Skip to content
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

Add timeout to BT action node #1745

Closed
wants to merge 2 commits into from
Closed

Add timeout to BT action node #1745

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
945dc46
Add timeout to BT action node
naiveHobo May 17, 2020
cb3a24e
Minor fix to retrigger CI
naiveHobo May 17, 2020
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
26 changes: 24 additions & 2 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_

#include <chrono>
#include <memory>
#include <string>

Expand Down Expand Up @@ -47,6 +48,11 @@ class BtActionNode : public BT::ActionNodeBase
}
createActionClient(action_name_);

// get action timeout duration in seconds
double timeout = 30.0;
getInput("timeout", timeout);
timeout_ = std::chrono::duration<double>(timeout);

// Give the derive class a chance to do any initialization
RCLCPP_INFO(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str());
}
Expand Down Expand Up @@ -74,7 +80,7 @@ class BtActionNode : public BT::ActionNodeBase
{
BT::PortsList basic = {
BT::InputPort<std::string>("server_name", "Action server name"),
BT::InputPort<std::chrono::milliseconds>("server_timeout")
BT::InputPort<double>("timeout", 30, "Timeout for action server")
};
basic.insert(addition.begin(), addition.end());

Expand Down Expand Up @@ -152,6 +158,14 @@ class BtActionNode : public BT::ActionNodeBase

// check if, after invoking spin_some(), we finally received the result
if (!goal_result_available_) {
// check if action has timed out
auto current_time = std::chrono::steady_clock::now();
if (current_time - start_time_ > timeout_) {
RCLCPP_WARN(
node_->get_logger(),
"Node timed out while executing action call to %s.", action_name_.c_str());
return BT::NodeStatus::FAILURE;
}
// Yield this Action, returning RUNNING
return BT::NodeStatus::RUNNING;
}
Expand Down Expand Up @@ -222,7 +236,7 @@ class BtActionNode : public BT::ActionNodeBase
auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);

if (rclcpp::spin_until_future_complete(node_, future_goal_handle) !=
rclcpp::executor::FutureReturnCode::SUCCESS)
rclcpp::FutureReturnCode::SUCCESS)
{
throw std::runtime_error("send_goal failed");
}
Expand All @@ -231,6 +245,9 @@ class BtActionNode : public BT::ActionNodeBase
if (!goal_handle_) {
throw std::runtime_error("Goal was rejected by the action server");
}

// Get time when execution of goal was started
start_time_ = std::chrono::steady_clock::now();
}

void increment_recovery_count()
Expand All @@ -253,6 +270,11 @@ class BtActionNode : public BT::ActionNodeBase

// The node that will be used for any ROS operations
rclcpp::Node::SharedPtr node_;

// The timeout value while to use in the tick loop while waiting for
// a result from the server
std::chrono::duration<double> timeout_;
std::chrono::steady_clock::time_point start_time_;
};

} // namespace nav2_behavior_tree
Expand Down