Navigation costmap pointcloud: only first message received -> no updates

asked 2021-08-29 07:41:01 -0500

Mk-_- gravatar image

updated 2021-08-29 07:42:53 -0500

Hi

I am trying to use an Intel realsence D455 as a source for a point cloud in the local costmap at navigation2. I'm using ros foxy and cyclone DDS

Now I have encountered the phenomenon that both the Obstacle Layer and Voxel Layer only use the first message when starting the realsense node and do not receive any further updates after that. So any further changes in the pointcloud will not be used in the costmap. If I display the point cloud in RVIZ, it always shows the current state and is also up-to-date. The update rate is according to ros2 topic hzalso 15 If I store this in my costmap config I repeatedly get the message: observation buffer has not been updated.

Even if I display the voxel grid map, it is not updated.

Furthermore I have written a short Point Cloud splitter that drops the rgb data and republishes the changed PointCloud, but that didn't help. My costmap config for nav2

local_costmap:
ros__parameters:
  update_frequency: 5.0
  publish_frequency: 2.0
  global_frame: odom
  robot_base_frame: base_link
  use_sim_time: False
  rolling_window: true
  width: 5
  height: 5
  resolution: 0.05
  robot_radius: 0.8
  plugins: ["obstacle_layer", "inflation_layer"]
  # plugins: ["obstacle_layer","voxel_layer", "inflation_layer"]
  inflation_layer:
    plugin: "nav2_costmap_2d::InflationLayer"
    cost_scaling_factor: 3.0
    inflation_radius: 0.55
  obstacle_layer:
    plugin: "nav2_costmap_2d::ObstacleLayer"
    enabled: True
    observation_sources: scan realsense
    scan:
      topic: /scan
      max_obstacle_height: 2.0
      clearing: True
      marking: True
      data_type: "LaserScan"
    realsense:
      topic: /camera/depth/color/points
      max_obstacle_height: 3.0
      obstacle_max_range: 3.0
      min_obstacle_height: 0.1
      #expected_update_rate: 0.7
      clearing: True
      marking: True
      data_type: "PointCloud2"
  # voxel_layer:
  #   plugin: "nav2_costmap_2d::VoxelLayer"
  #   enabled: True
  #   publish_voxel_map: True # True for debugging but generates unnecessary high cpu load if not needed
  #   origin_z: 0.0
  #   z_resolution: 0.1
  #   z_voxels: 16
  #   mark_threshold: 0
  #   observation_sources: realsense
  #   realsense:
  #     topic: /camera/depth/color/points
  #     max_obstacle_height: 3.0
  #     obstacle_max_range: 3.0
  #     min_obstacle_height: 0.1
  #     expected_update_rate: 0.7
  #     clearing: True
  #     marking: True
  #     data_type: "PointCloud2"
  static_layer:
    map_subscribe_transient_local: True
  always_send_full_costmap: True

Thanks for any help

edit retag flag offensive close merge delete