Rtabmap_ros on (Robotnik) Summit_XL with wheel odometry, laser scan and RGB-D camera

[grafoteka]

Hi all!

I'm trying to get a simulation with the robot from Robotnik, Summit-XL and the Rtabmap_ros package.

I have followed this question: rtabmap_ros on p3dx with wheel odometry and kinect data

But still having problems.

At this point, I can launch the robot, teleoperate it and can map with the lidar, the odom and gmapping.




The problem is when I launch rtabmap_ros. Now, I'm using this file:

<launch>
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
         
         
         
          <remap from="odom" to="/summit_xl_a_odom"/>
          <remap from="scan" to="/summit_xl_a/front_laser/scan"/>
          <remap from="rgb/image"       to="/summit_xl_a/front_rgbd_camera/rgb/image_raw"/>
          <remap from="depth/image"     to="/summit_xl_a/front_rgbd_camera/depth/image_raw"/>
          <remap from="rgb/camera_info" to="/summit_xl_a/front_rgbd_camera/rgb/camera_info"/>
         

         
         
         
         
         
         
           
         
         
         
         
         
    </node>
  </group>
</launch>

And these are my topics:

jorge@asus:~$ rostopic list
/clicked_point
/clock
/diagnostics
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/initialpose
/move_base_simple/goal
/rosout
/rosout_agg
/summit_xl_a/cmd_vel
/summit_xl_a/docker/cmd_vel
/summit_xl_a/front_laser/scan
/summit_xl_a/front_rgbd_camera/depth/camera_info
/summit_xl_a/front_rgbd_camera/depth/image_raw
/summit_xl_a/front_rgbd_camera/depth/points
/summit_xl_a/front_rgbd_camera/parameter_descriptions
/summit_xl_a/front_rgbd_camera/parameter_updates
/summit_xl_a/front_rgbd_camera/rgb/camera_info
/summit_xl_a/front_rgbd_camera/rgb/image_raw
/summit_xl_a/front_rgbd_camera/rgb/image_raw/compressed
/summit_xl_a/front_rgbd_camera/rgb/image_raw/compressed/parameter_descriptions
/summit_xl_a/front_rgbd_camera/rgb/image_raw/compressed/parameter_updates
/summit_xl_a/front_rgbd_camera/rgb/image_raw/compressedDepth
/summit_xl_a/front_rgbd_camera/rgb/image_raw/compressedDepth/parameter_descriptions
/summit_xl_a/front_rgbd_camera/rgb/image_raw/compressedDepth/parameter_updates
/summit_xl_a/front_rgbd_camera/rgb/image_raw/theora
/summit_xl_a/front_rgbd_camera/rgb/image_raw/theora/parameter_descriptions
/summit_xl_a/front_rgbd_camera/rgb/image_raw/theora/parameter_updates
/summit_xl_a/imu/data
/summit_xl_a/imu/data/accel/parameter_descriptions
/summit_xl_a/imu/data/accel/parameter_updates
/summit_xl_a/imu/data/bias
/summit_xl_a/imu/data/rate/parameter_descriptions
/summit_xl_a/imu/data/rate/parameter_updates
/summit_xl_a/imu/data/yaw/parameter_descriptions
/summit_xl_a/imu/data/yaw/parameter_updates
/summit_xl_a/joint_states
/summit_xl_a/map
/summit_xl_a/map_metadata
/summit_xl_a/move_base/cmd_vel
/summit_xl_a/odometry/filtered_odom
/summit_xl_a/pad_teleop/cmd_vel
/summit_xl_a/robotnik_base_control/cmd_vel
/summit_xl_a/robotnik_base_control/odom
/summit_xl_a/robotnik_base_control/parameter_descriptions
/summit_xl_a/robotnik_base_control/parameter_updates
/summit_xl_a/set_pose
/summit_xl_a/slam_gmapping/entropy
/summit_xl_a/summit_xl_a_front_ptz_camera/camera_info
/summit_xl_a/summit_xl_a_front_ptz_camera/image_raw
/summit_xl_a/summit_xl_a_front_ptz_camera/image_raw/compressed
/summit_xl_a/summit_xl_a_front_ptz_camera/image_raw/compressed/parameter_descriptions
/summit_xl_a/summit_xl_a_front_ptz_camera/image_raw/compressed/parameter_updates
/summit_xl_a/summit_xl_a_front_ptz_camera/image_raw/compressedDepth
/summit_xl_a/summit_xl_a_front_ptz_camera/image_raw/compressedDepth/parameter_descriptions
/summit_xl_a/summit_xl_a_front_ptz_camera/image_raw/compressedDepth/parameter_updates
/summit_xl_a/summit_xl_a_front_ptz_camera/image_raw/theora
/summit_xl_a/summit_xl_a_front_ptz_camera/image_raw/theora/parameter_descriptions
/summit_xl_a/summit_xl_a_front_ptz_camera/image_raw/theora/parameter_updates
/summit_xl_a/summit_xl_a_front_ptz_camera/parameter_descriptions
/summit_xl_a/summit_xl_a_front_ptz_camera/parameter_updates
/summit_xl_a/summit_xl_hw/emergency_stop
/summit_xl_a/twist_marker
/tf
/tf_static

