Navigation costmap pointcloud: only first message received -> no updates
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 hz
also 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
I know it's been awhile, but for other people coming to this post, this appears to be a middlewear problem. If you're able to, I would change your QOS profile to BEST_EFFORT. You should be able to tell what the node is outputting by doing a
ros2 topic info /scan --verbose