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..23f77970500 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 @@ -61,6 +61,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", 20); + } if (!node->has_parameter("action_server_result_timeout")) { node->declare_parameter("action_server_result_timeout", 900.0); } @@ -164,6 +167,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); @@ -181,6 +187,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", @@ -259,6 +266,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 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; }