Pass IDLE to on_tick, use that for initialize condition (#4744)

* Pass IDLE to on_tick, use that for initialize condition Signed-off-by: redvinaa <redvinaa@gmail.com> * Fix battery sub creation bug Signed-off-by: redvinaa <redvinaa@gmail.com> --------- Signed-off-by: redvinaa <redvinaa@gmail.com>
ros-navigation · Dec 13, 2024 · cf64728 · cf64728
1 parent 2da6aa8
commit cf64728
Show file tree

Hide file tree

Showing 25 changed files with 47 additions and 85 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
@@ -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
@@ -86,7 +86,6 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
 
 private:
   bool is_recovery_;
-  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
@@ -84,9 +84,6 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
           "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
@@ -61,7 +61,6 @@ class RemovePassedGoals : public BT::ActionNodeBase
   double transform_tolerance_;
   std::shared_ptr<tf2_ros::Buffer> 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
@@ -86,7 +86,6 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
 
 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
@@ -61,9 +61,6 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
         BT::InputPort<double>("wait_duration", 1.0, "Wait time")
       });
   }
-
-private:
-  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

diff --git a/...ehavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/...ehavior_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
@@ -90,7 +90,6 @@ class GoalReachedCondition : public BT::ConditionNode
   rclcpp::Node::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> 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
@@ -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
@@ -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
@@ -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/...avior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/...avior_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
@@ -64,7 +64,6 @@ class RateController : public BT::DecoratorNode
   std::chrono::time_point<std::chrono::high_resolution_clock> 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
@@ -24,8 +24,7 @@ AssistedTeleopAction::AssistedTeleopAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf),
-  initialized_(false)
+: BtActionNode<nav2_msgs::action::AssistedTeleop>(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
@@ -24,8 +24,7 @@ BackUpAction::BackUpAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::BackUp>(xml_tag_name, action_name, conf),
-  initialized_(false)
+: BtActionNode<nav2_msgs::action::BackUp>(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
@@ -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()
@@ -46,9 +45,7 @@ void RemovePassedGoals::initialize()
 
 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
@@ -23,10 +23,7 @@ SpinAction::SpinAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::Spin>(xml_tag_name, action_name, conf),
-  initialized_(false)
-{
-}
+: BtActionNode<nav2_msgs::action::Spin>(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
@@ -25,8 +25,7 @@ WaitAction::WaitAction(
   const std::string & xml_tag_name,
   const std::string & action_name,
   const BT::NodeConfiguration & conf)
-: BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf),
-  initialized_(false)
+: BtActionNode<nav2_msgs::action::Wait>(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
@@ -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<std::string>(
     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
@@ -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<rclcpp::Node::SharedPtr>("node");
 
@@ -52,13 +51,11 @@ void GoalReachedCondition::initialize()
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("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
@@ -27,35 +27,40 @@ IsBatteryLowCondition::IsBatteryLowCondition(
   battery_topic_("/battery_status"),
   min_battery_(0.0),
   is_voltage_(false),
-  is_battery_low_(false),
-  initialized_(false)
+  is_battery_low_(false)
 {
 }
 
 void IsBatteryLowCondition::initialize()
 {
   getInput("min_battery", min_battery_);
-  getInput("battery_topic", battery_topic_);
+  std::string battery_topic_new;
+  getInput("battery_topic", battery_topic_new);
   getInput("is_voltage", is_voltage_);
-  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
-  callback_group_ = node_->create_callback_group(
-    rclcpp::CallbackGroupType::MutuallyExclusive,
-    false);
-  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
-  rclcpp::SubscriptionOptions sub_option;
-  sub_option.callback_group = callback_group_;
-  battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
-    battery_topic_,
-    rclcpp::SystemDefaultsQoS(),
-    std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1),
-    sub_option);
-  initialized_ = true;
+  // Only create a new subscriber if the topic has changed or subscriber is empty
+  if (battery_topic_new != battery_topic_ || !battery_sub_) {
+    battery_topic_ = battery_topic_new;
+
+    node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+    callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      false);
+    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+
+    rclcpp::SubscriptionOptions sub_option;
+    sub_option.callback_group = callback_group_;
+    battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
+      battery_topic_,
+      rclcpp::SystemDefaultsQoS(),
+      std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1),
+      sub_option);
+  }
 }
 
 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
@@ -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<rclcpp::Node::SharedPtr>("node");
   client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
@@ -35,12 +34,11 @@ IsPathValidCondition::IsPathValidCondition(
 void IsPathValidCondition::initialize()
 {
   getInput<std::chrono::milliseconds>("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
@@ -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<rclcpp::Node::SharedPtr>("node");
   start_ = node_->now();
-  initialized_ = true;
 }
 
 BT::NodeStatus TimeExpiredCondition::tick()
 {
-  if (!initialized_) {
+  if (!BT::isStatusActive(status())) {
     initialize();
   }