Combining RTAB-Map with the navigation stack

[icolwell]

Hello There,
I have been using RTAB-Map to generate a map and localize within it. It is awesome! works really well.
My setup is a clearpath husky, hokuyo laser scanner, and a kinect.

I have two main questions/issues that have been bothering me:

Question 1:
I am trying to use the navigation stack with RTAB-Map. I am using a clearpath husky so I copied all the navigation configs from their gazebo amcl_demo and then simply remapped the RTAB-Map grid_map into the map topic for move_base navigation stack. I believe this is very similar to the turtlebot example:
http://wiki.ros.org/rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot

It worked, the global cost map was generated using grid_map and the local costmap was being created from the laser and kinect data. Now, the only thing that was strange was that the grid_map from RTAB-Map kept updating or something, it wasn't static. The following output kept being generated:

[ INFO] [1458945016.268542996]: Resizing costmap to 171 X 175 at 0.050000 m/pix

Why is the grid_map updating while in localization mode? I assumed RTAB-Map localization mode would simply load up the saved occupancy grid and try to place the robot in the saved static map. Maybe I am missing something.

Question 2:
When the laser data is fed into RTAB-Map in mapping mode it generates this weird shape behind the robot on grid_map (see below)

The shape is extending out directly behind the robot which is super strange because both the kinect and the laser are pointing 180 degrees the other way. The laser scanner has a FOV of 180 so I'm not sure why this shape is being generated. Also, it doesn't appear to actually have any effect on the grid_map. Basically, the shape does not clear out any free cells that were previously occupied or previously empty. Also, when we change the FOV setting of our laser scanner it changes the FOV of this strange shape, so it is definitely related to the laser scanner.
My best guess is that RTAB-Map is confused about the laser scanner config or settings or something.

Any clarification will be greatly appreciated!

Thanks!
Ian

[matlabbe]

Hi,

1) Which version do you use (binaries or latest from source)? Indeed, in localization mode and if "RGBD/OptimizeFromGraphEnd=false" (default), the map should not change unless a) the local map doesn't contain all nodes of the global map or b) current scan is added to map (probably your case).

a) when nodes are brought back from LTM to WM, the map should be optimized again. If you have "Rtabmap/TimeThr=0", this should not happen as all nodes will stay in WM. If it is set, in localization mode you can force to use all nodes from LTM with "Mem/InitWMWithAllNodes=true" (e.g., like demo_turtlebot_mapping.launch):

<param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/> 

b) the latest scan is added to map (clearing occupied cells in front of the robot if obstacles are gone), which would cause a map resize if the robot approaches the side of the grid map. I added a new issue to avoid regenerating/publishing maps if they have not changed:


2) This should be fixed by this commit (18 days ago): https://github.com/introlab/rtabmap/commit/152d47e62af3c19f9d3cc2f26cd15651351a372b
If you have already this commit, please let me know.

cheers,
Mathieu

[icolwell]

Hi Mathieu, thanks for the quick reply!

I was able to try your suggestions on the robot today but unfortunately no luck.

1) I changed "RGBD/OptimizeFromGraphEnd" from true to false and it seemed to help. It was still updating as you explained in b) but things are better now. I also saved the map in map saver and then served that into move_base and it worked pretty well for a static map. Only problem is the map saver saves the big shape as discussed in question 2.

2) I was building from source but hadn't updated for a while. I updated both rtabmap and rtabmap_ros and rebuilt but the shape was still there. Then I deleted ~/rtabmap and ~/catkin_ws/src/rtabmap_ros and followed the steps in the readme at https://github.com/introlab/rtabmap_ros and re-built it all. Still the strange shape appeared, I am using the following ROS package for the laser scanner: http://wiki.ros.org/hokuyo_node

I'm not sure how to proceed on question 2, is there a better way to "purge" rtabmap from the system to be 100% sure I am building the latest source?

