From 8c048b0b66bac7c2c30de15940f1366d73258d92 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Fri, 23 Apr 2021 00:12:20 +0530 Subject: [PATCH 01/11] Use incremental timeouts in spin_until_future_calls during BT execution Signed-off-by: Sarthak Mittal --- .../nav2_behavior_tree/bt_action_node.hpp | 90 ++++++++++++++++--- .../nav2_behavior_tree/bt_action_server.hpp | 6 ++ .../bt_action_server_impl.hpp | 28 ++++-- .../nav2_behavior_tree/bt_service_node.hpp | 67 ++++++++++---- .../params/nav2_multirobot_params_1.yaml | 2 + .../params/nav2_multirobot_params_2.yaml | 2 + nav2_bringup/bringup/params/nav2_params.yaml | 2 + 7 files changed, 161 insertions(+), 36 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 de94e697e6e..827af08cbc3 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 @@ -49,10 +49,15 @@ class BtActionNode : public BT::ActionNodeBase node_ = config().blackboard->template get("node"); // Get the required items from the blackboard + bt_loop_timeout_ = + config().blackboard->template get("bt_loop_timeout"); server_timeout_ = - config().blackboard->template get("server_timeout"); + config().blackboard->template get("default_server_timeout"); getInput("server_timeout", server_timeout_); + // Set spin_until_future_complete timeout + timeout_ = server_timeout_ < bt_loop_timeout_ ? server_timeout_ : bt_loop_timeout_; + // Initialize the input and output messages goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); @@ -174,7 +179,24 @@ class BtActionNode : public BT::ActionNodeBase // user defined callback on_tick(); - on_new_goal_received(); + send_new_goal(); + } + + // if new goal was sent and action server has not yet responded + // check the future goal handle + if (goal_sent_) { + if (!check_future_goal_handle()) { + // return RUNNING if there is still some time before timeout happens + auto elapsed = (node_->now() - sent_time_).to_chrono(); + if (elapsed < server_timeout_) { + return BT::NodeStatus::RUNNING; + } + // if server has taken more time to respond than the specified timeout value return FAILURE + RCLCPP_WARN( + node_->get_logger(), + "Timed out while waiting for action server to respond for %s", action_name_.c_str()); + return BT::NodeStatus::FAILURE; + } } // The following code corresponds to the "RUNNING" loop @@ -187,7 +209,17 @@ class BtActionNode : public BT::ActionNodeBase goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) { goal_updated_ = false; - on_new_goal_received(); + send_new_goal(); + if (!check_future_goal_handle()) { + auto elapsed = (node_->now() - sent_time_).to_chrono(); + if (elapsed < server_timeout_) { + return BT::NodeStatus::RUNNING; + } + RCLCPP_WARN( + node_->get_logger(), + "Timed out while waiting for action server to respond for %s", action_name_.c_str()); + return BT::NodeStatus::FAILURE; + } } rclcpp::spin_some(node_); @@ -257,7 +289,7 @@ class BtActionNode : public BT::ActionNodeBase /** * @brief Function to send new goal to action server */ - void on_new_goal_received() + void send_new_goal() { goal_result_available_ = false; auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); @@ -272,18 +304,42 @@ class BtActionNode : public BT::ActionNodeBase } }; - auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); + future_goal_handle_ = action_client_->async_send_goal(goal_, send_goal_options); + sent_time_ = node_->now(); + goal_sent_ = true; + } - if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) - { + /** + * @brief Function to check response from the action server when a new goal is sent + * @return boolen True if future_goal_handle_ returns SUCCESS, False when it returns TIMEOUT + */ + bool check_future_goal_handle() + { + auto elapsed = (node_->now() - sent_time_).to_chrono(); + auto remaining = server_timeout_ - elapsed; + + // server has already timed out, no need to sleep + if (remaining <= std::chrono::milliseconds(0)) { + return false; + } + + auto timeout = remaining > timeout_ ? timeout_ : remaining; + + auto result = rclcpp::spin_until_future_complete(node_, future_goal_handle_, timeout); + if (result == rclcpp::FutureReturnCode::INTERRUPTED) { throw std::runtime_error("send_goal failed"); } - goal_handle_ = future_goal_handle.get(); - if (!goal_handle_) { - throw std::runtime_error("Goal was rejected by the action server"); + if (result == rclcpp::FutureReturnCode::SUCCESS) { + goal_handle_ = future_goal_handle_.get(); + if (!goal_handle_) { + throw std::runtime_error("Goal was rejected by the action server"); + } + goal_sent_ = false; + return true; } + + return false; } /** @@ -313,6 +369,18 @@ class BtActionNode : public BT::ActionNodeBase // The timeout value while waiting for response from a server when a // new action goal is sent or canceled std::chrono::milliseconds server_timeout_; + + // The timeout value for BT loop execution + std::chrono::milliseconds bt_loop_timeout_; + + // spin_until_future_complete timeout value + std::chrono::milliseconds timeout_; + + // To track the action server response when a new goal is sent + std::shared_future::SharedPtr> + future_goal_handle_; + bool goal_sent_{false}; + rclcpp::Time sent_time_; }; } // namespace nav2_behavior_tree 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 857df122be4..14c96f00e9f 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 @@ -225,6 +225,12 @@ class BtActionServer // To publish BT logs std::unique_ptr topic_logger_; + // Timeout value for each iteration of BT execution + std::chrono::milliseconds bt_loop_timeout_; + + // Default timeout value while waiting for response from a server + std::chrono::milliseconds default_server_timeout_; + // Parameters for Groot monitoring bool enable_groot_monitoring_; int groot_zmq_publisher_port_; 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 82cb5b565a8..9019a2cf78d 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 @@ -53,6 +53,12 @@ BtActionServer::BtActionServer( clock_ = node->get_clock(); // Declare this node's parameters + if (!node->has_parameter("bt_loop_timeout")) { + node->declare_parameter("bt_loop_timeout", 10); + } + if (!node->has_parameter("default_server_timeout")) { + node->declare_parameter("default_server_timeout", 10); + } if (!node->has_parameter("enable_groot_monitoring")) { node->declare_parameter("enable_groot_monitoring", true); } @@ -91,6 +97,18 @@ bool BtActionServer::on_configure() node->get_node_waitables_interface(), action_name_, std::bind(&BtActionServer::executeCallback, this)); + // Get parameter for monitoring with Groot via ZMQ Publisher + node->get_parameter("enable_groot_monitoring", enable_groot_monitoring_); + node->get_parameter("groot_zmq_publisher_port", groot_zmq_publisher_port_); + node->get_parameter("groot_zmq_server_port", groot_zmq_server_port_); + + // Get parameters for BT timeouts + int timeout; + node->get_parameter("bt_loop_timeout", timeout); + bt_loop_timeout_ = std::chrono::milliseconds(timeout); + node->get_parameter("default_server_timeout", timeout); + default_server_timeout_ = std::chrono::milliseconds(timeout); + // Create the class that registers our custom nodes and executes the BT bt_ = std::make_unique(plugin_lib_names_); @@ -99,12 +117,8 @@ bool BtActionServer::on_configure() // Put items on the blackboard blackboard_->set("node", client_node_); // NOLINT - blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT - - // Get parameter for monitoring with Groot via ZMQ Publisher - node->get_parameter("enable_groot_monitoring", enable_groot_monitoring_); - node->get_parameter("groot_zmq_publisher_port", groot_zmq_publisher_port_); - node->get_parameter("groot_zmq_server_port", groot_zmq_server_port_); + blackboard_->set("default_server_timeout", default_server_timeout_); // NOLINT + blackboard_->set("bt_loop_timeout", bt_loop_timeout_); // NOLINT return true; } @@ -217,7 +231,7 @@ void BtActionServer::executeCallback() }; // Execute the BT that was previously created in the configure step - nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); + nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling, bt_loop_timeout_); // Make sure that the Bt is not in a running state from a previous execution // note: if all the ControlNodes are implemented correctly, this is not needed. 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 c8c3d24637f..ac81fc0959d 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 @@ -47,10 +47,15 @@ class BtServiceNode : public BT::SyncActionNode node_ = config().blackboard->template get("node"); // Get the required items from the blackboard + bt_loop_timeout_ = + config().blackboard->template get("bt_loop_timeout"); server_timeout_ = - config().blackboard->template get("server_timeout"); + config().blackboard->template get("default_server_timeout"); getInput("server_timeout", server_timeout_); + // Set spin_until_future_complete timeout + timeout_ = server_timeout_ < bt_loop_timeout_ ? server_timeout_ : bt_loop_timeout_; + // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); service_client_ = node_->create_client(service_name_); @@ -107,9 +112,13 @@ class BtServiceNode : public BT::SyncActionNode */ BT::NodeStatus tick() override { - on_tick(); - auto future_result = service_client_->async_send_request(request_); - return check_future(future_result); + if (!request_sent_) { + on_tick(); + future_result_ = service_client_->async_send_request(request_); + sent_time_ = node_->now(); + request_sent_ = true; + } + return check_future(); } /** @@ -122,24 +131,35 @@ class BtServiceNode : public BT::SyncActionNode /** * @brief Check the future and decide the status of BT - * @param future_result shared_future of service response * @return BT::NodeStatus SUCCESS if future complete before timeout, FAILURE otherwise */ - virtual BT::NodeStatus check_future( - std::shared_future future_result) + virtual BT::NodeStatus check_future() { - rclcpp::FutureReturnCode rc; - rc = rclcpp::spin_until_future_complete( - node_, - future_result, server_timeout_); - if (rc == rclcpp::FutureReturnCode::SUCCESS) { - return BT::NodeStatus::SUCCESS; - } else if (rc == rclcpp::FutureReturnCode::TIMEOUT) { - RCLCPP_WARN( - node_->get_logger(), - "Node timed out while executing service call to %s.", service_name_.c_str()); - on_wait_for_result(); + auto elapsed = (node_->now() - sent_time_).to_chrono(); + auto remaining = server_timeout_ - elapsed; + + if (remaining > std::chrono::milliseconds(0)) { + auto timeout = remaining > timeout_ ? timeout_ : remaining; + + auto rc = rclcpp::spin_until_future_complete(node_, future_result_, timeout); + if (rc == rclcpp::FutureReturnCode::SUCCESS) { + request_sent_ = false; + return BT::NodeStatus::SUCCESS; + } + + if (rc == rclcpp::FutureReturnCode::TIMEOUT) { + on_wait_for_result(); + elapsed = (node_->now() - sent_time_).to_chrono(); + if (elapsed < server_timeout_) { + return BT::NodeStatus::RUNNING; + } + } } + + RCLCPP_WARN( + node_->get_logger(), + "Node timed out while executing service call to %s.", service_name_.c_str()); + return BT::NodeStatus::FAILURE; } @@ -173,6 +193,17 @@ class BtServiceNode : public BT::SyncActionNode // The timeout value while to use in the tick loop while waiting for // a result from the server std::chrono::milliseconds server_timeout_; + + // The timeout value for BT loop execution + std::chrono::milliseconds bt_loop_timeout_; + + // spin_until_future_complete timeout value + std::chrono::milliseconds timeout_; + + // To track the server response when a new request is sent + std::shared_future future_result_; + bool request_sent_{false}; + rclcpp::Time sent_time_; }; } // namespace nav2_behavior_tree diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 37c2532818e..aff9c3c6af3 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -53,6 +53,8 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom + bt_loop_timeout: 10 + default_server_timeout: 10 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index f07e5858467..0abf779059f 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -53,6 +53,8 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom + bt_loop_timeout: 10 + default_server_timeout: 10 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index f6a32378125..4ade7a1f32b 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -53,6 +53,8 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom + bt_loop_timeout: 10 + default_server_timeout: 10 enable_groot_monitoring: True groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 From 497be42fc6bc2b6a277a77e790b9d6390cc44445 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Fri, 23 Apr 2021 02:05:12 +0530 Subject: [PATCH 02/11] Fix tests Signed-off-by: Sarthak Mittal --- .../include/nav2_behavior_tree/bt_action_node.hpp | 2 +- .../include/nav2_behavior_tree/bt_action_server_impl.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 827af08cbc3..f0bce4535f4 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 @@ -52,7 +52,7 @@ class BtActionNode : public BT::ActionNodeBase bt_loop_timeout_ = config().blackboard->template get("bt_loop_timeout"); server_timeout_ = - config().blackboard->template get("default_server_timeout"); + config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); // Set spin_until_future_complete timeout 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 9019a2cf78d..040d2ec1717 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 @@ -117,7 +117,7 @@ bool BtActionServer::on_configure() // Put items on the blackboard blackboard_->set("node", client_node_); // NOLINT - blackboard_->set("default_server_timeout", default_server_timeout_); // NOLINT + blackboard_->set("server_timeout", default_server_timeout_); // NOLINT blackboard_->set("bt_loop_timeout", bt_loop_timeout_); // NOLINT return true; 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 ac81fc0959d..f5e244474cd 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 @@ -50,7 +50,7 @@ class BtServiceNode : public BT::SyncActionNode bt_loop_timeout_ = config().blackboard->template get("bt_loop_timeout"); server_timeout_ = - config().blackboard->template get("default_server_timeout"); + config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); // Set spin_until_future_complete timeout From 5084f5617ae65c4973ced077f536263124d78d4e Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sun, 25 Apr 2021 15:10:30 +0530 Subject: [PATCH 03/11] Fix tests --- .../test/plugins/action/test_back_up_action.cpp | 3 +++ .../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_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_spin_action.cpp | 3 +++ .../test/plugins/action/test_wait_action.cpp | 3 +++ .../test/plugins/condition/test_transform_available.cpp | 3 +++ nav2_behavior_tree/test/test_behavior_tree_fixture.hpp | 3 +++ .../src/behavior_tree/test_behavior_tree_node.cpp | 2 ++ 13 files changed, 44 insertions(+) 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 3671fb02f8c..8565dac8903 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 @@ -65,6 +65,9 @@ class BackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); 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 17cd7cbb44e..31f2e503d39 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 @@ -50,6 +50,9 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -133,6 +136,9 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -222,6 +228,9 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); 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 fc7cfe36160..584faedad17 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 @@ -73,6 +73,9 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); 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 595f88e0e34..632ab4409a0 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 @@ -71,6 +71,9 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = 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 9c3c67fe4e7..93f4c3a5bde 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 @@ -64,6 +64,9 @@ class FollowPathActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); 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 0aa3500ff24..08a7fcad7d9 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 @@ -67,6 +67,9 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); 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 7ea3e858f8d..7b207530ab4 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 @@ -65,6 +65,9 @@ class NavigateToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); 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 0ce10e229f4..c278feea60f 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 @@ -50,6 +50,9 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); factory_->registerNodeType( 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 432a3c55bd4..9bf78f1162c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -65,6 +65,9 @@ class SpinActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); 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 193395f9401..acd0e85bab2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -56,6 +56,9 @@ class WaitActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp index 37dfc32809b..6fb1f1ed46b 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp @@ -45,6 +45,9 @@ class TransformAvailableConditionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); } diff --git a/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp b/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp index 20c9ace7bf9..f4895236cf6 100644 --- a/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp +++ b/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp @@ -52,6 +52,9 @@ class BehaviorTreeTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_timeout", + std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); transform_handler_->activate(); 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 56210581722..2c2c9dc0d8f 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 @@ -110,6 +110,8 @@ class BehaviorTreeHandler blackboard->set("node", node_); // NOLINT blackboard->set( "server_timeout", std::chrono::milliseconds(10)); // NOLINT + blackboard->set( + "bt_loop_timeout", std::chrono::milliseconds(10)); // NOLINT blackboard->set>("tf_buffer", tf_); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT From e3701f60f7fafdc113e039c78a13d0cf24bb22a8 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Tue, 27 Apr 2021 00:44:15 +0530 Subject: [PATCH 04/11] Remove unnecessary timeout_ variable Signed-off-by: Sarthak Mittal --- .../include/nav2_behavior_tree/bt_action_node.hpp | 8 +------- .../include/nav2_behavior_tree/bt_service_node.hpp | 8 +------- 2 files changed, 2 insertions(+), 14 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 f0bce4535f4..13a01936d56 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 @@ -55,9 +55,6 @@ class BtActionNode : public BT::ActionNodeBase config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); - // Set spin_until_future_complete timeout - timeout_ = server_timeout_ < bt_loop_timeout_ ? server_timeout_ : bt_loop_timeout_; - // Initialize the input and output messages goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); @@ -323,7 +320,7 @@ class BtActionNode : public BT::ActionNodeBase return false; } - auto timeout = remaining > timeout_ ? timeout_ : remaining; + auto timeout = remaining > bt_loop_timeout_ ? bt_loop_timeout_ : remaining; auto result = rclcpp::spin_until_future_complete(node_, future_goal_handle_, timeout); if (result == rclcpp::FutureReturnCode::INTERRUPTED) { @@ -373,9 +370,6 @@ class BtActionNode : public BT::ActionNodeBase // The timeout value for BT loop execution std::chrono::milliseconds bt_loop_timeout_; - // spin_until_future_complete timeout value - std::chrono::milliseconds timeout_; - // To track the action server response when a new goal is sent std::shared_future::SharedPtr> future_goal_handle_; 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 f5e244474cd..306afa277a2 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 @@ -53,9 +53,6 @@ class BtServiceNode : public BT::SyncActionNode config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); - // Set spin_until_future_complete timeout - timeout_ = server_timeout_ < bt_loop_timeout_ ? server_timeout_ : bt_loop_timeout_; - // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); service_client_ = node_->create_client(service_name_); @@ -139,7 +136,7 @@ class BtServiceNode : public BT::SyncActionNode auto remaining = server_timeout_ - elapsed; if (remaining > std::chrono::milliseconds(0)) { - auto timeout = remaining > timeout_ ? timeout_ : remaining; + auto timeout = remaining > bt_loop_timeout_ ? bt_loop_timeout_ : remaining; auto rc = rclcpp::spin_until_future_complete(node_, future_result_, timeout); if (rc == rclcpp::FutureReturnCode::SUCCESS) { @@ -197,9 +194,6 @@ class BtServiceNode : public BT::SyncActionNode // The timeout value for BT loop execution std::chrono::milliseconds bt_loop_timeout_; - // spin_until_future_complete timeout value - std::chrono::milliseconds timeout_; - // To track the server response when a new request is sent std::shared_future future_result_; bool request_sent_{false}; From 142fa676a04ff68055749330557a121ccb8fcd25 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Tue, 27 Apr 2021 01:17:18 +0530 Subject: [PATCH 05/11] Reset goal_sent on terminal conditions Signed-off-by: Sarthak Mittal --- .../include/nav2_behavior_tree/bt_action_node.hpp | 7 ++++++- .../include/nav2_behavior_tree/bt_service_node.hpp | 2 +- 2 files changed, 7 insertions(+), 2 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 13a01936d56..8a631ec6e85 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 @@ -192,6 +192,7 @@ class BtActionNode : public BT::ActionNodeBase RCLCPP_WARN( node_->get_logger(), "Timed out while waiting for action server to respond for %s", action_name_.c_str()); + goal_sent_ = false; return BT::NodeStatus::FAILURE; } } @@ -215,6 +216,7 @@ class BtActionNode : public BT::ActionNodeBase RCLCPP_WARN( node_->get_logger(), "Timed out while waiting for action server to respond for %s", action_name_.c_str()); + goal_sent_ = false; return BT::NodeStatus::FAILURE; } } @@ -260,6 +262,7 @@ class BtActionNode : public BT::ActionNodeBase } } + goal_sent_ = false; setStatus(BT::NodeStatus::IDLE); } @@ -317,6 +320,7 @@ class BtActionNode : public BT::ActionNodeBase // server has already timed out, no need to sleep if (remaining <= std::chrono::milliseconds(0)) { + goal_sent_ = false; return false; } @@ -324,15 +328,16 @@ class BtActionNode : public BT::ActionNodeBase auto result = rclcpp::spin_until_future_complete(node_, future_goal_handle_, timeout); if (result == rclcpp::FutureReturnCode::INTERRUPTED) { + goal_sent_ = false; throw std::runtime_error("send_goal failed"); } if (result == rclcpp::FutureReturnCode::SUCCESS) { + goal_sent_ = false; goal_handle_ = future_goal_handle_.get(); if (!goal_handle_) { throw std::runtime_error("Goal was rejected by the action server"); } - goal_sent_ = false; return true; } 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 306afa277a2..5bf63415a6f 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 @@ -156,7 +156,7 @@ class BtServiceNode : public BT::SyncActionNode RCLCPP_WARN( node_->get_logger(), "Node timed out while executing service call to %s.", service_name_.c_str()); - + request_sent_ = false; return BT::NodeStatus::FAILURE; } From d09074a7ce4a09057a455a857e517ec3b8f67278 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Wed, 28 Apr 2021 19:52:41 +0530 Subject: [PATCH 06/11] Rename variables and refactor timeout logic Signed-off-by: Sarthak Mittal --- .../nav2_behavior_tree/bt_action_node.hpp | 82 ++++++++++--------- .../nav2_behavior_tree/bt_action_server.hpp | 4 +- .../bt_action_server_impl.hpp | 12 +-- .../nav2_behavior_tree/bt_service_node.hpp | 8 +- .../plugins/action/test_back_up_action.cpp | 2 +- .../action/test_clear_costmap_service.cpp | 6 +- ...test_compute_path_through_poses_action.cpp | 2 +- .../test_compute_path_to_pose_action.cpp | 2 +- .../action/test_follow_path_action.cpp | 2 +- .../test_navigate_through_poses_action.cpp | 2 +- .../action/test_navigate_to_pose_action.cpp | 2 +- ...initialize_global_localization_service.cpp | 2 +- .../test/plugins/action/test_spin_action.cpp | 2 +- .../test/plugins/action/test_wait_action.cpp | 2 +- .../condition/test_transform_available.cpp | 2 +- .../test/test_behavior_tree_fixture.hpp | 2 +- .../params/nav2_multirobot_params_1.yaml | 2 +- .../params/nav2_multirobot_params_2.yaml | 2 +- nav2_bringup/bringup/params/nav2_params.yaml | 2 +- .../behavior_tree/test_behavior_tree_node.cpp | 2 +- 20 files changed, 74 insertions(+), 68 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 8a631ec6e85..92a7ffa38ec 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 @@ -49,8 +49,8 @@ class BtActionNode : public BT::ActionNodeBase node_ = config().blackboard->template get("node"); // Get the required items from the blackboard - bt_loop_timeout_ = - config().blackboard->template get("bt_loop_timeout"); + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); @@ -181,18 +181,18 @@ class BtActionNode : public BT::ActionNodeBase // if new goal was sent and action server has not yet responded // check the future goal handle - if (goal_sent_) { - if (!check_future_goal_handle()) { + if (!goal_handle_) { + if (!is_future_goal_handle_complete()) { // return RUNNING if there is still some time before timeout happens - auto elapsed = (node_->now() - sent_time_).to_chrono(); - if (elapsed < server_timeout_) { + if (time_elapsed_since_goal_sent_ < server_timeout_) { return BT::NodeStatus::RUNNING; } // if server has taken more time to respond than the specified timeout value return FAILURE RCLCPP_WARN( node_->get_logger(), - "Timed out while waiting for action server to respond for %s", action_name_.c_str()); - goal_sent_ = false; + "Timed out while waiting for action server to acknowledge goal request for %s", + action_name_.c_str()); + goal_handle_.reset(); return BT::NodeStatus::FAILURE; } } @@ -208,15 +208,15 @@ class BtActionNode : public BT::ActionNodeBase { goal_updated_ = false; send_new_goal(); - if (!check_future_goal_handle()) { - auto elapsed = (node_->now() - sent_time_).to_chrono(); - if (elapsed < server_timeout_) { + if (!is_future_goal_handle_complete()) { + if (time_elapsed_since_goal_sent_ < server_timeout_) { return BT::NodeStatus::RUNNING; } RCLCPP_WARN( node_->get_logger(), - "Timed out while waiting for action server to respond for %s", action_name_.c_str()); - goal_sent_ = false; + "Timed out while waiting for action server to acknowledge goal request for %s", + action_name_.c_str()); + goal_handle_.reset(); return BT::NodeStatus::FAILURE; } } @@ -230,19 +230,26 @@ class BtActionNode : public BT::ActionNodeBase } } + BT::NodeStatus status; switch (result_.code) { case rclcpp_action::ResultCode::SUCCEEDED: - return on_success(); + status = on_success(); + break; case rclcpp_action::ResultCode::ABORTED: - return on_aborted(); + status = on_aborted(); + break; case rclcpp_action::ResultCode::CANCELED: - return on_cancelled(); + status = on_cancelled(); + break; default: throw std::logic_error("BtActionNode::Tick: invalid status value"); } + + goal_handle_.reset(); + return status; } /** @@ -262,7 +269,7 @@ class BtActionNode : public BT::ActionNodeBase } } - goal_sent_ = false; + goal_handle_.reset(); setStatus(BT::NodeStatus::IDLE); } @@ -278,6 +285,11 @@ class BtActionNode : public BT::ActionNodeBase return false; } + // No need to cancel the goal if goal handle is invalid + if (!goal_handle_) { + return false; + } + rclcpp::spin_some(node_); auto status = goal_handle_->get_status(); @@ -304,36 +316,30 @@ class BtActionNode : public BT::ActionNodeBase } }; + goal_handle_.reset(); future_goal_handle_ = action_client_->async_send_goal(goal_, send_goal_options); - sent_time_ = node_->now(); - goal_sent_ = true; + time_goal_sent_ = node_->now(); + time_elapsed_since_goal_sent_ = std::chrono::milliseconds(0); } /** - * @brief Function to check response from the action server when a new goal is sent - * @return boolen True if future_goal_handle_ returns SUCCESS, False when it returns TIMEOUT + * @brief Function to check if the action server acknowledged a new goal + * @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise */ - bool check_future_goal_handle() + bool is_future_goal_handle_complete() { - auto elapsed = (node_->now() - sent_time_).to_chrono(); - auto remaining = server_timeout_ - elapsed; - - // server has already timed out, no need to sleep - if (remaining <= std::chrono::milliseconds(0)) { - goal_sent_ = false; - return false; - } + auto timeout = server_timeout_ > bt_loop_duration_ ? bt_loop_duration_ : server_timeout_; + auto result = rclcpp::spin_until_future_complete(node_, future_goal_handle_, timeout); - auto timeout = remaining > bt_loop_timeout_ ? bt_loop_timeout_ : remaining; + time_elapsed_since_goal_sent_ = + (node_->now() - time_goal_sent_).to_chrono(); - auto result = rclcpp::spin_until_future_complete(node_, future_goal_handle_, timeout); if (result == rclcpp::FutureReturnCode::INTERRUPTED) { - goal_sent_ = false; + goal_handle_.reset(); throw std::runtime_error("send_goal failed"); } if (result == rclcpp::FutureReturnCode::SUCCESS) { - goal_sent_ = false; goal_handle_ = future_goal_handle_.get(); if (!goal_handle_) { throw std::runtime_error("Goal was rejected by the action server"); @@ -373,13 +379,13 @@ class BtActionNode : public BT::ActionNodeBase std::chrono::milliseconds server_timeout_; // The timeout value for BT loop execution - std::chrono::milliseconds bt_loop_timeout_; + std::chrono::milliseconds bt_loop_duration_; - // To track the action server response when a new goal is sent + // To track the action server acknowledgement when a new goal is sent std::shared_future::SharedPtr> future_goal_handle_; - bool goal_sent_{false}; - rclcpp::Time sent_time_; + rclcpp::Time time_goal_sent_; + std::chrono::milliseconds time_elapsed_since_goal_sent_; }; } // namespace nav2_behavior_tree 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 14c96f00e9f..ec977944418 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 @@ -225,8 +225,8 @@ class BtActionServer // To publish BT logs std::unique_ptr topic_logger_; - // Timeout value for each iteration of BT execution - std::chrono::milliseconds bt_loop_timeout_; + // Duration for each iteration of BT execution + std::chrono::milliseconds bt_loop_duration_; // Default timeout value while waiting for response from a server std::chrono::milliseconds default_server_timeout_; 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 040d2ec1717..a9c4e1c79c7 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 @@ -53,8 +53,8 @@ BtActionServer::BtActionServer( clock_ = node->get_clock(); // Declare this node's parameters - if (!node->has_parameter("bt_loop_timeout")) { - node->declare_parameter("bt_loop_timeout", 10); + if (!node->has_parameter("bt_loop_duration")) { + node->declare_parameter("bt_loop_duration", 10); } if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 10); @@ -104,8 +104,8 @@ bool BtActionServer::on_configure() // Get parameters for BT timeouts int timeout; - node->get_parameter("bt_loop_timeout", timeout); - bt_loop_timeout_ = std::chrono::milliseconds(timeout); + node->get_parameter("bt_loop_duration", timeout); + bt_loop_duration_ = std::chrono::milliseconds(timeout); node->get_parameter("default_server_timeout", timeout); default_server_timeout_ = std::chrono::milliseconds(timeout); @@ -118,7 +118,7 @@ bool BtActionServer::on_configure() // Put items on the blackboard blackboard_->set("node", client_node_); // NOLINT blackboard_->set("server_timeout", default_server_timeout_); // NOLINT - blackboard_->set("bt_loop_timeout", bt_loop_timeout_); // NOLINT + blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT return true; } @@ -231,7 +231,7 @@ void BtActionServer::executeCallback() }; // Execute the BT that was previously created in the configure step - nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling, bt_loop_timeout_); + nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling, bt_loop_duration_); // Make sure that the Bt is not in a running state from a previous execution // note: if all the ControlNodes are implemented correctly, this is not needed. 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 5bf63415a6f..5925537568d 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 @@ -47,8 +47,8 @@ class BtServiceNode : public BT::SyncActionNode node_ = config().blackboard->template get("node"); // Get the required items from the blackboard - bt_loop_timeout_ = - config().blackboard->template get("bt_loop_timeout"); + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); @@ -136,7 +136,7 @@ class BtServiceNode : public BT::SyncActionNode auto remaining = server_timeout_ - elapsed; if (remaining > std::chrono::milliseconds(0)) { - auto timeout = remaining > bt_loop_timeout_ ? bt_loop_timeout_ : remaining; + auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; auto rc = rclcpp::spin_until_future_complete(node_, future_result_, timeout); if (rc == rclcpp::FutureReturnCode::SUCCESS) { @@ -192,7 +192,7 @@ class BtServiceNode : public BT::SyncActionNode std::chrono::milliseconds server_timeout_; // The timeout value for BT loop execution - std::chrono::milliseconds bt_loop_timeout_; + std::chrono::milliseconds bt_loop_duration_; // To track the server response when a new request is sent std::shared_future future_result_; 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 8565dac8903..9924abfdcc3 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 @@ -66,7 +66,7 @@ class BackUpActionTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); 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 31f2e503d39..c7381833d2f 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 @@ -51,7 +51,7 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -137,7 +137,7 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -229,7 +229,7 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); 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 584faedad17..79637dbae66 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 @@ -74,7 +74,7 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); 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 632ab4409a0..48bbbc0f6ad 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 @@ -72,7 +72,7 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); 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 93f4c3a5bde..93d0dd9f944 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 @@ -65,7 +65,7 @@ class FollowPathActionTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); 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 08a7fcad7d9..f04c149d0ac 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 @@ -68,7 +68,7 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); std::vector poses; 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 7b207530ab4..5727d80949f 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 @@ -66,7 +66,7 @@ class NavigateToPoseActionTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); 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 c278feea60f..367cc622704 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 @@ -51,7 +51,7 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); 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 9bf78f1162c..0f7f260819b 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -66,7 +66,7 @@ class SpinActionTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); 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 acd0e85bab2..6524366c49a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -57,7 +57,7 @@ class WaitActionTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp index 6fb1f1ed46b..40b15cec40e 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp @@ -46,7 +46,7 @@ class TransformAvailableConditionTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); } diff --git a/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp b/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp index f4895236cf6..6590aae2a7c 100644 --- a/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp +++ b/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp @@ -53,7 +53,7 @@ class BehaviorTreeTestFixture : public ::testing::Test "server_timeout", std::chrono::milliseconds(10)); config_->blackboard->set( - "bt_loop_timeout", + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index aff9c3c6af3..eae34ad590e 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -53,7 +53,7 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom - bt_loop_timeout: 10 + bt_loop_duration: 10 default_server_timeout: 10 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 0abf779059f..ccf9858145a 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -53,7 +53,7 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom - bt_loop_timeout: 10 + bt_loop_duration: 10 default_server_timeout: 10 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 4ade7a1f32b..75113861058 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -53,7 +53,7 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom - bt_loop_timeout: 10 + bt_loop_duration: 10 default_server_timeout: 10 enable_groot_monitoring: True groot_zmq_publisher_port: 1666 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 2c2c9dc0d8f..34c987ceaf4 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 @@ -111,7 +111,7 @@ class BehaviorTreeHandler blackboard->set( "server_timeout", std::chrono::milliseconds(10)); // NOLINT blackboard->set( - "bt_loop_timeout", std::chrono::milliseconds(10)); // NOLINT + "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT blackboard->set>("tf_buffer", tf_); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT From 6ec19b65777df45e71c2b9ae9253173c1b6f8fed Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Fri, 30 Apr 2021 03:20:05 +0530 Subject: [PATCH 07/11] Fix timeout logic and add tests Signed-off-by: Sarthak Mittal --- .../nav2_behavior_tree/bt_action_node.hpp | 31 +- .../test/plugins/action/CMakeLists.txt | 3 + .../plugins/action/test_bt_action_node.cpp | 317 ++++++++++++++++++ 3 files changed, 339 insertions(+), 12 deletions(-) create mode 100644 nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp 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 92a7ffa38ec..7e3577f8e03 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 @@ -182,9 +182,10 @@ class BtActionNode : public BT::ActionNodeBase // if new goal was sent and action server has not yet responded // check the future goal handle if (!goal_handle_) { - if (!is_future_goal_handle_complete()) { + auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + if (!is_future_goal_handle_complete(elapsed)) { // return RUNNING if there is still some time before timeout happens - if (time_elapsed_since_goal_sent_ < server_timeout_) { + if (elapsed < server_timeout_) { return BT::NodeStatus::RUNNING; } // if server has taken more time to respond than the specified timeout value return FAILURE @@ -208,8 +209,9 @@ class BtActionNode : public BT::ActionNodeBase { goal_updated_ = false; send_new_goal(); - if (!is_future_goal_handle_complete()) { - if (time_elapsed_since_goal_sent_ < server_timeout_) { + auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + if (!is_future_goal_handle_complete(elapsed)) { + if (elapsed < server_timeout_) { return BT::NodeStatus::RUNNING; } RCLCPP_WARN( @@ -316,23 +318,29 @@ class BtActionNode : public BT::ActionNodeBase } }; - goal_handle_.reset(); future_goal_handle_ = action_client_->async_send_goal(goal_, send_goal_options); time_goal_sent_ = node_->now(); - time_elapsed_since_goal_sent_ = std::chrono::milliseconds(0); } /** * @brief Function to check if the action server acknowledged a new goal + * @param elapsed Duration since the last goal was sent and future goal handle has not completed. + * After waiting for the future to complete, this value is incremented with the timeout value. * @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise */ - bool is_future_goal_handle_complete() + bool is_future_goal_handle_complete(std::chrono::milliseconds & elapsed) { - auto timeout = server_timeout_ > bt_loop_duration_ ? bt_loop_duration_ : server_timeout_; - auto result = rclcpp::spin_until_future_complete(node_, future_goal_handle_, timeout); + auto remaining = server_timeout_ - elapsed; - time_elapsed_since_goal_sent_ = - (node_->now() - time_goal_sent_).to_chrono(); + // server has already timed out, no need to sleep + if (remaining <= std::chrono::milliseconds(0)) { + goal_handle_.reset(); + return false; + } + + auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; + auto result = rclcpp::spin_until_future_complete(node_, future_goal_handle_, timeout); + elapsed += timeout; if (result == rclcpp::FutureReturnCode::INTERRUPTED) { goal_handle_.reset(); @@ -385,7 +393,6 @@ class BtActionNode : public BT::ActionNodeBase std::shared_future::SharedPtr> future_goal_handle_; rclcpp::Time time_goal_sent_; - std::chrono::milliseconds time_elapsed_since_goal_sent_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index e4ab440d33b..53c5f974fd8 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -1,3 +1,6 @@ +ament_add_gtest(test_bt_action_node test_bt_action_node.cpp) +ament_target_dependencies(test_bt_action_node ${dependencies}) + ament_add_gtest(test_action_spin_action test_spin_action.cpp) target_link_libraries(test_action_spin_action nav2_spin_action_bt_node) ament_target_dependencies(test_action_spin_action ${dependencies}) 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 new file mode 100644 index 00000000000..e72b1da7dd9 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -0,0 +1,317 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/bt_action_node.hpp" + +#include "test_msgs/action/fibonacci.hpp" + +using namespace std::chrono_literals; // NOLINT +using namespace std::placeholders; // NOLINT + +class FibonacciActionServer : public rclcpp::Node +{ +public: + FibonacciActionServer() + : rclcpp::Node("fibonacci_node", rclcpp::NodeOptions()), + sleep_duration_(0ms) + { + this->action_server_ = rclcpp_action::create_server( + this->get_node_base_interface(), + this->get_node_clock_interface(), + this->get_node_logging_interface(), + this->get_node_waitables_interface(), + "fibonacci", + std::bind(&FibonacciActionServer::handle_goal, this, _1, _2), + std::bind(&FibonacciActionServer::handle_cancel, this, _1), + std::bind(&FibonacciActionServer::handle_accepted, this, _1)); + } + + void setHandleGoalSleepDuration(std::chrono::milliseconds sleep_duration) + { + sleep_duration_ = sleep_duration; + } + +protected: + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr) + { + if (sleep_duration_ > 0ms) { + std::this_thread::sleep_for(sleep_duration_); + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr>) + { + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted( + const std::shared_ptr> handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + if (handle) { + const auto goal = handle->get_goal(); + auto result = std::make_shared(); + + if (goal->order < 0) { + handle->abort(result); + return; + } + + auto & sequence = result->sequence; + sequence.push_back(0); + sequence.push_back(1); + + for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { + sequence.push_back(sequence[i] + sequence[i - 1]); + } + + handle->succeed(result); + } + } + +protected: + rclcpp_action::Server::SharedPtr action_server_; + std::chrono::milliseconds sleep_duration_; +}; + +class FibonacciAction : public nav2_behavior_tree::BtActionNode +{ +public: + FibonacciAction( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf) + : nav2_behavior_tree::BtActionNode(xml_tag_name, "fibonacci", conf) + {} + + void on_tick() override + { + getInput("order", goal_.order); + } + + BT::NodeStatus on_success() override + { + config().blackboard->set>("sequence", result_.result->sequence); + return BT::NodeStatus::SUCCESS; + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({BT::InputPort("order", "Fibonacci order")}); + } +}; + +class BTActionNodeTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("bt_action_node_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + config_->blackboard->set("server_timeout", 10ms); + config_->blackboard->set("bt_loop_duration", 10ms); + config_->blackboard->set("initial_pose_received", false); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique(name, config); + }; + + factory_->registerBuilder("Fibonacci", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void SetUp() override + { + // initialize action server and spin on new thread + action_server_ = std::make_shared(); + server_thread_ = std::make_shared( + []() { + while (rclcpp::ok() && BTActionNodeTestFixture::action_server_ != nullptr) { + rclcpp::spin_some(BTActionNodeTestFixture::action_server_); + std::this_thread::sleep_for(100ns); + } + }); + } + + void TearDown() override + { + action_server_.reset(); + tree_.reset(); + server_thread_->join(); + server_thread_.reset(); + } + + static std::shared_ptr action_server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; + static std::shared_ptr server_thread_; +}; + +rclcpp::Node::SharedPtr BTActionNodeTestFixture::node_ = nullptr; +std::shared_ptr BTActionNodeTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * BTActionNodeTestFixture::config_ = nullptr; +std::shared_ptr BTActionNodeTestFixture::factory_ = nullptr; +std::shared_ptr BTActionNodeTestFixture::tree_ = nullptr; +std::shared_ptr BTActionNodeTestFixture::server_thread_ = nullptr; + +TEST_F(BTActionNodeTestFixture, test_short_action) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + // the server timeout is larger than the goal handling duration + config_->blackboard->set("server_timeout", 20ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // setting a small action server goal handling duration + action_server_->setHandleGoalSleepDuration(2ms); + + // to keep track of the number of ticks it took to reach a terminal result + int ticks = 0; + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + // BT loop execution rate + rclcpp::WallRate loopRate(10ms); + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // get calculated fibonacci sequence from blackboard + auto sequence = config_->blackboard->get>("sequence"); + + // expected fibonacci sequence for order 5 + std::vector expected = {0, 1, 1, 2, 3, 5}; + + // since the server timeout was larger than the action server goal handling duration + // the BT should have succeeded + EXPECT_EQ(result, BT::NodeStatus::SUCCESS); + + // even though the goal is accepted in the first tick, due to some timing issues + // the goal result is available in the next tick + EXPECT_EQ(ticks, 2); + + // checking the output fibonacci sequence + EXPECT_EQ(sequence.size(), expected.size()); + for (size_t i = 0; i < expected.size(); ++i) { + EXPECT_EQ(sequence[i], expected[i]); + } +} + +TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + // setting a server timeout smaller than the time the action server will take to accept the goal + // to simulate a server timeout scenario + config_->blackboard->set("server_timeout", 90ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // the action server will take 100ms before accepting the goal + action_server_->setHandleGoalSleepDuration(100ms); + + // to keep track of the number of ticks it took to reach a terminal result + int ticks = 0; + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + // BT loop execution rate + rclcpp::WallRate loopRate(10ms); + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // since the server timeout was smaller than the action server goal handling duration + // the BT should have failed + EXPECT_EQ(result, BT::NodeStatus::FAILURE); + + // since the server timeout is 90ms and bt loop duration is 10ms, number of ticks should be 9 + EXPECT_EQ(ticks, 9); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} From f83612d28ce0db76436cf711071129eb17099c8f Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Fri, 30 Apr 2021 03:57:49 +0530 Subject: [PATCH 08/11] Updates tests to ensure previous node state does not leak into new cycles Signed-off-by: Sarthak Mittal --- .../plugins/action/test_bt_action_node.cpp | 56 ++++++++++++++++++- 1 file changed, 55 insertions(+), 1 deletion(-) 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 e72b1da7dd9..8a8a6b14ece 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 @@ -201,7 +201,7 @@ std::shared_ptr BTActionNodeTestFixture::factory_ = nul std::shared_ptr BTActionNodeTestFixture::tree_ = nullptr; std::shared_ptr BTActionNodeTestFixture::server_thread_ = nullptr; -TEST_F(BTActionNodeTestFixture, test_short_action) +TEST_F(BTActionNodeTestFixture, test_server_timeout_success) { // create tree std::string xml_txt = @@ -255,6 +255,33 @@ TEST_F(BTActionNodeTestFixture, test_short_action) for (size_t i = 0; i < expected.size(); ++i) { EXPECT_EQ(sequence[i], expected[i]); } + + // start a new execution cycle with the previous BT to ensure previous state doesn't leak into + // the new cycle + + // halt BT for a new execution cycle + tree_->haltTree(); + + // setting a large action server goal handling duration + action_server_->setHandleGoalSleepDuration(100ms); + + // reset state variables + ticks = 0; + result = BT::NodeStatus::RUNNING; + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // since the server timeout was smaller than the action server goal handling duration + // the BT should have failed + EXPECT_EQ(result, BT::NodeStatus::FAILURE); + + // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should be 2 + EXPECT_EQ(ticks, 2); } TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) @@ -299,6 +326,33 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // since the server timeout is 90ms and bt loop duration is 10ms, number of ticks should be 9 EXPECT_EQ(ticks, 9); + + // start a new execution cycle with the previous BT to ensure previous state doesn't leak into + // the new cycle + + // halt BT for a new execution cycle + tree_->haltTree(); + + // setting a small action server goal handling duration + action_server_->setHandleGoalSleepDuration(25ms); + + // reset state variables + ticks = 0; + result = BT::NodeStatus::RUNNING; + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // since the server timeout was smaller than the action server goal handling duration + // the BT should have failed + EXPECT_EQ(result, BT::NodeStatus::SUCCESS); + + // takes 3 ticks waiting for the action server acknowledgement and 3 more ticks to get goal result + EXPECT_EQ(ticks, 6); } int main(int argc, char ** argv) From 07054d088acbd1e65fe45b5c5eefd9f668a11141 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 1 May 2021 03:06:01 +0530 Subject: [PATCH 09/11] Change goal_handle_ checks to future_goal_handle_ Signed-off-by: Sarthak Mittal --- .../nav2_behavior_tree/bt_action_node.hpp | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 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 7e3577f8e03..e4807b888f3 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 @@ -181,7 +181,7 @@ class BtActionNode : public BT::ActionNodeBase // if new goal was sent and action server has not yet responded // check the future goal handle - if (!goal_handle_) { + if (future_goal_handle_) { auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); if (!is_future_goal_handle_complete(elapsed)) { // return RUNNING if there is still some time before timeout happens @@ -193,7 +193,7 @@ class BtActionNode : public BT::ActionNodeBase node_->get_logger(), "Timed out while waiting for action server to acknowledge goal request for %s", action_name_.c_str()); - goal_handle_.reset(); + future_goal_handle_.reset(); return BT::NodeStatus::FAILURE; } } @@ -218,7 +218,7 @@ class BtActionNode : public BT::ActionNodeBase node_->get_logger(), "Timed out while waiting for action server to acknowledge goal request for %s", action_name_.c_str()); - goal_handle_.reset(); + future_goal_handle_.reset(); return BT::NodeStatus::FAILURE; } } @@ -318,7 +318,9 @@ class BtActionNode : public BT::ActionNodeBase } }; - future_goal_handle_ = action_client_->async_send_goal(goal_, send_goal_options); + future_goal_handle_ = std::make_shared< + std::shared_future::SharedPtr>>( + action_client_->async_send_goal(goal_, send_goal_options)); time_goal_sent_ = node_->now(); } @@ -334,21 +336,22 @@ class BtActionNode : public BT::ActionNodeBase // server has already timed out, no need to sleep if (remaining <= std::chrono::milliseconds(0)) { - goal_handle_.reset(); + future_goal_handle_.reset(); return false; } auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; - auto result = rclcpp::spin_until_future_complete(node_, future_goal_handle_, timeout); + auto result = rclcpp::spin_until_future_complete(node_, *future_goal_handle_, timeout); elapsed += timeout; if (result == rclcpp::FutureReturnCode::INTERRUPTED) { - goal_handle_.reset(); + future_goal_handle_.reset(); throw std::runtime_error("send_goal failed"); } if (result == rclcpp::FutureReturnCode::SUCCESS) { - goal_handle_ = future_goal_handle_.get(); + goal_handle_ = future_goal_handle_->get(); + future_goal_handle_.reset(); if (!goal_handle_) { throw std::runtime_error("Goal was rejected by the action server"); } @@ -390,7 +393,7 @@ class BtActionNode : public BT::ActionNodeBase std::chrono::milliseconds bt_loop_duration_; // To track the action server acknowledgement when a new goal is sent - std::shared_future::SharedPtr> + std::shared_ptr::SharedPtr>> future_goal_handle_; rclcpp::Time time_goal_sent_; }; From ade30aa6321a925bbf4e0782bbc7c592499ed431 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Tue, 4 May 2021 17:59:46 +0530 Subject: [PATCH 10/11] Remove goal handle reset in halt --- nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp | 1 - 1 file changed, 1 deletion(-) 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 e4807b888f3..99c0e249a7d 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 @@ -271,7 +271,6 @@ class BtActionNode : public BT::ActionNodeBase } } - goal_handle_.reset(); setStatus(BT::NodeStatus::IDLE); } From ec80f363967f8bbb1c60645cd4b031c18f54d468 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Wed, 5 May 2021 01:29:09 +0530 Subject: [PATCH 11/11] Update server_timeout to 20ms and inherit BtServiceNode from BT::ActionNodeBase Signed-off-by: Sarthak Mittal --- .gitignore | 4 ++++ .../nav2_behavior_tree/bt_action_server_impl.hpp | 2 +- .../include/nav2_behavior_tree/bt_service_node.hpp | 13 +++++++++++-- .../test/plugins/action/test_back_up_action.cpp | 2 +- .../test/plugins/action/test_bt_action_node.cpp | 9 +-------- .../plugins/action/test_clear_costmap_service.cpp | 2 +- .../test_compute_path_through_poses_action.cpp | 2 +- .../action/test_compute_path_to_pose_action.cpp | 2 +- .../test/plugins/action/test_follow_path_action.cpp | 2 +- .../action/test_navigate_through_poses_action.cpp | 2 +- .../plugins/action/test_navigate_to_pose_action.cpp | 2 +- ...est_reinitialize_global_localization_service.cpp | 2 +- .../test/plugins/action/test_spin_action.cpp | 2 +- .../test/plugins/action/test_wait_action.cpp | 2 +- .../plugins/condition/test_transform_available.cpp | 2 +- .../test/test_behavior_tree_fixture.hpp | 2 +- .../bringup/params/nav2_multirobot_params_1.yaml | 2 +- .../bringup/params/nav2_multirobot_params_2.yaml | 2 +- nav2_bringup/bringup/params/nav2_params.yaml | 2 +- nav2_rviz_plugins/src/nav2_panel.cpp | 2 +- .../src/behavior_tree/test_behavior_tree_node.cpp | 2 +- 21 files changed, 34 insertions(+), 28 deletions(-) diff --git a/.gitignore b/.gitignore index 173558e8c0e..88c775d303c 100644 --- a/.gitignore +++ b/.gitignore @@ -46,3 +46,7 @@ __pycache__/ *.py[cod] sphinx_doc/_build + +# CLion artifacts +.idea +cmake-build-debug/ 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 a9c4e1c79c7..8a0faaa93b8 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 @@ -57,7 +57,7 @@ BtActionServer::BtActionServer( node->declare_parameter("bt_loop_duration", 10); } if (!node->has_parameter("default_server_timeout")) { - node->declare_parameter("default_server_timeout", 10); + node->declare_parameter("default_server_timeout", 20); } if (!node->has_parameter("enable_groot_monitoring")) { node->declare_parameter("enable_groot_monitoring", true); 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 5925537568d..366ce0f87b9 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 @@ -31,7 +31,7 @@ namespace nav2_behavior_tree * @tparam ServiceT Type of service */ template -class BtServiceNode : public BT::SyncActionNode +class BtServiceNode : public BT::ActionNodeBase { public: /** @@ -42,7 +42,7 @@ class BtServiceNode : public BT::SyncActionNode BtServiceNode( const std::string & service_node_name, const BT::NodeConfiguration & conf) - : BT::SyncActionNode(service_node_name, conf), service_node_name_(service_node_name) + : BT::ActionNodeBase(service_node_name, conf), service_node_name_(service_node_name) { node_ = config().blackboard->template get("node"); @@ -118,6 +118,15 @@ class BtServiceNode : public BT::SyncActionNode return check_future(); } + /** + * @brief The other (optional) override required by a BT service. + */ + void halt() override + { + request_sent_ = false; + setStatus(BT::NodeStatus::IDLE); + } + /** * @brief Function to perform some user-defined operation on tick * Fill in service request with information if necessary 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 9924abfdcc3..db970202128 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 @@ -64,7 +64,7 @@ class BackUpActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); 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 8a8a6b14ece..d95cbd4a848 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 @@ -141,7 +141,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard = BT::Blackboard::create(); // Put items on the blackboard config_->blackboard->set("node", node_); - config_->blackboard->set("server_timeout", 10ms); + config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); config_->blackboard->set("initial_pose_received", false); @@ -246,10 +246,6 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // the BT should have succeeded EXPECT_EQ(result, BT::NodeStatus::SUCCESS); - // even though the goal is accepted in the first tick, due to some timing issues - // the goal result is available in the next tick - EXPECT_EQ(ticks, 2); - // checking the output fibonacci sequence EXPECT_EQ(sequence.size(), expected.size()); for (size_t i = 0; i < expected.size(); ++i) { @@ -350,9 +346,6 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // since the server timeout was smaller than the action server goal handling duration // the BT should have failed EXPECT_EQ(result, BT::NodeStatus::SUCCESS); - - // takes 3 ticks waiting for the action server acknowledgement and 3 more ticks to get goal result - EXPECT_EQ(ticks, 6); } int main(int argc, char ** argv) 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 c7381833d2f..f10177ecffb 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 @@ -49,7 +49,7 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); 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 79637dbae66..122c7646212 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 @@ -72,7 +72,7 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); 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 48bbbc0f6ad..5e35a1981de 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 @@ -70,7 +70,7 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); 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 93d0dd9f944..1325070e000 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 @@ -63,7 +63,7 @@ class FollowPathActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); 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 f04c149d0ac..c62e15798e2 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 @@ -66,7 +66,7 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); 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 5727d80949f..19b96d6f682 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 @@ -64,7 +64,7 @@ class NavigateToPoseActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); 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 367cc622704..1dbf1a7e6d1 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 @@ -49,7 +49,7 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); 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 0f7f260819b..332d5149417 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -64,7 +64,7 @@ class SpinActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); 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 6524366c49a..05cd388d7a6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -55,7 +55,7 @@ class WaitActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); diff --git a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp index 40b15cec40e..a24dcbf530d 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp @@ -44,7 +44,7 @@ class TransformAvailableConditionTestFixture : public ::testing::Test transform_handler_->getBuffer()); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); diff --git a/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp b/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp index 6590aae2a7c..2a377f9caad 100644 --- a/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp +++ b/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp @@ -51,7 +51,7 @@ class BehaviorTreeTestFixture : public ::testing::Test transform_handler_->getBuffer()); config_->blackboard->set( "server_timeout", - std::chrono::milliseconds(10)); + std::chrono::milliseconds(20)); config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index eae34ad590e..0c19287576a 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -54,7 +54,7 @@ bt_navigator: robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 - default_server_timeout: 10 + default_server_timeout: 20 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index ccf9858145a..07b069faf72 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -54,7 +54,7 @@ bt_navigator: robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 - default_server_timeout: 10 + default_server_timeout: 20 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 75113861058..b643828cfd2 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -54,7 +54,7 @@ bt_navigator: robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 - default_server_timeout: 10 + default_server_timeout: 20 enable_groot_monitoring: True groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 5c1b163bf3f..8fb4a481df5 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -38,7 +38,7 @@ GoalPoseUpdater GoalUpdater; Nav2Panel::Nav2Panel(QWidget * parent) : Panel(parent), - server_timeout_(10), + server_timeout_(20), client_nav_("lifecycle_manager_navigation"), client_loc_("lifecycle_manager_localization") { 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 34c987ceaf4..9b9ad869fe9 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 @@ -109,7 +109,7 @@ class BehaviorTreeHandler // Put items on the blackboard blackboard->set("node", node_); // NOLINT blackboard->set( - "server_timeout", std::chrono::milliseconds(10)); // NOLINT + "server_timeout", std::chrono::milliseconds(20)); // NOLINT blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT blackboard->set>("tf_buffer", tf_); // NOLINT