Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • Foxy release src build ros2/ros2@4109e26
• Version or commit hash:
  • main
• DDS implementation:
  • Fast-RTPS, cyclonedds

Steps to reproduce issue

• launch tb3 simulation and make it active
• repeat sending goal action and waiting for the result in a single loop, and wait for the deadlock
  https://gist.github.com/daisukes/46298b083a48e5db7016f3c0efba7e2d
  • I could reproduce the deadlock with this code usually in a few minutes

Expected behavior

• does not stop, keep working

Actual behavior

• the server will not accept goals when the deadlock happens (

Additional information

Here is gdb information
Thread 9 and 14 are trying to hold two mutex in different order
https://gist.github.com/daisukes/712316a97832f5d9ab851ad47c77ad98

Thread 9: server thread
Frame# 6 nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose, rclcpp::Node>::handle_goal
Frame# 14 rclcpp_action::ServerBase::execute_goal_request_received()

Thread 14: working thread started here
Frame# 6 rclcpp_action::ServerBase::notify_goal_terminal_state()
Frame# 11 nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose, rclcpp::Node>::succeeded_current

This happens once in ten navigations in my environment.
Luckily, I could find the deadlock loop, but I have no idea how to fix this so far.