Navigation Stack + rtabmap requesting map configuration problem

[mjedmonds]

Hi,

I've been trying to setup rtabmap and the standard ROS navigation stack. However, I can't get the move_base package to get past the "Requesting the map..." stage. Below is my complete configuration (largely followed from here). I'm using a Microsoft Kinect as point cloud sensor

Files

costmap_common_params.yaml

footprint: [[0.42, -0.43], [0.42, 0.43], [-0.42, 0.43], [-0.42, -0.43]]
footprint_padding: 0.02

inflation_layer:
  inflation_radius: 0.55

transform_tolerance: 2

obstacle_layer:
  obstacle_range: 3.0
  raytrace_range: 3.5
  max_obstacle_height: 0.7
  track_unknown_space: true


  observation_sources: point_cloud_sensor
  
  point_cloud_sensor: {
    #sensor_frame: /camera_link,
    data_type: PointCloud2,
    topic: /rtabmap/cloud_map,
    marking: true,
    clearing: true
  }

global_costmap_params.yaml

global_costmap:
  global_frame: /map
  robot_base_frame: base_footprint
  update_frequency: 0.5
  publish_frequency: 0.5
  static_map: true

  plugins:
     - {name: static_layer, type: "rtabmap_ros::StaticLayer"}
     - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
     - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

local_costmap_params.yaml

local_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  update_frequency: 2
  publish_frequency: 2
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.025
  origin_x: -3.0
  origin_y: -3.0

  plugins:
    - {name: obstacle_layer,   type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}

base_local_planner_params.yaml

TrajectoryPlannerROS:

  acc_lim_x:  0.05
  acc_lim_y:  0.05
  acc_lim_theta: 0.05
  max_vel_x:  0.1
  min_vel_x:  0.02
  max_vel_theta: 0.05
  min_vel_theta: -0.05
  min_in_place_vel_theta: 0.01
  holonomic_robot: true

  xy_goal_tolerance:  0.05
  yaw_goal_tolerance: 0.05
  latch_xy_goal_tolerance: true
  
  # make sure that the minimum velocity multiplied by the sim_period is less than twice the tolerance on a goal. Otherwise, the robot will prefer to rotate in place just outside of range of its target position rather than moving towards the goal.
  sim_time: 1.5 # set between 1 and 2. The higher he value, the smoother the path (though more samples would be required).
  sim_granularity: 0.025
  angular_sim_granularity: 0.05
  vx_samples: 12
  vtheta_samples: 20

  meter_scoring: true

  pdist_scale: 0.7 # The higher will follow more the global path.
  gdist_scale: 0.8
  occdist_scale: 0.01
  publish_cost_grid_pc: false

#move_base
controller_frequency: 5.0 #The robot can move faster when higher.

#global planner 
NavfnROS:
  allow_unknown: true
  visualize_potential: true

move_base.launch

<launch>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <!-- remap topics -->
    <remap from="cmd_vel" to="/mobility_base/cmd_vel" />
    <remap from="odom" to="/odometry/filtered" />
    

<remap from="map" to="/rtabmap/get_proj_map" />

    <rosparam file="$(find bender_2dnav)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find bender_2dnav)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find bender_2dnav)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find bender_2dnav)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find bender_2dnav)/config/base_local_planner_params.yaml" command="load" />
  </node>
</launch>

Note: I've bolded the line in the file above that corresponds to mapping the /map service call to rtabmap's get_proj_map. I believe this is the proper configuration; I don't have laser scan sources. I've verified that the get_proj_map indeeds returns a nav_msgs/GetMap and the /rtabmap/proj_map topic is publishing a correct occupancy grid.

Any guidance on where to go would be appreciated; I've been scratching my head as to what could be wrong.

I can provide rosbags if that would be helpful.

Thanks!

[matlabbe]

Hi,

I think it is the remapping of the /map topic that is wrong. Remap the proj_map topic, not the service and try to remap outside the move_base tag:

<remap from="map" to="/rtabmap/proj_map" />
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
...
</node>

Show up "rqt_graph" to see if the "/rtabmap/proj_map" is connected to move_base.

Side note: I would set "odom" for the global_frame of local_costmap_params.yaml. See this page about a standard configuration of move_base: http://wiki.ros.org/navigation/Tutorials/RobotSetup

cheers

[mjedmonds]

Thanks matlabbe! You were right. I was following the navigation tutorial, which shows a nav_msgs/GetMap, which is a service call. But it is working with the /proj_map topic.

Now, I'm having another problem with the Reg/Force3DoF parameter being respected. I'm quiet puzzled because it was working previously. Here's a screenshot of the odom I'm seeing:



And here's the corresponding roslaunch file:

rtabmap.launch

<launch>

  <arg name="publish_odom_tf" default="true"/>
  <arg name="publish_map_tf" default="true"/>

  <arg name="localization" default="true" />
  <arg if="$(arg localization)" name="mapping" value="false" />
  <arg unless="$(arg localization)" name="mapping" value="true" />

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

  <arg name="frame_id" default="base_link"/>
  <arg name="wait_for_transform_duration" default="0.5"/>
  <arg name="wait_for_transform" default="true"/>

  <arg name="rgb/image" default="/camera/rgb/image_rect_color"/>
  <arg name="depth/image" default="/camera/depth_registered/image_raw"/>
  <arg name="rgb/camera_info" default="/camera/rgb/camera_info"/>


  <group ns="rtabmap">

  <arg name="strategy" default="0" />
  <arg name="feature" default="6" />
  <arg name="nn" default="3" />
  <arg name="max_depth" default="6.0" />
  <arg name="min_inliers" default="20" />
  <arg name="inlier_distance" default="0.02" />
  <arg name="local_map" default="1000" />
  <arg name="gftt_max_corners" default="1000" />
  <arg name="gftt_min_distance" default="7" />

    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">

      <remap from="rgb/image" to="$(arg rgb/image)"/>
      <remap from="depth/image" to="$(arg depth/image)"/>
      <remap from="rgb/camera_info" to="$(arg rgb/camera_info)"/>

      <!-- Force 2D odometry -->
      <param name="Reg/Force2D" type="string" value="true"/>

      <param name="frame_id" type="string" value="$(arg frame_id)"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform_duration)" />
      <param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)" />

      <param name="publish_tf" type="bool" value="$(arg publish_odom_tf)" />      
      <param name="Vis/CorNNType" type="string" value="$(arg nn)"/>
      <param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>  
      <param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/> 
      <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>       
      <param name="OdomF2M/LocalHistorySize" type="string" value="$(arg local_map)"/>
      <param name="Vis/MaxFeatures" type="string" value="$(arg gftt_max_corners)"/>
      <param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
 
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="frame_id" type="string" value="$(arg frame_id)"/>
      <param name="publish_tf" type="bool" value="$(arg publish_map_tf)" />      
      <param name="subscribe_depth" type="bool" value="true"/>

      <remap from="odom" to="odom"/>
      <remap from="rgb/image" to="$(arg rgb/image)"/>
      <remap from="depth/image" to="$(arg depth/image)"/>
      <remap from="rgb/camera_info" to="$(arg rgb/camera_info)"/>

      <param name="queue_size" type="int" value="10"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform_duration)" />
      <param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)" />

      <!-- LOCALIZATION OR MAPPING MODE -->
      <param name="Mem/IncrementalMemory" type="string" value="$(arg mapping)"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
      <!--<param name="Mem/IncrementalMemory" type="string" value="true"/>-->
      <!-- RTAB-Map's parameters -->
      
      <!-- Force 2D mapping  (constrained to x-y plane + yaw) -->
      <param name="Reg/Force3DoF" type="string" value="true"/>
      <param name="Optimizer/Slam2D" type="string" value="true"/>
      <!--<param name="Reg/Strategy" type="string" value="1"/>     [> 0=Visual, 1=ICP, 2=Visual+ICP <]-->

      <param name="Rtabmap/DetectionRate" type="string" value="3.0" />
      <param name="DbSqlite3/InMemory" type="string" value="true" />
      <param name="Vis/MinInliers" type="string" value="10"/>
      <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
    </node>
  </group>
