STVL working very slowly Humble 22.04
I am working on creating a simple STVL localcostmap setup with only one camera. There are no warnings or errors outputted, and I am able to visualize the camera pointcloud with the robot on a map in rviz. The standard voxellayer that is provided with nav2 works as expected without any delays.
With the following STVL configuration I am experiencing a map update time of more than 1 second. The map update time was found through enable the debug log level for the controller server / local costmap.
I added some printing of timestamps within the code and found that the delay was coming from a combination of the voxel grid marking and transforming the pointcloud to include the odometry. The pointcloud transform was taking the longest time (close to a second) despite the transform being the same as is done in the other costmap plugins.
Please let me know if I'm doing anything wrong. Thanks for the help.
STVL (slow) config:
voxel_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.1 # meters
track_unknown_space: true # default space is known
unknown_threshold: 15 # voxel height
mark_threshold: 0 # voxel height
update_footprint_enabled: true
combination_method: 1 # 1=max, 0=override
origin_z: 0.0 # meters
publish_voxel_map: true # default off
transform_tolerance: 0.3 # seconds
mapping_mode: false # default off, saves map not for navigation
map_save_duration: 60.0 # default 60s, how often to autosave
observation_sources: rgbd1_mark rgbd1_clear
rgbd1_mark:
data_type: PointCloud2
topic: /rs_top/depth/points
marking: true
clearing: false
obstacle_range: 3.0 # meters
min_obstacle_height: 0.02 # default 0, meters
max_obstacle_height: 1.5 # default 3, meters
expected_update_rate: 0.0 # default 0,
observation_persistence: 0.0 # default 0,
inf_is_valid: true # default false, for laser scans
filter: "passthrough" # default passthrough, a
voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter
clear_after_reading: true # default false,
rgbd1_clear:
data_type: PointCloud2
topic: /rs_top/depth/points
marking: false
clearing: true
max_z: 7.0 # default 0, meters
min_z: 0.1 # default 10, meters
vertical_fov_angle: 1.0337 # default 0.7, radians
horizontal_fov_angle: 1.575 # default 1.04, radians
decay_acceleration: 5.0 # default 0, 1/s^2. If laser scanner MUST be 0
model_type: 0 # default 0, model type for frustum.
Standard Voxel Layer (fast) config:
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
footprint_clearing_enabled: true
max_obstacle_height: 2.0
publish_voxel_map: false
origin_z: 0.0
z_resolution: 0.1
z_voxels: 16
max_obstacle_height: 2.0
unknown_threshold: 4
mark_threshold: 2
observation_sources: rs_top_mark rs_top_clear
combination_method: 1
rs_top_mark: # no frame set, uses frame from message
topic: /rs_top/depth/points
max_obstacle_height: 1.5
min_obstacle_height: 0.02
obstacle_max_range: 3.0
obstacle_min_range: 0.0
raytrace_max_range: 3.2
raytrace_min_range: 0.0
clearing: True
marking: True
data_type: "PointCloud2"
rs_top_clear: # no frame set, uses frame from message
topic: /rs_top/depth/points
max_obstacle_height: 10.0
min_obstacle_height: 0.0
obstacle_max_range: 3.0
obstacle_min_range: 0.0
raytrace_max_range: 5.0
raytrace_min_range: 0.0
clearing: True
marking: False
data_type: "PointCloud2"
Asked by mattfield96 on 2023-03-08 11:08:34 UTC
Comments