fall back to wheel odometry, reset icp_odometry and keep mapping

[Alexander]

On my robot I have a 2D lidar, L515 and a wheel odometry sensor. I did a few tests using visual and wheel odometry. Unfortunatelly visual odometry gets lost during relatively fast robot rotations. At the same time our wheel odometry is not perfect either. I would like to use odometry from icp_odometry. When it gets lost I want rtabmap to fall back to wheel odometry, reset icp_odometry and  keep mapping temporary using wheel odometry without creating a new map until icp_odometry is back. When I want rtabmap to switch back to icp_odometry.

Is this approach possible via rtabmap configuration (ROS)? Or should I create a custom odometry nodelet which would contain the mentioned logic?

[matlabbe]

You could feed the wheel odometry to icp_odometry as guess to improve icp robustness:

<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
   <remap from="scan"      to="/scan"/>
   <remap from="odom"      to="/icp_odom"/>
	  
   <param name="frame_id"             type="string" value="base_footprint"/>   
   <param name="odom_guess_frame_id"  type="string" value="wheel_odom"/>
   <param name="odom_frame_id"        type="string" value="icp_odom"/>
</node>

However, if icp_odometry will eventually get lost because there are no walls at some point, you may use robot_localization to do fusion of wheel odometry and icp_odometry's twist. Enable reset countdown for icp_odometry and disable tf publishing:

<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
   <remap from="scan"      to="/scan"/>
   <remap from="odom"      to="/icp_odom"/> <!-- feed to robot_localization (use only twist as pose would diverge if getting lost) -->
	  
   <param name="frame_id"             type="string" value="base_footprint"/>   
   <param name="odom_guess_frame_id"  type="string" value="wheel_odom"/>
   <param name="odom_frame_id"        type="string" value="odom"/> <!-- if set to same robot_localization's odom frame, when icp_odometry resets after being lost, it will restart to latest pose from robot_localization. -->
   <param name="publish_tf"           type="bool" value="false"/>
   <param name="Odom/ResetCountdown"  type="int" value="1"/>
</node>