Skip to content

Fail a running goal whose action server has disappeared #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Aaron-VG merged 1 commit into from
Oct 23, 2026
Merged

Fail a running goal whose action server has disappeared #1

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 25 additions & 0 deletions behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -382,6 +384,7 @@ inline NodeStatus RosActionNode<T>::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_ = {};
Expand Down Expand Up @@ -479,6 +482,28 @@ inline NodeStatus RosActionNode<T>::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)
{
Expand Down
Loading