Add cancel timeout when cancelling actions

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<std::chrono::milliseconds>("bt_loop_duration");
     getInputOrBlackboard("server_timeout", server_timeout_);
+    getInputOrBlackboard("cancel_timeout", cancel_timeout_);
     wait_for_service_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("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_;
 

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_;
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -160,6 +160,9 @@ bool BtActionServer<ActionT, NodeT>::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<ActionT, NodeT>::on_configure()
   // Put items on the blackboard
   blackboard_->template set<nav2::LifecycleNode::SharedPtr>("node", client_node_);  // NOLINT
   blackboard_->template set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);  // NOLINT
+  blackboard_->template set<std::chrono::milliseconds>("cancel_timeout", default_cancel_timeout_);  // NOLINT
   blackboard_->template set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);  // NOLINT
   blackboard_->template set<std::chrono::milliseconds>(
     "wait_for_service_timeout",
@@ -331,6 +335,8 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
       blackboard->template set("node", client_node_);
       blackboard->template set<std::chrono::milliseconds>("server_timeout",
           default_server_timeout_);
+      blackboard->template set<std::chrono::milliseconds>("cancel_timeout",
+        default_cancel_timeout_);
       blackboard->template set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
       blackboard->template set<std::chrono::milliseconds>(
         "wait_for_service_timeout",

nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp

@@ -147,7 +147,9 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNode<test_msgs::actio
 
   BT::NodeStatus on_cancelled() override
   {
-    config().blackboard->set("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;
   }

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"]

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:

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"