The problem with this, is that [/rtabmap/rtabmap] and  [/summit_xl_a/slam_gmapping] are trying to write on [summit_xl_a_odom]

This is what roswtf says:
Found 2 error(s).

ERROR TF re-parenting contention:
 * reparenting of [summit_xl_a_odom] to [map] by [/rtabmap/rtabmap]
 * reparenting of [summit_xl_a_odom] to [/map] by [/summit_xl_a/slam_gmapping]

ERROR TF multiple authority contention:
 * node [/rtabmap/rtabmap] publishing transform [summit_xl_a_odom] with parent [map] already published by node [/summit_xl_a/slam_gmapping]
 * node [/summit_xl_a/slam_gmapping] publishing transform [summit_xl_a_odom] with parent [/map] already published by node [/rtabmap/rtabmap]

- - - - - - - - - -
The other option is to don't use gmapping.

There are no problem with the launchers, but the odom topic is not with all the frames and TFs





And this is the error

[ WARN] (2018-09-18 12:38:53.491) Rtabmap.cpp:2371::process() Rejecting all added loop closures (3) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 4.197081 (edge 40->41, type=0, abs error=0.360965, stddev=0.086004). The maximum error ratio parameter is 1.000000 of std deviation.
[ WARN] (2018-09-18 12:38:53.491) Rtabmap.cpp:2375::process() Loop closure 156->20 rejected!
[ WARN] (2018-09-18 12:38:53.492) Rtabmap.cpp:2375::process() Loop closure 156->30 rejected!
[ WARN] (2018-09-18 12:38:53.492) Rtabmap.cpp:2375::process() Loop closure 156->140 rejected!

[matlabbe]

Hi,

You cannot use gmapping/amcl at the same time than rtabmap. There will be a TF conflict between /map -> /odom as you observed. I downloaded/built summit_xl_sim package and tuned some parameters. For this example, I modified directly summit_xl_one_robot.launch by adding rtabmap under namespace of the robot based on the Kinect+Odometry+2D laser example:

<!-- BEGIN ROBOT-->
<group ns="$(arg id_robot)">

  <!-- rtabmap  -->
  <node pkg="rtabmap_ros" name="rtabmap" type="rtabmap" args="-d" output="screen">
        <param name="subscribe_depth" value="true"/>
        <param name="subscribe_scan" value="true"/>
        
        <param name="frame_id" value="$(arg id_robot)_base_footprint"/>
        <param name="map_frame_id" value="$(arg id_robot)_map"/>
        <param name="subscribe_scan" value="true"/>

        <remap from="odom" to="/$(arg id_robot)/odometry/filtered_odom"/>
        <remap from="scan" to="/$(arg id_robot)/front_laser/scan"/>
        <remap from="rgb/image" to="/$(arg id_robot)/front_rgbd_camera/rgb/image_raw"/>
        <remap from="rgb/camera_info" to="/$(arg id_robot)/front_rgbd_camera/rgb/camera_info"/>
        <remap from="depth/image" to="/$(arg id_robot)/front_rgbd_camera/depth/image_raw"/>

        <remap from="grid_map" to="map"/>

        <param name="queue_size" type="int" value="10"/>

        <!-- RTAB-Map's parameters -->
        <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
        <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
        <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
        <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
        <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
        <param name="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->
        <param name="Reg/Force3DoF"             type="string" value="true"/>
        <param name="Vis/MinInliers"            type="string" value="12"/>

         <!-- Custom summit_xl parameters -->
        <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.15"/>
        <param name="Icp/MaxTranslation" type="string" value="0.5"/>
  </node>
</group>

As odometry is drifting quite a lot, I relaxed some Icp parameters (see Custom summit_xl parameters in xml above).

Launch:

$ roslaunch summit_xl_sim_bringup summit_xl_complete.launch move_base_robot_a:=true

// Just for convenience to teleop by keyboard:
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/summit_xl_a/pad_teleop/cmd_vel

cheers,
Mathieu

[jdeleon]

Hi Mathieu!

Thank you for your help!!

I have it working on my computer, but as you say, there is a lot of driffting. In my computer is even worse!



So, neither the map corresponds with the rtabmap data collected, and even if I try to close the loop, nothing corresponds...

These values of Icp are relaxed?

param name="Icp/MaxCorrespondenceDistance" type="string" value="0.15"
param name="Icp/MaxTranslation" type="string" value="0.5"

And how can I relax them more, increasing or deacrising?

Thank you,
Jorge

[matlabbe]

