Navigation with RTAB-Map

[Noctis]

Hi matlabbe,

I am trying to get a custom robot car with an RGBD+IMU camera to autonomously navigate in my office. However, I am quite new to Robotics and am quite lost what to do after SLAM, hence, apologies for the stupid questoins. Tried reading the tutorials for turtlebot and stereo outdoor navigation but the turtlebot tutorials are just not working for me. I've read through navigation stack to understand the costmaps params.

1. During SLAM, do we just use RViz to subscribe to the /rtabmap/grid_map topic?

2. After which, I used map_server to save the .db file as map.pgm and map.yaml. Are these map files essentially the Costmap2D that can be used straight for navigation? Otherwise, what should I do to use these map files for navigation?

3. For navigation, what are the steps I have to do for RGBD navigation?
Do I first run
roslaunch rtabmap_ros rtabmap.launch localization:=true  ?
This will publish the /map -> /odom. Then move_base(?) will have /odom -> /base_footprint -> /URDF defined TFs??


After some trial and error, this is what I have right now:


when I click the 2D Pose Estimate, nothing happens but when I click 2D Nav Goal, the global path can be planned, but the robot doesnt move. My arduino nano is subscribed to the /planner/cmd_vel topic which I believe is the one being published by move_base.

This is my launch file:
I just added move_base stuff that I dug online and from their tutorials as well

<!-- -->
<launch>
     
  <!-- Choose between depth and stereo, set both to false to do only scan -->      
  <arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="true" />
 
  <!-- Localization-only mode -->
  <arg name="localization"            default="true"/>
  
  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
  
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/turtlebot_navigation.rviz" />
  
  <arg name="frame_id"                default="base_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
  <arg name="gdb"                     default="false"/>         <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
  <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
  <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
  <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->
  <arg name="publish_tf_map"          default="true"/>

  <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/>         
   
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
  
  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
  
  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="false"/>         <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
  <arg name="approx_rgbd_sync"        default="true"/>          <!-- false=exact synchronization -->
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />         <!-- Deprecated, use rgbd_depth_scale instead -->
  <arg name="rgbd_depth_scale"        default="$(arg depth_scale)" />
  <arg name="rgbd_decimation"         default="1" />
  
  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="subscribe_scan_descriptor" default="false"/>
  <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="false"/> <!-- use filtered cloud from icp_odometry for mapping -->
  <arg name="gen_scan"                default="false"/> <!-- only works with depth image and if not subscribing to scan topic-->
   
  <arg name="visual_odometry"          default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="icp_odometry"             default="false"/>         <!-- Launch rtabmap icp odometry node -->
  <arg name="odom_topic"               default="odom"/>          <!-- Odometry topic name -->
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
  <arg name="publish_tf_odom"          default="true"/>
  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default=""/>              <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
  <arg name="odom_sensor_sync"         default="false"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/>
  <arg name="odom_max_rate"            default="0"/>
  <arg name="odom_expected_rate"       default="0"/>
  <arg name="imu_topic"                default="/imu/data"/>          <!-- only used with VIO approaches -->
  <arg name="wait_imu_to_init"         default="false"/>
  <arg name="use_odom_features"        default="false"/>
  
  <arg name="scan_cloud_assembling"              default="false"/>
  <arg name="scan_cloud_assembling_time"         default="1"/>
  <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
  <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
  <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>
  
  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
  
  <arg name="gps_topic"                default="/gps/fix" />         <!-- gps async subscription -->
  
  <arg name="tag_topic"                default="/tag_detections" />  <!-- apriltags async subscription -->
  <arg name="tag_linear_variance"      default="0.0001" />
  <arg name="tag_angular_variance"     default="9999" />             <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
  
  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>
  <arg if="$(arg rgbd_sync)"      name="rgbd_topic_relay"          default="$(arg rgbd_topic)"/>
  <arg unless="$(arg rgbd_sync)"  name="rgbd_topic_relay"          default="$(arg rgbd_topic)_relay"/>

