Skip to content

Commit 640196a

Browse files
mini-1235mini-1235
authored
Add cancel timeout when cancelling actions (#5895)
* Add halt timeout when cancelling actions Signed-off-by: mini-1235 <[email protected]> * Add to config Signed-off-by: mini-1235 <[email protected]> * Fix flaky test Signed-off-by: mini-1235 <[email protected]> * Retrigger Signed-off-by: mini-1235 <[email protected]> * Rename Signed-off-by: mini-1235 <[email protected]> * Lint Signed-off-by: mini-1235 <[email protected]> --------- Signed-off-by: mini-1235 <[email protected]>
1 parent 0cd9288 commit 640196a

7 files changed

Lines changed: 20 additions & 2 deletions

File tree

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,6 +62,7 @@ class BtActionNode : public BT::ActionNodeBase
6262
auto bt_loop_duration =
6363
config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
6464
getInputOrBlackboard("server_timeout", server_timeout_);
65+
getInputOrBlackboard("cancel_timeout", cancel_timeout_);
6566
wait_for_service_timeout_ =
6667
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
6768

@@ -334,7 +335,7 @@ class BtActionNode : public BT::ActionNodeBase
334335
"Failed to cancel action server for %s", action_name_.c_str());
335336
}
336337

337-
if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) !=
338+
if (callback_group_executor_.spin_until_future_complete(future_result, cancel_timeout_) !=
338339
rclcpp::FutureReturnCode::SUCCESS)
339340
{
340341
RCLCPP_ERROR(
@@ -485,6 +486,9 @@ class BtActionNode : public BT::ActionNodeBase
485486
// new action goal is sent or canceled
486487
std::chrono::milliseconds server_timeout_;
487488

489+
// The timeout value when cancelling actions during halt
490+
std::chrono::milliseconds cancel_timeout_;
491+
488492
// The timeout value for BT loop execution
489493
std::chrono::milliseconds max_timeout_;
490494

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -285,6 +285,9 @@ class BtActionServer
285285
// Default timeout value while waiting for response from a server
286286
std::chrono::milliseconds default_server_timeout_;
287287

288+
// Default timeout value when cancelling actions during node halt
289+
std::chrono::milliseconds default_cancel_timeout_;
290+
288291
// The timeout value for waiting for a service to response
289292
std::chrono::milliseconds wait_for_service_timeout_;
290293

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,9 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
160160
default_server_timeout_ = std::chrono::milliseconds(
161161
node->declare_or_get_parameter("default_server_timeout", 20));
162162

163+
default_cancel_timeout_ = std::chrono::milliseconds(
164+
node->declare_or_get_parameter("default_cancel_timeout", 50));
165+
163166
wait_for_service_timeout_ = std::chrono::milliseconds(
164167
node->declare_or_get_parameter("wait_for_service_timeout", 1000));
165168

@@ -178,6 +181,7 @@ bool BtActionServer<ActionT, NodeT>::on_configure()
178181
// Put items on the blackboard
179182
blackboard_->template set<nav2::LifecycleNode::SharedPtr>("node", client_node_); // NOLINT
180183
blackboard_->template set<std::chrono::milliseconds>("server_timeout", default_server_timeout_); // NOLINT
184+
blackboard_->template set<std::chrono::milliseconds>("cancel_timeout", default_cancel_timeout_); // NOLINT
181185
blackboard_->template set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_); // NOLINT
182186
blackboard_->template set<std::chrono::milliseconds>(
183187
"wait_for_service_timeout",
@@ -331,6 +335,8 @@ bool BtActionServer<ActionT, NodeT>::loadBehaviorTree(const std::string & bt_xml
331335
blackboard->template set("node", client_node_);
332336
blackboard->template set<std::chrono::milliseconds>("server_timeout",
333337
default_server_timeout_);
338+
blackboard->template set<std::chrono::milliseconds>("cancel_timeout",
339+
default_cancel_timeout_);
334340
blackboard->template set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
335341
blackboard->template set<std::chrono::milliseconds>(
336342
"wait_for_service_timeout",

‎nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -147,7 +147,9 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNode<test_msgs::actio
147147

148148
BT::NodeStatus on_cancelled() override
149149
{
150-
config().blackboard->set("sequence", result_.result->sequence);
150+
if (result_.result) {
151+
config().blackboard->set("sequence", result_.result->sequence);
152+
}
151153
config().blackboard->set("on_cancelled_triggered", true);
152154
return BT::NodeStatus::SUCCESS;
153155
}

‎nav2_bringup/params/nav2_params.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@ bt_navigator:
4848
bt_loop_duration: 10
4949
filter_duration: 0.3
5050
default_server_timeout: 20
51+
default_cancel_timeout: 50
5152
wait_for_service_timeout: 1000
5253
introspection_mode: "disabled"
5354
navigators: ["navigate_to_pose", "navigate_through_poses"]

‎nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ bt_navigator:
66
bt_loop_duration: 10
77
filter_duration: 0.3
88
default_server_timeout: 20
9+
default_cancel_timeout: 50
910
wait_for_service_timeout: 1000
1011
navigators: ["navigate_to_pose", "navigate_through_poses"]
1112
navigate_to_pose:

‎nav2_system_tests/src/system/nav2_system_params.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ bt_navigator:
4646
bt_loop_duration: 10
4747
filter_duration: 0.3
4848
default_server_timeout: 20
49+
default_cancel_timeout: 50
4950
navigators: ["navigate_to_pose", "navigate_through_poses"]
5051
navigate_to_pose:
5152
plugin: "nav2_bt_navigator::NavigateToPoseNavigator"

0 commit comments

Comments
 (0)