Skip to content

Commit 10ef868

Browse files
christophfroehlichembeddedadambi0ha2ardkaichie
authored and
RBT22
committed
Make BT nodes have configurable wait times (Backport ros-navigation#3960 ros-navigation#4178 ros-navigation#4203) (ros-navigation#4409)
* WIP: Make BT nodes have configurable wait times. (ros-navigation#3960) * Make BT nodes have configurable wait times. Previous solution provided hardcoded 1s value. Right now the value can be configured for BT Action, Cancel and Service nodes. [ros-navigation#3920] Signed-off-by: Adam Galecki <[email protected]> * Make BT nodes have configurable wait times. Previous solution provided hardcoded 1s value. Right now the value can be configured for BT Action, Cancel and Service nodes. [ros-navigation#3920] Signed-off-by: Adam Galecki <[email protected]> * Fix typos, linting errors and value type from float to int * Fix extra underscores * Fix extra underscore * Update unit tests with blackboard parameter Signed-off-by: Adam Galecki <[email protected]> * Fix formatting errors Signed-off-by: Adam Galecki <[email protected]> * Update system tests to match new parameter Signed-off-by: Adam Galecki <[email protected]> --------- Signed-off-by: Adam Galecki <[email protected]> Signed-off-by: Christoph Froehlich <[email protected]> * chore(nav2_behavior_tree): log actual wait period in bt_action_node (ros-navigation#4178) Signed-off-by: Felix <[email protected]> Co-authored-by: Felix <[email protected]> Signed-off-by: Christoph Froehlich <[email protected]> * fix missing param declare (ros-navigation#4203) Signed-off-by: nelson <[email protected]> Signed-off-by: Christoph Froehlich <[email protected]> * Fix error messages (ros-navigation#4411) Signed-off-by: Christoph Froehlich <[email protected]> --------- Signed-off-by: Adam Galecki <[email protected]> Signed-off-by: Christoph Froehlich <[email protected]> Signed-off-by: Felix <[email protected]> Signed-off-by: nelson <[email protected]> Co-authored-by: Adam Gałecki <[email protected]> Co-authored-by: bi0ha2ard <[email protected]> Co-authored-by: Felix <[email protected]> Co-authored-by: nelson <[email protected]>
1 parent f1957a1 commit 10ef868

30 files changed

+110
-10
lines changed

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp‎

Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,8 @@ class BtActionNode : public BT::ActionNodeBase
6161
server_timeout_ =
6262
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
6363
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
64+
wait_for_service_timeout_ =
65+
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
6466

6567
// Initialize the input and output messages
6668
goal_ = typename ActionT::Goal();
@@ -93,10 +95,11 @@ class BtActionNode : public BT::ActionNodeBase
9395

9496
// Make sure the server is actually there before continuing
9597
RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
96-
if (!action_client_->wait_for_action_server(10s)) {
98+
if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
9799
RCLCPP_ERROR(
98-
node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
99-
action_name.c_str());
100+
node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
101+
action_name.c_str(),
102+
wait_for_service_timeout_.count() / 1000.0);
100103
throw std::runtime_error(
101104
std::string("Action server ") + action_name +
102105
std::string(" not available"));
@@ -463,6 +466,9 @@ class BtActionNode : public BT::ActionNodeBase
463466
// The timeout value for BT loop execution
464467
std::chrono::milliseconds bt_loop_duration_;
465468

469+
// The timeout value for waiting for a service to response
470+
std::chrono::milliseconds wait_for_service_timeout_;
471+
466472
// To track the action server acknowledgement when a new goal is sent
467473
std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
468474
future_goal_handle_;

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -242,6 +242,9 @@ class BtActionServer
242242
// Default timeout value while waiting for response from a server
243243
std::chrono::milliseconds default_server_timeout_;
244244

245+
// The timeout value for waiting for a service to response
246+
std::chrono::milliseconds wait_for_service_timeout_;
247+
245248
// User-provided callbacks
246249
OnGoalReceivedCallback on_goal_received_callback_;
247250
OnLoopCallback on_loop_callback_;

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,9 @@ BtActionServer<ActionT>::BtActionServer(
6161
if (!node->has_parameter("default_server_timeout")) {
6262
node->declare_parameter("default_server_timeout", 20);
6363
}
64+
if (!node->has_parameter("wait_for_service_timeout")) {
65+
node->declare_parameter("wait_for_service_timeout", 1000);
66+
}
6467

6568
std::vector<std::string> error_code_names = {
6669
"follow_path_error_code",
@@ -90,7 +93,7 @@ BtActionServer<ActionT>::BtActionServer(
9093
error_codes_str += " " + error_code;
9194
}
9295
RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:" << error_codes_str);
93-
}
96+
}
9497
}
9598
}
9699

@@ -144,6 +147,9 @@ bool BtActionServer<ActionT>::on_configure()
144147
bt_loop_duration_ = std::chrono::milliseconds(timeout);
145148
node->get_parameter("default_server_timeout", timeout);
146149
default_server_timeout_ = std::chrono::milliseconds(timeout);
150+
int wait_for_service_timeout;
151+
node->get_parameter("wait_for_service_timeout", wait_for_service_timeout);
152+
wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout);
147153

148154
// Get error code id names to grab off of the blackboard
149155
error_code_names_ = node->get_parameter("error_code_names").as_string_array();
@@ -158,6 +164,9 @@ bool BtActionServer<ActionT>::on_configure()
158164
blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_); // NOLINT
159165
blackboard_->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_); // NOLINT
160166
blackboard_->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_); // NOLINT
167+
blackboard_->set<std::chrono::milliseconds>(
168+
"wait_for_service_timeout",
169+
wait_for_service_timeout_);
161170

162171
return true;
163172
}
@@ -225,6 +234,9 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
225234
blackboard->set<rclcpp::Node::SharedPtr>("node", client_node_);
226235
blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
227236
blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
237+
blackboard->set<std::chrono::milliseconds>(
238+
"wait_for_service_timeout",
239+
wait_for_service_timeout_);
228240
}
229241
} catch (const std::exception & e) {
230242
RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
5959
server_timeout_ =
6060
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
6161
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
62+
wait_for_service_timeout_ =
63+
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
6264

6365
std::string remapped_action_name;
6466
if (getInput("server_name", remapped_action_name)) {
@@ -89,10 +91,10 @@ class BtCancelActionNode : public BT::ActionNodeBase
8991

9092
// Make sure the server is actually there before continuing
9193
RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
92-
if (!action_client_->wait_for_action_server(1s)) {
94+
if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
9395
RCLCPP_ERROR(
94-
node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
95-
action_name.c_str());
96+
node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
97+
action_name.c_str(), wait_for_service_timeout_.count() / 1000.0);
9698
throw std::runtime_error(
9799
std::string("Action server ") + action_name +
98100
std::string(" not available"));
@@ -168,6 +170,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
168170
// The timeout value while waiting for response from a server when a
169171
// new action goal is canceled
170172
std::chrono::milliseconds server_timeout_;
173+
// The timeout value for waiting for a service to response
174+
std::chrono::milliseconds wait_for_service_timeout_;
171175
};
172176

173177
} // namespace nav2_behavior_tree

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp‎

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -62,6 +62,8 @@ class BtServiceNode : public BT::ActionNodeBase
6262
server_timeout_ =
6363
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
6464
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
65+
wait_for_service_timeout_ =
66+
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
6567

6668
// Now that we have node_ to use, create the service client for this BT service
6769
getInput("service_name", service_name_);
@@ -77,10 +79,10 @@ class BtServiceNode : public BT::ActionNodeBase
7779
RCLCPP_DEBUG(
7880
node_->get_logger(), "Waiting for \"%s\" service",
7981
service_name_.c_str());
80-
if (!service_client_->wait_for_service(1s)) {
82+
if (!service_client_->wait_for_service(wait_for_service_timeout_)) {
8183
RCLCPP_ERROR(
82-
node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
83-
service_node_name.c_str());
84+
node_->get_logger(), "\"%s\" service server not available after waiting for %.2fs",
85+
service_name_.c_str(), wait_for_service_timeout_.count() / 1000.0);
8486
throw std::runtime_error(
8587
std::string(
8688
"Service server %s not available",
@@ -249,6 +251,9 @@ class BtServiceNode : public BT::ActionNodeBase
249251
// The timeout value for BT loop execution
250252
std::chrono::milliseconds bt_loop_duration_;
251253

254+
// The timeout value for waiting for a service to response
255+
std::chrono::milliseconds wait_for_service_timeout_;
256+
252257
// To track the server response when a new request is sent
253258
std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;
254259
bool request_sent_{false};

‎nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,9 @@ class AssistedTeleopActionTestFixture : public ::testing::Test
6868
config_->blackboard->set<std::chrono::milliseconds>(
6969
"bt_loop_duration",
7070
std::chrono::milliseconds(10));
71+
config_->blackboard->set<std::chrono::milliseconds>(
72+
"wait_for_service_timeout",
73+
std::chrono::milliseconds(1000));
7174
config_->blackboard->set<bool>("initial_pose_received", false);
7275
config_->blackboard->set<int>("number_recoveries", 0);
7376

‎nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,9 @@ class CancelAssistedTeleopActionTestFixture : public ::testing::Test
6565
config_->blackboard->set<std::chrono::milliseconds>(
6666
"bt_loop_duration",
6767
std::chrono::milliseconds(10));
68+
config_->blackboard->set<std::chrono::milliseconds>(
69+
"wait_for_service_timeout",
70+
std::chrono::milliseconds(1000));
6871
client_ = rclcpp_action::create_client<nav2_msgs::action::AssistedTeleop>(
6972
node_, "assisted_teleop");
7073

‎nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,9 @@ class BackUpActionTestFixture : public ::testing::Test
6868
config_->blackboard->set<std::chrono::milliseconds>(
6969
"bt_loop_duration",
7070
std::chrono::milliseconds(10));
71+
config_->blackboard->set<std::chrono::milliseconds>(
72+
"wait_for_service_timeout",
73+
std::chrono::milliseconds(1000));
7174
config_->blackboard->set<bool>("initial_pose_received", false);
7275
config_->blackboard->set<int>("number_recoveries", 0);
7376

‎nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,9 @@ class CancelBackUpActionTestFixture : public ::testing::Test
6464
config_->blackboard->set<std::chrono::milliseconds>(
6565
"bt_loop_duration",
6666
std::chrono::milliseconds(10));
67+
config_->blackboard->set<std::chrono::milliseconds>(
68+
"wait_for_service_timeout",
69+
std::chrono::milliseconds(1000));
6770
client_ = rclcpp_action::create_client<nav2_msgs::action::BackUp>(
6871
node_, "back_up");
6972

‎nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -173,6 +173,7 @@ class BTActionNodeTestFixture : public ::testing::Test
173173
config_->blackboard->set<rclcpp::Node::SharedPtr>("node", node_);
174174
config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 20ms);
175175
config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
176+
config_->blackboard->set<std::chrono::milliseconds>("wait_for_service_timeout", 1000ms);
176177
config_->blackboard->set<bool>("initial_pose_received", false);
177178
config_->blackboard->set<bool>("on_cancelled_triggered", false);
178179

0 commit comments

Comments
 (0)