</launch>

Any tips on what might be wrong, or is this expected behavior? Force 3DoF should zero any z-translation...

Thanks!

[mjedmonds]

Actually, I'm fairly confident this the result of an incorrect base_link -> camera_link transform. I think the angle is off.

I'm going to try to fix that, but if there's another solution please let me know!

[matlabbe]

Hi,

For rgbd_odometry, you set "Reg/Force2D" instead of "Reg/Force3DoF". To reduce the confusion between Force2D and Force3DoF, I added a compatibility match in this commit so that both can be used for the same parameter (though Reg/Force2D will throw a warning but will be still used).

You may also want to update your rtabmap.launch file with the new one. See here for migration issues.

cheers,
Mathieu

[mjedmonds]

Great, got that sorted out!

Now on to the next problem...is there a reason /rtabmap/cloud_map wouldn't begin publishing immediately?

I run the launch file below (updated to v0.11 as you recommended). I'm trying to use the navigation stack, with an observation source of /rtabmap/cloud_map (since I'm using a Kinect). After launching rtabmap, subscribing to the /rtabmap/cloud_map topic yields the following warning:

[ WARN] [1466095980.174555762]: Cloud map is empty! (poses=1 clouds=1)

After a certain amount of time, but cloud_map is eventually published. Do you have any guess as to why the cloud_map isn't immediately available, but becomes available later? I can provide a more detailed log of what's happening if necessary. I'm running in localization mode with the following launch file:

rtabmap.launch

<launch>

  <!-- additional params -->
  <arg name="publish_odom_tf" default="true"/>
  <arg name="publish_map_tf" default="true"/>

  <!-- Localization-only mode -->
  <arg name="localization"            default="true"/>

  <arg if="$(arg localization)" name="mapping" value="false" />
  <arg unless="$(arg localization)" name="mapping" value="true" />

  <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->

  <!-- For rgbd:=true
        Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
           
  <!-- For stereo:=true
        Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->
     
  <!-- Choose between RGB-D and stereo -->      
  <arg name="rgbd"            default="true"/>
  <arg name="stereo"          default="false"/>
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="false" />
  
  <!-- 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="-d $(find rtabmap_ros)/launch/config/rgbd.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="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.5"/>

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

  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
  
  <!-- 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" />
  
  <!-- 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" />
  <arg name="approx_sync"             default="false"/>         <!-- if timestamps of the stereo images are not synchronized -->
  
  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
                                                                <!-- For depth_topic, "compressedDepth" image_transport is used. --> 
                                                                <!-- For rgb_topic, see "rgb_image_transport" argument. -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (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="visual_odometry"         default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
  
  <!-- 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)"/>

  <!-- algorithm params -->
  <arg name="strategy" default="0" />
  <arg name="feature" default="6" />
  <arg name="nn" default="3" />
  <arg name="max_depth" default="6.0" />
  <arg name="min_inliers" default="20" />
  <arg name="inlier_distance" default="0.02" />
  <arg name="local_map" default="1000" />
  <arg name="gftt_max_corners" default="1000" />
  <arg name="gftt_min_distance" default="7" />

  <!-- Nodes -->
  <group ns="$(arg namespace)">

  
    <!-- RGB-D Odometry -->
    <group if="$(arg 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="compressedDepth in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
  
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" 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)"/>

        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
        <param name="config_path"                 type="string" value="$(arg cfg)"/>
        <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
        <param name="publish_tf"                  type="bool"   value="$(arg publish_odom_tf)" />      

        <!-- Force 2D odometry -->
        <param name="Reg/Force2D" type="string" value="true"/> 
        <param name="Reg/Force3DoF" type="string" value="true"/> 

        <param name="Vis/EstimationType" type="string" value="$(arg strategy)"/>
        <param name="Vis/CorNNType" type="string" value="$(arg nn)"/>
        <param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>  
        <param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/> 
        <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>       
        <param name="OdomF2M/LocalHistorySize" type="string" value="$(arg local_map)"/>
        <param name="Vis/MaxFeatures" type="string" value="$(arg gftt_max_corners)"/>
        <param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
      </node>
    </group>
    
    <!-- Stereo Odometry -->
    <group if="$(arg stereo)">
      <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="compressed 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="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
  
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="$(arg rtabmap_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)"/>
	  
        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
        <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)"/>
      </node>
    </group>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"      type="bool"   value="$(arg rgbd)"/>
      <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="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="publish_tf"           type="bool"   value="$(arg publish_map_tf)" />      
      <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="stereo_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)"/>
      
      <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="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 from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>

      <!-- Force 2D mapping  (constrained to x-y plane + yaw) -->
      <param name="Reg/Force3DoF" type="string" value="true"/>
      <param name="Reg/Force2D" type="string" value="true"/>
      <param name="Optimizer/Slam2D" type="string" value="true"/>

      <param name="Rtabmap/DetectionRate" type="string" value="3.0" />
      <param name="DbSqlite3/InMemory" type="string" value="true" />
      <param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
      <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>

      <!-- 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="screen" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"      type="bool"   value="$(arg rgbd)"/>
      <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_odom_info"  type="bool"   value="$(arg visual_odometry)"/>
      <param name="frame_id"             type="string" value="$(arg 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)"/>
    
      <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="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 from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap from="right/image"       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="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="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="2"/>
    <param name="voxel_size"  type="double" value="0.02"/>
    <param if="$(arg stereo)" name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

