Navigation Stack

Welcome to the Navigation Stack wiki!

This page gives useful information on the navigation stack that runs on Summit XL such as navigation modes, parameter descriptions, and debugging.

Viewing Navigation with RViz

Launching the simulation launch files automatically brings up RViz with the navigation config. For ITR execute the following command to bring up RViz.

rviz -d ~/catkin_ws/src/icclab_summit_xl/rviz/irlab_summitxl.rviz

Navigation Modes

NOTE: currently only differential driving works and has been tested properly in autonomous mode (velocity limits of planner seem to be too low for omni driving to be possible). The following parameter in teb_local_planner_steel_params.yaml determines the differential driving behavior: weight_kinematics_nh: 5000. Also note the default argument omni:=true in irlab_summit_xl_complete.launch sets everything in the stack for omni-drive even though actual driving behavior is diff. The argument is left as motivation for proper omni driving to be implemented in the future.

Arguments to irlab_summit_xl_complete.launch: Below are all the possible navigation stack arguments that can be set. Their descriptions are given within the wiki.

        <arg name="teleop" default="false"/> 
        <arg name="launch_robot_localization" default="false"/>
        <arg name="omni" default="true"/> 
        <arg name="launch_move_base" default="false"/>
        <arg name="launch_gmapping" default="false"/>
        <arg name="launch_amcl_and_mapserver" default="false"/>
        <arg name="map_file" default="icclab/icclab_latest_map.yaml"/>
        <arg name="x_init_pose" default="0"/>
        <arg name="y_init_pose" default="0"/>
        <arg name="z_init_pose" default="0"/>
        <arg name="a_init_pose" default="0"/>
        <arg name="debug_mode" default="false"/>

Teleop

Run the following command to manually control the robot with a keyboard (ROS wiki source).

roslaunch icclab_summit_xl irlab_summit_xl_complete.launch teleop:=true 

AMCL

Run the following command to drive the robot by sending navigation goals in a pre-built map using AMCL and map_server.

Sim:

roslaunch icclab_summit_xl irlab_sim_summit_xls_complete.launch move_base_robot_a:=true amcl_and_mapserver_a:=true

ITR:

roslaunch icclab_summit_xl irlab_summit_xl_complete.launch launch_move_base:=true launch_amcl_and_mapserver:=true

• By default the brought up map is icclab/icclab_latest_map.yaml specified by the map_file argument
• Maps are found and should be saved in the icclab_summit_xl/maps directory
• Optional arguments are x_init_pose, y_init_pose, z_init_pose, and a_init_pose which sets the initial pose of the robot on the map (can also be done with RViz 2D Pose Estimate tool).

NOTE: AMCL only accepts LaserScan messages from a single topic and a single frame which is problematic with a 2-lidar setup. To solve this issue the laserscan_multi_merger node from ira_laser_tools is used to merge the laser scan topics to scan_combined and to the frame summit_xl_base_link (the relay ROS node is insufficient for this issue since it just 'relays' two topics to one topic).

GMapping

Run the following command to do SLAM (Simultaneous Localization and Mapping) with GMapping. Maps should be saved in icclab_summit_xl/maps.

Sim:

roslaunch icclab_summit_xl irlab_sim_summit_xls_complete move_base_robot_a:=true gmapping_robot_a:=true

ITR:

roslaunch icclab_summit_xl irlab_summit_xl_complete.launch launch_move_base:=true launch_gmapping:=true

NOTE: Like AMCL, GMapping requires a single LaserScan topic + frame message so the laserscan_multi_merger node is again used here which also fixes the sweep lidar angle increment + GMapping issue.

Cartographer

Cartographer is another SLAM algorithm (like GMapping) made by Google. The forked repository is here which contains gathered notes and useful information while experimenting with the cartographer.

Sim:

roslaunch icclab_summit_xl irlab_sim_summit_xls_complete move_base_robot_a:=true
ROS_NAMESPACE=summit_xl roslaunch cartographer_ros summit_xl_slam.launch use_sim_time:=true

Tuning/Debugging

NOTE: Tuning parameters should be done in a systematic and controlled way. It is crucial to start experimenting with the system defaults and then tune parameters individually according to needs. Documentation of parameters should be added as comments in configuration files.

Effective way of tuning parameters: rosrun rqt_reconfigure rqt_reconfigure

Changing Logger level: rosrun rqt_logger_level rqt_logger_level

Good tuning guides:

• Here is a very good Navigation Tuning Guide. Specifically look at the Odometry section which explains how to do sanity checks of the odometry performance.

• This is a good reference for tuning the teb local planner

• A MUST READ -> ROS Navigation Tuning Guide

Costmap debug argument: Add as an argument when bringing up robot debug_mode:=true which will publish voxel map. Click checkbox next to Voxel Grid topic in RViz to view markers. Example shown below.

@ROBOT:$ roslaunch icclab_summit_xl irlab_summit_xl_complete.launch \ 
> launch_move_base:=true \
> launch_amcl_and_mapserver:=true \
> map_file:=icclab/icclab_latest_map.yaml \
> debug_mode:=true
@REMOTE_PC:$ rviz -d ~/catkin_ws/src/icclab_summit_xl/rviz/irlab_summitxl.rviz

Fixed Issues

laserscan_multi_merger node

ISSUE: Node did not subscribe to LaserScan topics when it was in the same launch file as bring-up of sensors.

SOLUTION: The ROS package timed_roslaunch is used to delay the bring-up of the laserscan_multi_merger node by 5 seconds which allows enough time for LaserScan topics to be published and ready to be subscribed by the laserscan_multi_merger node.

Clearing 2D obstacle layer (ObstacleLayer) on costmap

ISSUE: "Ghost obstacles" (as the online community likes to refer them as) are points on the costmap that indicate no-longer-existing obstacles. This issue was seen with the Scanse Sweep LIDARs.

SOLUTION: The following parameters were modified:

• In costmap_common_params.yaml the parameter inf_is_valid was set to true (should be set for all sensors that return inf for invalid measurements)
  • move_base/obstacle_2d_layer/scan_front/inf_is_valid true
  • move_base/obstacle_2d_layer/scan_rear/inf_is_valid true
• In irlab_sweep2scan.launch the parameter range_max was changed from 40.0 to 6.0
  • pointcloud_to_laserscan_front/range_max 6.0
  • pointcloud_to_laserscan_rear/range_max 6.0
• In costmap_common_params.yaml the parameter raytrace_range was set higher than the max valid measurement returned by the sensor
  • move_base/obstacle_2d_layer/raytrace_range 6.1

Clearing 3D obstacle layer (VoxelLayer) on costmap

ISSUE: The robot sometimes had an issue with clearing 3-D obstacles in the costmap specifically at short distances from the front depth camera. This distance was observed to hover around the camera's blind spot.

SOLUTION: Recovery behavior was setup such that it would clear the obstacle_3d_layer whenever the planner would fail to find a plan. This commit corresponds to the changes made. Below are screenshots of the solution working in simulation.

Unresolved Issues

Planning in unknown areas

Robot sometimes plans in areas that are unknown.