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..977c7397d0 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -15,12 +15,14 @@ #ifndef RCLCPP_ACTION__SERVER_HPP_ #define RCLCPP_ACTION__SERVER_HPP_ +#include #include #include #include #include #include #include +#include #include "action_msgs/srv/cancel_goal.hpp" #include "rcl/event_callback.h" @@ -30,6 +32,8 @@ #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/timer.hpp" #include "rclcpp/waitable.hpp" #include "rclcpp_action/visibility_control.hpp" @@ -173,6 +177,31 @@ 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 + RCLCPP_ACTION_PUBLIC + void + reset_timer_to_next_goal(); + /// Unset the callback to be called whenever the waitable becomes ready. RCLCPP_ACTION_PUBLIC void @@ -187,6 +216,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 +360,14 @@ 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 goal_entry_times_mutex_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; + 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 void @@ -396,6 +434,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..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 @@ -29,6 +30,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 +119,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_(std::chrono::nanoseconds(options.result_timeout.nanoseconds)) { auto deleter = [node_base](rcl_action_server_t * ptr) { @@ -704,6 +709,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) { @@ -720,6 +819,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_) { + set_expire_goal_timer(); + } + { /** * NOTE: There is a potential deadlock issue if both unordered_map_mutex_ and @@ -788,6 +891,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