Note sure to understand why the 3D cloud map doesn't correspond to 2D occupancy grid, as it should. Can you share the database? For the ICP parameter, increasing Icp/MaxCorrespondenceDistance will make ICP corresponding points more far apart. So if odom error is large, we can increase it. ICP will then be able to recover from bad odom transforms, but final refined transformation will be less accurate.

cheers,
Mathieu

[jdeleon]

Hi Mathieu, sorry for this question, but what is the database?

cheers,
Jorge

[matlabbe]

It is saved by default in ~/.ros/rtabmap.db

[jdeleon]

Hi Mathieu!

sorry for this time without answer you, I had some problems with my computer and need to reinstall everything.

Here I send you the database that is from this path.



And this is the database you can download here: https://www.dropbox.com/s/dvs7evqq1gs1rvj/rtabmap.db?dl=0

Kinds regards,
Jorge

EDIT: I don't know why the image is not uploaded

[matlabbe]

Hi,

I think why the 3D map is not matching the 2D map is that in the rviz's Global Options, the Fixed Frame is set to summit_xl_a_odom as it should be map to correctly visualize rtabmap/MapCloud.

For the laser scan registration, we have set Icp/MaxCorrespondenceDistance to 0.15 because the odometry is drifting a lot. Thus the registrations are not as accurate as they could be with lower Icp/MaxCorrespondenceDistance. If rtabmap is built with libpointmatcher, we can use it to get better registrations. For example adding those following parameters:

<param name="Vis/MinInliers"    type="string" value="20"/> <!-- Increased from 12 as the environment (only geometric without any visual features) can cause wrong loop closure detections -->
<param name="Icp/PM"            type="string" value="true"/> <!-- Use libpointmatcher for ICP -->
<param name="Icp/PointToPlane"  type="string" value="true"/> <!-- USe point to plane ICP -->

I can get this final map:


Note how right how bad the visual images for loop closure detection. During the mapping, I've seen image not correctly rendered by the simulator, having some aliasing causing false features on the ground and that the opengl far clipping plane of the camera is too close (the light gray zone in the left pictures are the clipping plane, not actually a wall).

If you cannot tuned better your odometry, you may consider using rtabmap_ros/icp_odometry node for odometry instead or hector slam like in this example.

cheers,
Mathieu

[jdeleon]

Hi again Mathieu,

I have now make a test with turtlebot, and I think that is much better than with the summit_xl.

Those are the commands that I launch:

1.- roslaunch turtlebot_gazebo turtlebot_office.launch
2.- roslaunch rtabmap_ros demo_turtlebot_mapping.launch simulation:=true
3.- roslaunch rtabmap_ros demo_turtlebot_rviz.launch
4.- roslaunch turtlebot_teleop keyboard_teleop.launch



And here is the database: https://www.dropbox.com/s/44uq7t4hpmdaezh/rtabmap_turtlebot_willowgarage.db?dl=0

[matlabbe]

Yes, odometry looks a lot better. You may add the long-range lidar on it, as only using the kinect for a fake laser scan, it can be quite difficult in that kind of environment (without any visual features) for loop closure detection.

[grafoteka]

Hi Mathieu,

please, could you verify if the model that you have download from Robotnik doesn't have this error in the rotation.

Video1

Video2

Thank you!
Jorge

[matlabbe]

Hi,

I tried again the example of my previous post and the robot doesn't have odometry anymore and the model seems not having wheels anymore (move_base doesn't work too).


I used latest code from summit_xl_common and summit_xl_sim. To answer your question, there is just no odometry anymore in the default summit launch.

I tried adding the following to be able to generate the missing odometry topic/tf:

<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
     <remap from="scan"      to="/$(arg id_robot)/front_laser/scan"/>
     <remap from="odom"      to="/$(arg id_robot)/odometry/filtered_odom"/>
	  
     <param name="frame_id"        type="string" value="$(arg id_robot)_base_footprint"/>   
     <param name="odom_frame_id"        type="string" value="$(arg id_robot)_odom"/> 
     
     <param name="Icp/PointToPlane"  type="string" value="false"/>
     <param name="Icp/VoxelSize"     type="string" value="0.05"/>
     <param name="Icp/Epsilon"       type="string" value="0.001"/>
     <param name="Icp/PointToPlaneK"  type="string" value="5"/>
     <param name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
     <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
     <param name="Icp/PM"             type="string" value="false"/> 
     <param name="Icp/PMOutlierRatio" type="string" value="0.95"/>
     <param name="Odom/Strategy"        type="string" value="0"/>
     <param name="Odom/GuessMotion"     type="string" value="true"/>
     <param name="Odom/ResetCountdown"  type="string" value="0"/>
     <param name="Odom/ScanKeyFrameThr"  type="string" value="0.9"/>
</node>

But we cannot move the robot, so I cannot test more.

cheers,
Mathieu