Removed Object is not cleared from Local Costmap

Question

I am experiencing an issue with my local_costmap not updating in Nav2. Despite configuring the local costmap to use a rolling window, the costmap does not reflect changes in the environment when objects are removed.

When I echo the /local_costmap/costmap_updates topic, no updates are received, even though the topic is correctly advertised and the publisher count is 1. Here is the output from ros2 topic info -v /local_costmap/costmap_updates:

Type: map_msgs/msg/OccupancyGridUpdate

Publisher count: 1

Node name: local_costmap
Node namespace: /local_costmap
Topic type: map_msgs/msg/OccupancyGridUpdate
Topic type hash: RIHS01_c4095b0be2e4363f8978a1570e0293ee093436f1e1c61d99b48502ac317025e8
Endpoint type: PUBLISHER
GID: 01.10.49.1e.1f.69.ce.37.7b.91.99.76.00.01.55.03
QoS profile:
  Reliability: RELIABLE
  History (Depth): KEEP_LAST (1)
  Durability: TRANSIENT_LOCAL
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 2

Node name: _ros2cli_28377
Node namespace: /
Topic type: map_msgs/msg/OccupancyGridUpdate
Topic type hash: RIHS01_c4095b0be2e4363f8978a1570e0293ee093436f1e1c61d99b48502ac317025e8
Endpoint type: SUBSCRIPTION
GID: 01.10.6c.f7.b0.b9.10.d2.4d.6f.70.76.00.00.0a.04
QoS profile:
  Reliability: RELIABLE
  History (Depth): KEEP_LAST (5)
  Durability: TRANSIENT_LOCAL
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Node name: rviz
Node namespace: /
Topic type: map_msgs/msg/OccupancyGridUpdate
Topic type hash: RIHS01_c4095b0be2e4363f8978a1570e0293ee093436f1e1c61d99b48502ac317025e8
Endpoint type: SUBSCRIPTION
GID: 01.10.a7.1c.ad.1f.8e.5d.dc.9c.c0.17.00.00.be.04
QoS profile:
  Reliability: RELIABLE
  History (Depth): KEEP_ALL
  Durability: TRANSIENT_LOCAL
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC

Despite having a properly configured local costmap and sensor data available, the costmap does not update when objects are removed. There are no transform errors or other significant warnings in the logs. Interestingly, if I remove an obstacle and then place a new obstacle behind it, the costmap correctly reflects the removal and addition of objects. However, if the environment is left empty after removing an obstacle, the costmap does not update to show the cleared space. I have read that some NaN or 0 values could be the cause of this weird behavior: local cost map not updated Does it proceed?

Obstacle Added (Correctly update occupancy grid costmap)

Obstacle Removed (does not clear the costmap):

Configuration Files Below are the relevant sections of my configuration files:

local_costmap:
  local_costmap:
    ros__parameters:
      lethal_cost_threshold: 100 
      update_frequency: 5.0 
      transform_tolerance: 0.5
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_footprint
      rolling_window: true
      width: 100
      height: 50
      origin_x: 50.0
      origin_y: 0.0
      track_unknown_space: true 
      resolution: 1.0
      footprint:  "[[-4.8025, -1.739], [4.8025, -1.739], [4.8025, 1.739], [-4.8025, 1.739]]"
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 1.0
        inflation_radius: 0.01
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 1.0
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: sick_scan_2D
        sick_scan_2D:
          topic: /sick_scan_2D
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 50.0
          raytrace_min_range: 0.0
          obstacle_max_range: 50.0
          obstacle_min_range: 1.0
          marking_threshold: 100
      always_send_full_costmap: True

Additional Information I followed the Nav2 GPS tutorial, which does not use a static map, and adapted the parameters to my custom simulation. I mean I have carefully compared my nav2 configuration parameters available here:

https://pastebin.com/8HJRrwm3

against tutorial navigation2 tutorial parameter files (using or not static layer). In tutorials, removing some obstacles updates the costmap correctly (local costmap is cleared), but for my simulation, this does not happen for some underlying reason I could not diagnose. Could it be something configured in the launch file or what...?

A video of this weird behavior: https://youtu.be/c3U44__Fho4

Answer 1

Your costmap is not correctly configured, because if it were, this wouldn't be happening ;-)

Costmap updates are deltas on segments of the costmap updated in a particular iteration. You can see in the configuration guide that there is a parameter along the lines of "send full map each time" (always_send_full_costmap) that is on in the default nav2 configuration files that you're using as the base for your project. Turning that off should then send deltas where useful.

You need to check your raytracing settings and if Inf is Valid (and if your lidar even produces an Inf at max range) for raytracing out spaces where there are no returns.