Cherry-pick of get-input PR

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp

@@ -65,6 +65,11 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
    */
   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
@@ -82,6 +87,7 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
 
 private:
   bool is_recovery_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

@@ -65,6 +65,11 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
    */
   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<nav2_msgs::action::BackUp>
           "error_code_id", "The back up behavior server error code")
       });
   }
+
+private:
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp

@@ -44,6 +44,11 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
     const std::string & action_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 basic ports along with node-specific ports
@@ -62,6 +67,11 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
       });
   }
 
+  /**
+ * @brief Function to perform some user-defined operation on tick
+ */
+  void on_tick() override;
+
   /**
  * @brief Function to perform some user-defined operation upon successful completion of the action
  */
@@ -76,6 +86,9 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
    * @brief Function to perform some user-defined operation upon cancellation of the action
    */
   BT::NodeStatus on_cancelled() override;
+
+private:
+  bool initalized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp

@@ -36,6 +36,10 @@ class RemovePassedGoals : public BT::ActionNodeBase
     const std::string & xml_tag_name,
     const BT::NodeConfiguration & conf);
 
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
 
   static BT::PortsList providedPorts()
   {
@@ -56,6 +60,7 @@ class RemovePassedGoals : public BT::ActionNodeBase
   std::string robot_base_frame_, global_frame_;
   double transform_tolerance_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp

@@ -49,6 +49,11 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
    */
   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<nav2_msgs::action::Spin>
 
 private:
   bool is_recovery_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp

@@ -45,6 +45,11 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
    */
   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<nav2_msgs::action::Wait>
         BT::InputPort<double>("wait_duration", 1.0, "Wait time")
       });
   }
+
+private:
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

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
@@ -73,8 +78,8 @@ class DistanceTraveledCondition : public BT::ConditionNode
 
   double distance_;
   double transform_tolerance_;
-  std::string global_frame_;
-  std::string robot_base_frame_;
+  std::string global_frame_, robot_base_frame_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

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

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

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

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

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<std::chrono::high_resolution_clock> start_;
   double period_;
   bool first_time_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp

@@ -24,18 +24,27 @@ 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)
+: BtActionNode<nav2_msgs::action::AssistedTeleop>(xml_tag_name, action_name, conf),
+  initialized_(false)
+{}
+
+void AssistedTeleopAction::initialize()
 {
   double time_allowance;
   getInput("time_allowance", time_allowance);
   getInput("is_recovery", is_recovery_);
 
   // 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();
   }

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<nav2_msgs::action::BackUp>(xml_tag_name, action_name, conf)
+: BtActionNode<nav2_msgs::action::BackUp>(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();
 }
 

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<nav2_msgs::action::DriveOnHeading>(xml_tag_name, action_name, conf)
+: BtActionNode<nav2_msgs::action::DriveOnHeading>(xml_tag_name, action_name, conf),
+  initalized_(false)
+{
+}
+
+void DriveOnHeadingAction::initialize()
 {
   double dist;
   getInput("dist_to_travel", dist);
@@ -45,6 +50,14 @@ DriveOnHeadingAction::DriveOnHeadingAction(
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
   goal_.free_goal_vel = free_goal_vel;
   goal_.check_local_costmap = check_local_costmap;
+  initalized_ = true;
+}
+
+void DriveOnHeadingAction::on_tick()
+{
+  if (!initalized_) {
+    initialize();
+  }
 }
 
 BT::NodeStatus DriveOnHeadingAction::on_success()

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_);
 
@@ -37,12 +41,17 @@ RemovePassedGoals::RemovePassedGoals(
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
   auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   node->get_parameter("transform_tolerance", transform_tolerance_);
+  initialized_ = true;
 }
 
 inline BT::NodeStatus RemovePassedGoals::tick()
 {
   setStatus(BT::NodeStatus::RUNNING);
 
+  if (!initialized_) {
+    initialize();
+  }
+
   Goals goal_poses;
   getInput("input_goals", goal_poses);