Add cancel timeout when cancelling actions (backport #5895)

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<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");
 
@@ -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_;
 

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

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -158,6 +158,7 @@ bool BtActionServer<ActionT>::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<ActionT>::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<ActionT>::on_configure()
   blackboard_ = BT::Blackboard::create();
 
   // Put items on the blackboard
+<<<<<<< HEAD
   blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_);  // NOLINT
   blackboard_->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);  // NOLINT
   blackboard_->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);  // NOLINT
   blackboard_->set<std::chrono::milliseconds>(
+=======
+  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>(
+>>>>>>> 640196af (Add cancel timeout when cancelling actions (#5895))
     "wait_for_service_timeout",
     wait_for_service_timeout_);
 
@@ -257,10 +282,20 @@ bool BtActionServer<ActionT>::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<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
       blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
       blackboard->set<std::chrono::milliseconds>(
+=======
+      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>(
+>>>>>>> 640196af (Add cancel timeout when cancelling actions (#5895))
         "wait_for_service_timeout",
         wait_for_service_timeout_);
     }

nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp

@@ -146,7 +146,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

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

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:

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"