STVL working very slowly Humble 22.04

edit

I am working on creating a simple STVL local_costmap 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 voxel_layer 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"