From 1424534f321ce8f810daf242ea164c288c7556d9 Mon Sep 17 00:00:00 2001 From: redvinaa Date: Thu, 7 Nov 2024 17:00:45 +0100 Subject: [PATCH] Pass IDLE to on_tick, use that for initialize condition --- .../include/nav2_behavior_tree/bt_action_node.hpp | 6 +++--- .../plugins/action/assisted_teleop_action.hpp | 1 - .../nav2_behavior_tree/plugins/action/back_up_action.hpp | 3 --- .../plugins/action/remove_passed_goals_action.hpp | 1 - .../nav2_behavior_tree/plugins/action/spin_action.hpp | 1 - .../nav2_behavior_tree/plugins/action/wait_action.hpp | 3 --- .../plugins/condition/distance_traveled_condition.hpp | 1 - .../plugins/condition/goal_reached_condition.hpp | 1 - .../plugins/condition/is_battery_low_condition.hpp | 1 - .../plugins/condition/is_path_valid_condition.hpp | 1 - .../plugins/condition/time_expired_condition.hpp | 1 - .../plugins/condition/transform_available_condition.hpp | 1 - .../plugins/decorator/rate_controller.hpp | 1 - .../plugins/action/assisted_teleop_action.cpp | 6 ++---- nav2_behavior_tree/plugins/action/back_up_action.cpp | 6 ++---- .../plugins/action/remove_passed_goals_action.cpp | 8 ++------ nav2_behavior_tree/plugins/action/spin_action.cpp | 9 ++------- nav2_behavior_tree/plugins/action/wait_action.cpp | 6 ++---- .../plugins/condition/distance_traveled_condition.cpp | 6 ++---- .../plugins/condition/goal_reached_condition.cpp | 7 ++----- .../plugins/condition/is_battery_low_condition.cpp | 6 ++---- .../plugins/condition/is_path_valid_condition.cpp | 6 ++---- .../plugins/condition/time_expired_condition.cpp | 6 ++---- .../plugins/condition/transform_available_condition.cpp | 6 ++---- nav2_behavior_tree/plugins/decorator/rate_controller.cpp | 6 ++---- 25 files changed, 27 insertions(+), 73 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 index 4e723f1ac5f..505a7895c51 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 @@ -191,15 +191,15 @@ class BtActionNode : public BT::ActionNodeBase { // first step to be done only at the beginning of the Action if (!BT::isStatusActive(status())) { - // setting the status to RUNNING to notify the BT Loggers (if any) - setStatus(BT::NodeStatus::RUNNING); - // 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(); + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(BT::NodeStatus::RUNNING); + if (!should_send_goal_) { return BT::NodeStatus::FAILURE; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp index 97511608d50..6466948b313 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp @@ -86,7 +86,6 @@ class AssistedTeleopAction : public BtActionNode "error_code_id", "The back up behavior server error code") }); } - -private: - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index 33552a24f06..fcfade6d17b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -61,7 +61,6 @@ class RemovePassedGoals : public BT::ActionNodeBase double transform_tolerance_; std::shared_ptr tf_; std::string robot_base_frame_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index 4f9b300e445..cad5e8ce4a8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp @@ -86,7 +86,6 @@ class SpinAction : public BtActionNode private: bool is_recovery_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp index fdc320c16b0..1749a032cb1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp @@ -61,9 +61,6 @@ class WaitAction : public BtActionNode BT::InputPort("wait_duration", 1.0, "Wait time") }); } - -private: - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index 77a80728ddf..dd200f318b3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -80,7 +80,6 @@ class DistanceTraveledCondition : public BT::ConditionNode double distance_; double transform_tolerance_; std::string global_frame_, robot_base_frame_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index 44e582c5f53..ff65da09a18 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -90,7 +90,6 @@ class GoalReachedCondition : public BT::ConditionNode rclcpp::Node::SharedPtr node_; std::shared_ptr tf_; - bool initialized_; double goal_reached_tol_; double transform_tolerance_; std::string robot_base_frame_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 59a023ff3c2..40f0ac5804b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -86,7 +86,6 @@ class IsBatteryLowCondition : public BT::ConditionNode double min_battery_; bool is_voltage_; bool is_battery_low_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index 5a9f255a9b7..a22942f9a56 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -73,7 +73,6 @@ class IsPathValidCondition : public BT::ConditionNode // The timeout value while waiting for a responce from the // is path valid service std::chrono::milliseconds server_timeout_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp index 26a3431b5db..2e34689c56a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp @@ -68,7 +68,6 @@ class TimeExpiredCondition : public BT::ConditionNode rclcpp::Node::SharedPtr node_; rclcpp::Time start_; double period_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp index 511c381321b..732e862ff4c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp @@ -80,7 +80,6 @@ class TransformAvailableCondition : public BT::ConditionNode std::string child_frame_; std::string parent_frame_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp index c390357b342..ccc346eb16d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp @@ -64,7 +64,6 @@ class RateController : public BT::DecoratorNode std::chrono::time_point start_; double period_; bool first_time_; - bool initialized_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index e876d2ce40b..7354b05f370 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -24,8 +24,7 @@ AssistedTeleopAction::AssistedTeleopAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) {} void AssistedTeleopAction::initialize() @@ -36,12 +35,11 @@ void AssistedTeleopAction::initialize() // Populate the input message goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); - initialized_ = true; } void AssistedTeleopAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index e17580fe71c..3df77f98d7c 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -24,8 +24,7 @@ BackUpAction::BackUpAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) { } @@ -44,12 +43,11 @@ void nav2_behavior_tree::BackUpAction::initialize() goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); - initialized_ = true; } void BackUpAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 665bf81bc3f..723e704e437 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -28,8 +28,7 @@ RemovePassedGoals::RemovePassedGoals( const std::string & name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(name, conf), - viapoint_achieved_radius_(0.5), - initialized_(false) + viapoint_achieved_radius_(0.5) {} void RemovePassedGoals::initialize() @@ -42,14 +41,11 @@ void RemovePassedGoals::initialize() robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); - initialized_ = true; } inline BT::NodeStatus RemovePassedGoals::tick() { - setStatus(BT::NodeStatus::RUNNING); - - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index d10bb83f32b..cb3459006fe 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -23,10 +23,7 @@ SpinAction::SpinAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) -{ -} +: BtActionNode(xml_tag_name, action_name, conf) {} void SpinAction::initialize() { @@ -37,13 +34,11 @@ void SpinAction::initialize() goal_.target_yaw = dist; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); getInput("is_recovery", is_recovery_); - - initialized_ = true; } void SpinAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index b607d026e59..03a18c9f947 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -25,8 +25,7 @@ WaitAction::WaitAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf), - initialized_(false) +: BtActionNode(xml_tag_name, action_name, conf) { } @@ -42,12 +41,11 @@ void WaitAction::initialize() } goal_.time = rclcpp::Duration::from_seconds(duration); - initialized_ = true; } void WaitAction::on_tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 991e0ab7cf2..2ebf9c486ea 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -29,8 +29,7 @@ DistanceTraveledCondition::DistanceTraveledCondition( const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), distance_(1.0), - transform_tolerance_(0.1), - initialized_(false) + transform_tolerance_(0.1) { } @@ -46,12 +45,11 @@ void DistanceTraveledCondition::initialize() node_, "global_frame", this); robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); - initialized_ = true; } BT::NodeStatus DistanceTraveledCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e93ba5cc360..4b5e20a733e 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -27,8 +27,7 @@ namespace nav2_behavior_tree GoalReachedCondition::GoalReachedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf), - initialized_(false) +: BT::ConditionNode(condition_name, conf) { auto node = config().blackboard->get("node"); @@ -52,13 +51,11 @@ void GoalReachedCondition::initialize() tf_ = config().blackboard->get>("tf_buffer"); node_->get_parameter("transform_tolerance", transform_tolerance_); - - initialized_ = true; } BT::NodeStatus GoalReachedCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index a0e3761f28c..efeb33c6402 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -27,8 +27,7 @@ IsBatteryLowCondition::IsBatteryLowCondition( battery_topic_("/battery_status"), min_battery_(0.0), is_voltage_(false), - is_battery_low_(false), - initialized_(false) + is_battery_low_(false) { } @@ -50,12 +49,11 @@ void IsBatteryLowCondition::initialize() rclcpp::SystemDefaultsQoS(), std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), sub_option); - initialized_ = true; } BT::NodeStatus IsBatteryLowCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 7274743e1e9..cd76df97495 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -23,8 +23,7 @@ namespace nav2_behavior_tree IsPathValidCondition::IsPathValidCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf), - initialized_(false) +: BT::ConditionNode(condition_name, conf) { node_ = config().blackboard->get("node"); client_ = node_->create_client("is_path_valid"); @@ -35,12 +34,11 @@ IsPathValidCondition::IsPathValidCondition( void IsPathValidCondition::initialize() { getInput("server_timeout", server_timeout_); - initialized_ = true; } BT::NodeStatus IsPathValidCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp index f03c9711aa8..23fd614cb7c 100644 --- a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -27,8 +27,7 @@ TimeExpiredCondition::TimeExpiredCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - period_(1.0), - initialized_(false) + period_(1.0) { } @@ -37,12 +36,11 @@ void TimeExpiredCondition::initialize() getInput("seconds", period_); node_ = config().blackboard->get("node"); start_ = node_->now(); - initialized_ = true; } BT::NodeStatus TimeExpiredCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index 1afe0e14331..ab589048c82 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -31,8 +31,7 @@ TransformAvailableCondition::TransformAvailableCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - was_found_(false), - initialized_(false) + was_found_(false) { node_ = config().blackboard->get("node"); tf_ = config().blackboard->get>("tf_buffer"); @@ -56,12 +55,11 @@ void TransformAvailableCondition::initialize() } RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node"); - initialized_ = true; } BT::NodeStatus TransformAvailableCondition::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); } diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index 9592d119c96..5263da09eb2 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -24,8 +24,7 @@ RateController::RateController( const std::string & name, const BT::NodeConfiguration & conf) : BT::DecoratorNode(name, conf), - first_time_(false), - initialized_(false) + first_time_(false) { } @@ -34,12 +33,11 @@ void RateController::initialize() double hz = 1.0; getInput("hz", hz); period_ = 1.0 / hz; - initialized_ = true; } BT::NodeStatus RateController::tick() { - if (!initialized_) { + if (!BT::isStatusActive(status())) { initialize(); }