diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
index 9208cd2284fa7092fc4b5a06d68fdb1dc3ffc3a3..af4b70b5e73b84ffc740692a86b9d437e7c883e6 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -41,8 +41,9 @@ public:
result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
// Get the required items from the blackboard
- node_loop_timeout_ =
- config().blackboard->get<std::chrono::milliseconds>("node_loop_timeout");
+ server_timeout_ =
+ config().blackboard->get<std::chrono::milliseconds>("server_timeout");
+ getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
// Now that we have the ROS node to use, create the action client for this BT action
action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name_);
@@ -61,8 +62,16 @@ public:
{
}
+ // Any BT node that accepts parameters must provide a requiredNodeParameters method
+ static BT::PortsList providedPorts()
+ {
+ return {
+ BT::InputPort<std::chrono::milliseconds>("server_timeout")
+ };
+ }
+
// Derived classes can override any of the following methods to hook into the
- // processing for the action: on_tick, on_loop_timeout, and on_success
+ // processing for the action: on_tick, on_server_timeout, and on_success
// Could do dynamic checks, such as getting updates to values on the blackboard
virtual void on_tick()
@@ -71,7 +80,7 @@ public:
// There can be many loop iterations per tick. Any opportunity to do something after
// a timeout waiting for a result that hasn't been received yet
- virtual void on_loop_timeout()
+ virtual void on_server_timeout()
{
}
@@ -106,9 +115,9 @@ new_goal_received:
auto future_result = goal_handle_->async_result();
rclcpp::executor::FutureReturnCode rc;
do {
- rc = rclcpp::spin_until_future_complete(node_, future_result, node_loop_timeout_);
+ rc = rclcpp::spin_until_future_complete(node_, future_result, server_timeout_);
if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) {
- on_loop_timeout();
+ on_server_timeout();
// We can handle a new goal if we're still executing
auto status = goal_handle_->get_status();
@@ -197,7 +206,7 @@ protected:
// The timeout value while to use in the tick loop while waiting for
// a result from the server
- std::chrono::milliseconds node_loop_timeout_;
+ std::chrono::milliseconds server_timeout_;
};
} // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
index ead7ed686a3d74306265f60b076e079b131efd5d..ed3a4cc7d33f4d0e3dae62e8b9301dd95a7eea3d 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
@@ -59,6 +59,12 @@ inline geometry_msgs::msg::Quaternion convertFromString(const StringView key)
}
}
+template <>
+inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(StringView str)
+{
+ return std::chrono::milliseconds(std::stoul(str.data()));
+}
+
} // namespace BT
#endif // NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
index 8f40866bfcd40eef08652a71087140531efb807d..1a7eb037b64301d5ca18418a03d061bc235a57da 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
@@ -37,8 +37,9 @@ public:
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
// Get the required items from the blackboard
- node_loop_timeout_ =
- config().blackboard->get<std::chrono::milliseconds>("node_loop_timeout");
+ server_timeout_ =
+ config().blackboard->get<std::chrono::milliseconds>("server_timeout");
+ getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
// Now that we have node_ to use, create the service client for this BT service
getInput("service_name", service_name_);
@@ -63,7 +64,8 @@ public:
static BT::PortsList providedPorts()
{
return {
- BT::InputPort<std::string>("service_name", "please_set_service_name_in_BT_Node")
+ BT::InputPort<std::string>("service_name", "please_set_service_name_in_BT_Node"),
+ BT::InputPort<std::chrono::milliseconds>("server_timeout")
};
}
@@ -75,12 +77,15 @@ public:
rclcpp::executor::FutureReturnCode rc;
rc = rclcpp::spin_until_future_complete(node_,
- future_result, node_loop_timeout_);
- if (rc != rclcpp::executor::FutureReturnCode::SUCCESS) {
- return BT::NodeStatus::FAILURE;
- } else {
+ future_result, server_timeout_);
+ if (rc == rclcpp::executor::FutureReturnCode::SUCCESS) {
return BT::NodeStatus::SUCCESS;
+ } else if(rc == rclcpp::executor::FutureReturnCode::TIMEOUT) {
+ RCLCPP_WARN(node_->get_logger(),
+ "Node timed out while executing service call to %s.", service_name_.c_str());
+ on_server_timeout();
}
+ return BT::NodeStatus::FAILURE;
}
// Fill in service request with information if necessary
@@ -89,6 +94,12 @@ public:
request_ = std::make_shared<typename ServiceT::Request>();
}
+ // An opportunity to do something after
+ // a timeout waiting for a result that hasn't been received yet
+ virtual void on_server_timeout()
+ {
+ }
+
protected:
std::string service_name_, service_node_name_;
typename std::shared_ptr<rclcpp::Client<ServiceT>> service_client_;
@@ -99,7 +110,7 @@ protected:
// The timeout value while to use in the tick loop while waiting for
// a result from the server
- std::chrono::milliseconds node_loop_timeout_;
+ std::chrono::milliseconds server_timeout_;
};
} // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp
index ecbe918ba9d8cb14bdd363b9bdc7f4e4e97dc2b9..0ddcb3bb0900dc5e26110cfca5ca79e4920679c6 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp
@@ -40,7 +40,7 @@ public:
getInput("path", goal_.path);
}
- void on_loop_timeout() override
+ void on_server_timeout() override
{
// Check if the goal has been updated
if (config().blackboard->get<bool>("path_updated")) {
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index c06b2ea2206b67771f50db5960c27177e5a00f1a..9a9a0ad1e5221a93ffd45ff98a7f382475c3599f 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -82,7 +82,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
// Put items on the blackboard
blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_); // NOLINT
blackboard_->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", tf_); // NOLINT
- blackboard_->set<std::chrono::milliseconds>("node_loop_timeout", std::chrono::milliseconds(10)); // NOLINT
+ blackboard_->set<std::chrono::milliseconds>("server_timeout", std::chrono::milliseconds(10)); // NOLINT
blackboard_->set<bool>("path_updated", false); // NOLINT
blackboard_->set<bool>("initial_pose_received", false); // NOLINT