From 7006a84c03e1cecf9e7ef03951d94338295f32ea Mon Sep 17 00:00:00 2001 From: Aaron-VG Date: Thu, 22 Oct 2026 16:12:43 +0200 Subject: [PATCH] fix(action node): fail a running goal whose server has disappeared A RosActionNode whose action server crashed or was restarted while its goal ran stayed RUNNING for ever: the result could never arrive. A tree that starts an action only when it is not already running then never started it again. Seen on Reachy Mini: restarting the gaze server left the controller's gaze tracking dead until the controller was restarted. While running, the node now checks that the server is still there; gone for longer than server_timeout (so a discovery hiccup does not count), the goal fails with SERVER_UNREACHABLE. --- .../behaviortree_ros2/bt_action_node.hpp | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index 3bd5fc10..c952a7f2 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -230,6 +230,8 @@ class RosActionNode : public BT::ActionNodeBase rclcpp::Time time_goal_sent_; NodeStatus on_feedback_state_change_; bool goal_received_; + // When the server holding the running goal was last seen missing; zero while it is there. + rclcpp::Time server_missing_since_{ 0, 0, RCL_ROS_TIME }; WrappedResult result_; bool createClient(const std::string& action_name); @@ -382,6 +384,7 @@ inline NodeStatus RosActionNode::tick() setStatus(NodeStatus::RUNNING); goal_received_ = false; + server_missing_since_ = rclcpp::Time(0, 0, now().get_clock_type()); future_goal_handle_ = {}; on_feedback_state_change_ = NodeStatus::RUNNING; result_ = {}; @@ -479,6 +482,28 @@ inline NodeStatus RosActionNode::tick() } } + // The server holding the goal is gone (it crashed or was restarted): its result can never + // arrive, and without this the node stayed RUNNING for ever, so a tree that starts the + // action only when it is not already running never started it again. Missing for longer + // than server_timeout, to ride out a discovery hiccup, counts as gone. + if(!client_instance_->action_client->action_server_is_ready()) + { + if(server_missing_since_.nanoseconds() == 0) + { + server_missing_since_ = now(); + } + else if((now() - server_missing_since_) > + rclcpp::Duration::from_seconds(double(server_timeout_.count()) / 1000)) + { + goal_handle_.reset(); + return CheckStatus(onFailure(SERVER_UNREACHABLE)); + } + } + else + { + server_missing_since_ = rclcpp::Time(0, 0, now().get_clock_type()); + } + // SECOND case: onFeedback requested a stop if(on_feedback_state_change_ != NodeStatus::RUNNING) {