Local Costmap in ROS2 not updating as robot moves

[osama-z-salah]

I'm currently working on a ROS2 project where I'm using a local costmap for robot navigation. The costmap is generated based on a point cloud data obtained from a OAK-D Pro camera. However, I'm encountering an issue where the local costmap freezes and does not update as the robot moves.

I have configured the local costmap using the following parameters:

local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link use_sim_time: True rolling_window: true width: 10 height: 10 resolution: 0.05 footprint: "[ [0.4665, 0.2890], [0.4665, -0.2890], [-0.4665, -0.2890], [-0.4665, 0.2890] ]" footprint_padding: 0.02 plugins: ["voxel_layer", "inflation_layer"] voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True footprint_clearing_enabled: true publish_voxel_map: True origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 max_obstacle_height: 2.0 unknown_threshold: 15 mark_threshold: 0 observation_sources: pointcloud combination_method: 1 pointcloud: topic: /stereo/points sensor_frame: oak_rgb_camera_optical_frame max_obstacle_height: 2.0 min_obstacle_height: 0.0 obstacle_max_range: 9.5 obstacle_min_range: 0.0 raytrace_max_range: 10.0 raytrace_min_range: 0.0 clearing: True marking: True data_type: "PointCloud2" inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 inflation_radius: 3.0

always_send_full_costmap: True

I have verified that the point cloud topic /stereo/points is publishing data correctly, and the sensor_frame parameter is set to match the frame ID of my 3D camera's optical frame.

However, despite these configurations, the local costmap freezes after receiving the first data from the point cloud and does not update as the robot moves. I suspect there may be an issue with the configuration or some missing parameters.

I would appreciate any insights or suggestions on how to resolve this problem. Thank you in advance for your help!

[Serafadam]

Hi, I guess if the camera keeps publishing data after costmap freeze, then this is more Nav2 oriented question, I'd suggest asking on official robotics stack exchange/discord or on Nav2 project Github repository itself, nevertheless here are some pointers:

• ros2 doctor --report will print out some general diagnostics
• Some problems with ROS2 topics come from different QoS used by publishers/subscribers
• ros2 topic info /oak/points -v will give you pub/sub QoS information
• I'd also check out if all timestamps on headers are correct in Image/Pointcloud/TF data

[osama-z-salah]

Thank you for your response. In fact, I have already attempted the same process in ROS1 Noetic, and it worked without any issues. Therefore, I wanted to inquire about the potential disparities in Quality of Service (QoS) for the point cloud topic between ROS1 and ROS2. I have been unable to access the QoS information in ROS1, and I noticed that in ROS2, I couldn't visualize the point cloud data in RViz until I adjusted the History Policy to "system default" and the Reliability to "best effort."

[osama-z-salah]

Futhermore, when i check the QoS profile for publisher(camera) and subscriber(costmap), both showing Reliability: BEST_EFFORT. However, the point cloud data is not updating in the costmap.

[saching13]

This might be an issue coming in from depth_image_proc which handles the depth to point cloud conversion.