From b1b525b1c6f1b298f5858352008622df0c8bf938 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 22 May 2020 11:41:34 -0700 Subject: [PATCH] [rclcpp_action] Action client holds weak pointers to goal handles (#1122) * [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 * Move cleanup logic to the end of the function Signed-off-by: Jacob Perron * Add TODO Signed-off-by: Jacob Perron * Log debug messages when dropping a weak references to goal handles Signed-off-by: Jacob Perron * Improve documentation Signed-off-by: Jacob Perron --- .../include/rclcpp_action/client.hpp | 57 +++++++++++++++---- 1 file changed, 45 insertions(+), 12 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 27ced3dbdb..71b0c866a0 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 @@ -386,6 +388,26 @@ 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 + { + 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()) { + 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; + } + } + } + return future; } @@ -494,7 +516,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 +571,15 @@ 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) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during feedback callback"); + 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 +608,16 @@ 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) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during status callback"); goal_handles_.erase(goal_id); + continue; } + goal_handle->set_status(status.status); } } @@ -639,7 +672,7 @@ class Client : public ClientBase return future; } - std::map goal_handles_; + std::map goal_handles_; std::mutex goal_handles_mutex_; }; } // namespace rclcpp_action