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 58c1f422b5a..03411b18086 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 @@ -63,6 +63,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"); @@ -335,7 +336,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( @@ -486,6 +487,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 7af8b8eca0a..ee6c3eb7374 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 @@ -281,6 +281,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 bf9ecf203eb..c4653f31f49 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 @@ -67,6 +67,9 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } + if (!node->has_parameter("default_cancel_timeout")) { + node->declare_parameter("default_cancel_timeout", 50); + } if (!node->has_parameter("always_reload_bt_xml")) { node->declare_parameter("always_reload_bt_xml", false); } @@ -178,6 +181,9 @@ bool BtActionServer::on_configure() int default_server_timeout; node->get_parameter("default_server_timeout", default_server_timeout); default_server_timeout_ = std::chrono::milliseconds(default_server_timeout); + int default_cancel_timeout; + node->get_parameter("default_cancel_timeout", default_cancel_timeout); + default_cancel_timeout_ = std::chrono::milliseconds(default_cancel_timeout); int wait_for_service_timeout; node->get_parameter("wait_for_service_timeout", wait_for_service_timeout); wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout); @@ -195,6 +201,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("cancel_timeout", default_cancel_timeout_); // NOLINT blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT blackboard_->set( "wait_for_service_timeout", @@ -296,6 +303,7 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena auto & blackboard = subtree->blackboard; blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); + blackboard->set("cancel_timeout", default_cancel_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); blackboard->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 56a66a4602c..690708b6cba 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; }