From 3ed934a16719734a8377a6e230699c995975f320 Mon Sep 17 00:00:00 2001
From: matt-chen <chenbo@yunji.ai>
Date: Tue, 22 Oct 2019 21:19:26 +0800
Subject: [PATCH] Add an input port to control the action/service servers'
timeout
---
.../nav2_behavior_tree/bt_action_node.hpp | 23 +++++++++++-----
.../nav2_behavior_tree/bt_conversions.hpp | 6 +++++
.../nav2_behavior_tree/bt_service_node.hpp | 27 +++++++++++++------
.../nav2_behavior_tree/follow_path_action.hpp | 2 +-
nav2_bt_navigator/src/bt_navigator.cpp | 2 +-
5 files changed, 43 insertions(+), 17 deletions(-)
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 9208cd22..af4b70b5 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 ead7ed68..ed3a4cc7 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 8f40866b..1a7eb037 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 ecbe918b..0ddcb3bb 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 c06b2ea2..9a9a0ad1 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
--
GitLab