Broken tf when starting rtabmap with icp_odometry and odom_guess

[thohemp]

Hello,

I want to use generate the robots odometry via rtabmap's icp_odometry node using the robots wheel odom as guess. This is the launch file:

<launch>

  <!-- Choose visualization -->
  <arg name="rviz" default="false" />
  <arg name="rtabmapviz" default="false" />

  <!-- If "hector" above is false, this option feeds wheel odometry to
       icp_odometry as guess ( to be more robust to corridor-like environments).
       If so, use original demo_mapping.bag containing wheel odometry! -->
  <arg name="odom_guess" default="true" />

  <!-- Example with camera or not -->
  <arg name="camera" default="true" />

  <!-- Limit lidar range if > 0 (has effect only when hector:=false) -->
  <arg name="max_range" default="0" />

  <!-- Point to Plane ICP? (has effect only when hector:=false) -->
  <arg name="p2n" default="true" />

  <!-- Use libpointmatcher for ICP? (has effect only when hector:=false) -->
  <arg name="pm" default="true" />

  <param name="use_sim_time" type="bool" value="True"/>

  <!-- If argument "hector" is false, we use rtabmap's icp odometry to generate odometry for us -->
  <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
     <remap from="scan"      to="/scan"/>
     <remap from="odom"      to="/scanmatch_odom"/>
     <remap from="odom_info"      to="/rtabmap/odom_info"/>

     <param name="frame_id"        type="string" value="base_footprint"/>

     <param if="$(arg odom_guess)" name="odom_frame_id"   type="string" value="icp_odom"/>
     <param if="$(arg odom_guess)" name="guess_frame_id"  type="string" value="odom"/>

     <param name="Icp/VoxelSize"     type="string" value="0.05"/>
     <param name="Icp/RangeMax"      type="string" value="$(arg max_range)"/>
     <param name="Icp/Epsilon"       type="string" value="0.001"/>
     <param unless="$(arg odom_guess)" name="Icp/MaxTranslation" type="string" value="0"/> <!-- can be set to reject large ICP jumps -->
     <param if="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="false"/>
     <param if="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="5"/>
     <param if="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
     <param unless="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="false"/>
     <param unless="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="0"/>
     <param unless="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0"/>
     <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
     <param name="Icp/PM"             type="string" value="$(arg pm)"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
     <param name="Icp/PMOutlierRatio" type="string" value="0.85"/>
     <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>

  <group ns="rtabmap">
    <node if="$(arg camera)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
   <remap from="rgb/image"        to="/xtion/rgb/image_rect_color"/>
      <remap from="depth/image"      to="/xtion/depth_registered/image_raw"/>
      <remap from="rgb/camera_info"  to="/xtion/rgb/camera_info"/>
      <remap from="rgbd_image"       to="rgbd_image"/> <!-- output -->
    </node>

    <!-- SLAM -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id" type="string" value="base_footprint"/>

      <param name="subscribe_rgb"   type="bool" value="false"/>
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_rgbd"  type="bool" value="$(arg camera)"/>
      <param name="subscribe_scan"  type="bool" value="true"/>

      <remap from="scan" to="/scan"/>

      <remap  from="odom" to="/scanmatch_odom"/>
      <param  name="subscribe_odom_info" type="bool" value="true"/>

      <!-- RTAB-Map's parameters -->
      <param name="Reg/Strategy"       type="string" value="1"/>    <!-- 0=
, 1=ICP, 2=Visual+ICP -->
      <param name="Reg/Force3DoF"      type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"    type="string" value="true"/>
      <param name="Icp/CorrespondenceRatio"  type="string" value="0.2"/>
      <param name="Icp/VoxelSize"            type="string" value="0.05"/>
      <param name="Icp/RangeMax"             type="string" value="$(arg max_range)"/>
      <param name="Grid/RangeMax"            type="string" value="$(arg max_range)"/>
    </node>

  </group>
</launch>

Both nodes start and without error, but it results in a missing map tf:

  | /robot_pose_publisher | Could not find a connection between 'map' and 'odom' because they are not part of the same tree.Tf has two or more unconnected trees.

[matlabbe]

It is like rtabmap node is not running. Does it show warnings each 5 seconds about not able to synchronize input topics?

[thohemp]

No, there are no warnings. This is from the standard log output:

