Best way to avoid small relocalization jumps when pursuing a goal

[felipeduque]

From what I've seen, this is a somewhat common issue on this forum. Let's say my robot is tasked to move to a XY position in the map frame - i.e., the goal is not a node ID. Importantly, the goal also includes a desired yaw angle. The robot moves accordingly and upon reaching a close enough XY position to the goal, starts to rotate in place to reach the yaw goal.

However, the rotation needs to proceed cautiously, otherwise the visual odometry may not keep up. So, within the long period of time between getting to the XY position and reaching the yaw angle, RTAB-Map keeps publishing new localization poses, giving rise to small jumps in localization. These, in turn, are followed by constant replanning by move_base. It's not uncommon for the robot to be caught in a quasi-infinite loop, just a few centimeters/degrees away from the desired goal!

My question: what would be the more appropriate way to handle those small jumps in this delicate scenario?

Foolishly, I've tried halting new pose publications by RTAB-Map if the new calculated pose is sufficiently close to the previous pose - I found out that whenever it's not being published, the pose is assumed to be (0, 0, 0), which breaks other things.

Also, based on several threads in this forum, I have set RGBD/OptimizeFromGraphEnd to true, but although the robot itself does not jump anymore, the map does, so the "relative jump" seems to be unaltered...

[matlabbe]

When using move_base, you could increase the goal position error. However, if accurate position in the map is required (like under 1 or 2 cm), /map -> /odom should be somewhat filtered. robot_localization package does something similar to filter GPS (which is likely to jump too). Here is an example in 2D:

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen">
    <param name="Mem/IncrementalMemory" type="string" value="false"/> <!-- localization mode -->
    <param name="publish_tf"            type="bool"   value="false"/> <!-- robot_localization will publish /map->/odom -->
    <param name="frame_id"              type="string" value="base_footprint"/> 
    <param name="Reg/Force3DoF"         type="string" value="true"/>
    <remap from="odom" to="/odom"/> <!-- robot's odometry -->
    ...
</node>

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
      <param name="frequency" value="20"/>
      <param name="two_d_mode" value="true"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_footprint"/>
      <param name="world_frame" value="map"/>

      <param name="odom0" value="/odom"/> <!-- robot's odometry -->
      <param name="pose0" value="localization_pose"/> <!-- pose published by rtabmap on localization -->

      <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
      <rosparam param="odom0_config">[true, true, false,
                                      false, false, true,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam param="pose0_config">[
                                     true,   true,  false,
                                     false,  false, true,
                                     false,  false, false,
                                     false,  false, false,
                                     false,  false, false] </rosparam>

      <param name="odom0_differential" value="true"/>
      <param name="pose0_differential" value="false"/>

      <param name="odom0_relative" value="true"/>
      <param name="pose0_relative" value="false"/>

      <!-- ======== ADVANCED PARAMETERS ======== -->
      <param name="odom0_queue_size" value="5"/>
      <param name="pose0_queue_size" value="5"/>

</node>

Your approach could have also worked, but not sure where you did your modification.

cheers,
Mathieu