feat(Client): Added function to drop a goal handle in a thread safe way

This function allows us to drop the handle in a locked context.
If we do not do this within a lock, there will be a race condition between
the deletion of the shared_ptr of the handle and the result / feedback
callbacks.

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

Author: Janosch Machowinski

rclcpp_action/include/rclcpp_action/client.hpp

@@ -562,6 +562,21 @@ class Client : public ClientBase
     return async_cancel(cancel_request, cancel_callback);
   }
 
+  void drop_goal_handle(typename GoalHandle::SharedPtr goal_handle)
+  {
+    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_DEBUG(
+        this->get_logger(),
+        "Given goal is unknown. Ignoring...");
+      return;
+    }
+    goal_handles_.erase(goal_id);
+  }
+
   /// Asynchronously request all goals at or before a specified time be canceled.
   /**
    * \param[in] stamp The timestamp for the cancel goal request.