Spatial_temporal_voxel_layer not being populated
Greetings , I am trying to setup the tutorial of Spatial Temporal Voxel Layer (STVL) from the offical website on ROS Foxy (installed using debs), Ubuntu 20.04. STVL is compiled from source (foxy-devel branch).
The costmap layer is not creating any obstacles based on the points read from the sensor. The following msgs are displayed on the console:
[controller_server-2] [WARN] [1626235911.078158591] [local_costmap.local_costmap]: intel_realsense_r200_depth/points buffer updated in 51.86s, it should be updated every 0.00s.
[controller_server-2] [WARN] [1626235911.078251423] [local_costmap.local_costmap]: intel_realsense_r200_depth/points buffer updated in 51.86s, it should be updated every 0.00s.
[planner_server-3] [WARN] [1626235911.085046724] [global_costmap.global_costmap]: intel_realsense_r200_depth/points buffer updated in 51.86s, it should be updated every 0.00s.
[planner_server-3] [WARN] [1626235911.085151591] [global_costmap.global_costmap]: intel_realsense_r200_depth/points buffer updated in 51.86s, it should be updated every 0.00s.
The params are as follows:
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: True
footprint_padding: 0.03
update_frequency: 1.0
publish_frequency: 1.0
origin_x: -5.0 # Otherwise the robot is lying outside the costmap
origin_y: -5.0
global_frame: map
robot_base_frame: base_link
robot_radius: 0.22 # radius set and used, so no footprint points
resolution: 0.05
width: 10
height: 10
plugins: ["inflation_layer", "vlp16_stv_layer", "static_layer"] #
vlp16_stv_layer:
plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
enabled: true
voxel_decay: 1.0 # seconds if linear, e^n if exponential
decay_model: 0 # 0=linear, 1=exponential, -1=persistent
voxel_size: 0.05 # meters
track_unknown_space: true # default space is known
max_obstacle_height: 2.5 # meters
unknown_threshold: 15 # voxel height
mark_threshold: 0 # voxel height
update_footprint_enabled: true
combination_method: 1 # 1=max, 0=override
obstacle_range: 5.0 # meters
origin_z: 0.0 # meters
publish_voxel_map: false # default off
transform_tolerance: 1.0 # seconds
mapping_mode: false # default off, saves map not for navigation
map_save_duration: 60.0 # default 60s, how often to autosave
observation_sources: realsense
realsense:
data_type: PointCloud2
topic: intel_realsense_r200_depth/points
marking: true
clearing: true
min_obstacle_height: 0.2 # default 0, meters
max_obstacle_height: 2.5 # default 3, meters
expected_update_rate: 1.0 # default 0, if not updating at this rate at least, remove from buffer
observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest
inf_is_valid: false # default false, for laser scans
voxel_filter: false # default off, apply voxel filter to sensor, recommend on
clear_after_reading: true # default false, clear the buffer after the layer gets readings from it
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
enabled: true
inflation_radius: 0.55
cost_scaling_factor: 1.0
inflate_unknown: false
inflate_around_unknown: true
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
always_send_full_costmap: True
I am using identical params for local-costmap.
I am able to run simulation with 2D LiDAR but not with Realsense sensor.
EDIT 1: The echo of topic 'intel_realsense_r200_depth/points' is as follows:
header:
stamp:
sec: 32
nanosec: 600000000
frame_id: camera_depth_frame
height: 1
width: 76800
fields: '<sequence type: sensor_msgs/msg/PointField, length: 4>'
is_bigendian: false
point_step: 32
row_step: 2457600
data: '<sequence type: uint8, length: 2457600>'
is_dense: false
Given that height is read as 1, is sensor data correctly being published?
Am I missing something in settings? Thanks