From 3b7b61287830998acfaf3c5a0a9932d6bca09748 Mon Sep 17 00:00:00 2001 From: Maurice Alexander Purnawan Date: Tue, 27 Jan 2026 02:50:26 +0700 Subject: [PATCH] Add cancel timeout when cancelling actions (#5895) * Add halt timeout when cancelling actions Signed-off-by: mini-1235 * Add to config Signed-off-by: mini-1235 * Fix flaky test Signed-off-by: mini-1235 * Retrigger Signed-off-by: mini-1235 * Rename Signed-off-by: mini-1235 * Lint Signed-off-by: mini-1235 --------- Signed-off-by: mini-1235 (cherry picked from commit 640196af92900d073390b07bcd81935d486fd3bb) # Conflicts: # nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp # nav2_system_tests/src/system/nav2_system_params.yaml --- .../nav2_behavior_tree/bt_action_node.hpp | 6 +++- .../nav2_behavior_tree/bt_action_server.hpp | 3 ++ .../bt_action_server_impl.hpp | 35 +++++++++++++++++++ .../plugins/action/test_bt_action_node.cpp | 4 ++- nav2_bringup/params/nav2_params.yaml | 1 + .../gps_navigation/nav2_no_map_params.yaml | 1 + .../src/system/nav2_system_params.yaml | 4 +++ 7 files changed, 52 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 a56ae31b8b6..c149de90d98 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,7 @@ class BtActionNode : public BT::ActionNodeBase auto bt_loop_duration = config().blackboard->template get("bt_loop_duration"); getInputOrBlackboard("server_timeout", server_timeout_); + getInputOrBlackboard("cancel_timeout", cancel_timeout_); wait_for_service_timeout_ = config().blackboard->template get("wait_for_service_timeout"); @@ -322,7 +323,7 @@ class BtActionNode : public BT::ActionNodeBase "Failed to cancel action server for %s", action_name_.c_str()); } - if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + if (callback_group_executor_.spin_until_future_complete(future_result, cancel_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( @@ -473,6 +474,9 @@ class BtActionNode : public BT::ActionNodeBase // new action goal is sent or canceled std::chrono::milliseconds server_timeout_; + // The timeout value when cancelling actions during halt + std::chrono::milliseconds cancel_timeout_; + // The timeout value for BT loop execution std::chrono::milliseconds max_timeout_; 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 e61dcefae8b..60771bb697e 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 @@ -255,6 +255,9 @@ class BtActionServer // Default timeout value while waiting for response from a server std::chrono::milliseconds default_server_timeout_; + // Default timeout value when cancelling actions during node halt + std::chrono::milliseconds default_cancel_timeout_; + // The timeout value for waiting for a service to response std::chrono::milliseconds wait_for_service_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 ac5fc20da41..8b4b2dfe294 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,7 @@ bool BtActionServer::on_configure() nullptr, std::chrono::milliseconds(500), false, server_options); // Get parameters for BT timeouts +<<<<<<< HEAD int bt_loop_duration; node->get_parameter("bt_loop_duration", bt_loop_duration); bt_loop_duration_ = std::chrono::milliseconds(bt_loop_duration); @@ -168,6 +169,22 @@ bool BtActionServer::on_configure() node->get_parameter("wait_for_service_timeout", wait_for_service_timeout); wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout); node->get_parameter("always_reload_bt_xml", always_reload_bt_xml_); +======= + bt_loop_duration_ = std::chrono::milliseconds( + node->declare_or_get_parameter("bt_loop_duration", 10)); + + default_server_timeout_ = std::chrono::milliseconds( + node->declare_or_get_parameter("default_server_timeout", 20)); + + default_cancel_timeout_ = std::chrono::milliseconds( + node->declare_or_get_parameter("default_cancel_timeout", 50)); + + wait_for_service_timeout_ = std::chrono::milliseconds( + node->declare_or_get_parameter("wait_for_service_timeout", 1000)); + + always_reload_bt_ = node->declare_or_get_parameter( + "always_reload_bt_xml", false); +>>>>>>> 640196af (Add cancel timeout when cancelling actions (#5895)) // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -179,10 +196,18 @@ bool BtActionServer::on_configure() blackboard_ = BT::Blackboard::create(); // Put items on the blackboard +<<<<<<< HEAD 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( +======= + blackboard_->template set("node", client_node_); // NOLINT + blackboard_->template set("server_timeout", default_server_timeout_); // NOLINT + blackboard_->template set("cancel_timeout", default_cancel_timeout_); // NOLINT + blackboard_->template set("bt_loop_duration", bt_loop_duration_); // NOLINT + blackboard_->template set( +>>>>>>> 640196af (Add cancel timeout when cancelling actions (#5895)) "wait_for_service_timeout", wait_for_service_timeout_); @@ -257,10 +282,20 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena tree_ = bt_->createTreeFromFile(filename, blackboard_); for (auto & subtree : tree_.subtrees) { auto & blackboard = subtree->blackboard; +<<<<<<< HEAD blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); blackboard->set( +======= + blackboard->template set("node", client_node_); + blackboard->template set("server_timeout", + default_server_timeout_); + blackboard->template set("cancel_timeout", + default_cancel_timeout_); + blackboard->template set("bt_loop_duration", bt_loop_duration_); + blackboard->template set( +>>>>>>> 640196af (Add cancel timeout when cancelling actions (#5895)) "wait_for_service_timeout", wait_for_service_timeout_); } 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 d22ed27c7a2..b90be0d8a9b 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 @@ -146,7 +146,9 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNodeset("sequence", result_.result->sequence); + if (result_.result) { + config().blackboard->set("sequence", result_.result->sequence); + } config().blackboard->set("on_cancelled_triggered", true); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index ac52c787f58..3ccf1af181c 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 + default_cancel_timeout: 50 wait_for_service_timeout: 1000 action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] 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 96dc593d824..acffda43762 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 + default_cancel_timeout: 50 wait_for_service_timeout: 1000 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index e0d901907fb..46a44914ee5 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -45,7 +45,11 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 +<<<<<<< HEAD action_server_result_timeout: 900.0 +======= + default_cancel_timeout: 50 +>>>>>>> 640196af (Add cancel timeout when cancelling actions (#5895)) navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator"