rtabmap localization

edit

Hi

I'm trying to run rtabmap in localization mode.

This is my launch file:

<launch>
<arg name="localization" value="true"/>

<arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
<arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>

<node pkg="tf2_ros" type="static_transform_publisher" name="camera_broadcaster" args="0.613 0.012 0.425 0.002 0.008 -0.010 1.000 base_link camera_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="footprint_broadcaster" args="0 0 0.125 0 0 0 1 base_footprint base_link" />

<param name="robot_description" textfile="$(find robot_slam)/urdf/robot.urdf" />

<node pkg="robot_slam" name="control" type="control.py" output="screen">
</node>

<!-- ROS navigation stack move_base -->
<group ns="planner">
    <remap from="point_cloud" to="/planner/planner_cloud"/>
    <remap from="map" to="/rtabmap/grid_map"/>
    <remap from="move_base_simple/goal" to="/move_base_simple/goal"/>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
        <rosparam file="$(find robot_slam)/launch/config/move_base/costmap_common.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find robot_slam)/launch/config/move_base/costmap_common.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find robot_slam)/launch/config/move_base/costmap_local.yaml" command="load" />
        <rosparam file="$(find robot_slam)/launch/config/move_base/costmap_global.yaml" command="load"/>
        <rosparam file="$(find robot_slam)/launch/config/move_base/base_local_planner.yaml" command="load" />
    </node>
</group>

<!-- encoder odometry -->
<node pkg="robot_slam" name="encoder_odometry" type="odometry.py" output="screen">
    <remap from="odom" to="/planner/odom"/>

    <param name="odom_frame_id" type="string" value="odom"/>
    <param name="robot_frame_id" type="string" value="base_footprint"/>
</node>

<group ns="rtabmap">
    <!-- RGB-D Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="log">
        <remap from="rgb/image"       to="/camera/color/image_raw"/>
        <remap from="depth/image"     to="/camera/aligned_depth_to_color/image_raw"/>
        <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
        <remap from="rgbd_image"      to="rgbd_image"/>
        <remap from="odom"            to="rgbd_odom"/>

        <param name="frame_id"                    type="string" value="base_footprint"/>
        <param name="odom_frame_id"               type="string" value="odom"/>
        <param name="publish_tf"                  type="bool"   value="true"/>
        <param name="ground_truth_frame_id"       type="string" value=""/>
        <param name="ground_truth_base_frame_id"  type="string" value=""/>
        <param name="wait_for_transform"          type="bool"   value="true"/>
        <param name="wait_for_transform_duration" type="double" value="0.2"/>
        <param name="approx_sync"                 type="bool"   value="false"/>
        <param name="config_path"                 type="string" value=""/>
        <param name="queue_size"                  type="int"    value="10"/>
        <param name="subscribe_rgbd"              type="bool"   value="false"/>
        <param name="guess_frame_id"              type="string" value=""/>
        <param name="guess_min_translation"       type="double" value="0"/>
        <param name="guess_min_rotation"          type="double" value="0"/>

        <param name="Odom/ResetCountdown"         type="int"    value="10"/>
        <param name="Odom/Holonomic" type="bool" value="false"/>
        <param name="Vis/MaxFeatures" type="string" value="1000"/>
        <param name="Reg/Force3DoF"             type="bool" value="false"/>

        <param name="RGBD/SavedLocalizationIgnored" type="bool" value="true"/>
    </node>

    <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" args="$(arg rtabmap_args)" output="log">
        <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
        <param if="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="true"/>

        <remap from="odom" to="/rtabmap/rgbd_odom"/> 
        <remap from="rgb/image" to="/camera/color/image_raw"/>
        <remap from="depth/image" to="/camera/aligned_depth_to_color/image_raw"/>
        <remap from="rgb/camera_info" to="/camera/color/camera_info"/>

        <param name="frame_id"          type="string"   value="base_footprint"/>
        <param name="odom_frame_id"     type="string"   value="odom"/>
        <param name="map_frame_id"      type="string"   value="map"/>
        <param name ...