From f8f5f84d2e14b99d058f6e0d4e9f6fda65b1e96c Mon Sep 17 00:00:00 2001 From: maksymdidukh <142302397+maksymdidukh@users.noreply.github.com> Date: Tue, 2 Jan 2024 22:52:30 +0100 Subject: [PATCH] BT nodes not able to accept parameter values (#3988) * fix simple cases * fix on_tick type in drive_on_heading node * fix linting and fixed other nodes * Revert "fix linting and fixed other nodes" This reverts commit c1cd575b50580305b56a70b4a0fd0e399acaf39c. * fix linting * fixes for subscription cases in action nodes * fixes in condition nodes * fix input usage * fix uncrustify * . * fix typo in back_up_action.cpp * fix in drive_on_heading_action * remove unnecessary first_use variables * typo * typo * typo * fixed typos * move initialize() method above tick() * revert changes in truncate_path_action node * revert changes in case of creating subscription in constructor * remove global_frame_ in remove_passed_goals_action node * changes in is_path_valid and rate_controller nodes * change bt_cancel_action node * change bt_action_node * add xml_tag parameter * revert changes in bt_action and bt_cancel_action nodes * pre-commit * . --- .../plugins/action/assisted_teleop_action.hpp | 6 +++++ .../plugins/action/back_up_action.hpp | 8 +++++++ .../action/drive_on_heading_action.hpp | 13 ++++++++++ .../action/remove_passed_goals_action.hpp | 5 ++++ .../plugins/action/spin_action.hpp | 6 +++++ .../plugins/action/wait_action.hpp | 8 +++++++ .../condition/distance_traveled_condition.hpp | 6 +++++ .../condition/is_battery_low_condition.hpp | 6 +++++ .../condition/is_path_valid_condition.hpp | 6 +++++ .../condition/time_expired_condition.hpp | 6 +++++ .../transform_available_condition.hpp | 6 +++++ .../plugins/decorator/rate_controller.hpp | 6 +++++ .../plugins/action/assisted_teleop_action.cpp | 11 ++++++++- .../plugins/action/back_up_action.cpp | 12 +++++++++- .../action/drive_on_heading_action.cpp | 15 +++++++++++- .../action/remove_passed_goals_action.cpp | 10 +++++++- .../plugins/action/spin_action.cpp | 13 +++++++++- .../plugins/action/truncate_path_action.cpp | 2 +- .../plugins/action/wait_action.cpp | 12 +++++++++- .../condition/distance_traveled_condition.cpp | 9 +++++++ .../condition/goal_reached_condition.cpp | 24 +++++++++---------- .../condition/is_battery_low_condition.cpp | 12 +++++++++- .../condition/is_path_valid_condition.cpp | 12 +++++++++- .../path_expiring_timer_condition.cpp | 2 +- .../condition/time_expired_condition.cpp | 12 +++++++++- .../transform_available_condition.cpp | 21 +++++++++++----- .../plugins/control/recovery_node.cpp | 2 +- .../plugins/decorator/rate_controller.cpp | 12 +++++++++- 28 files changed, 232 insertions(+), 31 deletions(-) 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 a151bc62920..587b790d3a7 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 @@ -65,6 +65,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 @@ -80,6 +85,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 877132441c5..172200c9a07 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 @@ -44,6 +44,11 @@ class DriveOnHeadingAction : public BtActionNode tf_; + 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 d692ba6756d..d9520a6e5bf 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 @@ -49,6 +49,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 @@ -82,6 +87,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 f452d24d320..497b35f0f68 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, "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 6373a5600c1..0b65f61f888 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 @@ -75,6 +80,7 @@ class DistanceTraveledCondition : public BT::ConditionNode double transform_tolerance_; std::string global_frame_; std::string 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 1f63a281c3e..590aea5f206 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 9013a1244c1..9ffd078bae6 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 73036ccddd6..0f1b1b3c7dc 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 99568f933f9..1e94169b081 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -28,7 +28,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_); @@ -43,6 +47,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 dd203499502..d3e8457ec10 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 d2b0e6484be..9cf145c1f3c 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -24,7 +24,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() { int duration; getInput("wait_duration", duration); @@ -36,10 +41,15 @@ WaitAction::WaitAction( } goal_.time.sec = 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 00be8be846b..7141f9043bc 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -32,6 +32,11 @@ DistanceTraveledCondition::DistanceTraveledCondition( transform_tolerance_(0.1), global_frame_("map"), robot_base_frame_("base_link") + initialized_(false) +{ +} + +void DistanceTraveledCondition::initialize() { getInput("distance", distance_); getInput("global_frame", global_frame_); @@ -43,6 +48,10 @@ DistanceTraveledCondition::DistanceTraveledCondition( 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 3a5b8d821c9..4a9073454e4 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -41,18 +41,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"); @@ -68,6 +56,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 current_pose; 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 ddc67534469..b87f22a3129 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 2c90e2e4c6d..a26fb26c923 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -27,13 +27,13 @@ RecoveryNode::RecoveryNode( retry_count_(0), last_recovery_time_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { - getInput("number_of_retries", number_of_retries_); getInput("timeout", timeout_); node_ = config().blackboard->get("node"); } 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)