<!-- 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">
        <param name="base_global_planner" value="navfn/NavfnROS"/>
        <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/>

        <rosparam file="$(find navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find navigation)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find navigation)/param/global_costmap_params.yaml" command="load"/>
        <rosparam file="$(find navigation)/param/base_local_planner_params.yaml" command="load" />
    </node>
</group>

  <!-- Nodes -->
  <group ns="$(arg namespace)">
     
    <!-- relays -->
    <group if="$(arg depth)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
          <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
          <param name="depth_scale"     type="double" value="$(arg rgbd_depth_scale)"/>
          <param name="decimation"     type="double" value="$(arg rgbd_decimation)"/>
        </node>
      </group>
    </group>
    <group if="$(arg stereo)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)">
          <remap from="left/image_rect"   to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"  to="$(arg right_image_topic_relay)"/>
          <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
          <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
        </node>
      </group>
    </group>
    
    <group unless="$(arg rgbd_sync)">
       <group if="$(arg subscribe_rgbd)">
         <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros">
           <remap     if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
           <remap     if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
           <remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
           <param if="$(arg compressed)" name="uncompress" value="true"/>
         </node>
      </group>
    </group>

    <!-- Visual odometry -->
    <group unless="$(arg icp_odometry)">
      <group if="$(arg visual_odometry)">
      
        <!-- RGB-D Odometry -->
        <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"            to="$(arg odom_topic)"/>
          <remap from="imu"             to="$(arg imu_topic)"/>
      
          <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
          <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
          <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
          <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
          <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
          <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
          <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
          <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
          <param name="config_path"                 type="string" value="$(arg cfg)"/>
          <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
          <param name="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
          <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
          <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
          <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
          <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
          <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
          <param name="keep_color"                  type="bool"   value="$(arg use_odom_features)"/>

          <param name="OdomORBSLAM/Bf"              type="double" value="46.356317139"/>
          <param name="OdomORBSLAM/VocPath"         type="string" value="/home/jetson/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt"/>
          <param name="Odom/Strategy"               type="int"    value="5"/>
          <param name="ORB/Gpu"                     type="bool"   value="true"/>
          <param name="Reg/Force3DoF"               type="bool"   value="true"/>
          <param name="Optimizer/GravitySigma"      type="double" value="0.0"/>
          <param name="GFTT/MinDistance"            type="int"    value="10"/>
        </node>

        <node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
          <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"             to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"                   to="$(arg odom_topic)"/>
          <remap from="imu"                    to="$(arg imu_topic)"/>
      
          <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
          <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
          <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
          <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
          <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
          <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
          <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
          <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
          <param name="config_path"                 type="string" value="$(arg cfg)"/>
          <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
          <param name="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
          <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
          <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
          <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
          <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
          <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
          <param name="keep_color"                  type="bool"   value="$(arg use_odom_features)"/>
        </node>
      </group>
    </group>
    
    <!-- ICP Odometry -->
    <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
      
      <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
      <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
      <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
      <param name="config_path"                 type="string" value="$(arg cfg)"/>
      <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
      <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
      <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
      <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
      <param name="scan_cloud_max_points"       type="int"    value="$(arg scan_cloud_max_points)"/>
      <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
      <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
    </node>
    
    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
      <remap     if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>

      <remap from="odom"            to="$(arg odom_topic)"/>
      <param name="assembling_time" type="double" value="$(arg scan_cloud_assembling_time)"/>
      <param name="fixed_frame_id"  type="string" value="$(arg scan_cloud_assembling_fixed_frame)"/>
      <param name="voxel_size"      type="double" value="$(arg scan_cloud_assembling_voxel_size)"/>
      <param name="noise_radius"        type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
      <param name="noise_min_neighbors" type="int"    value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
    </node>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgb"        type="bool"   value="$(arg depth)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="publish_tf"           type="bool"   value="$(arg publish_tf_map)"/>
      <param name="gen_scan"             type="bool"   value="$(arg gen_scan)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
      <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>
      <param name="odom_sensor_sync"         type="bool"   value="$(arg odom_sensor_sync)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"        type="string" value="$(arg database_path)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param name="scan_cloud_max_points"     type="int" value="$(arg scan_cloud_max_points)"/>
      <param name="landmark_linear_variance"   type="double" value="$(arg tag_linear_variance)"/>
      <param name="landmark_angular_variance"  type="double" value="$(arg tag_angular_variance)"/>      
      
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
	
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
      <remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap from="gps/fix"                to="$(arg gps_topic)"/>
      <remap from="tag_detections"         to="$(arg tag_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
      
      <param name="Odom/Strategy"               type="int"    value="5"/>
      <param name="ORB/Gpu"                     type="bool"   value="true"/>
      <param name="Reg/Force3DoF"               type="bool"   value="true"/>
      <param name="Optimizer/GravitySigma"      type="double" value="0.0"/>
      <param name="GFTT/MinDistance"            type="int"    value="10"/>

      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param     if="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(eval subscribe_scan or subscribe_scan_cloud)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
      <param name="queue_size"           type="int"    value="$(arg queue_size)"/>
      <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
    
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
      
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
      <remap     if="$(arg icp_odometry)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg icp_odometry)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
    <remap if="$(arg stereo)" from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="rgbd_image"        to="$(arg rgbd_topic_relay)"/>
    <remap from="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="4"/>
    <param name="voxel_size"  type="double" value="0.0"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