[ INFO] [1617865606.652304465]: Odom: ratio=0.982301, std dev=0.001629m|0.001629rad, update time=0.002295s [ INFO] [1617865606.722559689]: Odom: ratio=0.982036, std dev=0.001787m|0.001787rad, update time=0.005420s [ INFO] [1617865606.787609240]: Odom: ratio=0.978979, std dev=0.001476m|0.001476rad, update time=0.003078s [ INFO] [1617865606.852523816]: Odom: ratio=0.982036, std dev=0.001485m|0.001485rad, update time=0.002267s [ INFO] [1617865606.918608403]: Odom: ratio=0.981818, std dev=0.001507m|0.001507rad, update time=0.001560s [ INFO] [1617865606.988248273]: Odom: ratio=0.982036, std dev=0.001602m|0.001602rad, update time=0.001779s [ INFO] [1617865607.050915231]: Odom: ratio=0.976119, std dev=0.001782m|0.001782rad, update time=0.001553s [ INFO] [1617865607.118828921]: Odom: ratio=0.973054, std dev=0.001804m|0.001804rad, update time=0.002707s [ INFO] [1617865607.187879573]: Odom: ratio=0.981928, std dev=0.001702m|0.001702rad, update time=0.002402s [ INFO] [1617865607.252143863]: Odom: ratio=0.978916, std dev=0.001636m|0.001636rad, update time=0.002390s [ INFO] [1617865607.318148109]: Odom: ratio=0.985119, std dev=0.001613m|0.001613rad, update time=0.001727s [ INFO] [1617865607.386088197]: Odom: ratio=0.982196, std dev=0.001680m|0.001680rad, update time=0.001349s [ INFO] [1617865607.452474076]: Odom: ratio=0.982143, std dev=0.001777m|0.001777rad, update time=0.001919s [ INFO] [1617865607.521183203]: Odom: ratio=0.985030, std dev=0.001613m|0.001613rad, update time=0.004207s [ INFO] [1617865607.585607087]: Odom: ratio=0.978916, std dev=0.001411m|0.001411rad, update time=0.001464s

[ INFO] [1617865607.651256076]: Odom: ratio=0.982249, std dev=0.001581m|0.001581rad, update time=0.001699s [ INFO] [1617865607.717716455]: Odom: ratio=0.985075, std dev=0.001442m|0.001442rad, update time=0.001505s [ INFO] [1617865607.786033679]: Odom: ratio=0.981982, std dev=0.001702m|0.001702rad, update time=0.002055s [ INFO] [1617865607.851313601]: Odom: ratio=0.982090, std dev=0.001443m|0.001443rad, update time=0.001855s [ INFO] [1617865607.918231860]: Odom: ratio=0.985163, std dev=0.001506m|0.001506rad, update time=0.001482s [ INFO] [1617865607.987730474]: Odom: ratio=0.982196, std dev=0.001602m|0.001602rad, update time=0.002371s [ INFO] [1617865608.052019849]: Odom: ratio=0.979167, std dev=0.001559m|0.001559rad, update time=0.001787s [ INFO] [1617865608.119064880]: Odom: ratio=0.981928, std dev=0.001588m|0.001588rad, update time=0.001857s [ INFO] [1617865608.190515979]: Odom: ratio=0.982143, std dev=0.001710m|0.001710rad, update time=0.003018s [ INFO] [1617865608.253483224]: Odom: ratio=0.982301, std dev=0.001635m|0.001635rad, update time=0.003642s [ INFO] [1617865608.319068621]: Odom: ratio=0.982249, std dev=0.001659m|0.001659rad, update time=0.001462s [ INFO] [1617865608.384223700]: Odom: ratio=0.978788, std dev=0.001685m|0.001685rad, update time=0.001796s [ INFO] [1617865608.453781710]: Odom: ratio=0.979290, std dev=0.001778m|0.001778rad, update time=0.004317s [ INFO] [1617865608.517989112]: Odom: ratio=0.982558, std dev=0.001810m|0.001810rad, update time=0.001664s [ INFO] [1617865608.588280704]: Odom: ratio=0.985075, std dev=0.001543m|0.001543rad, update time=0.003443s

[ INFO] [1617865608.653739214]: Odom: ratio=0.976190, std dev=0.001576m|0.001576rad, update time=0.004277s [ INFO] [1617865608.719160193]: Odom: ratio=0.981651, std dev=0.001593m|0.001593rad, update time=0.002142s [ INFO] [1617865608.788409453]: Odom: ratio=0.979532, std dev=0.001593m|0.001593rad, update time=0.003807s [ INFO] [1617865608.851882899]: Odom: ratio=0.982301, std dev=0.001490m|0.001490rad, update time=0.001551s [ INFO] [1617865608.917946688]: Odom: ratio=0.975976, std dev=0.001439m|0.001439rad, update time=0.001575s [ INFO] [1617865608.985846731]: Odom: ratio=0.982609, std dev=0.001558m|0.001558rad, update time=0.002017s [ INFO] [1617865609.052030354]: Odom: ratio=0.981928, std dev=0.001376m|0.001376rad, update time=0.002514s [ INFO] [1617865609.119081582]: Odom: ratio=0.985251, std dev=0.001470m|0.001470rad, update time=0.002702s [ INFO] [1617865609.186437827]: Odom: ratio=0.982143, std dev=0.001813m|0.001813rad, update time=0.002671s [ INFO] [1617865609.251388602]: Odom: ratio=0.982353, std dev=0.001493m|0.001493rad, update time=0.001864s [ INFO] [1617865609.318291089]: Odom: ratio=0.981982, std dev=0.001611m|0.001611rad, update time=0.001720s [ INFO] [1617865609.389247146]: Odom: ratio=0.985030, std dev=0.001751m|0.001751rad, update time=0.001753s [ INFO] [1617865609.455009003]: Odom: ratio=0.976331, std dev=0.001358m|0.001358rad, update time=0.004290s [ INFO] [1617865609.524283754]: Odom: ratio=0.976119, std dev=0.001496m|0.001496rad, update time=0.004613s [ INFO] [1617865609.586614169]: Odom: ratio=0.984756, std dev=0.001335m|0.001335rad, update time=0.003165s

