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

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_);
+    bt_wait_for_service_time_ =
+      config().blackboard->template get<std::chrono::milliseconds>("bt_wait_for_service_time");
 
     // 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(bt_wait_for_service_time_)) {
       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 bt_wait_for_service_time_;
+
   // 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_;

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_);
+    bt_wait_for_service_time_ =
+      config().blackboard->template get<std::chrono::milliseconds>("bt_wait_for_service_time");
 
     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(bt_wait_for_service_time_)) {
       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 bt_wait_for_service_time_;
 };
 
 }  // namespace nav2_behavior_tree

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_);
+    bt_wait_for_service_time_ =
+      config().blackboard->template get<std::chrono::milliseconds>("bt_wait_for_service_time");
 
     // 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(bt_wait_for_service_time_)) {
       RCLCPP_ERROR(
         node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
         service_node_name.c_str());
@@ -248,6 +250,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 bt_wait_for_service_time_;
 
   // To track the server response when a new request is sent
   std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;