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 2c13b8f9c78..aef2cbb972a 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 @@ -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("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::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