Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• Installation type:
  • binaries or source
• Version or commit hash:
  • 47f21da
• DDS implementation:
  • Fast-RTPS
• Client library (if applicable):
  • rclcpp (rclcpp_action)

Steps to reproduce issue

• I commented the code std::lock_guard<std::recursive_mutex> lock(update_mutex_); in this file https://github.com/ros-planning/navigation2/blob/main/nav2_util/include/nav2_util/simple_action_server.hpp#L125, because it would cause deadlock, although this issue [Backport] Fix action server deadlock (#1285) (#1313)  #1518 claims that it is fixed. ( due to there are two threads tring to acquire the the mutex update_mutex_, https://github.com/ros-planning/navigation2/blob/main/nav2_util/include/nav2_util/simple_action_server.hpp#L384 )

  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>/*handle*/)
  {
    // std::lock_guard<std::recursive_mutex> lock(update_mutex_);
    debug_msg("Received request for goal cancellation");
    return rclcpp_action::CancelResponse::ACCEPT;
  }

• then run navigation2 as usual, send goal and cancel goal
• then sometimes will crush and print this infomation

terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
  what():  goal_handle attempted invalid transition from state SUCCEEDED with event CANCEL_GOAL, at /tmp/binarydeb/ros-foxy-rcl-action-1.1.10/src/rcl_action/goal_handle.c:95

So far as I am aware, https://github.com/ros2/rcl/blob/foxy/rcl_action/src/rcl_action/goal_state_machine.c#L82 shows that the state SUCCEEDED can not be transformed to any state. https://github.com/ros2/rclcpp/blob/foxy/rclcpp_action/src/server_goal_handle.cpp#L81 the _cancel_goal() function will be called internally only. so if the handle_cancel_ user callback return rclcpp_action::CancelResponse::ACCEPT https://github.com/ros2/rclcpp/blob/foxy/rclcpp_action/include/rclcpp_action/server.hpp#L363, and it crushes.
I can't catch the exception because it occurs internally.

So I think this exception should be catched internally, or do not throw exception, just handle it. Make sure it will not crush.

Of course, I think it can be avoid in the action server implementation too. Because the action server should know it is going to publish succeeded state, and the the handle_cancel_ user callback return rclcpp_action::CancelResponse::REJECT, so it would not to call goal_handle->_cancel_goal();.

But the second solution demand that I have to know more about the implementation of rclcpp and rcl.

So do you think it's the issue of rclcpp?
Or do you have any good suggestions?