Filtering rtabmap localization jumps with robot_localization in 2D

classic Classic list List threaded Threaded
1 message Options
Reply | Threaded
Open this post in threaded view
|

Filtering rtabmap localization jumps with robot_localization in 2D

matlabbe
Administrator
Here is a small example (working in 2D) to filter jumps caused by relocalization in localization mode. The idea is to smooth /map -> /odom transform with robot_localization package like a GPS.

First, we need to disable tf published from rtabmap with publish_tf:=false, as it will be robot_localization that will publish /map -> /odom:
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">
   ...
   <param name="publish_tf" value="false"/>
   <param name="Reg/Force3DoF" value="true"/> <!-- 2D mode -->
</node>

Setup robot_localization like the GPS example:
<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"/>
      <param name="pose0" value="rtabmap/localization_pose"/> 

      <!-- 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"/>

      <param name="odom0_queue_size" value="5"/>
      <param name="pose0_queue_size" value="5"/> 

</node>

cheers,
Mathieu