0
$\begingroup$

I am having issues adding PointCloud data to the costmap as obstacles. The data is coming from a /marker_to_pointcloud topic that takes visualization_msgs/MarkerArray and publishes into PointCloud2.

I then add that topic in my list of observation sources for the costmap as such:


global_costmap:
  plugins:
    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: virtual_layer,   type: "virtual_costmap_layer::VirtualLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
...
  obstacle_layer:
    enabled: true
    observation_sources:  laser_scan_sensor camera_sensor
    laser_scan_sensor: {sensor_frame: lidar_link, data_type: LaserScan, topic: /scan, marking: true, clearing: true}
    camera_sensor: {sensor_frame: camera_link, data_type: PointCloud2, topic: /realsense_pointcloud2, marking: true, clearing: true}
    obstacle_range: 10.0
    raytrace_range: 10.0
    min_obstacle_height: -0.5
    max_obstacle_height: 1.5
    combination_method: 1
    debug: true

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

  # 各TFフレームの設定
  global_frame: odom
  robot_base_frame: base_footprint

  # #inflation_layerの設定
  inflation_layer:
    enabled: true
    inflation_radius: 0.2
    cost_scaling_factor: 15.8


  # 更新頻度の設定
  update_frequency: 3.0
  publish_frequency: 3.0

  # ローカルコストマップの計算範囲
  rolling_window: true
  width: 6.0
  height: 6.0

  # ローカルコストマップの解像度
  resolution: 0.05

  # タイムスタンプのずれの許容量
  transform_tolerance: 0.5

  static_layer:
    enabled: true
    map_topic: "/map"
    track_unknown_space: false
    subscribe_to_updates: true
  
  obstacle_layer:
    observation_sources: laser_scan_sensor camera_sensor
    laser_scan_sensor: {sensor_frame: lidar_link, data_type: LaserScan, topic: /scan, marking: true, clearing: true}
    camera_sensor: {sensor_frame: camera_link, data_type: PointCloud2, topic: /realsense_pointcloud2, marking: true, clearing: true}
    obstacle_range: 10.0
    raytrace_range: 10.0
    min_obstacle_height: -0.5
    max_obstacle_height: 1.5
    debug: true

I made sure I am calling those yaml files in my move_base launch file.

Additionally, I checked the ranges for the PointCloud to make sure the min_obstacle_height , max_obstacle_height are within range by listening to /realsense_pointcloud2 in the following way:

def callback(data):
    points = list(pc2.read_points(data, field_names=("x", "y", "z"), skip_nans=True))
    for point in points:
        x, y, z = point
        print(f"x: {x}, y: {y}, z: {z}")

a snippet of the results:

x: 0.6240000128746033, y: -0.1398918330669403, z: 0.0 x: 0.6506802439689636, y: -0.16479024291038513, z: 0.0

I`ve added screenshots of Rviz with and without the laserscan topic, since this does get taken as an obstacle, for clarity

Rviz with laserscan topic

Rviz without laserscan topic

and also I have added the visualization of just the costmap, you can see some overlapping with the pointcloud but that is coming from the laserscan that detects the obstacle as well.

Additional info: The Marker comes from a YOLO bounding box for humans using a RealSense D435 camera, which gets projected into the map.

marker example

I am using Ubuntu 20.04,ROS1 Noetic. Any help is appreciated and I`m glad to give more info :))

$\endgroup$

1 Answer 1

0
$\begingroup$

You've set the "clearing" flag for both ObstacleLayer sources. In other words, I think you've asked the lidar source to remove depth data, and you've asked the depth source to remove lidar data.

Try using a separate ObstacleLayer for each source.

$\endgroup$
2
  • $\begingroup$ I added a separate layer for each source but it does not solve the issue as the PointCloud is still not reflected in the costmap ` global_costmap: plugins: - {name: obstacle_layer1, type: "costmap_2d::ObstacleLayer"} - {name: obstacle_layer2, type: "costmap_2d::ObstacleLayer"} obstacle_layer1: observation_sources: laser_scan_sensor obstacle_layer2: observation_sources: camera_sensor ` For good measure I also tried to remove the laser_sensor and use only the camera as observation source but it was unsuccessful $\endgroup$
    – pca1
    Commented Jul 5 at 2:04
  • $\begingroup$ You need to debug why depth camera is not giving you obstacle data. It's common to get the frame transform wrong for an optical frame where z-axis does not point upward. $\endgroup$
    – Mike973
    Commented Jul 5 at 11:19

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.