BT nodes not able to accept parameter values (ros-navigation#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 c1cd575.

* 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

* .

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

@@ -64,6 +64,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
@@ -81,6 +86,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

@@ -64,6 +64,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
@@ -79,6 +84,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

@@ -43,6 +43,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
@@ -59,6 +64,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
  */
@@ -73,6 +83,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
   double transform_tolerance_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string robot_base_frame_;
+  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

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

@@ -48,6 +48,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
@@ -81,6 +86,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
@@ -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

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);
@@ -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()

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);
 

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