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 9c9dc88be1..b76403ff31 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 @@ -196,9 +196,10 @@ class BtActionNode : public BT::ActionNodeBase // user defined callback, may modify "should_send_goal_". on_tick(); - if (should_send_goal_) { - send_new_goal(); + if (!should_send_goal_) { + return BT::NodeStatus::FAILURE; } + send_new_goal(); } try { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 200b4c08dc..cd0cd524b9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -131,7 +131,17 @@ class BtServiceNode : public BT::ActionNodeBase BT::NodeStatus tick() override { if (!request_sent_) { + // reset the flag to send the request or not, + // allowing the user the option to set it in on_tick + should_send_request_ = true; + + // user defined callback, may modify "should_send_request_". on_tick(); + + if (!should_send_request_) { + return BT::NodeStatus::FAILURE; + } + future_result_ = service_client_->async_send_request(request_).share(); sent_time_ = node_->now(); request_sent_ = true; @@ -243,6 +253,9 @@ class BtServiceNode : public BT::ActionNodeBase std::shared_future future_result_; bool request_sent_{false}; rclcpp::Time sent_time_; + + // Can be set in on_tick or on_wait_for_result to indicate if a request should be sent. + bool should_send_request_; }; } // namespace nav2_behavior_tree