Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • Foxy binaries
• Version or commit hash:
  • 41996f4
• DDS implementation:
  • Fast-RTPS

Steps to reproduce issue

Actually It's similar to this issue #1961

Expected behavior

always accept goals

Actual behavior

the server will not accept goals when the deadlock happens

Additional information

• here is the backtrace, https://gist.github.com/KavenYau/562d2180c69d22ee87bfbd5306ced44d

backtrace

• Thread 9: main thread
  Frame# 6 nav2_util::SimpleActionServer<nav2_msgs::action::ComputePathToPose, rclcpp::Node>::handle_cancel
  Frame# 13 rclcpp_action::Server<nav2_msgs::action::ComputePathToPose>::call_handle_cancel_callback

• Thread 3612: working thread
  Frame# 5 rclcpp_action::Server<nav2_msgs::action::ComputePathToPose>::call_goal_accepted_callback
  Frame# 9 rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose>::succeed

deadlock

• Thread 9: main thread

1. std::lock_guard<std::mutex> lock(goal_handles_mutex_);
2. std::lock_guard<std::recursive_mutex> lock(update_mutex_);

• Thread 3612: working thread

1. std::lock_guard<std::recursive_mutex> lock(update_mutex_);
2. std::lock_guard<std::mutex> lock(goal_handles_mutex_);

The lock sequences are different.

But if I commented the code std::lock_guard<std::recursive_mutex> lock(update_mutex_);, it would cause this rclcpp issue ros2/rclcpp#1599

So if using another mutex or some atomic variables to mark the action state. I think it could be solved.

• If it is publishing succeeded state, so reject the cancel request. It returns succeeded state.
• If it is canceling, so abort publishing succeeded state. It returns canceled state.