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