</launch>

Thanks so much for RTAB-Map and your help!

[matlabbe]

Hi,

To keep the launch clean and to be easier to see the differences with original rtabmap.launch, create a new launch file and include rtabmap.launch instead, like this:

<launch>

<arg name="localization" default="true"/>

<!-- 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">
        <param name="base_global_planner" value="navfn/NavfnROS"/>
        <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/>

        <rosparam file="$(find navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find navigation)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find navigation)/param/global_costmap_params.yaml" command="load"/>
        <rosparam file="$(find navigation)/param/base_local_planner_params.yaml" command="load" />
    </node>
</group>

<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
   <arg name="base_link" value="base_link"/>
   <arg name="localization" value="$(arg localization)"/>

   <!-- if you are doing 2D slam -->
   <arg name="args" value="--Reg/Force3DoF true"/>
   ...
</include>

</launch>

Other option is not using rtabmap.launch, and cherry pick only nodes you need following the examples here to create your own launch file.

For your questions, /rtabmap/grid_map can be visualized in RVIZ, but its purpose with move_base is to feed it the map required for its global costmap. When using rtabmap, there is then no need to use map_server, as rtabmap in localization mode will already provide the map. move_base uses this map to update the global costmap, which is then used to do safe global planning.

Your base_footprint is confusing, as base_link is used in the launch file. Do you have external odometry or are you using rtabmap odometry? /odom->/base_footprint would be published by rtabmap's odometry node or your own odometry node.
 /map -> /odom will be published by rtabmap.

For the 2D pose estimate button to work with rtabmap, you should open Tools properties and set 2D pose estimate topic to  /rtabmap/initialpose.

cheers,
Mathieu

[Noctis]

Hi Mattieu, thanks for the reply! Will take some time to tidy up the launch file!

Actually.. what is the difference between choosing base_link vs base_footprint for frame_id? For odometry, I am using ORB-SLAM2.
By the way, if I use ORB-SLAM2 as odometry for mapping, must I also use ORB-SLAM2 during localisation? Or would you recommend using RTAB-Map's?

If my robot is able to localize itself in the /rtabmap/grid_map, do i still have to use the 2D Pose Estimate?

Also, for move_base related topics, I see that my rostopic list outputs:

