[ROS2] costmap does not clear when voxels are cleared

[tal-grossman]

Bug report

I'm new in using the the stvl layer (b.t.w - another great job!), and might be doing something wrong.
But according to the tutorials and to the answer given here Spatio temporal voxel layer: Clearing voxels not seen but included in fov - which explicitly saying that if a voxel is removed than the costmap should be updated as well , I think there might be a sync issue - when voxels are cleared after decaying but the costmap is not.

please look at the video -
voxels are green, taken from depth pcl. The 2D local costmap layer is pink.

stvl_sync_clear_issue-2020-12-24_10.06.25.mp4

Required Info:

• Operating System:
  Ubuntu 20.04
• Version or commit hash:
  • stvl branch: build from foxy-devel, commit: 4f538989191ae29325ddfc912ea452172ea765f8
  • nav2: foxy binaries: 0.4.5-1focal.20201210.084248

Steps to reproduce issue

• Implement the stvl layer to the local costmap - seee parameters:

  local_costmap:
    local_costmap:
      ros__parameters:
        update_frequency: 5.0
        publish_frequency: 2.0
        global_frame: map # default odom
        robot_base_frame: base_link
        use_sim_time: True
        rolling_window: true
        width: 4
        height: 4
        resolution: 0.05
        footprint: "[[0.083, 0.16033], [-0.36511, 0.16033], [-0.36511, -0.16033], [0.083,-0.16033]]"
        footprint_padding: 0.01
        robot_radius: 0.16 # not in use when has footprint
        lethal_cost_threshold: 50
        plugins: [ "inflation_layer",  "head_stvl_layer" ] 
        inflation_layer:
          plugin: "nav2_costmap_2d::InflationLayer"
          enabled: true
          inflation_radius: 0.08 #0.55 default
          cost_scaling_factor: 5.0
          inflate_unknown: false
          inflate_around_unknown: false

        head_stvl_layer:
          plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
          enabled: true
          voxel_decay: 10.0  # seconds if linear, e^n if exponential
          decay_model: 0     # 0=linear, 1=exponential, -1=persistent
          voxel_size: 0.2  # meters
          track_unknown_space: false  # default space is known
          max_obstacle_height: 2.0   # meters
          unknown_threshold: 15
          mark_threshold: 2
          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.2   # seconds
          mapping_mode: false # default off, saves map not for navigation
          map_save_duration: 60.0  # default 60s, how often to autosave
          observation_sources: head_depth_pointcloud_mark head_depth_pointcloud_clear
          head_depth_pointcloud_mark:
            data_type: PointCloud2
            topic: /sensors/depth_cameras/head/points
            marking: true
            clearing: false
            obstacle_range: 2.0          # meters
            min_obstacle_height: 0.3     # default 0, meters
            max_obstacle_height: 1.5     # default 3, meters
            expected_update_rate: 0.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
            filter: "passthrough"        # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on
            voxel_min_points: 8         # default 0, minimum points per voxel for voxel filter
            clear_after_reading: true    # default false, clear the buffer after the layer gets readings from it
          head_depth_pointcloud_clear:
            data_type: PointCloud2
            topic: /sensors/depth_cameras/head/points
            marking: false
            clearing: true
            max_z: 3.0                  # default 0, meters # maximum distance from camera it can see
            min_z: 0.2                  # default 10, meters
            vertical_fov_angle: 0.8745  # default 0.7, radians
            horizontal_fov_angle: 1.048 # default 1.04, radians
            decay_acceleration: 5.0     # default 0, 1/s^2. If laser scanner MUST be 0 #acceleration scales the model's decay in presence of readings
            model_type: 0                # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar

• PCL is directed to a wall, voxels are created and projected to the local costmap
• turn the robot and watch the voxels decay
• costmap is still "dirty" with decayed voxels.

Expected behavior

costmap is synced to stvl layer. when voxeled are cleared the costmap should be updated accordingly

Actual behavior

After voxels decayed and cleared from layer the costmap is not totally cleared

Additional information

• When reproducing this scenrio only the STVL and inflation layer are active

[SteveMacenski]

voxel_min_points: 8

this is a new user-contributed feature, so that’s suspect to me to consider the source of the issue. I haven’t seen that behavior before with a similar configuration on my robots. If you try the default file for standard env, that’s exactly what I use, I’d be interested if you see it there.

If the internal voxel grid decays them all, the 2D costmap should definitely fully-clear. I’d venture to guess that the new feature addition wasn’t fully thought through and I didn’t catch it.

[tal-grossman]

I tried both.

• just use the default voxel_min_points: 0
  and
• default file: standard_indoor_environment_config.yaml

got the same issue

stvl_sync_clear_issue_standart-2020-12-27_18.29.31.mp4

[SteveMacenski]

I'd recommend trying on a commit hash after the voxel min was added to validate that that is indeed the issue

[tal-grossman]

I'd recommend trying on a commit hash after the voxel min was added to validate that that is indeed the issue

