Add flag to not send request in BTServiceNode (#3431)

* Add flag to not send request in BTServiceNode * rename goal to request * Fail if should not send goal * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp Co-authored-by: Steve Macenski <[email protected]> * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp Co-authored-by: Steve Macenski <[email protected]> * . * fix linter * fix CI --------- Co-authored-by: Steve Macenski <[email protected]>
ros-navigation · Mar 20, 2023 · c2537a8 · c2537a8
1 parent e725903
commit c2537a8
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 2 deletions.
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
@@ -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
@@ -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<typename ServiceT::Response::SharedPtr> 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