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]>
embeddedadam · Nov 14, 2023 · 2812ce2 · 2812ce2
1 parent 8fde00d
commit 2812ce2
Show file tree

Hide file tree

Showing 10 changed files with 25 additions and 12 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
@@ -61,8 +61,8 @@ class BtActionNode : public BT::ActionNodeBase
     server_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
     getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
-    bt_wait_for_service_time_ =
-      config().blackboard->template get<std::chrono::milliseconds>("bt_wait_for_service_time");
+    bt_wait_for_service_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("bt_wait_for_service_timeout_");
 
     // Initialize the input and output messages
     goal_ = typename ActionT::Goal();
@@ -95,7 +95,7 @@ class BtActionNode : public BT::ActionNodeBase
 
     // Make sure the server is actually there before continuing
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
-    if (!action_client_->wait_for_action_server(bt_wait_for_service_time_)) {
+    if (!action_client_->wait_for_action_server(bt_wait_for_service_timeout_)) {
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
         action_name.c_str());
@@ -465,7 +465,7 @@ class BtActionNode : public BT::ActionNodeBase
   std::chrono::milliseconds bt_loop_duration_;
 
   // The timeout value for waiting for a service to response
-  std::chrono::milliseconds bt_wait_for_service_time_;
+  std::chrono::milliseconds bt_wait_for_service_timeout_;
 
   // To track the action server acknowledgement when a new goal is sent
   std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
@@ -248,6 +248,9 @@ class BtActionServer
   // Default timeout value while waiting for response from a server
   std::chrono::milliseconds default_server_timeout_;
 
+  // The timeout value for waiting for a service to response
+  std::chrono::milliseconds bt_wait_for_service_timeout_;
+
   // User-provided callbacks
   OnGoalReceivedCallback on_goal_received_callback_;
   OnLoopCallback on_loop_callback_;

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -158,6 +158,9 @@ bool BtActionServer<ActionT>::on_configure()
   int default_server_timeout;
   node->get_parameter("default_server_timeout", default_server_timeout);
   default_server_timeout_ = std::chrono::milliseconds(default_server_timeout);
+  int bt_wait_for_service_timeout;
+  node->get_parameter("bt_wait_for_service_timeout", bt_wait_for_service_timeout);
+  bt_wait_for_service_timeout_ = std::chrono::milliseconds(bt_wait_for_service_timeout);
 
   // Get error code id names to grab off of the blackboard
   error_code_names_ = node->get_parameter("error_code_names").as_string_array();
@@ -172,6 +175,7 @@ bool BtActionServer<ActionT>::on_configure()
   blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_);  // NOLINT
   blackboard_->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);  // NOLINT
   blackboard_->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);  // NOLINT
+  blackboard_->set<std::chrono::milliseconds>("bt_wait_for_service_timeout_", bt_wait_for_service_timeout_);  // NOLINT
 
   return true;
 }
@@ -235,6 +239,7 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
       blackboard->set<rclcpp::Node::SharedPtr>("node", client_node_);
       blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
       blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
+      blackboard->set<std::chrono::milliseconds>("bt_wait_for_service_timeout", bt_wait_for_service_timeout_);
     }
   } catch (const std::exception & e) {
     RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp
@@ -59,8 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
     server_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
     getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
-    bt_wait_for_service_time_ =
-      config().blackboard->template get<std::chrono::milliseconds>("bt_wait_for_service_time");
+    bt_wait_for_service_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("bt_wait_for_service_timeout_");
 
     std::string remapped_action_name;
     if (getInput("server_name", remapped_action_name)) {
@@ -91,7 +91,7 @@ class BtCancelActionNode : public BT::ActionNodeBase
 
     // Make sure the server is actually there before continuing
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
-    if (!action_client_->wait_for_action_server(bt_wait_for_service_time_)) {
+    if (!action_client_->wait_for_action_server(bt_wait_for_service_timeout_)) {
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
         action_name.c_str());
@@ -171,7 +171,7 @@ class BtCancelActionNode : public BT::ActionNodeBase
   // new action goal is canceled
   std::chrono::milliseconds server_timeout_;
   // The timeout value for waiting for a service to response
-  std::chrono::milliseconds bt_wait_for_service_time_;
+  std::chrono::milliseconds bt_wait_for_service_timeout_;
 };
 
 }  // namespace nav2_behavior_tree

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
@@ -62,8 +62,8 @@ class BtServiceNode : public BT::ActionNodeBase
     server_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
     getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
-    bt_wait_for_service_time_ =
-      config().blackboard->template get<std::chrono::milliseconds>("bt_wait_for_service_time");
+    bt_wait_for_service_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("bt_wait_for_service_timeout_");
 
     // Now that we have node_ to use, create the service client for this BT service
     getInput("service_name", service_name_);
@@ -79,7 +79,7 @@ class BtServiceNode : public BT::ActionNodeBase
     RCLCPP_DEBUG(
       node_->get_logger(), "Waiting for \"%s\" service",
       service_name_.c_str());
-    if (!service_client_->wait_for_service(bt_wait_for_service_time_)) {
+    if (!service_client_->wait_for_service(bt_wait_for_service_timeout_)) {
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
         service_node_name.c_str());
@@ -252,7 +252,7 @@ class BtServiceNode : public BT::ActionNodeBase
   std::chrono::milliseconds bt_loop_duration_;
 
   // The timeout value for waiting for a service to response
-  std::chrono::milliseconds bt_wait_for_service_time_;
+  std::chrono::milliseconds bt_wait_for_service_timeout_;
 
   // To track the server response when a new request is sent
   std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;

diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml
@@ -45,6 +45,7 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    bt_wait_for_service_timeout: 1000.0
     action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:

diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml
@@ -45,6 +45,7 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    bt_wait_for_service_timeout: 1000.0
     action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:

diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml
@@ -45,6 +45,7 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    bt_wait_for_service_timeout: 1000.0
     action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:

diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
@@ -45,6 +45,7 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    bt_wait_for_service_timeout: 1000.0
     action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:

diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml
@@ -5,6 +5,7 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    bt_wait_for_service_timeout: 1000.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"