feat: De-register ClientGoalHandles in a thread safe way.

Signed-off-by: Janosch Machowinski <[email protected]>

Author: Janosch Machowinski

rclcpp_action/include/rclcpp_action/client.hpp

@@ -333,7 +333,7 @@ class ClientBase : public rclcpp::Waitable
  *  - calling user callbacks.
  */
 template<typename ActionT>
-class Client : public ClientBase
+class Client : public ClientBase, public std::enable_shared_from_this<Client<ActionT>>
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client<ActionT>)
@@ -446,7 +446,9 @@ class Client : public ClientBase
         goal_info.stamp = goal_response->stamp;
         // Do not use std::make_shared as friendship cannot be forwarded.
         std::shared_ptr<GoalHandle> goal_handle(
-          new GoalHandle(goal_info, options.feedback_callback, options.result_callback));
+          new GoalHandle(
+            this->shared_from_this(), goal_info, options.feedback_callback,
+            options.result_callback));
         {
           std::lock_guard<std::mutex> guard(goal_handles_mutex_);
           goal_handles_[goal_handle->get_goal_id()] = goal_handle;
@@ -563,9 +565,13 @@ class Client : public ClientBase
   }
 
   void drop_goal_handle(typename GoalHandle::SharedPtr goal_handle)
+  {
+    drop_goal_handle(goal_handle->get_goal_id());
+  }
+
+  void drop_goal_handle(const GoalUUID & goal_id)
   {
     std::lock_guard<std::mutex> guard(goal_handles_mutex_);
-    const GoalUUID & goal_id = goal_handle->get_goal_id();
     if (goal_handles_.count(goal_id) == 0) {
       // someone else already deleted the entry
       // e.g. the result callback

rclcpp_action/include/rclcpp_action/client_goal_handle.hpp

@@ -107,6 +107,7 @@ class ClientGoalHandle
   friend class Client<ActionT>;
 
   ClientGoalHandle(
+    const typename Client<ActionT>::SharedPtr & client,
     const GoalInfo & info,
     FeedbackCallback feedback_callback,
     ResultCallback result_callback);
@@ -163,6 +164,8 @@ class ClientGoalHandle
   ResultCallback result_callback_{nullptr};
   int8_t status_{GoalStatus::STATUS_ACCEPTED};
 
+  typename std::weak_ptr<Client<ActionT>> client_weak_ptr_;
+
   std::mutex handle_mutex_;
 };
 }  // namespace rclcpp_action

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -28,17 +28,23 @@ namespace rclcpp_action
 
 template<typename ActionT>
 ClientGoalHandle<ActionT>::ClientGoalHandle(
+  const typename Client<ActionT>::SharedPtr & client,
   const GoalInfo & info, FeedbackCallback feedback_callback, ResultCallback result_callback)
 : info_(info),
   result_future_(result_promise_.get_future()),
   feedback_callback_(feedback_callback),
-  result_callback_(result_callback)
+  result_callback_(result_callback),
+  client_weak_ptr_(client)
 {
 }
 
 template<typename ActionT>
 ClientGoalHandle<ActionT>::~ClientGoalHandle()
 {
+  typename Client<ActionT>::SharedPtr client = client_weak_ptr_.lock();
+  if (client) {
+    client->drop_goal_handle(get_goal_id());
+  }
 }
 
 template<typename ActionT>