I can see the the rtabmap node in the rqt graph:

rostopic bw /rtabmap/mapData 
subscribed to [/rtabmap/mapData]
average: 637.43KB/s
    mean: 359.63KB min: 358.52KB max: 360.74KB window: 2
average: 507.09KB/s
    mean: 359.92KB min: 358.52KB max: 360.74KB window: 3
average: 459.87KB/s
    mean: 359.94KB min: 358.52KB max: 360.74KB window: 4
average: 435.82KB/s
    mean: 360.17KB min: 358.52KB max: 361.10KB window: 5
average: 401.17KB/s
    mean: 360.17KB min: 358.52KB max: 361.10KB window: 5

And echo the topic gives me actually output data.

[matlabbe]

rtabmap node is working, tf map->icp_odom should have been published. You are using use_sim_time=true, make sure you launch with --clock your bag. Do you see something with:

rosrun tf tf_echo map icp_odom
rosrun tf tf_monitor

[thohemp]

You just pointed out the root of the problem. I used use_sim_time=true on a real robot. Setting it to false solved it.

Thanks for your help!

[thohemp]

Sorry for bringing this back up. Mapping works fine, but I faces tf errors when I tried to use the navigation stack. I modified the launch file to set goals using rviz by adding

      <param name="use_action_for_goal" type="bool" value="true"/>
      <remap from="move_base" to="/move_base"/>

When I now selected a 2d navigation goal in rviz it shows:

Could not get local plan (compute velocity function)
Global Frame: odom Plan Frame size 146: map
Extrapolation  Error: Lookup would require extrapolation into the future.  Requested  time 1618236676.892129422 but the latest data is at time  1618236676.738440990, when looking up transform from frame [odom] to  frame [map]
Could not get local plan (compute velocity function)
Global Frame: odom Plan Frame size 146: map
Extrapolation  Error: Lookup would require extrapolation into the future.  Requested  time 1618236676.592079407 but the latest data is at time  1618236676.405219316, when looking up transform from frame [odom] to  frame [map]

I don't find the reason why this is happening. All nodes except rviz are running on the same machine. The rviz machine is synchronized with NTP.

tf_monitor between odom and map outputs:

RESULTS: for odom to map
Chain is: odom -> icp_odom -> map
Net delay     avg = 0.0299466: max = 0.189169

Frames:
Frame: icp_odom published by unknown_publisher Average Delay: -0.0979914 Max Delay: 0
Frame: odom published by unknown_publisher Average Delay: 0.126581 Max Delay: 0.128093

All Broadcasters:
Node: unknown_publisher 122.741 Hz, Average Delay: -0.00496997 Max Delay: 0.128093
Node: unknown_publisher(static) 1e+08 Hz, Average Delay: 0 Max Delay: 0

[matlabbe]

It is strange that odometry publishes at 15 Hz but has a delay of 146 ms. I would suspect the stamp set in scan topic has a delay of ~140 ms. Odometry publishes odom topic and TF with same stamp than the received scan topic.

[thohemp]

When I use the slam_kartopackage that uses the same laser scan it works fine:

So it shouldn't be an issue with the scan topic afaik. Is the icp_odoom maybe too slow? I had to set Icp/PointToPlane=false to avoid getting this warning:

RegistrationIcp.cpp:983::computeTransformationImpl() ICP PointToPlane ignored for 2d scans with PCL registration (some crash issues). Use libpointmatcher (Icp/PM) or disable Icp/PointToPlane to avoid this warning.

[thohemp]

@matlabbe You are right about the delay of the scan topics: average delay: 0.123

While I don't understand where this is coming from, other slam packages don't have any problem at dealing this this. Beside slam_karto I also tested acml (https://github.com/ros-planning/navigation/tree/noetic-devel/amcl) and it worked out of the box. With rtabmap the issue only occures when I send a goal to move_base. It will then throw the error here: https://github.com/ros-planning/navigation/blob/noetic-devel/base_local_planner/src/goal_functions.cpp#L163

I searched the internet for similar errors and I found a workaround by setting the global_frame for the local_costmap from odom to map. This enables setting goals over rviz, but raises other issues. If you have any other idea how to approach this I would really appreciate as I still don't really get why it this problem only appears with rtabmap in combination with sending 2D goals.

[matlabbe]

If you use icp_odom instead of odom in the local_costmap?

[thohemp]

Makes totally sense, I don't know why I didn't come up with this. Thanks.