From 7ac6114ceb6f70fa49ed1809f810682cdfb81045 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 19 May 2020 17:46:25 -0700 Subject: [PATCH 1/5] [rclcpp_action] Action client holds weak pointers to goal handles Fixes #861 It is against the design of ROS actions to rely on the status topic for the core implementation, instead it should just be used for introspection. Rather than relying on the status topic to remove references to goal handles, the action client instead holds weak pointers to the goal handles. This way as long as a user holds a reference to the goal handle they can use it to interact with the action client. Signed-off-by: Jacob Perron --- .../include/rclcpp_action/client.hpp | 41 ++++++++++++++----- 1 file changed, 30 insertions(+), 11 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 7133e8ce14..9a36665ba2 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -342,6 +342,20 @@ class Client : public ClientBase std::shared_future async_send_goal(const Goal & goal, const SendGoalOptions & options = SendGoalOptions()) { + // To prevent the list from growing out of control, forget about any goals + // with no more user references + { + std::lock_guard guard(goal_handles_mutex_); + auto goal_handle_it = goal_handles_.begin(); + while (goal_handle_it != goal_handles_.end()) { + if (!goal_handle_it->second.lock()) { + goal_handle_it = goal_handles_.erase(goal_handle_it); + } else { + ++goal_handle_it; + } + } + } + // Put promise in the heap to move it around. auto promise = std::make_shared>(); std::shared_future future(promise->get_future()); @@ -494,7 +508,10 @@ class Client : public ClientBase std::lock_guard guard(goal_handles_mutex_); auto it = goal_handles_.begin(); while (it != goal_handles_.end()) { - it->second->invalidate(); + typename GoalHandle::SharedPtr goal_handle = it->second.lock(); + if (goal_handle) { + goal_handle->invalidate(); + } it = goal_handles_.erase(it); } } @@ -546,7 +563,12 @@ class Client : public ClientBase "Received feedback for unknown goal. Ignoring..."); return; } - typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; + typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock(); + // Forget about the goal if there are no more user references + if (!goal_handle) { + goal_handles_.erase(goal_id); + return; + } auto feedback = std::make_shared(); *feedback = feedback_message->feedback; goal_handle->call_feedback_callback(goal_handle, feedback); @@ -575,16 +597,13 @@ class Client : public ClientBase "Received status for unknown goal. Ignoring..."); continue; } - typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id]; - goal_handle->set_status(status.status); - const int8_t goal_status = goal_handle->get_status(); - if ( - goal_status == GoalStatus::STATUS_SUCCEEDED || - goal_status == GoalStatus::STATUS_CANCELED || - goal_status == GoalStatus::STATUS_ABORTED) - { + typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock(); + // Forget about the goal if there are no more user references + if (!goal_handle) { goal_handles_.erase(goal_id); + continue; } + goal_handle->set_status(status.status); } } @@ -639,7 +658,7 @@ class Client : public ClientBase return future; } - std::map goal_handles_; + std::map goal_handles_; std::mutex goal_handles_mutex_; }; } // namespace rclcpp_action From 9489d489274f6d44074aa68d0d3efab47f9b654e Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 20 May 2020 18:35:08 -0700 Subject: [PATCH 2/5] Move cleanup logic to the end of the function Signed-off-by: Jacob Perron --- .../include/rclcpp_action/client.hpp | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 9a36665ba2..3090946104 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -342,20 +342,6 @@ class Client : public ClientBase std::shared_future async_send_goal(const Goal & goal, const SendGoalOptions & options = SendGoalOptions()) { - // To prevent the list from growing out of control, forget about any goals - // with no more user references - { - std::lock_guard guard(goal_handles_mutex_); - auto goal_handle_it = goal_handles_.begin(); - while (goal_handle_it != goal_handles_.end()) { - if (!goal_handle_it->second.lock()) { - goal_handle_it = goal_handles_.erase(goal_handle_it); - } else { - ++goal_handle_it; - } - } - } - // Put promise in the heap to move it around. auto promise = std::make_shared>(); std::shared_future future(promise->get_future()); @@ -400,6 +386,21 @@ class Client : public ClientBase } } }); + + // To prevent the list from growing out of control, forget about any goals + // with no more user references + { + std::lock_guard guard(goal_handles_mutex_); + auto goal_handle_it = goal_handles_.begin(); + while (goal_handle_it != goal_handles_.end()) { + if (!goal_handle_it->second.lock()) { + goal_handle_it = goal_handles_.erase(goal_handle_it); + } else { + ++goal_handle_it; + } + } + } + return future; } From 089e304288cff7920d02acd1cf5ba42fd51251b2 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 20 May 2020 18:36:33 -0700 Subject: [PATCH 3/5] Add TODO Signed-off-by: Jacob Perron --- rclcpp_action/include/rclcpp_action/client.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 3090946104..1ee5e1b972 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -387,6 +387,8 @@ class Client : public ClientBase } }); + // TODO(jacobperron): Encapsulate into it's own function and + // consider exposing an option to disable this cleanup // To prevent the list from growing out of control, forget about any goals // with no more user references { From d06e8e08545d93c905a5d9591c4f1fc26a77268e Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 20 May 2020 18:42:31 -0700 Subject: [PATCH 4/5] Log debug messages when dropping a weak references to goal handles Signed-off-by: Jacob Perron --- rclcpp_action/include/rclcpp_action/client.hpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 1ee5e1b972..54562a608b 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -396,6 +396,9 @@ class Client : public ClientBase auto goal_handle_it = goal_handles_.begin(); while (goal_handle_it != goal_handles_.end()) { if (!goal_handle_it->second.lock()) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during send_goal()"); goal_handle_it = goal_handles_.erase(goal_handle_it); } else { ++goal_handle_it; @@ -569,6 +572,9 @@ class Client : public ClientBase typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock(); // Forget about the goal if there are no more user references if (!goal_handle) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during feedback callback"); goal_handles_.erase(goal_id); return; } @@ -603,6 +609,9 @@ class Client : public ClientBase typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock(); // Forget about the goal if there are no more user references if (!goal_handle) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during status callback"); goal_handles_.erase(goal_id); continue; } From 7cda497ce9c35782073a078f2be9c8ea5defd552 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 21 May 2020 15:14:22 -0700 Subject: [PATCH 5/5] Improve documentation Signed-off-by: Jacob Perron --- rclcpp_action/include/rclcpp_action/client.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 54562a608b..f6d553a4b4 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -331,7 +331,9 @@ class Client : public ClientBase * If the goal is accepted by an action server, the returned future is set to a `ClientGoalHandle`. * If the goal is rejected by an action server, then the future is set to a `nullptr`. * - * The goal handle is used to monitor the status of the goal and get the final result. + * The returned goal handle is used to monitor the status of the goal and get the final result. + * It is valid as long as you hold a reference to the shared pointer or until the + * rclcpp_action::Client is destroyed at which point the goal status will become UNKNOWN. * * \param[in] goal The goal request. * \param[in] options Options for sending the goal request. Contains references to callbacks for