/mobile_base/sensors/bumper_pointcloud
/move_base/TrajectoryPlannerROS/global_plan
/move_base/TrajectoryPlannerROS/local_plan
/move_base/global_costmap/costmap
/move_base/global_costmap/costmap_updates
/move_base/local_costmap/costmap
/move_base/local_costmap/costmap_updates
/move_base_simple/goal
/particlecloud
/planner/cmd_vel
/planner/move_base/DWAPlannerROS/cost_cloud
/planner/move_base/DWAPlannerROS/global_plan
/planner/move_base/DWAPlannerROS/local_plan
/planner/move_base/DWAPlannerROS/parameter_descriptions
/planner/move_base/DWAPlannerROS/parameter_updates
/planner/move_base/DWAPlannerROS/trajectory_cloud
/planner/move_base/NavfnROS/plan
/planner/move_base/cancel
/planner/move_base/current_goal
/planner/move_base/feedback
/planner/move_base/global_costmap/costmap
/planner/move_base/global_costmap/costmap_updates
/planner/move_base/global_costmap/footprint
/planner/move_base/global_costmap/inflation_layer/parameter_descriptions
/planner/move_base/global_costmap/inflation_layer/parameter_updates
/planner/move_base/global_costmap/obstacle_layer/parameter_descriptions
/planner/move_base/global_costmap/obstacle_layer/parameter_updates
/planner/move_base/global_costmap/parameter_descriptions
/planner/move_base/global_costmap/parameter_updates
/planner/move_base/global_costmap/static_layer/parameter_descriptions
/planner/move_base/global_costmap/static_layer/parameter_updates
/planner/move_base/goal
/planner/move_base/local_costmap/costmap
/planner/move_base/local_costmap/costmap_updates
/planner/move_base/local_costmap/footprint
/planner/move_base/local_costmap/inflation_layer/parameter_descriptions
/planner/move_base/local_costmap/inflation_layer/parameter_updates
/planner/move_base/local_costmap/obstacle_layer/parameter_descriptions
/planner/move_base/local_costmap/obstacle_layer/parameter_updates
/planner/move_base/local_costmap/parameter_descriptions
/planner/move_base/local_costmap/parameter_updates
/planner/move_base/local_costmap/static_layer/parameter_descriptions
/planner/move_base/local_costmap/static_layer/parameter_updates
/planner/move_base/parameter_descriptions
/planner/move_base/parameter_updates
/planner/move_base/result
/planner/move_base/status
/planner/odom



In Tool Properties, the 2D Nav Goal topic is subscribed to /move_base_simple/goal

The global and local costmaps are subscribed to /move_base/global_costmap/costmap and /move_base/local_costmap/costmap respectively. When i clicked on the topics, only planner/move_base/global_costmap/costmap and planner/move_base/local_costmap/costmap appeared and those are working. Could I get an explanation whats the difference between the planner and the non-planner topics and should I change the 2D Nav Goal topic to anything else?

I had to change the Planner under the Global Map to /planner/move_base/NavfnROS/plan for the global path to show up. Is that supposed to be the case?

I also changed my subscribed topic from my arduino from /cmd_vel to /planner/cmd_vel but it just rotates in place

[matlabbe]

For the base frame, see https://www.ros.org/reps/rep-0105.html#coordinate-frames. base_footprint is often used as parent to base_link at ground level. In this case, frame_id should be base_footprint. If your robot uses only base_link as the root of the robot, use base_link. You don't need to use ORB_SLAM2 again in localization, you can use any odometry approaches, but if you already satisfied with ORB_SLAM2, continue to use it.

You don't need 2D Pose Estimate if the robot is already able to localize itself in the map.

Topics not in planner namespace are maybe not published topics, but subscribed topics. To "rosnode info" on your move_base node to see the actual subscribed and published topics.

Your robot is rotating onplace maybe because move_base is in recovery behavior. "rostopic echo" the cmd_vel topic and see if there is indeed only twist over z axis. If so, check for errors or warnings on move_base terminal.

cheers,
Mathieu