Merge pull request #13 from logivations/add-flag-to-not-send-goal-in-…

…BTactionnode

Add-flag-to-not-send-goal-in-BTactionnode

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -47,7 +47,7 @@ class BtActionNode : public BT::ActionNodeBase
     const std::string & xml_tag_name,
     const std::string & action_name,
     const BT::NodeConfiguration & conf)
-  : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
+  : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name), should_send_goal_(true)
   {
     node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
     callback_group_ = node_->create_callback_group(
@@ -188,10 +188,15 @@ class BtActionNode : public BT::ActionNodeBase
       // setting the status to RUNNING to notify the BT Loggers (if any)
       setStatus(BT::NodeStatus::RUNNING);
 
-      // user defined callback
+      // reset the flag to send the goal or not, allowing the user the option to set it in on_tick
+      should_send_goal_ = true;
+
+      // user defined callback, may modify "should_send_goal_".
       on_tick();
 
-      send_new_goal();
+      if (should_send_goal_) {
+        send_new_goal();
+      }
     }
 
     try {
@@ -223,7 +228,8 @@ class BtActionNode : public BT::ActionNodeBase
         feedback_.reset();
 
         auto goal_status = goal_handle_->get_status();
-        if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
+        if (goal_updated_ &&
+          (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
           goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
         {
           goal_updated_ = false;
@@ -444,6 +450,9 @@ class BtActionNode : public BT::ActionNodeBase
   std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
   future_goal_handle_;
   rclcpp::Time time_goal_sent_;
+
+  // Can be set in on_tick or on_wait_for_result to indicate if a goal should be sent.
+  bool should_send_goal_;
 };
 
 }  // namespace nav2_behavior_tree