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 7cdc305c3e3..0c5581d3a4f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -62,6 +62,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"); @@ -334,7 +335,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( @@ -485,6 +486,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 aae15723e0f..4dfb5cb485c 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 @@ -285,6 +285,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 6736b6f0c3b..c03b9b94737 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 @@ -160,6 +160,9 @@ bool BtActionServer::on_configure() 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)); @@ -178,6 +181,7 @@ bool BtActionServer::on_configure() // Put items on the blackboard 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( "wait_for_service_timeout", @@ -331,6 +335,8 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml 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( "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 96e9a594447..d44cf7c73af 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 @@ -147,7 +147,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 269d7b34882..590b30b91c1 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -48,6 +48,7 @@ bt_navigator: bt_loop_duration: 10 filter_duration: 0.3 default_server_timeout: 20 + default_cancel_timeout: 50 wait_for_service_timeout: 1000 introspection_mode: "disabled" 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 c1fca8802dc..9c96fc7d18a 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 @@ -6,6 +6,7 @@ bt_navigator: bt_loop_duration: 10 filter_duration: 0.3 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 954ec7388c0..fade1bbdb5d 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -46,6 +46,7 @@ bt_navigator: bt_loop_duration: 10 filter_duration: 0.3 default_server_timeout: 20 + default_cancel_timeout: 50 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator"