From 91c69df23faf3c357e1cfd651d95bbcfab6c2af0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Adam=20Ga=C5=82ecki?=
 <73710544+embeddedadam@users.noreply.github.com>
Date: Mon, 20 Nov 2023 20:02:03 +0100
Subject: [PATCH] WIP: Make BT nodes have configurable wait times. (#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.

[#3920]

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

* 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.

[#3920]

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

* 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 <embeddedadam@gmail.com>

* Fix formatting errors

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

* Update system tests to match new parameter

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

---------

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>
Signed-off-by: gg <josho.wallace@gmail.com>
---
 .../include/nav2_behavior_tree/bt_action_node.hpp        | 7 ++++++-
 .../include/nav2_behavior_tree/bt_action_server.hpp      | 3 +++
 .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 9 +++++++++
 .../include/nav2_behavior_tree/bt_cancel_action_node.hpp | 6 +++++-
 .../include/nav2_behavior_tree/bt_service_node.hpp       | 7 ++++++-
 .../test/plugins/action/test_assisted_teleop_action.cpp  | 3 +++
 .../plugins/action/test_assisted_teleop_cancel_node.cpp  | 3 +++
 .../test/plugins/action/test_back_up_action.cpp          | 3 +++
 .../test/plugins/action/test_back_up_cancel_node.cpp     | 3 +++
 .../test/plugins/action/test_bt_action_node.cpp          | 1 +
 .../test/plugins/action/test_clear_costmap_service.cpp   | 9 +++++++++
 .../action/test_compute_path_through_poses_action.cpp    | 3 +++
 .../plugins/action/test_compute_path_to_pose_action.cpp  | 3 +++
 .../test/plugins/action/test_controller_cancel_node.cpp  | 3 +++
 .../test/plugins/action/test_drive_on_heading_action.cpp | 3 +++
 .../plugins/action/test_drive_on_heading_cancel_node.cpp | 3 +++
 .../test/plugins/action/test_follow_path_action.cpp      | 3 +++
 .../action/test_navigate_through_poses_action.cpp        | 3 +++
 .../test/plugins/action/test_navigate_to_pose_action.cpp | 3 +++
 .../test_reinitialize_global_localization_service.cpp    | 3 +++
 .../test/plugins/action/test_smooth_path_action.cpp      | 3 +++
 .../test/plugins/action/test_spin_action.cpp             | 3 +++
 .../test/plugins/action/test_spin_cancel_node.cpp        | 3 +++
 .../test/plugins/action/test_wait_action.cpp             | 3 +++
 .../test/plugins/action/test_wait_cancel_node.cpp        | 3 +++
 nav2_bringup/params/nav2_multirobot_params_1.yaml        | 1 +
 nav2_bringup/params/nav2_multirobot_params_2.yaml        | 1 +
 nav2_bringup/params/nav2_multirobot_params_all.yaml      | 1 +
 nav2_bringup/params/nav2_params.yaml                     | 1 +
 .../src/behavior_tree/test_behavior_tree_node.cpp        | 2 ++
 .../src/gps_navigation/nav2_no_map_params.yaml           | 1 +
 31 files changed, 100 insertions(+), 3 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 e6eef2992f..39c5c5f7d9 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
@@ -61,6 +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_);
+    wait_for_service_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
     // Initialize the input and output messages
     goal_ = typename ActionT::Goal();
@@ -93,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(1s)) {
+    if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
         action_name.c_str());
@@ -462,6 +464,9 @@ class BtActionNode : public BT::ActionNodeBase
   // The timeout value for BT loop execution
   std::chrono::milliseconds bt_loop_duration_;
 
+  // The timeout value for waiting for a service to response
+  std::chrono::milliseconds 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>>
   future_goal_handle_;
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
index 467e4c80d0..331ca237c8 100644
--- 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 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
index 8d06ee7075..cd089692b8 100644
--- 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 wait_for_service_timeout;
+  node->get_parameter("wait_for_service_timeout", wait_for_service_timeout);
+  wait_for_service_timeout_ = std::chrono::milliseconds(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,9 @@ 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>(
+    "wait_for_service_timeout",
+    wait_for_service_timeout_);
 
   return true;
 }
@@ -235,6 +241,9 @@ 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>(
+        "wait_for_service_timeout",
+        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
index 179c93e4d2..ea3e41d859 100644
--- 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,6 +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_);
+    wait_for_service_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
     std::string remapped_action_name;
     if (getInput("server_name", remapped_action_name)) {
@@ -89,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(1s)) {
+    if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
         action_name.c_str());
@@ -168,6 +170,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
   // The timeout value while waiting for response from a server when a
   // new action goal is canceled
   std::chrono::milliseconds server_timeout_;
+  // The timeout value for waiting for a service to response
+  std::chrono::milliseconds 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
index a027ac7760..1afdb5adfd 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
@@ -62,6 +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_);
+    wait_for_service_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
     // Now that we have node_ to use, create the service client for this BT service
     getInput("service_name", service_name_);
@@ -77,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(1s)) {
+    if (!service_client_->wait_for_service(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
         service_node_name.c_str());
@@ -249,6 +251,9 @@ class BtServiceNode : public BT::ActionNodeBase
   // The timeout value for BT loop execution
   std::chrono::milliseconds bt_loop_duration_;
 
+  // The timeout value for waiting for a service to response
+  std::chrono::milliseconds wait_for_service_timeout_;
+
   // To track the server response when a new request is sent
   std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;
   bool request_sent_{false};
diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp
index 38033598d0..4bb9905759 100644
--- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp
@@ -68,6 +68,9 @@ class AssistedTeleopActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
     config_->blackboard->set<int>("number_recoveries", 0);
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp
index a7af3efef6..40f6f65c88 100644
--- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp
@@ -65,6 +65,9 @@ class CancelAssistedTeleopActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     client_ = rclcpp_action::create_client<nav2_msgs::action::AssistedTeleop>(
       node_, "assisted_teleop");
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp
index 6f0d54e7ca..85e3d3764d 100644
--- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp
@@ -68,6 +68,9 @@ class BackUpActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
     config_->blackboard->set<int>("number_recoveries", 0);
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp
index 35ab9d2f88..99e6eb1b45 100644
--- a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp
@@ -64,6 +64,9 @@ class CancelBackUpActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     client_ = rclcpp_action::create_client<nav2_msgs::action::BackUp>(
       node_, "back_up");
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp
index 12c25d30d6..a283dee33e 100644
--- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp
@@ -173,6 +173,7 @@ class BTActionNodeTestFixture : public ::testing::Test
     config_->blackboard->set<rclcpp::Node::SharedPtr>("node", node_);
     config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 20ms);
     config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
+    config_->blackboard->set<std::chrono::milliseconds>("wait_for_service_timeout", 1000ms);
     config_->blackboard->set<bool>("initial_pose_received", false);
     config_->blackboard->set<bool>("on_cancelled_triggered", false);
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp
index 4351152acf..88173e8420 100644
--- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp
@@ -53,6 +53,9 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
     config_->blackboard->set<int>("number_recoveries", 0);
 
@@ -139,6 +142,9 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
     config_->blackboard->set<int>("number_recoveries", 0);
 
@@ -231,6 +237,9 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
     config_->blackboard->set<int>("number_recoveries", 0);
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp
index 40bbeb1a37..ec619b0120 100644
--- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp
@@ -76,6 +76,9 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
 
     BT::NodeBuilder builder =
diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp
index ca5bcea666..d726186cd3 100644
--- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp
@@ -74,6 +74,9 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
 
     BT::NodeBuilder builder =
diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp
index 5128391d03..290c9133a5 100644
--- a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp
@@ -64,6 +64,9 @@ class CancelControllerActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     client_ = rclcpp_action::create_client<nav2_msgs::action::FollowPath>(
       node_, "follow_path");
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp
index 043f1aedb6..06478df588 100644
--- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp
@@ -69,6 +69,9 @@ class DriveOnHeadingActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
 
     BT::NodeBuilder builder =
diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp
index cc2e7f0693..5378123a46 100644
--- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp
@@ -65,6 +65,9 @@ class CancelDriveOnHeadingTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     client_ = rclcpp_action::create_client<nav2_msgs::action::DriveOnHeading>(
       node_, "drive_on_heading_cancel");
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp
index 4fa291c4b7..c4fcc57519 100644
--- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp
@@ -67,6 +67,9 @@ class FollowPathActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
 
     BT::NodeBuilder builder =
diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp
index bd028827af..5fa2c9aa03 100644
--- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp
@@ -70,6 +70,9 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
     std::vector<geometry_msgs::msg::PoseStamped> poses;
     config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>(
diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp
index 13e3a2a2ca..264b775a68 100644
--- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp
@@ -68,6 +68,9 @@ class NavigateToPoseActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
 
     BT::NodeBuilder builder =
diff --git a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp
index 122e6d0e0b..2f7a36e7fa 100644
--- a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp
@@ -53,6 +53,9 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
 
     factory_->registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(
diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp
index d1c930b5e4..c2e4389021 100644
--- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp
@@ -67,6 +67,9 @@ class SmoothPathActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
 
     BT::NodeBuilder builder =
diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp
index 56f5cdae74..a6d9f36d57 100644
--- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp
@@ -68,6 +68,9 @@ class SpinActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
     config_->blackboard->set<int>("number_recoveries", 0);
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp
index d773e88bca..ea3b01b983 100644
--- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp
@@ -64,6 +64,9 @@ class CancelSpinActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     client_ = rclcpp_action::create_client<nav2_msgs::action::Spin>(
       node_, "spin");
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp
index 2df39599a9..f54250a2b6 100644
--- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp
@@ -59,6 +59,9 @@ class WaitActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     config_->blackboard->set<bool>("initial_pose_received", false);
     config_->blackboard->set<int>("number_recoveries", 0);
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp
index e8d4d053c7..0773c72c6d 100644
--- a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp
+++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp
@@ -64,6 +64,9 @@ class CancelWaitActionTestFixture : public ::testing::Test
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout",
+      std::chrono::milliseconds(1000));
     client_ = rclcpp_action::create_client<nav2_msgs::action::Wait>(
       node_, "wait");
 
diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml
index 692ca6ec80..d0bdce21db 100644
--- 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
+    wait_for_service_timeout: 1000
     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
index 2d57aef383..cd51b1332e 100644
--- 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
+    wait_for_service_timeout: 1000
     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
index 6f7e66cd61..b6e5712092 100644
--- 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
+    wait_for_service_timeout: 1000
     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
index 112a1de98b..e70fa9216c 100644
--- 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
+    wait_for_service_timeout: 1000
     action_server_result_timeout: 900.0
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp
index 48ba35038c..48b97848f8 100644
--- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp
+++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp
@@ -138,6 +138,8 @@ class BehaviorTreeHandler
       "server_timeout", std::chrono::milliseconds(20));  // NOLINT
     blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration", std::chrono::milliseconds(10));  // NOLINT
+    blackboard->set<std::chrono::milliseconds>(
+      "wait_for_service_timeout", std::chrono::milliseconds(1000));  // NOLINT
     blackboard->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", tf_);  // NOLINT
     blackboard->set<bool>("initial_pose_received", false);  // NOLINT
     blackboard->set<int>("number_recoveries", 0);  // NOLINT
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
index 2539cef7b5..7b3125afba 100644
--- 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
+    wait_for_service_timeout: 1000
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
       plugin: "nav2_bt_navigator/NavigateToPoseNavigator"