Thanks for taking the time to answer my questions!

[matlabbe]

Hi Ian,

1) I added "map_negative_poses_ignored" (boolean) parameter to rtabmap node (see this commit). If you set this parameter to true and rtabmap is in localization mode, the generated map should not change anymore because of 1-b) (see my previous post).

2) Well, if you set "map_negative_poses_ignored" to true, this problem should not happen anymore as the last scan will not be added to the map. Otherwise, you can try to uncomment these lines to see if the unknown cells are filled in the wrong side (if laser scanner is rotating clockwise or counterclockwise).

cheers

[icolwell]

Hi Mathieu,

Good news and bad news. Good news is that when I uncommented those lines you mentioned, it worked great! The shape is gone now, looking much better. One thing that I thought of is that we actually have our laser scanner mounted upside down, could that be part of the problem leading to the generation of the shape?

Bad news is that the grid is still "morphing" every couple of seconds in localization mode. I noticed no new points appearing on the grid where there was previously free space but the grid is still being republished and looks slightly different every couple of seconds. I will post my launch file below, perhaps I am missing something obvious.

RTABMAP portion of the launch file:

		<!-- start the rtabmap node -->
		<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
			<!-- Remaps for RTABMAP -->
			<remap from="odom" to="$(arg odom_topic)" />
			<remap from="scan" to="$(arg scan_topic)" />

			<!-- RTABMAP parameters (http://wiki.ros.org/rtabmap_ros#rtabmap) -->
			<param name="frame_id" type="string" value="$(arg frame_id)" />
			<param name="subscribe_depth" type="bool" value="true" />
			<param name="subscribe_scan" type="bool" value="true" />

			<param name="RGBD/NeighborLinkRefining" type="string" value="true"/> <!-- Do odometry correction with consecutive laser scans -->

			<!-- Parameters used for flat surface environments -->
			<param name="Optimizer/Slam2D" type="string" value="true"/> <!-- Optimize on only 3 degrees of freedom (x, y, theta) -->
			<param name="Reg/Force3DoF" type="string" value="true"/> <!-- Force 3 degrees of freedom transform -->
			<param name="Reg/Strategy" type="string" value="2"/> <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->

			<!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
			<param if="$(arg localization)" name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
			<param unless="$(arg localization)" name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
			<param name="Mem/MapLabelsAdded" type="string" value="true"/> <!-- Add labels to the map -->

			<!-- Are these even RTABMAP params? -->
			<param name="rgb/image_transport"   type="string" value="compressed"/>
			<param name="depth/image_transport" type="string" value="compressedDepth"/>

			<!-- Localization mode -->
			<param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
			<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>

			<!-- Initialize the Working Memory with all nodes in Long-Term Memory.
				When false, it is initialized with nodes of the previous session -->
			<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>

			<!-- Prevent scan from updating map -->
			<param if="$(arg localization)" name="map_negative_poses_ignored" type="bool" value="true" />

			<!-- Maximum time allowed to update the map (ms) (0 means infinity). -->
			<param unless="$(arg localization)" name="Rtabmap/TimeThr" type="string" value="700" />
		</node>

Thanks!
Ian

[matlabbe]

Hi,

Yes, it is probably because the laser scanner is mounted upside down. The empty space is not filled in the good direction. In that case, "map_negative_poses_ignored" should be set to true. Maybe I should set "map_negative_poses_ignored" default to true (I opened an issue).

I tested with your parameters and I reproduced the problem (I hope it is). You have to set "map_filter_radius" to 0 in order to disable node filtering when creating maps:

<param name="map_filter_radius"  type="double" value="0"/>

When the robot is moving, it seems it is not always the same nodes that are filtered (maybe because of numerical precision), so the generated grid maps can slightly be different. I tested with demo_mapping.bag, your parameters (in localization mode) and "map_filter_radius=0", and the map doesn't change anymore.

Hope it will fix your problem,
cheers