Hi community,

env: Galactic

I am trying to implement the Spatio-temporal voxel layer package to avoid obstacles that are not detected by 2d scan. I am using gazebo depth camera.

I have some questions:

Do I need to add the stvl_layer to both local and global costmap or I can use it only for local costmap ?

Is it enough to specify the pointcloud observation source in stvllayer only to avoid obstacles or I need to add it also to obstaclelayer ?

If I add pointcloud observation source in stvl_layer only, the plannar plans through the obstacle: Note that the obstacle in front of the robot is not detectable by the laserscan

here is the log:

[INFO 1626931421.514384579] [controller_server]: Received a goal, begin computing control effort.
[WARN 1626931421.520858397] [controller_server]: No goal checker was specified in parameter 'current_goal_checker'. Server will use only plugin loaded general_goal_checker . This warning will appear once.
[INFO 1626931423.029277935] [controller_server]: Passing new path to controller. 
[INFO 1626931424.029268647] [controller_server]: Passing new path to controller. 
[INFO 1626931425.029358744] [controller_server]: Passing new path to controller. 

Then if I specify pointcloud observation source for stvl_layer and obstacle layer, the plannar sometimes plans the path through the obstacle and sometimes it avoids the obstacle, but i got errors/warns in the log

[INFO 1626931789.900263594] [controller_server]: Passing new path to controller. 
[WARN 1626931790.902719138] [bt_navigator_rclcpp_node]: Timed out while waiting for action server to acknowledge goal request for follow_path
[INFO 1626931790.903188264] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap 
[WARN 1626931790.946699561] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[INFO 1626931791.640286147] [controller_server]: Passing new path to controller. 
[INFO 1626931791.900269273] [controller_server]: Passing new path to controller. 
[rviz2-5] [INFO 1626931792.152939240] [rviz]: Message Filter dropping message: frame 'odom' at time 3749.690 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-5] [INFO 1626931792.153129298] [rviz]: Message Filter dropping message: frame 'odom' at time 3749.690 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[WARN 1626931792.987636210] [controller_server]: Control loop missed its desired rate of 2.0000Hz 
[INFO 1626931792.990406154] [controller_server]: Passing new path to controller. 
[rviz2-5] [INFO 1626931793.331718135] [rviz]: Message Filter dropping message: frame 'odom' at time 3750.192 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[rviz2-5] [INFO 1626931793.332792560] [rviz]: Message Filter dropping message: frame 'odom' at time 3750.192 for reason 'the timestamp on the message is earlier than all the data in the transform cache'
[WARN 1626931794.089128166] [controller_server]: Control loop missed its desired rate of 2.0000Hz 
[WARN 1626931967.647490865] [local_costmap.local_costmap]: Sensor origin at (4.91, 3.95) is out of map bounds. The costmap cannot raytrace for it.
[WARN 1626931968.646334343] [local_costmap.local_costmap]: Sensor origin at (4.91, 3.95) is out of map bounds. The costmap cannot raytrace for it.
[WARN 1626931969.649034248] [local_costmap.local_costmap]: Sensor origin at (4.91, 3.95) is out of map bounds. The costmap cannot raytrace for it.
[WARN 1626931970.646960699] [local_costmap.local_costmap]: Sensor origin at (4.91, 3.95) is out of map bounds. The costmap cannot raytrace for it.
[WARN 1626931971.649522944] [local_costmap.local_costmap]: Sensor origin at (4.91, 3.95) is out of map bounds. The costmap cannot raytrace for it.
[WARN 1626931972.648788006] [local_costmap.local_costmap]: Sensor origin at (4.91, 3.95) is out of map bounds. The costmap cannot raytrace for it.
[WARN 1626931973.648738112] [local_costmap.local_costmap]: Sensor origin at (4.91, 3.95) is out of map bounds. The costmap cannot raytrace for it.

