Local costmap is empty

[ryleymcc]

I can see rtabmap publishing the cloud_obstacle topic in rviz.
I can't see move_base subscribing to cloud_obstacles in rqt_graph.
The local costmap is empty and TEB local planner does not avoid local obstacles.
My global costmap seems to be working fine and a global path is created without any problems.
I am using D435i and T265 real-sense cameras.

This is my costmap_common_params.yaml

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[-0.35, 0.6], [1.32 , 0.6], [1.32, -0.6], [-0.35, -0.6]]
#robot_radius: 1
inflation_layer:
  inflation_radius: 10

transform_tolerance: 2

obstacle_layer:
  obstacle_range: 3.5
  raytrrace_range: 4
  max_obstacle_height: -99999
  min_obstacle_height: 99999

  track_unknown_space: true


This is my local_costmap_params.yaml

local_costmap:
  global_frame: t265_odom_frame
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.05
  #transform_tolerance: 10
  observation_sources: point_cloud_sensor
  point_cloud_sensor: {sensor_frame: odom,
  data_type: PointCloud2,
  topic: /rtabmap/cloud_obstacles,
  marking: true, clearing: false,
  min_obstacle_height: -99999,
  max_obstacle_height: 999999}


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

this is my working global_costmap_params.yaml

global_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 1.0
  static_map: true
  observation_sources: point_cloud_sensor
  point_cloud_sensor: {sensor_frame: odom, data_type: PointCloud2, topic: /rtabmap/cloud_obstacles, marking: true, clearing: false}

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


This is my launch file
<launch>
  <include file="$(find realsense2_camera)/launch/rs_d400_and_t265.launch" />
  <include file="$(find rtabmap_ros)/launch/rtabmap.launch"  >
 
    <arg name="odom_topic" value="/t265/odom/sample"/>
    <arg name="frame_id" value="t265_link"/>
    <arg name="rgbd_sync" value="true"/>
    <arg name="depth_topic" value="/d400/aligned_depth_to_color/image_raw"/>
    <arg name="rgb_topic" value="/d400/color/image_raw"/>
    <arg name="camera_info_topic" value="/d400/color/camera_info"/>
    <arg name="approx_rgbd_sync" value="false"/>
    <arg name="visual_odometry" value="false"/> 
    <arg name="localization" value="false"/>

  </include>
 
</launch>

When i go into localization mode cloud_obstacles is not published anymore so how could i avoid obstacles in localization mode?

[matlabbe]

Hi,

You should not use /rtabmap/cloud_obstacles for the local costmap. You should send the camera point cloud directly to local costmap. You may have to downsample the point cloud for performance issue or segment the ground from the obstacles. See his example: http://wiki.ros.org/rtabmap_ros/Tutorials/StereoOutdoorNavigation#Local_costmap

In that example, a disparity image is used in rtabmap_ros/point_cloud_xyz, but that node can also subscribe to a depth image instead (by remapping depth/image and depth/camera_info).

In localization mode, cloud_obstacles would be published only after the robot has been localized.

cheers,
Mathieu

[ryleymcc]

ok, so I should be using the obstacle topic published by rtabmap_ros/obstacles_detection nodelet to update my local costmap. Why can't I use the rtabmap/local_grid_obstacle topic? I don't really understand why my local costmap doesn't work with the rtabmap/local_grid_obstacle since I can see the topic publishing the obstacle point cloud in rviz. My observation source in the local_costmap_param.yaml should force move_base to subscribe to the rtabmap/local_grid_obstacle right? EDIT: move_base is now subscribing to  rtabmap/local_grid_obstacle and I can see it in rqt_graph however local costmap still does not get filled in with obstacles.

Clearly, I am missing some fundamental knowledge... maybe you know wht i need to know...
Thanks a lot for your help.

[ryleymcc]

rostopic echo /rtabmap/local_grid_obstacle:



rostopic echo /move_base/local_costmap/costmap:

[ryleymcc]

Solved the issue. I was simply using the wrong observation source frame_id. I changed it to the same frame as the local costmap. Duhhh.

I also did some research on using the obstacle detection node for higher frequency generated local costmap.