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 fb3f683b462..97511608d50 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 @@ -64,6 +64,11 @@ class AssistedTeleopAction : public BtActionNode */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -79,6 +84,9 @@ class BackUpAction : 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/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp index 7e5b27af9ce..dd1a6f29086 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp @@ -43,6 +43,11 @@ class DriveOnHeadingAction : public BtActionNode 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 2e442c0e112..4f9b300e445 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 @@ -48,6 +48,11 @@ class SpinAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -81,6 +86,7 @@ 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 5143f11351e..fdc320c16b0 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 @@ -45,6 +45,11 @@ class WaitAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing basic ports along with node-specific ports @@ -56,6 +61,9 @@ 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 d29f40e66d8..3c87c7370c7 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 @@ -52,6 +52,11 @@ class DistanceTraveledCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -74,6 +79,7 @@ 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/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 5c4753cf9a1..ff479105807 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 @@ -51,6 +51,11 @@ class IsBatteryLowCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -81,6 +86,7 @@ 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 a04b1263f4b..36890f2a8a0 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 @@ -50,6 +50,11 @@ class IsPathValidCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -68,6 +73,7 @@ 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 9f8c47afab0..356a79857db 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 @@ -48,6 +48,11 @@ class TimeExpiredCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -63,6 +68,7 @@ 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 64572e21b7a..5fdefa9b8bf 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 @@ -55,6 +55,11 @@ class TransformAvailableCondition : public BT::ConditionNode */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -75,6 +80,7 @@ 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 201f4868cfd..24a0207e3bd 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 @@ -38,6 +38,11 @@ class RateController : public BT::DecoratorNode const std::string & name, const BT::NodeConfiguration & conf); + /** + * @brief Function to read parameters and initialize class variables + */ + void initialize(); + /** * @brief Creates list of BT ports * @return BT::PortsList Containing node-specific ports @@ -59,6 +64,7 @@ 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 1d2bad9f376..64eb61d788e 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -24,7 +24,11 @@ AssistedTeleopAction::AssistedTeleopAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{} + +void AssistedTeleopAction::initialize() { double time_allowance; getInput("time_allowance", time_allowance); @@ -32,10 +36,15 @@ AssistedTeleopAction::AssistedTeleopAction( // Populate the input message goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + initialized_ = true; } void AssistedTeleopAction::on_tick() { + if (!initialized_) { + initialize(); + } + if (is_recovery_) { increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index cb15fc93336..09d79c7e230 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -24,7 +24,12 @@ BackUpAction::BackUpAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{ +} + +void nav2_behavior_tree::BackUpAction::initialize() { double dist; getInput("backup_dist", dist); @@ -39,10 +44,15 @@ BackUpAction::BackUpAction( 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_) { + initialize(); + } + increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index be062357b16..7e0b613f62f 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -24,7 +24,12 @@ DriveOnHeadingAction::DriveOnHeadingAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initalized_(false) +{ +} + +void DriveOnHeadingAction::initialize() { double dist; getInput("dist_to_travel", dist); @@ -39,6 +44,14 @@ DriveOnHeadingAction::DriveOnHeadingAction( goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + initalized_ = true; +} + +void DriveOnHeadingAction::on_tick() +{ + if (!initalized_) { + initialize(); + } } BT::NodeStatus DriveOnHeadingAction::on_success() 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 7a5816973dc..bf8b95c84f6 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -29,7 +29,11 @@ RemovePassedGoals::RemovePassedGoals( const std::string & name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(name, conf), - viapoint_achieved_radius_(0.5) + viapoint_achieved_radius_(0.5), + initialized_(false) +{} + +void RemovePassedGoals::initialize() { getInput("radius", viapoint_achieved_radius_); @@ -45,6 +49,10 @@ inline BT::NodeStatus RemovePassedGoals::tick() { setStatus(BT::NodeStatus::RUNNING); + if (!initialized_) { + initialize(); + } + Goals goal_poses; getInput("input_goals", goal_poses); diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index 78fa1311752..cab9c8f8211 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -23,7 +23,12 @@ SpinAction::SpinAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{ +} + +void SpinAction::initialize() { double dist; getInput("spin_dist", dist); @@ -32,10 +37,16 @@ SpinAction::SpinAction( 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_) { + initialize(); + } + if (is_recovery_) { increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp index 7f919a30f3a..b394a804c73 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp @@ -33,12 +33,12 @@ TruncatePath::TruncatePath( : BT::ActionNodeBase(name, conf), distance_(1.0) { - getInput("distance", distance_); } inline BT::NodeStatus TruncatePath::tick() { setStatus(BT::NodeStatus::RUNNING); + getInput("distance", distance_); nav_msgs::msg::Path input_path; diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index f08fed31476..b9317076737 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -25,7 +25,12 @@ WaitAction::WaitAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf), + initialized_(false) +{ +} + +void WaitAction::initialize() { double duration; getInput("wait_duration", duration); @@ -37,10 +42,15 @@ WaitAction::WaitAction( } goal_.time = rclcpp::Duration::from_seconds(duration); + initialized_ = true; } void WaitAction::on_tick() { + if (!initialized_) { + initialize(); + } + increment_recovery_count(); } diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 5840b8882df..64394ee3782 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -30,7 +30,12 @@ DistanceTraveledCondition::DistanceTraveledCondition( const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), distance_(1.0), - transform_tolerance_(0.1) + transform_tolerance_(0.1), + initialized_(false) +{ +} + +void DistanceTraveledCondition::initialize() { getInput("distance", distance_); @@ -42,10 +47,15 @@ DistanceTraveledCondition::DistanceTraveledCondition( node_, "global_frame", this); robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); + initialized_ = true; } BT::NodeStatus DistanceTraveledCondition::tick() { + if (!initialized_) { + initialize(); + } + if (status() == BT::NodeStatus::IDLE) { if (!nav2_util::getCurrentPose( start_pose_, *tf_, global_frame_, robot_base_frame_, diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index d81df60a9ec..2697680415b 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -42,18 +42,6 @@ GoalReachedCondition::~GoalReachedCondition() cleanup(); } -BT::NodeStatus GoalReachedCondition::tick() -{ - if (!initialized_) { - initialize(); - } - - if (isGoalReached()) { - return BT::NodeStatus::SUCCESS; - } - return BT::NodeStatus::FAILURE; -} - void GoalReachedCondition::initialize() { node_ = config().blackboard->get("node"); @@ -69,6 +57,18 @@ void GoalReachedCondition::initialize() initialized_ = true; } +BT::NodeStatus GoalReachedCondition::tick() +{ + if (!initialized_) { + initialize(); + } + + if (isGoalReached()) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + bool GoalReachedCondition::isGoalReached() { geometry_msgs::msg::PoseStamped goal; 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 b8630261e6e..9d221cf8c81 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -27,7 +27,12 @@ IsBatteryLowCondition::IsBatteryLowCondition( battery_topic_("/battery_status"), min_battery_(0.0), is_voltage_(false), - is_battery_low_(false) + is_battery_low_(false), + initialized_(false) +{ +} + +void IsBatteryLowCondition::initialize() { getInput("min_battery", min_battery_); getInput("battery_topic", battery_topic_); @@ -45,10 +50,15 @@ IsBatteryLowCondition::IsBatteryLowCondition( rclcpp::SystemDefaultsQoS(), std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), sub_option); + initialized_ = true; } BT::NodeStatus IsBatteryLowCondition::tick() { + if (!initialized_) { + initialize(); + } + callback_group_executor_.spin_some(); if (is_battery_low_) { return BT::NodeStatus::SUCCESS; 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 4a02dede9aa..59d593a1475 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -23,17 +23,27 @@ namespace nav2_behavior_tree IsPathValidCondition::IsPathValidCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) -: BT::ConditionNode(condition_name, conf) +: BT::ConditionNode(condition_name, conf), + initialized_(false) { node_ = config().blackboard->get("node"); client_ = node_->create_client("is_path_valid"); server_timeout_ = config().blackboard->template get("server_timeout"); +} + +void IsPathValidCondition::initialize() +{ getInput("server_timeout", server_timeout_); + initialized_ = true; } BT::NodeStatus IsPathValidCondition::tick() { + if (!initialized_) { + initialize(); + } + nav_msgs::msg::Path path; getInput("path", path); diff --git a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp index e018e025352..1347998602f 100644 --- a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp @@ -29,13 +29,13 @@ PathExpiringTimerCondition::PathExpiringTimerCondition( period_(1.0), first_time_(true) { - getInput("seconds", period_); node_ = config().blackboard->get("node"); } BT::NodeStatus PathExpiringTimerCondition::tick() { if (first_time_) { + getInput("seconds", period_); getInput("path", prev_path_); first_time_ = false; start_ = node_->now(); diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp index 4dc7e588937..64afd1a34be 100644 --- a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -27,15 +27,25 @@ TimeExpiredCondition::TimeExpiredCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - period_(1.0) + period_(1.0), + initialized_(false) +{ +} + +void TimeExpiredCondition::initialize() { getInput("seconds", period_); node_ = config().blackboard->get("node"); start_ = node_->now(); + initialized_ = true; } BT::NodeStatus TimeExpiredCondition::tick() { + if (!initialized_) { + initialize(); + } + if (status() == BT::NodeStatus::IDLE) { start_ = node_->now(); return BT::NodeStatus::FAILURE; diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index 8daca5afede..33e94178deb 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -27,11 +27,20 @@ TransformAvailableCondition::TransformAvailableCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - was_found_(false) + was_found_(false), + initialized_(false) { node_ = config().blackboard->get("node"); tf_ = config().blackboard->get>("tf_buffer"); +} +TransformAvailableCondition::~TransformAvailableCondition() +{ + RCLCPP_DEBUG(node_->get_logger(), "Shutting down TransformAvailableCondition BT node"); +} + +void TransformAvailableCondition::initialize() +{ getInput("child", child_frame_); getInput("parent", parent_frame_); @@ -43,15 +52,15 @@ TransformAvailableCondition::TransformAvailableCondition( } RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node"); -} - -TransformAvailableCondition::~TransformAvailableCondition() -{ - RCLCPP_DEBUG(node_->get_logger(), "Shutting down TransformAvailableCondition BT node"); + initialized_ = true; } BT::NodeStatus TransformAvailableCondition::tick() { + if (!initialized_) { + initialize(); + } + if (was_found_) { return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 42fc8076be6..44e56dc55ec 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -26,11 +26,11 @@ RecoveryNode::RecoveryNode( number_of_retries_(1), retry_count_(0) { - getInput("number_of_retries", number_of_retries_); } BT::NodeStatus RecoveryNode::tick() { + getInput("number_of_retries", number_of_retries_); const unsigned children_count = children_nodes_.size(); if (children_count != 2) { diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index 13e98e0d29d..b710ec30091 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -24,15 +24,25 @@ RateController::RateController( const std::string & name, const BT::NodeConfiguration & conf) : BT::DecoratorNode(name, conf), - first_time_(false) + first_time_(false), + initialized_(false) +{ +} + +void RateController::initialize() { double hz = 1.0; getInput("hz", hz); period_ = 1.0 / hz; + initialized_ = true; } BT::NodeStatus RateController::tick() { + if (!initialized_) { + initialize(); + } + if (status() == BT::NodeStatus::IDLE) { // Reset the starting point since we're starting a new iteration of // the rate controller (moving from IDLE to RUNNING)