here is my local and global costmap params:

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 5
      height: 5
      resolution: 0.05
      footprint_padding: 0.02
      footprint: "[[-0.79, -0.39], [-0.79, 0.39], [0.87, 0.39], [0.87, -0.39]]"
      plugins: ["obstacle_layer", "stvl_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan1 scan2 pointcloud
        scan1:
          topic: /scan_front
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 6.0
          raytrace_min_range: 0.6
          obstacle_max_range: 6.0
          obstacle_min_range: 0.6
        scan2:
          topic: /scan_back
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 6.0
          raytrace_min_range: 0.6
          obstacle_max_range: 6.0
          obstacle_min_range: 0.6
        pointcloud:
          topic: /realsense_depth_front/points
          max_obstacle_height: 3.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
          raytrace_max_range: 6.0
          raytrace_min_range: 0.6
          obstacle_max_range: 6.0
          obstacle_min_range: 0.6
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 10.0
        inflation_radius: 0.7
      stvl_layer:
        plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
        enabled: true
        voxel_decay: 15.
        decay_model: 0
        voxel_size: 0.05
        track_unknown_space: true
        max_obstacle_height: 3.0
        unknown_threshold: 15
        mark_threshold: 0
        update_footprint_enabled: true
        combination_method: 1
        origin_z: 0.0
        publish_voxel_map: true
        transform_tolerance: 0.2
        mapping_mode: false
        map_save_duration: 60.0
        observation_sources: pointcloud
        pointcloud:
          data_type: PointCloud2
          topic: /realsense_depth_front/points
          marking: true
          clearing: true
          obstacle_range: 6.0
          min_obstacle_height: 0.0
          max_obstacle_height: 3.0
          expected_update_rate: 0.0
          observation_persistence: 0.0
          inf_is_valid: false
          voxel_filter: false
          clear_after_reading: true
          max_z: 6.0
          min_z: 0.6
          vertical_fov_angle: 0.8745
          horizontal_fov_angle: 1.048
          decay_acceleration: 15.0
          model_type: 0
      static_layer:
        map_subscribe_transient_local: True
      # keepout_filter:
      #   plugin: "nav2_costmap_2d::KeepoutFilter"
      #   enabled: True
      #   filter_info_topic: "/costmap_filter_info"
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      footprint_padding: 0.02
      footprint: "[[-0.79, -0.39], [-0.79, 0.39], [0.87, 0.39], [0.87, -0.39]]"
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer", "stvl_layer"]
      # filters: ["keepout_filter"] # , "speed_filter"
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan1 scan2 pointcloud
        scan1:
          topic: /scan_front
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_min_range: 0.6
          obstacle_max_range: 20.0
          obstacle_min_range: 0.6
        scan2:
          topic: /scan_back
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 20.0
          raytrace_min_range: 0.6
          obstacle_max_range: 20.0
          obstacle_min_range: 0.6
        pointcloud:
          topic: /realsense_depth_front/points
          max_obstacle_height: 3.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
          raytrace_max_range: 6.0
          raytrace_min_range: 0.6
          obstacle_max_range: 6.0
          obstacle_min_range: 0.6
      stvl_layer:
        plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
        enabled: true
        voxel_decay: 15.
        decay_model: 0
        voxel_size: 0.05
        track_unknown_space: true
        max_obstacle_height: 3.0
        unknown_threshold: 15
        mark_threshold: 0
        update_footprint_enabled: true
        combination_method: 1
        origin_z: 0.0
        publish_voxel_map: true
        transform_tolerance: 0.2
        mapping_mode: false
        map_save_duration: 60.0
        observation_sources: pointcloud
        pointcloud:
          data_type: PointCloud2
          topic: /realsense_depth_front/points
          marking: true
          clearing: true
          obstacle_range: 6.0
          min_obstacle_height: 0.0
          max_obstacle_height: 3.0
          expected_update_rate: 0.0
          observation_persistence: 0.0
          inf_is_valid: false
          voxel_filter: false
          clear_after_reading: true
          max_z: 6.0
          min_z: 0.6
          vertical_fov_angle: 0.8745
          horizontal_fov_angle: 1.048
          decay_acceleration: 15.0
          model_type: 0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 10.0
        inflation_radius: 0.7
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True