Obstacles are cleared from the voxel layer but not from the local costmap

[siferati]

Edit: forgot to mention, I'm using ros melodic. STVL was installed by sudo apt-get install ros-melodic-spatio-temporal-voxel-layer

Hello, I'm having a problem where the obstacles are cleared from the voxel layer but not from the local costmap. However, this only affects the local costmap, the global costmap works fine!

You can see here a short clip I recorded of it happening. The green lines you see once I give it a new nav goal are from the costmap_converter, proof that the local costmap was indeed not cleared and it's not just a visual bug.

Below are my costmap params:

# common_costmap.yaml

robot_base_frame: base_footprint
footprint: [[-0.7, -0.55], [0.7, -0.55], [0.7, 0.55], [-0.7, 0.55]]
publish_frequency: 30

rgdb_obstacles:
  enabled:                  true
  voxel_decay:              30
  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
  obstacle_range:           8.5
  origin_z:                 0.0
  publish_voxel_map:        true
  observation_sources:      realsense_mark realsense_clear

  realsense_mark:
    data_type: PointCloud2
    topic: realsense/depth/points
    marking: true
    clearing: false
    min_obstacle_height: 0.01
    max_obstacle_height: 3.0
    voxel_filter: true
    
  realsense_clear:
    data_type: PointCloud2
    topic: realsense/depth/points
    marking: false
    clearing: true
    min_z: 0.01
    max_z: 9.0
    vertical_fov_angle: 0.96
    horizontal_fov_angle: 1.22
    decay_acceleration: 5
    model_type: 0

# global_costmap.yaml

global_costmap:
  plugins:
    - {name: map, type: 'costmap_2d::StaticLayer'}
    - {name: rgdb_obstacles, type: 'spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer'}
    - {name: inflation, type: 'costmap_2d::InflationLayer'}

  map:
    subscribe_to_updates: true

  inflation:
    inflation_radius: 1
    cost_scaling_factor: 5

# local_costmap.yaml

local_costmap:
  plugins:
    - {name: rgdb_obstacles, type: 'spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer'}

  rolling_window: true
  width: 6
  height: 6
  resolution: 0.05

[SteveMacenski]

admittedly, its been a long time since I've looked at the ROS1 nav configuration file hierarchy, so I'd make sure first of all that your parameters are actually making it to the layer in the first place.

Try using our provided configuration file for indoor use: https://github.com/SteveMacenski/spatio_temporal_voxel_layer/blob/melodic-devel/example/standard_indoor_environment_config.yaml I know companies using that exactly so if that starts working for you, we can back out the issue one param change at a time.

Unfortunately, I'm so far removed from ROS1 that I can't test this easily. Another thing to test is rolling back melodic to this commit 9ef8784 and seeing if that fixes the issue, that's the last personally tested to be known working commit I have. If you still see it, then there's something very odd going on.

What happens if you put the static layer under the STVL layer in the local costmap?

@tasilb are you still on Melodic? If so, you see any issues in the binaries?

[siferati]

make sure first of all that your parameters are actually making it to the layer in the first place.

They are, otherwise I wouldn't be able to see the voxels in rviz (default is off) nor would the layer be aware of the correct observation sources and topics.

Try using our provided configuration file for indoor use

Problem persisted.

What happens if you put the static layer under the STVL layer in the local costmap?

(by under, you mean to place the static layer first in the plugin list, right?)

That fixes the problem! Obstacles are now correctly removed from the local costmap when the voxels are removed from the STVL layer. I guess this was the reason why the global costmap was working fine while the local costmap wasn't - the global costmap already had a static layer as the bottom layer.

Any clue as to what the problem might be? For navigation purposes I would much prefer to not have the static map present in the local costmap. A temporary workaround could be to have a static layer pointing to a dummy map topic, so for all intents and purposes the static layer wouldn't actually be doing anything - but that doesn't solve the underlying issue.

[nickvaras]

The video that you shared is 26 seconds long, but your voxel decay is set to 30 seconds, so maybe part of the box was outside of the camera frustum and hence kept for the full 30 seconds (without decay acceleration)? There's a tool to visualize the frustum if I remember correctly.

[tal-grossman]

@SteveMacenski although this ticket is ros1 it seems very similar to #187 don't you think?

[siferati]

The video that you shared is 26 seconds long, but your voxel decay is set to 30 seconds, so maybe part of the box was outside of the camera frustum and hence kept for the full 30 seconds (without decay acceleration)?

The obstacle stays in the local costmap indefinitely - I just tested it again to make sure, by waiting ~5min.

[SteveMacenski]

Did you try the older version I suggested before the min points in voxel filter was added?

[siferati]

Did you try the older version I suggested before the min points in voxel filter was added?

Yes, same problem.

[SteveMacenski]

My guess then it has something to do with the sizing. Since the static layer will set the size of the costmap by having content, when there's nothing behind it and we cleared the costmap grid then this function https://github.com/SteveMacenski/spatio_temporal_voxel_layer/blob/ros2/src/spatio_temporal_voxel_layer.cpp#L672-L691 wouldn't touch any cells to update the bounds to then update the appropriate costs.

https://github.com/SteveMacenski/spatio_temporal_voxel_layer/blob/ros2/src/spatio_temporal_voxel_layer.cpp#L733-L755 we clear than mark, so if you had the parameters set so aggressively they instantly disappear, then the internal voxel grid is empty on clearing, then nothing new marks it because it instantly disappeared, and then when it comes time to update the bounds, there's nothing to update them with (and there's no static layer to set the reasonable update bounds).

The fix I could see would be to touch the cells cleared as they were cleared to resize the bounds, that way if you totally clear your local costmap by instantly removing a single obstacle, then that are would be marked for update even though the costmap itself is now zero-sized. If fact, if the analog was also implemented for marking, then we could remove that iteration through the entire costmap altogether.

Another fix would just be have some costmap layer below this of all FREE space just to size the costmap for updates - though a bit more hacky

[SteveMacenski]

@siferati can you also submit one for the new galactic branch? Its basically identical to ROS2. Can close out this ticket then

[siferati]

can you also submit one for the new galactic branch?

Done.

[SteveMacenski]

Patch has now been merged in all distributions. Thanks @siferati !