diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp
index abc421cbc197cc838d380f2e5e5c1df71f57e592..8eb8518cabf0d4fdfd50c0037e7965acdfba8d1a 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp
@@ -52,10 +52,10 @@ public:
static BT::PortsList providedPorts()
{
- return {
- BT::InputPort<double>("backup_dist", -0.15, "Distance to backup"),
- BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup")
- };
+ return providedBasicPorts({
+ BT::InputPort<double>("backup_dist", -0.15, "Distance to backup"),
+ BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup")
+ });
}
};
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..e069794f104af7780c20630f48822bcf10bcc424 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,25 @@ public:
{
}
+ // Any subclass of BtActionNode that accepts parameters must provide a providedPorts method
+ // and call providedBasicPorts in it.
+ static BT::PortsList providedBasicPorts(BT::PortsList addition)
+ {
+ BT::PortsList basic = {
+ BT::InputPort<std::chrono::milliseconds>("server_timeout")
+ };
+ basic.insert(addition.begin(), addition.end());
+
+ return basic;
+ }
+
+ static BT::PortsList providedPorts()
+ {
+ return providedBasicPorts({});
+ }
+
// 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 +89,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 +124,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 +215,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..6b7d783511b15876a0dffa14e9ae735f1748e5e6 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>(const StringView key)
+{
+ return std::chrono::milliseconds(std::stoul(key.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..76fbdcdfb11235ac192cc63b8d947a7dc9f4beff 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_);
@@ -59,12 +60,22 @@ public:
{
}
- // Any BT node that accepts parameters must provide a requiredNodeParameters method
- static BT::PortsList providedPorts()
+ // Any subclass of BtServiceNode that accepts parameters must provide a providedPorts method
+ // and call providedBasicPorts in it.
+ static BT::PortsList providedBasicPorts(BT::PortsList addition)
{
- return {
- BT::InputPort<std::string>("service_name", "please_set_service_name_in_BT_Node")
+ BT::PortsList basic = {
+ BT::InputPort<std::string>("service_name", "please_set_service_name_in_BT_Node"),
+ BT::InputPort<std::chrono::milliseconds>("server_timeout")
};
+ basic.insert(addition.begin(), addition.end());
+
+ return basic;
+ }
+
+ static BT::PortsList providedPorts()
+ {
+ return providedBasicPorts({});
}
// The main override required by a BT service
@@ -75,12 +86,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 +103,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 +119,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/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
index 91e8bc488274641e1dd8dece446c97dcbeb93cf4..d610563b0f41631878ace76c2c07efeff20ca54b 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
@@ -53,10 +53,10 @@ public:
static BT::PortsList providedPorts()
{
- return {
- BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
- BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to")
- };
+ return providedBasicPorts({
+ BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
+ BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to")
+ });
}
private:
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..023cb3780bbae0a097aaa47305d74e0b6b107342 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")) {
@@ -56,9 +56,9 @@ public:
static BT::PortsList providedPorts()
{
- return {
- BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
- };
+ return providedBasicPorts({
+ BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
+ });
}
};
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp
index e3dca52f37c773cb67548144fe8da15899b8431b..46f40fec5530664ab1d1b306a79167a57ff19c30 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp
@@ -58,10 +58,10 @@ public:
// Any BT node that accepts parameters must provide a requiredNodeParameters method
static BT::PortsList providedPorts()
{
- return {
- BT::InputPort<geometry_msgs::msg::Point>("position", "0;0;0", "Position"),
- BT::InputPort<geometry_msgs::msg::Quaternion>("orientation", "0;0;0;0", "Orientation")
- };
+ return providedBasicPorts({
+ BT::InputPort<geometry_msgs::msg::Point>("position", "0;0;0", "Position"),
+ BT::InputPort<geometry_msgs::msg::Quaternion>("orientation", "0;0;0;0", "Orientation")
+ });
}
};
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
index 38c11dc3619d2bb4dcf70544f4e1ba4b3daa193f..2324a917e434b2fce958116d58d780dc56f301dd 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
@@ -43,9 +43,9 @@ public:
static BT::PortsList providedPorts()
{
- return {
- BT::InputPort<double>("spin_dist", 1.57, "Spin distance")
- };
+ return providedBasicPorts({
+ BT::InputPort<double>("spin_dist", 1.57, "Spin distance")
+ });
}
};
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp
index f31a8c28bf1e60935e0f9d0282968f8b95e54d08..1bd4997f72fb62994b3fa49d52cbd44fedff992a 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp
@@ -47,9 +47,9 @@ public:
// Any BT node that accepts parameters must provide a requiredNodeParameters method
static BT::PortsList providedPorts()
{
- return {
- BT::InputPort<int>("wait_duration", 1, "Wait time")
- };
+ return providedBasicPorts({
+ BT::InputPort<int>("wait_duration", 1, "Wait time")
+ });
}
};
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