@@ -47,7 +47,7 @@ class BtActionNode : public BT::ActionNodeBase
4747 const std::string & xml_tag_name,
4848 const std::string & action_name,
4949 const BT::NodeConfiguration & conf)
50- : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
50+ : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name), should_send_goal_( true )
5151 {
5252 node_ = config ().blackboard ->template get <rclcpp::Node::SharedPtr>(" node" );
5353 callback_group_ = node_->create_callback_group (
@@ -190,7 +190,10 @@ class BtActionNode : public BT::ActionNodeBase
190190 // setting the status to RUNNING to notify the BT Loggers (if any)
191191 setStatus (BT::NodeStatus::RUNNING);
192192
193- // user defined callback
193+ // reset the flag to send the goal or not, allowing the user the option to set it in on_tick
194+ should_send_goal_ = true ;
195+
196+ // user defined callback, may modify "should_send_goal_".
194197 on_tick ();
195198
196199 if (!should_send_goal_) {
@@ -449,6 +452,9 @@ class BtActionNode : public BT::ActionNodeBase
449452 std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
450453 future_goal_handle_;
451454 rclcpp::Time time_goal_sent_;
455+
456+ // Can be set in on_tick or on_wait_for_result to indicate if a goal should be sent.
457+ bool should_send_goal_;
452458};
453459
454460} // namespace nav2_behavior_tree
0 commit comments