I am not sure which commit hash did you mean but I tried

• b963de782c5c07266f18f994311a155221a6d59e which is the one before the Add voxel_min_points param for voxel filter (ROS2)
• cf9cd2c9340b15467fee7065a5726f28a5030c5a which is Add voxel_min_points param for voxel filter (ROS2)
• and every commit hash that followed after that one until the current one.

They all got the same undesired behavior, So I can't say that this new user-contributed feature is the source of the issue

[SteveMacenski]

Ok. I’m back to work on Monday and I can take a look at this sometime next week. A minimum reproduction example would be helpful if you have a short dataset you can share with that config file.

[tal-grossman]

After some more digging trying to find better possible causes for that behavior we think we might find something.

• There seem to be that the costmap resolution and the inflation_layer: inflation_radius are some how a major influence on this. when the inflation_radius is big there is lesser chance the costmap is not cleared when the voxels decayed.
  The easiest way do see it is to use the nav2_tb3_simulation with inflation_radius: 0.08 .
  please see params:

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["rgbd_obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.08
      rgbd_obstacle_layer:
        plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
        enabled:                  true
        voxel_decay:              10.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
        max_obstacle_height:      2.0   # meters
        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.2   # 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_mark:
          data_type: PointCloud2
          topic: /intel_realsense_r200_depth/points
          marking: true
          clearing: false
          obstacle_range: 3.0          # meters
          min_obstacle_height: 0.3     # default 0, meters
          max_obstacle_height: 2.0     # default 3, meters
          expected_update_rate: 0.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
          filter: "passthrough"        # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on
          voxel_min_points: 0          # default 0, minimum points per voxel for voxel filter
          clear_after_reading: true    # default false, clear the buffer after the layer gets readings from it
        rgbd1_clear:
          data_type: PointCloud2
          topic: /intel_realsense_r200_depth/points
          marking: false
          clearing: true
          max_z: 7.0                  # default 0, meters
          min_z: 0.1                  # default 10, meters
          vertical_fov_angle: 0.8745  # default 0.7, radians
          horizontal_fov_angle: 1.048 # 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. 0=depth camera, 1=3d lidar like VLP16 or similar
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

https://user-images.githubusercontent.com/54407092/104180976-afc6ff00-5416-11eb-9ca2-f7d1bb7f95ad.mp4
rviz configuration:
rviz_config_stvl_tb3.zip

• Another possible bad behavior is when the inflation layer plugin is delacred first before the stvl_layer (But we are not sure about it). like such:
  plugins: [ "inflation_layer" , "rgbd_obstacle_layer"]

The bottom line is that we are not sure it is a STVL issue but rather a nav2_costmap issue. any thoughts?

[SteveMacenski]

The inflation stuff should have nothing to do with STVL. The layers in the costmap don't impact each other except for the final pixel assemblies for which to take: max or last, which you're using combination method max so that's not an issue. I think what you're seeing isn't an actual trend that generalizes.

voxel_size: 0.1 # meters

Have you tried making this match your actual costmap resolution? Maybe your actual issue is raytracing-based. It's really bizarre to me that your STVL resolution doesn't match the master costmap's resolution. I see the lethal-marked voxels on the costmap with space between them like its a multiple of the costmap resolution.

That video is strange because I think that light blue color left over isn't actually from STVL. STVL doesn't do "some cost" obstacles, its either lethal or its not. So I don't think that's actually (maybe?) cause by STVL if I'm seeing that correctly as a non-lethal cost. Maybe a weird interaction in the inflation layer not really handling layer data exported that don't match the costmap resolution?

[tal-grossman]

I tried running the tb3 simulation again but with matching the voxel_size to the costmap resolution (both 0.05) and had no inflation layer and got the same bad result, see params and attached video:

local_costmap:
  local_costmap:
    ros__parameters:
      always_send_full_costmap: True
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["rgbd_obstacle_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.1
      rgbd_obstacle_layer:
        plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
        enabled:                  true
        voxel_decay:              10.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.0   # meters
        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.2   # 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_mark:
          data_type: PointCloud2
          topic: /intel_realsense_r200_depth/points
          marking: true
          clearing: false
          obstacle_range: 3.0          # meters
          min_obstacle_height: 0.3     # default 0, meters
          max_obstacle_height: 2.0     # default 3, meters
          expected_update_rate: 0.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
          filter: "passthrough"        # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on
          voxel_min_points: 0          # default 0, minimum points per voxel for voxel filter
          clear_after_reading: true    # default false, clear the buffer after the layer gets readings from it
        rgbd1_clear:
          data_type: PointCloud2
          topic: /intel_realsense_r200_depth/points
          marking: false
          clearing: true
          max_z: 7.0                  # default 0, meters
          min_z: 0.1                  # default 10, meters
          vertical_fov_angle: 0.8745  # default 0.7, radians
          horizontal_fov_angle: 1.048 # 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. 0=depth camera, 1=3d lidar like VLP16 or similar
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

stvl_sync_clear_issue_nav2_tb3_sim_no_inflation_same_res-2021-01-12_12.23.36.mp4

than, I added the inflation layer with inflation_radius: 0.1 (so its costmap resolution by factor of 2) and got GOOD expected result, see params and attached video:

local_costmap:
  local_costmap:
    ros__parameters:
      always_send_full_costmap: True
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["rgbd_obstacle_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.1
      rgbd_obstacle_layer:
        plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
        enabled:                  true
        voxel_decay:              10.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.0   # meters
        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.2   # 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_mark:
          data_type: PointCloud2
          topic: /intel_realsense_r200_depth/points
          marking: true
          clearing: false
          obstacle_range: 3.0          # meters
          min_obstacle_height: 0.3     # default 0, meters
          max_obstacle_height: 2.0     # default 3, meters
          expected_update_rate: 0.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
          filter: "passthrough"        # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on
          voxel_min_points: 0          # default 0, minimum points per voxel for voxel filter
          clear_after_reading: true    # default false, clear the buffer after the layer gets readings from it
        rgbd1_clear:
          data_type: PointCloud2
          topic: /intel_realsense_r200_depth/points
          marking: false
          clearing: true
          max_z: 7.0                  # default 0, meters
          min_z: 0.1                  # default 10, meters
          vertical_fov_angle: 0.8745  # default 0.7, radians
          horizontal_fov_angle: 1.048 # 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. 0=depth camera, 1=3d lidar like VLP16 or similar
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

stvl_sync_clear_issue_nav2_tb3_sim_with_inflation_0.1_same_res-2021-01-12_12.31.28.mp4

Since we have the same issue without the inflation layer but got expected result with inflation layer with a certain inflation radius I still suspect an issue with the nav2_costmap.

[SteveMacenski]

I cannot emphasize enough how much I don't think the layers interact at all unless its an issue with the master costmap itself. This is possible, but doesn't seem like the most likely candidate.

I think maybe a good next step is to publish the STVL 2d costmap to visualize so you can compare against the master costmap here and that one. That way when you see things left over, you can see if STVL state is wrong or the costmap layer isn't being updated correctly. Maybe a bounds issue?

I'm also vaguely wondering if this is an rviz issue too where the /updates topic is dropping a message so the diffs are wrong. What happens if you send as https://github.com/ros-planning/navigation2/blob/053d0c8cc96553f951d21be843f86099dba627d5/nav2_costmap_2d/src/costmap_2d_ros.cpp#L286 send full costmap as true?

This just seems like such a bizarre issue to not actually be caused by STVL (or if STVL, where it came from out of the blue if not caused by the only recent change with the min voxel count)

[tal-grossman]

I cannot emphasize enough how much I don't think the layers interact at all unless its an issue with the master costmap itself. This is possible, but doesn't seem like the most likely candidate.

This just seems like such a bizarre issue to not actually be caused by STVL (or if STVL, where it came from out of the blue if not caused by the only recent change with the min voxel count)

As I said, I do not think it is necessarily a STVL issue and it seems more sense that it is a "master" nav2_costmap issue, due to the impact the costmap's resolution and other layers (inflation) have on this bug. regarding the recent change with the min voxel count, tried it already with 0 and with commits before this change.

I think maybe a good next step is to publish the STVL 2d costmap to visualize so you can compare against the master costmap here and that one. That way when you see things left over, you can see if STVL state is wrong or the costmap layer isn't being updated correctly. Maybe a bounds issue?

Can you elaborate on how should we do this? should be possible as I can easily reprocuce it with the tb3 simulation

I'm also vaguely wondering if this is an rviz issue too where the /updates topic is dropping a message so the diffs are wrong.

I dont believe it is an rviz issue since my actual robot would stuck in these ghost obstacles and the nav_to_goal would fail.

What happens if you send as https://github.com/ros-planning/navigation2/blob/053d0c8cc96553f951d21be843f86099dba627d5/nav2_costmap_2d/src/costmap_2d_ros.cpp#L286 send full costmap as true?

as you can see from the params we already had this one set to "true". did you mean something else?

[SteveMacenski]

Can you elaborate on how should we do this? should be possible as I can easily reprocuce it with the tb3 simulation

When we populate the master grid (this area of the code) https://github.com/SteveMacenski/spatio_temporal_voxel_layer/blob/foxy-devel/src/spatio_temporal_voxel_layer.cpp#L642-L661, create a new occupancy grid object and then publish it so you can see what STVL sees -- or just translate costmap_ itself at the end. If you don't see it there, then we know its a costmap master issue we can go sorting down. If its there, then its an STVL / configuration / porting issue.

I dont believe it is an rviz issue since my actual robot would stuck in these ghost obstacles and the nav_to_goal would fail.

Good to know

as you can see from the params we already had this one set to "true". did you mean something else?

👍

[SteveMacenski]

Any update on this? I haven't heard any other reports

Edit: whoops, hit the wrong button