Deadlock when a static layer gets map updated

[daisukes]

I found a deadlock bug when I tested our new prototype, which frequently updates the static costmap.

Bug report

Required Info:

• Operating System:
  • Ubuntu20.04
• ROS2 Version:
  • Galactic (ros:galactic docker image)
• Version or commit hash:
  • Galactic sync 2-based (a25cd43) custom build
• DDS implementation:
  • default (cyclone dds)

Steps to reproduce issue

• frequently update the static costmap (2hz)

Expected behavior

• no deadlock

Actual behavior

• deadlock
• gdb traceback
  • https://gist.github.com/daisukes/5e6e1d3758daa0c34996d60822b41170

Additional information

• main thread locks (Thread 15 in the gdb log)
  1. the mutex of the layered costmap
     https://github.com/ros-planning/navigation2/blob/6a9508b772eb6f07cbfd350b3578aa9de2567d59/nav2_costmap_2d/src/layered_costmap.cpp#L130
  2. then, the mutex of the static layer
     https://github.com/ros-planning/navigation2/blob/6a9508b772eb6f07cbfd350b3578aa9de2567d59/nav2_costmap_2d/plugins/static_layer.cpp#L336
• updating map thread locks (Thread 13 in the gdb log)
  1. the mutex of the static layer
     https://github.com/ros-planning/navigation2/blob/6a9508b772eb6f07cbfd350b3578aa9de2567d59/nav2_costmap_2d/plugins/static_layer.cpp#L272
  2. then, the mutex of the layered costmap, when the layered costmap needs to be resized
     https://github.com/ros-planning/navigation2/blob/6a9508b772eb6f07cbfd350b3578aa9de2567d59/nav2_costmap_2d/src/layered_costmap.cpp#L104

Implementation considerations

The mutex of the static layer should be unlocked before calling resizeMap of the layered costmap
https://github.com/ros-planning/navigation2/blob/6a9508b772eb6f07cbfd350b3578aa9de2567d59/nav2_costmap_2d/plugins/static_layer.cpp#L194-L198

I think this lock can be removed. What do you think?
https://github.com/ros-planning/navigation2/blob/6a9508b772eb6f07cbfd350b3578aa9de2567d59/nav2_costmap_2d/plugins/static_layer.cpp#L272

[SteveMacenski]

@daisukes thanks for filing, back from PTO now and took a look at this.

Basically the issue to solve is how to unlock the updateBounds/incomingMap locks within processMap for layered costmap resize. I thought about how to remove the locks from those functions but I don't think it works out because we need to still protect map_buffer_. Maybe the solution is to have a class member lock object that we're locking / unlocking so we can access it in multiple scopes. Or even just rewriting the update semantics, this atomic / multiple location of updates stuff with the buffer mid-update is really quite convoluted. It might be better to have a class member guard using the mutex that is locked at the start of updateBounds and unlocked at the end of updateCosts and similarly with incomingMap / incomingUpdate to just block the threads and have the map changes only occur within the incomingXYZ functions and remove this map_buffer_ stuff (or vise versa: only ever buffer the map in the incomingXYZ and process in the updateBounds method. That might actaully be better not to block the the subscriber thread when a map is coming in that could block sensor data).

However, I don't think just removing https://github.com/ros-planning/navigation2/blob/6a9508b772eb6f07cbfd350b3578aa9de2567d59/nav2_costmap_2d/plugins/static_layer.cpp#L272 works, because that then exposes map_buffer_ outside of the mutex which both threads modify. There could be a situation where update_in_progress_ is set to true in updateBounds and while processMap(*map_buffer) is being called, map_buffer_ = new_map; is being set in incomingMap. Its unlikely to happen, but it can happen.

Thoughts?

[daisukes]

Thank you for the comments.
How about this change? I made a test code that reproduces the deadlock, and this works fine with the test code.

• I think it does not need to call mapProcess twice when map_received_ is false
• map_buffer_ = nullptr is not needed when update_in_progress_ is false
• isolate processMap from the lock_guard

void
StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map)
{
  {
    std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
    if (!map_received_) {
      map_received_ = true;
    }

    if (update_in_progress_.load()) {
      map_buffer_ = new_map;
      return;
    }
  }
  processMap(*new_map);
}

original code
https://github.com/ros-planning/navigation2/blob/6a9508b772eb6f07cbfd350b3578aa9de2567d59/nav2_costmap_2d/plugins/static_layer.cpp#L269-L283

[SteveMacenski]

I think that would work if in updateBounds the check on map_received_ https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/plugins/static_layer.cpp#L340-L342 is put under the mutex in L334 that way if map_received_ is set to true but before processMap is called within incomingMap, we're still protected by the mutex.

Edit: Actually, I don't think that works since processMap from incomingMap could then be running at the same time as updateBounds -- which in the "bad" situation would have the bounds updated for the old map, then processMap is unblocked to set new values, and then updateCosts is run in the different map. In the worst case, the resizing could make some of those bounds invalid.

I think these semantics of having the map processed in both locations is really unnecessary and the cause of all of these problems. This was not the case in ROS 1 so I have no problem moving it elsewhere. In ROS 1, we did it in the map callbacks (which did not synchronize between updates on updateBounds/updateCosts). I think here instead we should do it at the start of updateBounds which then makes any changes to the map entirely sequential. So we should always buffer maps and never process in the map callbacks (except for maybe that very first map we receive just to get going).

We can keep incomingUpdate the same since it doesn't mess with the bounds and the setting of costmap_ values is under a lock that is shared with the updatebounds/costs, as calling that function should never actually impact the bounds to invalidate updateCosts bounds to consider. So really, its just removing the processing from incomingMap, moving it to always in updateBounds -- except for the very first map

[daisukes]

I made a test code and a PR for the patch which changes the static map update location as you suggested.
One thing I am not sure if it is okay assuming the costmap update is frequent enough compared to map receiving.

[SteveMacenski]

For a new map, does it matter? If we have a map pending to be processed (which would completely overwrite the existing one) but a new one comes in to overwrite that -- does it matter if we didn't process the one in the middle if no costmap update was requested? I would think not.

For just localized updates, that would be a problem, but the incomingUpdate handles those semantics