</launch>

[matlabbe]

Hi,

You are starting rtabmap in localization mode. Until a relocalization happens with a location saved in the map, cloud_map will be empty. I slightly updated the code so that current cloud data is published in cloud_map even if there is no localization yet. A first localization is still required to show up the whole map (similar to this video).

cheers

[mjedmonds]

Thank you for your help! It is greatly appreciated. Everything is working :)

[mjedmonds]

Er - I spoke too soon. Everything is working, but the move_base node is consistently crashing. The behavior is very consistent; eventually it crashes with the same error every time. The amount of time it takes to crash does vary. I'm going to post this same question on http://answers.ros.org/question/237927/move_base-corrupt-linked-list/

Here's the exact error, each time corresponding to the following warning:

[ WARN] [1466704185.100935930]: InflationLayer::updateCosts(): seen_ array size is wrong

and the error:

*** Error in `/opt/ros/indigo/lib/move_base/move_base': corrupted double-linked list: 0x00000000029ad9b0 ***
[move_base-1] process has died [pid 8680, exit code -6, cmd /opt/ros/indigo/lib/move_base/move_base cmd_vel:=/mobility_base/cmd_vel odom:=/odometry/filtered map:=/rtabmap/proj_map __name:=move_base __log:=/home/mb/.ros/log/4d826ab2-396a-11e6-ad82-b8aeed747f2b/move_base-1.log].
log file: /home/mb/.ros/log/4d826ab2-396a-11e6-ad82-b8aeed747f2b/move_base-1*.log
all processes on machine have died, roslaunch will exit

Unfortunately, the log file isn't actually written, so I haven't been able to get any useful information from there. I've done some googling and the only thing I can turn up is line 192 of the source of costmap_2d's inflation_layer.cpp (http://docs.ros.org/jade/api/costmap_2d/html/inflation__layer_8cpp_source.html). So it seems deleting the seen_ array of costmap_2d's inflation layer is causing a corruption of a linked list somewhere else. What's odd is the warning doesn't always cause the error. I'm thinking it might be cause by how rtabmap is updating proj_map, but I'm somewhat shooting in the dark. Any insight would be helpful.

Here's every related file:

base_local_planner_params.yaml

TrajectoryPlannerROS:

  acc_lim_x:  1.0
  acc_lim_y:  1.0
  acc_lim_theta: 2.0
  max_vel_x:  0.4
  min_vel_x:  0.05
  escape_vel: -0.05
  max_vel_theta: 0.5
  min_vel_theta: -0.5
  min_in_place_vel_theta: 0.01
  holonomic_robot: true
  y_vels: [-0.3, -0.05, 0.05, 0.3]

  xy_goal_tolerance:  0.03
  yaw_goal_tolerance: 0.05
  latch_xy_goal_tolerance: true
  
  # make sure that the minimum velocity multiplied by the sim_period is less than twice the tolerance on a goal. Otherwise, the robot will prefer to rotate in place just outside of range of its target position rather than moving towards the goal.
  sim_time: 1.5 # set between 1 and 2. The higher he value, the smoother the path (though more samples would be required).
  sim_granularity: 0.025
  angular_sim_granularity: 0.025
  vx_samples: 12
  vtheta_samples: 20

  meter_scoring: true

  pdist_scale: 0.7 # The higher will follow more the global path.
  gdist_scale: 0.8
  occdist_scale: 0.03
  publish_cost_grid_pc: false

# move_base controller_frequency
controller_frequency: 5.0 

# global planner 
NavfnROS:
  allow_unknown: true
  visualize_potential: false

costmap_common_params.yaml

footprint: [[0.42, -0.43], [0.42, 0.43], [-0.42, 0.43], [-0.42, -0.43]]
footprint_padding: 0.02

inflation_layer:
  inflation_radius: 0.02

transform_tolerance: 2

obstacle_layer:
  obstacle_range: 3.0
  raytrace_range: 3.5
  track_unknown_space: false

  observation_sources: point_cloud_sensor
  
  point_cloud_sensor: {
    sensor_frame: /camera_link,
    data_type: PointCloud2,
    topic: /rtabmap/cloud_map,
    expected_update_rate: 1.0,
    max_obstacle_height: 3.0,
    min_obstacle_height: 0.1,
    marking: true,
    clearing: true
  }

global_costmap_params.yaml

global_costmap:
  global_frame: /map
  robot_base_frame: base_footprint
  update_frequency: 0.5
  publish_frequency: 0.5
  always_send_full_costmap: true
  static_map: true

  plugins:
     - {name: static_layer, type: "rtabmap_ros::StaticLayer"}
     - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
     - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

local_costmap_params.yaml

local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint
  update_frequency: 1
  publish_frequency: 1
  static_map: false
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.025
  #origin_x: -3.0
  #origin_y: -3.0

  plugins:
    - {name: obstacle_layer,   type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}

rtabmap.launch

<launch>

  <!-- huroco specific additional params -->
  <arg name="publish_odom_tf"       default="true"/>
  <arg name="publish_map_tf"        default="true"/>
  <arg name="proj_min_cluster_size" default="2.0"/>

  <!-- Localization-only mode -->
  <arg name="localization"            default="true"/>

  <arg if="$(arg localization)" name="mapping" value="false" />
  <arg unless="$(arg localization)" name="mapping" value="true" />

  <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->

  <!-- For rgbd:=true
        Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
           
  <!-- For stereo:=true
        Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->
     
  <!-- Choose between RGB-D and stereo -->      
  <arg name="rgbd"            default="true"/>
  <arg name="stereo"          default="false"/>
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="false" />
  
  <!-- 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="-d $(find rtabmap_ros)/launch/config/rgbd.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="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.5"/>

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

  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
  
  <!-- 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" />
  
  <!-- 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" />
  <arg name="approx_sync"             default="false"/>         <!-- if timestamps of the stereo images are not synchronized -->
  
  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
                                                                <!-- For depth_topic, "compressedDepth" image_transport is used. --> 
                                                                <!-- For rgb_topic, see "rgb_image_transport" argument. -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (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="visual_odometry"         default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
  
  <!-- 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)"/>

  <!-- algorithm params -->
  <arg name="strategy" default="0" />
  <arg name="feature" default="6" />
  <arg name="nn" default="3" />
  <arg name="max_depth" default="5.0" />
  <arg name="min_inliers" default="15" />
  <arg name="inlier_distance" default="0.02" />
  <arg name="local_map" default="2000" />
  <arg name="gftt_max_corners" default="1000" />
  <arg name="gftt_min_distance" default="7" />

  <!-- Nodes -->
  <group ns="$(arg namespace)">

  
    <!-- RGB-D Odometry -->
    <group if="$(arg 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="compressedDepth in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
  
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" 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)"/>

        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
        <param name="config_path"                 type="string" value="$(arg cfg)"/>
        <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
        <param name="publish_tf"                  type="bool"   value="$(arg publish_odom_tf)" />      

        <!-- Force 2D odometry -->
        <param name="Reg/Force2D" type="string" value="true"/> 
        <param name="Reg/Force3DoF" type="string" value="true"/> 

        <param name="Vis/EstimationType" type="string" value="$(arg strategy)"/>
        <param name="Vis/CorNNType" type="string" value="$(arg nn)"/>
        <param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>  
        <param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/> 
        <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>       
        <param name="OdomF2M/LocalHistorySize" type="string" value="$(arg local_map)"/>
        <param name="Vis/MaxFeatures" type="string" value="$(arg gftt_max_corners)"/>
        <param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
      </node>
    </group>
    
    <!-- Stereo Odometry -->
    <group if="$(arg stereo)">
      <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="compressed 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="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
  
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="$(arg rtabmap_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)"/>
	  
        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
        <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)"/>
      </node>
    </group>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"      type="bool"   value="$(arg rgbd)"/>
      <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="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="publish_tf"           type="bool"   value="$(arg publish_map_tf)" />      
      <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="stereo_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)"/>
      
      <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="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 from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>

      <!-- Force 2D mapping  (constrained to x-y plane + yaw) -->
      <param name="Reg/Force3DoF" type="string" value="true"/>
      <param name="Reg/Force2D" type="string" value="true"/>
      <param name="Optimizer/Slam2D" type="string" value="true"/>

      <param name="Rtabmap/DetectionRate" type="string" value="3.0" />
      <param name="DbSqlite3/InMemory" type="string" value="true" />
      <param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
      <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
      <param name="proj_min_cluster_size" type="double" value="$(arg proj_min_cluster_size)" />

      <!-- 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="screen" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"      type="bool"   value="$(arg rgbd)"/>
      <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_odom_info"  type="bool"   value="$(arg visual_odometry)"/>
      <param name="frame_id"             type="string" value="$(arg 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)"/>
    
      <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="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 from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap from="right/image"       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="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="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="2"/>
    <param name="voxel_size"  type="double" value="0.02"/>
    <param if="$(arg stereo)" name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

</launch>

[matlabbe]

At first look, "inflation_radius: 0.02" seems very small to me. Generally, it is set >footprint radius. In your case, I would set it between 0.5 and 0.7 (the highest the robot will plan farther from obstacles):

inflation_layer:
  inflation_radius: 0.6

cheers

[mjedmonds]

Thanks for the quick reply. I changed the inflation radius to 0.5; the footprint itself is a tad inflated. (also thanks for the general info - it's very difficult to find documentation for the navigation stack and little things like that make a huge difference).

I'm still getting the same error, though it occurs much less frequently. Any other ideas?

[matlabbe]

Can you track if the error is coming from the inflation layer of the local costmap or the global costmap?

The global costmap is created from /map (which is a remap from /rtabmap/proj_map?) and the obstacle_layer.
The local costmap is created from the obstacle_layer.

The obstacle_layer should not be /rtabmap/cloud_map topic. It should be the sensor data. I assume that you are using a Kinect-like sensor, I use this for an obstacle_layer:

obstacle_layer:
  obstacle_range: 2.5
  raytrace_range: 3
  max_obstacle_height: 0.4
  track_unknown_space: true

  observation_sources: point_cloud_sensorA point_cloud_sensorB

  point_cloud_sensorA: {
    sensor_frame: base_link,
    data_type: PointCloud2,
    topic: /obstacles_cloud,
    expected_update_rate: 0.5,
    marking: true,
    clearing: true,
    min_obstacle_height: 0.04
  }

  point_cloud_sensorB: {
    sensor_frame: base_link,
    data_type: PointCloud2,
    topic: /ground_cloud,
    expected_update_rate: 0.5,
    marking: false,
    clearing: true,
    min_obstacle_height: -1.0 # make sure the ground is not filtered
  }

With this in the launch file (note that openni or freenect's camera_nodelet_manager is used for efficiency):

<!-- Throttling messages -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
      <param name="rate" type="double" value="5"/>
      <param name="decimation" type="int" value="2"/>
   
      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
    
      <remap from="rgb/image_out"       to="data_resized_image"/>
      <remap from="depth/image_out"     to="data_resized_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
    </node>

    <!-- for the planner -->
    <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
      <remap from="depth/image"            to="data_resized_image_depth"/>
      <remap from="depth/camera_info"      to="data_resized_camera_info"/>
      <remap from="cloud"                  to="cloudXYZ" />
      <param name="decimation" type="int" value="1"/>                     <!-- already decimated above -->
      <param name="max_depth"  type="double" value="3.0"/>
      <param name="voxel_size" type="double" value="0.02"/>
    </node>
  
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection camera_nodelet_manager">
      <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>

      <param name="frame_id"             type="string" value="base_link"/>		
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
      <param name="min_cluster_size"     type="int" value="20"/>
      <param name="max_obstacles_height" type="double" value="0.4"/>
    </node>  
  </group>

Note also to simplify your launch file, you can create a new one and include rtabmap.launch unless there are ROS parameters you want to change not available as arguments of rtabmap.launch:

<launch>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
   <arg name="frame_id" value="base_link"/>
   <arg name="rtabmap_args" value="--delete_db_on_start --Reg/Force3DoF true --Optimizer/Slam2D true --DbSqlite3/InMemory true "/>
    ... (other arguments from rtabmap.launch)
  </include>
</launch>

For "Rtabmap/DetectionRate" parameter, I don't recommend increasing it over 1 Hz as the map size and loop closure detection computation time will increase faster. Increasing the rate would be in cases where there are holes between consecutive point clouds because the robot would move very fast.

cheers

[mjedmonds]

Matlabbe,

Sorry for the delayed response, but thank you very much! Extremely helpful. Now, everything is working robustly. If anyone would like the full setup I'm using, please reply and I'll post it.

Thanks

[ibd]

Mjedmonds, I am running into the same crash as you experienced. Could you post your full setup? :)

Thank you.

- ibd

[mjedmonds]

Hi ibd,

I'm using a VLP-16 now, but my costmap_common_params.yaml is very similar to what matlabbe posted:

footprint: [[0.42, -0.43], [0.42, 0.43], [-0.42, 0.43], [-0.42, -0.43]]
footprint_padding: 0.02

inflation_layer:
  inflation_radius: 0.65

transform_tolerance: 2

obstacle_layer:
  obstacle_range: 4.5
  raytrace_range: 5.0
  track_unknown_space: false

  observation_sources: point_cloud_sensorA point_cloud_sensorB
  
  point_cloud_sensorA: {
    sensor_frame: /base_link,
    data_type: PointCloud2,
    topic: /obstacles_cloud,
    expected_update_rate: 0.5,
    max_obstacle_height: 3.0,
    min_obstacle_height: 0.04,
    marking: true,
    clearing: true
  }

  point_cloud_sensorB: {
    sensor_frame: /base_link,
    data_type: PointCloud2,
    topic: /ground_cloud,
    expected_update_rate: 1.0,
    max_obstacle_height: 0.5,
    min_obstacle_height: -1.0, # make sure the ground is filtered
    marking: false,
    clearing: true
  }

[ibd]

BTW, I proposed a fix for this crash here:
https://github.com/ros-planning/navigation/issues/584

It has completely fixed the problem for me, including when I use rtabmap at higher update rates.