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