Navigation Stack
This page gives useful information on the navigation stack that runs on Summit XL such as navigation modes, parameter descriptions, and debugging.
Launching the simulation launch files by default bring 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
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="-16.910"/>
<arg name="y_init_pose" default="-0.777"/>
<arg name="z_init_pose" default="0.0"/>
<arg name="a_init_pose" default="-1.597"/>
<arg name="debug_mode" default="false"/>
<arg name="slam" default="false"/>
NOTE: The arg launch_robot_localization fuses IMU data with odometry to theoretically provide a more accurate robot pose in the odom frame (i.e. less drift, more info on odom frame here).
Arguments to irlab_sim_summit_xls_complete.launch:
<!-- GAZEBO -->
<arg name="ros_planar_move_plugin" default="true"/> <!-- disables gazebo ros_control plugin -->
<arg name="gazebo_world" default="$(find icclab_summit_xl)/worlds/summit_xl_office.world"/>
<arg name="gazebo_gui" default="true"/>
<!-- NAV STACK PARAMS -->
<arg name="launch_rviz_nav" default="true"/>
<arg name="map_file_a" default="willow_garage/willow_garage.yaml"/>
<arg name="localization_robot_a" default="false"/>
<arg name="gmapping_robot_a" default="false"/>
<arg name="move_base_robot_a" default="true"/>
<arg name="amcl_and_mapserver_a" default="true"/>
<arg name="slam" default="false"/>
The argument omni determines the drive mode of the robot. By default it is set to true which enables omnidirectional drive by configuring the teb planner and executing the command below. Mode 2 corresponds to omni kinematics while mode 1 is diff.
rosservice call /summit_xl/set_mode "mode: 2"
Run the following command to manually control the robot with a keyboard (ROS wiki source).
ITR:
roslaunch icclab_summit_xl irlab_summit_xl_complete.launch teleop:=true
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_summit_xl_amcl.launch
- By default the world brought up is
worlds/summit_xl_office.worldspecified by thegazebo_worldargument (world is same as the willow garage) - Set
gazebo_gui:=falseto disable the bring-up of thegazebo_guinode (decreases the computational load of gazebo)
ITR:
roslaunch icclab_summit_xl irlab_summit_xl_amcl.launch sim:=false
- By default the brought up map is
icclab/icclab_latest_map.yamlspecified by themap_fileargument - 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, anda_init_posewhich 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).
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_summit_xl_gmapping.launch
- By default the world brought up is
worlds/summit_xl_office.worldspecified by thegazebo_worldargument (world is same as the willow garage) - Set
gazebo_gui:=falseto disable the bring-up of thegazebo_guinode.
ITR:
roslaunch icclab_summit_xl irlab_summit_xl_gmapping.launch sim:=false
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.
Saving map with map_server:
rosrun map_server map_saver map:=/summit_xl/map -f ~/catkin_ws/src/icclab_summit_xl/maps/icclab/map
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 cartographer.
Sim:
roslaunch icclab_summit_xl irlab_summit_xl_cartographer.launch
ROS_NAMESPACE=summit_xl roslaunch cartographer_ros summit_xl_slam.launch use_sim_time:=true launch_rviz:=false
ITR:
roslaunch icclab_summit_xl irlab_summit_xl_cartographer.launch sim:=false
ROS_NAMESPACE=summit_xl roslaunch cartographer_ros summit_xl_slam.launch use_sim_time:=false launch_rviz:=false
NOTE: Cartographer is currently not installed on the summit and needs to launched on a separate machine.
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_slam.launch debug_mode:=true
@REMOTE_PC:$ rviz -d ~/catkin_ws/src/icclab_summit_xl/rviz/irlab_summitxl.rviz
A blog post was written to expand on the issues discussed below.
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.
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.yamlthe parameterinf_is_validwas 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 truemove_base/obstacle_2d_layer/scan_rear/inf_is_valid true
- In
irlab_sweep2scan.launchthe parameterrange_maxwas changed from 40.0 to 6.0pointcloud_to_laserscan_front/range_max 6.0pointcloud_to_laserscan_rear/range_max 6.0
- In
costmap_common_params.yamlthe parameterraytrace_rangewas set higher than the max valid measurement returned by the sensormove_base/obstacle_2d_layer/raytrace_range 6.1
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 clears the obstacle_3d_layer whenever the planner fails to find a plan. This commit corresponds to the changes made. Below are screenshots of the solution working in simulation.
To further mitigate this issue, specifically when the planner does indeed find a valid plan, the Spatio-Temporal Voxel Layer was implemented to replace the default one. This improved voxel grid package has a voxel_decay parameter which clears voxels and hence costmap overtime. Commit d4aae66 corresponds to the modifications made to incorporate this package.
ISSUE: Global planner was occasionally planning in unknown areas.
SOLUTION: The config file global_planner_params.yaml was added to move_base.launch to replace the default Navfn global planner with GlobalPlanner. The parameter allow_unknown was set to false and in costmap_common_params.yaml the parameter track_unknown_space was set to true.
Another modification is the obstacle_2d_layer was removed from the global costmap due to the layer's high raytrace_range (set to properly clear 2D obstacles in local costmap) which was wrongly setting "unknown" space to "free".
ISSUE: Planner would attempt to only find one plan before executing recovery behavior. Hence, the robot would give up finding a plan very quickly.
SOLUTION: The following commit 93eea82 solves the issue. The key parameters in the file move_base_params.yaml to tune to solve this issue are the following: max_planning_retries, planner_frequency, planner_patience, and controller_patience. Their descriptions are described in the move_base wiki.
ISSUE: Data from the IMU was very unreliable. When the robot was stationary acceleration values were non-zero. Used rqt_plot to produce image showing inaccurate imu data.
SOLUTION: We sent our imu to Robotnik. They fixed it and sent it back.
The rear Scanse LIDAR mostly outputs inf values, hence very few valid data points. It should be replaced by another LDS-01 (the current sensor on the front).
The scan_combined topic does not seem to include data from the front LIDAR even though the laserscan_multi_merger node is subscribed to the topic scan_front.