From 8fde00deddf760da76ee995b0ef7c57772e878eb Mon Sep 17 00:00:00 2001 From: Adam Galecki Date: Mon, 13 Nov 2023 09:53:29 +0100 Subject: [PATCH 1/8] 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 --- .../include/nav2_behavior_tree/bt_action_node.hpp | 7 ++++++- .../include/nav2_behavior_tree/bt_cancel_action_node.hpp | 6 +++++- .../include/nav2_behavior_tree/bt_service_node.hpp | 7 ++++++- 3 files changed, 17 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..9e1e9c86f8 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("server_timeout"); getInput("server_timeout", server_timeout_); + bt_wait_for_service_time_ = + config().blackboard->template get("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::SharedPtr>> future_goal_handle_; 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..88add8e8c4 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("server_timeout"); getInput("server_timeout", server_timeout_); + bt_wait_for_service_time_ = + config().blackboard->template get("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 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..9c0d051ecd 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("server_timeout"); getInput("server_timeout", server_timeout_); + bt_wait_for_service_time_ = + config().blackboard->template get("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 future_result_; From 2812ce2017981a83e0cee870aae8709802e10e6a Mon Sep 17 00:00:00 2001 From: Adam Galecki Date: Mon, 13 Nov 2023 09:53:29 +0100 Subject: [PATCH 2/8] 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 --- .../include/nav2_behavior_tree/bt_action_node.hpp | 8 ++++---- .../include/nav2_behavior_tree/bt_action_server.hpp | 3 +++ .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 5 +++++ .../include/nav2_behavior_tree/bt_cancel_action_node.hpp | 8 ++++---- .../include/nav2_behavior_tree/bt_service_node.hpp | 8 ++++---- 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/gps_navigation/nav2_no_map_params.yaml | 1 + 10 files changed, 25 insertions(+), 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 index 9e1e9c86f8..e6f3aedc2d 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,8 +61,8 @@ class BtActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); - bt_wait_for_service_time_ = - config().blackboard->template get("bt_wait_for_service_time"); + bt_wait_for_service_timeout_ = + config().blackboard->template get("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::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 index 467e4c80d0..79d55751d0 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 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 index 8d06ee7075..65235232f3 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::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::on_configure() blackboard_->set("node", client_node_); // NOLINT blackboard_->set("server_timeout", default_server_timeout_); // NOLINT blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT + blackboard_->set("bt_wait_for_service_timeout_", bt_wait_for_service_timeout_); // NOLINT return true; } @@ -235,6 +239,7 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); + blackboard->set("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 index 88add8e8c4..d31f819c04 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,8 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); - bt_wait_for_service_time_ = - config().blackboard->template get("bt_wait_for_service_time"); + bt_wait_for_service_timeout_ = + config().blackboard->template get("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 index 9c0d051ecd..8a40693d2e 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,8 +62,8 @@ class BtServiceNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); - bt_wait_for_service_time_ = - config().blackboard->template get("bt_wait_for_service_time"); + bt_wait_for_service_timeout_ = + config().blackboard->template get("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 future_result_; diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 692ca6ec80..a82e2e6a10 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 + 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 index 2d57aef383..7c5cf1dd7a 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 + 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 index 6f7e66cd61..97356daa06 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 + 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 index 112a1de98b..09c65876f5 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 + 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 index 2539cef7b5..e270cc55df 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 + bt_wait_for_service_timeout: 1000.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" From 99a5f899118aab43e265be0bc639f87eed2f634e Mon Sep 17 00:00:00 2001 From: Adam Galecki Date: Fri, 17 Nov 2023 18:12:06 +0100 Subject: [PATCH 3/8] Fix typos, linting errors and value type from float to int --- .../include/nav2_behavior_tree/bt_action_node.hpp | 8 ++++---- .../include/nav2_behavior_tree/bt_action_server.hpp | 2 +- .../nav2_behavior_tree/bt_action_server_impl.hpp | 10 +++++----- .../nav2_behavior_tree/bt_cancel_action_node.hpp | 8 ++++---- .../include/nav2_behavior_tree/bt_service_node.hpp | 10 +++++----- nav2_bringup/params/nav2_multirobot_params_1.yaml | 2 +- nav2_bringup/params/nav2_multirobot_params_2.yaml | 2 +- nav2_bringup/params/nav2_multirobot_params_all.yaml | 2 +- nav2_bringup/params/nav2_params.yaml | 2 +- .../src/gps_navigation/nav2_no_map_params.yaml | 2 +- 10 files changed, 24 insertions(+), 24 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 e6f3aedc2d..f39e90c36b 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,8 +61,8 @@ class BtActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); - bt_wait_for_service_timeout_ = - config().blackboard->template get("bt_wait_for_service_timeout_"); + wait_for_service_timeout_ = + config().blackboard->template get("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_timeout_)) { + 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()); @@ -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_timeout_; + std::chrono::milliseconds wait_for_service_timeout_; // To track the action server acknowledgement when a new goal is sent std::shared_ptr::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 index 79d55751d0..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 @@ -249,7 +249,7 @@ class BtActionServer std::chrono::milliseconds default_server_timeout_; // The timeout value for waiting for a service to response - std::chrono::milliseconds bt_wait_for_service_timeout_; + std::chrono::milliseconds wait_for_service_timeout_; // User-provided callbacks OnGoalReceivedCallback on_goal_received_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 65235232f3..f8a196b480 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,9 +158,9 @@ bool BtActionServer::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); + 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(); @@ -175,7 +175,7 @@ bool BtActionServer::on_configure() blackboard_->set("node", client_node_); // NOLINT blackboard_->set("server_timeout", default_server_timeout_); // NOLINT blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT - blackboard_->set("bt_wait_for_service_timeout_", bt_wait_for_service_timeout_); // NOLINT + blackboard_->set("wait_for_service_timeout_", wait_for_service_timeout_); // NOLINT return true; } @@ -239,7 +239,7 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); - blackboard->set("bt_wait_for_service_timeout", bt_wait_for_service_timeout_); + blackboard->set("wait_for_service_timeout", wait_for_service_timeout_); // NOLINT } } 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 d31f819c04..e064b19faf 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,8 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); - bt_wait_for_service_timeout_ = - config().blackboard->template get("bt_wait_for_service_timeout_"); + wait_for_service_timeout_ = + config().blackboard->template get("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_timeout_)) { + 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()); @@ -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_timeout_; + 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 8a40693d2e..586540529b 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,8 +62,8 @@ class BtServiceNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); - bt_wait_for_service_timeout_ = - config().blackboard->template get("bt_wait_for_service_timeout_"); + wait_for_service_timeout_ = + config().blackboard->template get("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_timeout_)) { + 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()); @@ -250,9 +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_timeout_; + std::chrono::milliseconds wait_for_service_timeout_; // To track the server response when a new request is sent std::shared_future future_result_; diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index a82e2e6a10..d0bdce21db 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -45,7 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 - bt_wait_for_service_timeout: 1000.0 + 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 7c5cf1dd7a..cd51b1332e 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -45,7 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 - bt_wait_for_service_timeout: 1000.0 + 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 97356daa06..b6e5712092 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -45,7 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 - bt_wait_for_service_timeout: 1000.0 + 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 09c65876f5..e70fa9216c 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -45,7 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 - bt_wait_for_service_timeout: 1000.0 + 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/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index e270cc55df..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,7 +5,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 - bt_wait_for_service_timeout: 1000.0 + wait_for_service_timeout: 1000 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" From db767d65230153fdb53bbbd368f43e480c609187 Mon Sep 17 00:00:00 2001 From: Adam Galecki Date: Fri, 17 Nov 2023 21:18:42 +0100 Subject: [PATCH 4/8] Fix extra underscores --- .../include/nav2_behavior_tree/bt_action_node.hpp | 2 +- .../include/nav2_behavior_tree/bt_cancel_action_node.hpp | 2 +- .../include/nav2_behavior_tree/bt_service_node.hpp | 2 +- 3 files changed, 3 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 f39e90c36b..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 @@ -62,7 +62,7 @@ class BtActionNode : public BT::ActionNodeBase config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); wait_for_service_timeout_ = - config().blackboard->template get("wait_for_service_timeout_"); + config().blackboard->template get("wait_for_service_timeout"); // Initialize the input and output messages goal_ = typename ActionT::Goal(); 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 e064b19faf..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 @@ -60,7 +60,7 @@ class BtCancelActionNode : public BT::ActionNodeBase config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); wait_for_service_timeout_ = - config().blackboard->template get("wait_for_service_timeout_"); + config().blackboard->template get("wait_for_service_timeout"); std::string remapped_action_name; if (getInput("server_name", remapped_action_name)) { 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 586540529b..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 @@ -63,7 +63,7 @@ class BtServiceNode : public BT::ActionNodeBase config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); wait_for_service_timeout_ = - config().blackboard->template get("wait_for_service_timeout_"); + config().blackboard->template get("wait_for_service_timeout"); // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); From 9fb2da9e4c45466c709dff75c10a4456aab00c4c Mon Sep 17 00:00:00 2001 From: Adam Galecki Date: Fri, 17 Nov 2023 21:32:56 +0100 Subject: [PATCH 5/8] Fix extra underscore --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 f8a196b480..b59176143a 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 @@ -175,7 +175,7 @@ bool BtActionServer::on_configure() blackboard_->set("node", client_node_); // NOLINT blackboard_->set("server_timeout", default_server_timeout_); // NOLINT blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT - blackboard_->set("wait_for_service_timeout_", wait_for_service_timeout_); // NOLINT + blackboard_->set("wait_for_service_timeout", wait_for_service_timeout_); // NOLINT return true; } From 6507a605748e5826bf01fb7bc6ae16db9929fe01 Mon Sep 17 00:00:00 2001 From: Adam Galecki Date: Sun, 19 Nov 2023 14:03:10 +0100 Subject: [PATCH 6/8] Update unit tests with blackboard parameter Signed-off-by: Adam Galecki --- .../test/plugins/action/test_assisted_teleop_action.cpp | 3 +++ .../plugins/action/test_assisted_teleop_cancel_node.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 +++ .../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 +++ 18 files changed, 58 insertions(+) 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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("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..2049f7f839 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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "assisted_teleop"); 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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( 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("node", node_); config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); + config_->blackboard->set("wait_for_service_timeout", 1000ms); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -139,6 +142,9 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -231,6 +237,9 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "follow_path"); 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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( 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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); std::vector poses; config_->blackboard->set>( 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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); factory_->registerNodeType( 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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( 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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "wait"); From aa9492bf54ff083b4bce04cba957a4b9c3b975e6 Mon Sep 17 00:00:00 2001 From: Adam Galecki Date: Sun, 19 Nov 2023 15:12:54 +0100 Subject: [PATCH 7/8] Fix formatting errors Signed-off-by: Adam Galecki --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 8 ++++++-- .../plugins/action/test_assisted_teleop_cancel_node.cpp | 2 +- .../test/plugins/action/test_back_up_action.cpp | 3 +++ .../test/plugins/action/test_drive_on_heading_action.cpp | 3 +++ 4 files changed, 13 insertions(+), 3 deletions(-) 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 b59176143a..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 @@ -175,7 +175,9 @@ bool BtActionServer::on_configure() blackboard_->set("node", client_node_); // NOLINT blackboard_->set("server_timeout", default_server_timeout_); // NOLINT blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT - blackboard_->set("wait_for_service_timeout", wait_for_service_timeout_); // NOLINT + blackboard_->set( + "wait_for_service_timeout", + wait_for_service_timeout_); return true; } @@ -239,7 +241,9 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); - blackboard->set("wait_for_service_timeout", wait_for_service_timeout_); // NOLINT + blackboard->set( + "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/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp index 2049f7f839..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,7 +65,7 @@ class CancelAssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); - config_->blackboard->set( + config_->blackboard->set( "wait_for_service_timeout", std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( 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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); 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( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = From e5e08e4f563fc6480be146f459874708cad03a86 Mon Sep 17 00:00:00 2001 From: Adam Galecki Date: Sun, 19 Nov 2023 16:25:02 +0100 Subject: [PATCH 8/8] Update system tests to match new parameter Signed-off-by: Adam Galecki --- nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp | 2 ++ 1 file changed, 2 insertions(+) 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( "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT + blackboard->set( + "wait_for_service_timeout", std::chrono::milliseconds(1000)); // NOLINT blackboard->set>("tf_buffer", tf_); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT