From 9d839fd775a442944fc68d569b968834f1870ae5 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 4 Nov 2024 16:11:13 +0000 Subject: [PATCH 1/3] Fix Actions memory leak: Add timer to cleanup Signed-off-by: Mauro Passerino --- .../include/rclcpp_action/create_server.hpp | 5 +++ .../include/rclcpp_action/server.hpp | 22 ++++++++++ rclcpp_action/src/server.cpp | 41 ++++++++++++++++++- 3 files changed, 67 insertions(+), 1 deletion(-) diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index c333a60581..4bf1a8db61 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -24,6 +24,7 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_waitables_interface.hpp" #include "rclcpp_action/server.hpp" @@ -42,6 +43,7 @@ namespace rclcpp_action * \param[in] node_base_interface The node base interface of the corresponding node. * \param[in] node_clock_interface The node clock interface of the corresponding node. * \param[in] node_logging_interface The node logging interface of the corresponding node. + * \param[in] node_timers_interface The node timers interface of the corresponding node. * \param[in] node_waitables_interface The node waitables interface of the corresponding node. * \param[in] name The action name. * \param[in] handle_goal A callback that decides if a goal should be accepted or rejected. @@ -59,6 +61,7 @@ create_server( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_interface, rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, const std::string & name, typename Server::GoalCallback handle_goal, @@ -100,6 +103,7 @@ create_server( node_base_interface, node_clock_interface, node_logging_interface, + node_timers_interface, name, options, handle_goal, @@ -142,6 +146,7 @@ create_server( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), + node->get_node_timers_interface(), node->get_node_waitables_interface(), name, handle_goal, diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index a885383614..f6dfdb6118 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -30,6 +30,7 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/waitable.hpp" #include "rclcpp_action/visibility_control.hpp" @@ -173,6 +174,17 @@ class ServerBase : public rclcpp::Waitable void set_on_ready_callback(std::function callback) override; + /// \internal + /// Set up a one-shot timer to trigger when a goal's expiration time is reached + /** + * Initializes a timer to trigger a callback when a goal expires, + * enabling cleanup of expired goals. The timer is removed after the callback + * is called + */ + RCLCPP_ACTION_PUBLIC + void + setup_expire_goal_timer(); + /// Unset the callback to be called whenever the waitable becomes ready. RCLCPP_ACTION_PUBLIC void @@ -187,6 +199,7 @@ class ServerBase : public rclcpp::Waitable rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers, const std::string & name, const rosidl_action_type_support_t * type_support, const rcl_action_server_options_t & options); @@ -330,6 +343,13 @@ class ServerBase : public rclcpp::Waitable // Storage for std::function callbacks to keep them in scope std::unordered_map> entity_type_to_on_ready_callback_; + // Store elements required to create goal expiration timers + std::mutex expire_goal_timers_mutex_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; + std::vector> expire_goal_timers_; + rcl_duration_value_t goal_expire_timeout_; + /// Set a callback to be called when the specified entity is ready RCLCPP_ACTION_PUBLIC void @@ -396,6 +416,7 @@ class Server : public ServerBase, public std::enable_shared_from_this(), options), diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index fefc02d6ad..51d67b5dc8 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -29,6 +29,7 @@ #include "action_msgs/msg/goal_status_array.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/create_timer.hpp" #include "rclcpp_action/server.hpp" using rclcpp_action::ServerBase; @@ -117,12 +118,15 @@ ServerBase::ServerBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers, const std::string & name, const rosidl_action_type_support_t * type_support, const rcl_action_server_options_t & options ) : pimpl_(new ServerBaseImpl( - node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action"))) + node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action"))), + node_base_(node_base), node_timers_(node_timers), + goal_expire_timeout_(options.result_timeout.nanoseconds) { auto deleter = [node_base](rcl_action_server_t * ptr) { @@ -645,6 +649,37 @@ ServerBase::execute_check_expired_goals() } } +void ServerBase::setup_expire_goal_timer() +{ + std::lock_guard lock(expire_goal_timers_mutex_); + unsigned int timer_id = expire_goal_timers_.empty() ? 1 : expire_goal_timers_.back().first + 1; + + auto one_shot_timer_callback = [this, timer_id]() mutable { + execute_check_expired_goals(); + + std::lock_guard lock_mutex(expire_goal_timers_mutex_); + auto it = std::find_if( + expire_goal_timers_.begin(), expire_goal_timers_.end(), + [timer_id](const std::pair& element) { + return element.first == timer_id; + }); + + if (it != expire_goal_timers_.end()) { + expire_goal_timers_.erase(it); + } + }; + + auto timer = rclcpp::create_wall_timer( + std::chrono::nanoseconds(goal_expire_timeout_), + std::function(one_shot_timer_callback), + nullptr, + node_base_.get(), + node_timers_.get() + ); + + expire_goal_timers_.emplace_back(timer_id, timer); +} + void ServerBase::publish_status() { @@ -720,6 +755,10 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_m throw std::runtime_error("Asked to publish result for goal that does not exist"); } + if (on_ready_callback_set_) { + setup_expire_goal_timer(); + } + { /** * NOTE: There is a potential deadlock issue if both unordered_map_mutex_ and From f4408aa2ddaf1613b583e2602e99fdefbab8df6f Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Tue, 5 Nov 2024 13:39:02 +0000 Subject: [PATCH 2/3] Use a single timer to expire goals Signed-off-by: Mauro Passerino --- .../include/rclcpp_action/server.hpp | 36 +++-- rclcpp_action/src/server.cpp | 130 +++++++++++++----- 2 files changed, 123 insertions(+), 43 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index f6dfdb6118..1ba3fe53c6 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "action_msgs/srv/cancel_goal.hpp" #include "rcl/event_callback.h" @@ -174,16 +175,30 @@ class ServerBase : public rclcpp::Waitable void set_on_ready_callback(std::function callback) override; + /// Initialize the goal expiration timer. + /// \internal + RCLCPP_ACTION_PUBLIC + void + initialize_expire_goal_timer(); + + /// Set timer to trigger on the goal expiration time + /// \internal + RCLCPP_ACTION_PUBLIC + void + set_expire_goal_timer(); + + /// Timer callback to handle goal expiration. + /// \internal + RCLCPP_ACTION_PUBLIC + void + expire_goal_timer_callback(); + + /// Calculates the time until the next goal expiration and updates the timer period. + /// Cancels the timer if no goals remain or invokes the callback directly if expiration is immediate. /// \internal - /// Set up a one-shot timer to trigger when a goal's expiration time is reached - /** - * Initializes a timer to trigger a callback when a goal expires, - * enabling cleanup of expired goals. The timer is removed after the callback - * is called - */ RCLCPP_ACTION_PUBLIC void - setup_expire_goal_timer(); + reset_timer_to_next_goal(); /// Unset the callback to be called whenever the waitable becomes ready. RCLCPP_ACTION_PUBLIC @@ -344,11 +359,12 @@ class ServerBase : public rclcpp::Waitable std::unordered_map> entity_type_to_on_ready_callback_; // Store elements required to create goal expiration timers - std::mutex expire_goal_timers_mutex_; + std::mutex goal_entry_times_mutex_; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; - std::vector> expire_goal_timers_; - rcl_duration_value_t goal_expire_timeout_; + std::set goal_entry_times_; + rclcpp::TimerBase::SharedPtr expire_goal_timer_; + std::chrono::nanoseconds goal_expire_timeout_; /// Set a callback to be called when the specified entity is ready RCLCPP_ACTION_PUBLIC diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 51d67b5dc8..829c7bbc72 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -126,7 +126,7 @@ ServerBase::ServerBase( : pimpl_(new ServerBaseImpl( node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action"))), node_base_(node_base), node_timers_(node_timers), - goal_expire_timeout_(options.result_timeout.nanoseconds) + goal_expire_timeout_(std::chrono::nanoseconds(options.result_timeout.nanoseconds)) { auto deleter = [node_base](rcl_action_server_t * ptr) { @@ -649,37 +649,6 @@ ServerBase::execute_check_expired_goals() } } -void ServerBase::setup_expire_goal_timer() -{ - std::lock_guard lock(expire_goal_timers_mutex_); - unsigned int timer_id = expire_goal_timers_.empty() ? 1 : expire_goal_timers_.back().first + 1; - - auto one_shot_timer_callback = [this, timer_id]() mutable { - execute_check_expired_goals(); - - std::lock_guard lock_mutex(expire_goal_timers_mutex_); - auto it = std::find_if( - expire_goal_timers_.begin(), expire_goal_timers_.end(), - [timer_id](const std::pair& element) { - return element.first == timer_id; - }); - - if (it != expire_goal_timers_.end()) { - expire_goal_timers_.erase(it); - } - }; - - auto timer = rclcpp::create_wall_timer( - std::chrono::nanoseconds(goal_expire_timeout_), - std::function(one_shot_timer_callback), - nullptr, - node_base_.get(), - node_timers_.get() - ); - - expire_goal_timers_.emplace_back(timer_id, timer); -} - void ServerBase::publish_status() { @@ -739,6 +708,100 @@ ServerBase::publish_status() } } +void +ServerBase::initialize_expire_goal_timer() +{ + expire_goal_timer_ = rclcpp::create_wall_timer( + std::chrono::nanoseconds(goal_expire_timeout_), + std::bind(&ServerBase::expire_goal_timer_callback, this), + nullptr, + node_base_.get(), + node_timers_.get() + ); + + expire_goal_timer_->cancel(); +} + +void +ServerBase::expire_goal_timer_callback() +{ + { + std::lock_guard lock(goal_entry_times_mutex_); + auto now = std::chrono::steady_clock::now(); + auto it = goal_entry_times_.begin(); + + // Remove expired time entries for goals. + while (it != goal_entry_times_.end() && *it + goal_expire_timeout_ <= now) { + it = goal_entry_times_.erase(it); + } + } + + execute_check_expired_goals(); + reset_timer_to_next_goal(); +} + +void +ServerBase::reset_timer_to_next_goal() +{ + std::lock_guard lock(goal_entry_times_mutex_); + + // If no more goals, cancel the timer. + if (goal_entry_times_.empty()) { + expire_goal_timer_->cancel(); + return; + } + + auto now = std::chrono::steady_clock::now(); + auto earliest_entry_time = *goal_entry_times_.begin(); + auto expiration_time = earliest_entry_time + goal_expire_timeout_; + + // Calculate time until expiration of the next goal. + auto time_until_expiration = expiration_time > now + ? expiration_time - now + : std::chrono::nanoseconds(0); + + // If the time is zero, directly call the callback. + if (time_until_expiration.count() == 0) { + expire_goal_timer_->cancel(); + expire_goal_timer_callback(); + return; + } + + // Update the timer to the calculated next expiration time. + rcl_timer_t * rcl_timer_handle = const_cast(expire_goal_timer_->get_timer_handle().get()); + int64_t old_period; + rcl_ret_t ret = rcl_timer_exchange_period(rcl_timer_handle, time_until_expiration.count(), &old_period); + + if (ret != RCL_RET_OK) { + RCLCPP_ERROR(pimpl_->logger_, "Failed to update expire timer period"); + } + + // Reset timer to count down from the new period. + expire_goal_timer_->reset(); +} + +void +ServerBase::set_expire_goal_timer() +{ + { + std::lock_guard lock(goal_entry_times_mutex_); + goal_entry_times_.insert(std::chrono::steady_clock::now()); + } + + // Only reset timer if it is canceled; otherwise, let the current timer run its full duration. + if (expire_goal_timer_->is_canceled()) { + rcl_timer_t * rcl_timer_handle = const_cast(expire_goal_timer_->get_timer_handle().get()); + int64_t old_period; + rcl_ret_t ret = rcl_timer_exchange_period(rcl_timer_handle, goal_expire_timeout_.count(), &old_period); + + if (ret != RCL_RET_OK) { + RCLCPP_ERROR(pimpl_->logger_, "Failed to reset timer period to goal_expire_timeout_"); + } + + expire_goal_timer_->reset(); + } +} + void ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_msg) { @@ -756,7 +819,7 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_m } if (on_ready_callback_set_) { - setup_expire_goal_timer(); + set_expire_goal_timer(); } { @@ -827,6 +890,7 @@ ServerBase::set_on_ready_callback(std::function callback) set_callback_to_entity(EntityType::GoalService, callback); set_callback_to_entity(EntityType::ResultService, callback); set_callback_to_entity(EntityType::CancelService, callback); + initialize_expire_goal_timer(); } void From bfebe0ab82fa3ccbbae8e6592904bea88e9e0b11 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Tue, 5 Nov 2024 14:26:25 +0000 Subject: [PATCH 3/3] Add missing includes Signed-off-by: Mauro Passerino --- rclcpp_action/include/rclcpp_action/server.hpp | 2 ++ rclcpp_action/src/server.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 1ba3fe53c6..977c7397d0 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_ACTION__SERVER_HPP_ #define RCLCPP_ACTION__SERVER_HPP_ +#include #include #include #include @@ -32,6 +33,7 @@ #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/timer.hpp" #include "rclcpp/waitable.hpp" #include "rclcpp_action/visibility_control.hpp" diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 829c7bbc72..de37c09242 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include