fix linting and fixed other nodes

papertowel123 · papertowel123 · commit c1cd575b5058 · 2023-11-27T14:34:03.000+01:00
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp
@@ -82,7 +82,7 @@ class GoalCheckerSelector : public BT::SyncActionNode
    * @param msg the message with the id of the goal_checker_selector
    */
   void callbackGoalCheckerSelect(const std_msgs::msg::String::SharedPtr msg);
-
+  bool first_time;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr goal_checker_selector_sub_;
 
   std::string last_selected_goal_checker_;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp
@@ -83,7 +83,7 @@ class PlannerSelector : public BT::SyncActionNode
    */
   void callbackPlannerSelect(const std_msgs::msg::String::SharedPtr msg);
 
-
+  bool first_time;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr planner_selector_sub_;
 
   std::string last_selected_planner_;
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
@@ -52,6 +52,7 @@ class RemovePassedGoals : public BT::ActionNodeBase
   void halt() override {}
   BT::NodeStatus tick() override;
 
+  bool first_time;
   double viapoint_achieved_radius_;
   double transform_tolerance_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp
@@ -36,7 +36,7 @@ void AssistedTeleopAction::on_tick()
 
   // Populate the input message
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
-  
+
   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
@@ -43,7 +43,7 @@ void BackUpAction::on_tick()
   goal_.target.z = 0.0;
   goal_.speed = speed;
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
-  
+
   increment_recovery_count();
 }
 
diff --git a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp
@@ -30,30 +30,30 @@ using std::placeholders::_1;
 ControllerSelector::ControllerSelector(
   const std::string & name,
   const BT::NodeConfiguration & conf)
-: BT::SyncActionNode(name, conf)
+: BT::SyncActionNode(name, conf),
+  first_time(true)
 {
   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());
+}
 
+BT::NodeStatus ControllerSelector::tick()
+{
   getInput("topic_name", topic_name_);
 
   rclcpp::QoS qos(rclcpp::KeepLast(1));
   qos.transient_local().reliable();
-
   rclcpp::SubscriptionOptions sub_option;
   sub_option.callback_group = callback_group_;
   controller_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
     topic_name_,
     qos,
     std::bind(&ControllerSelector::callbackControllerSelect, this, _1),
     sub_option);
-}
 
-BT::NodeStatus ControllerSelector::tick()
-{
   callback_group_executor_.spin_some();
 
   // This behavior always use the last selected controller received from the topic input.
diff --git a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp
@@ -30,21 +30,25 @@ using std::placeholders::_1;
 GoalCheckerSelector::GoalCheckerSelector(
   const std::string & name,
   const BT::NodeConfiguration & conf)
-: BT::SyncActionNode(name, conf)
+: BT::SyncActionNode(name, conf),
+  first_time(true)
 {
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
-
-  getInput("topic_name", topic_name_);
-
-  rclcpp::QoS qos(rclcpp::KeepLast(1));
-  qos.transient_local().reliable();
-
-  goal_checker_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
-    topic_name_, qos, std::bind(&GoalCheckerSelector::callbackGoalCheckerSelect, this, _1));
 }
 
 BT::NodeStatus GoalCheckerSelector::tick()
 {
+  if (first_time) {
+    first_time = false;
+    getInput("topic_name", topic_name_);
+
+    rclcpp::QoS qos(rclcpp::KeepLast(1));
+    qos.transient_local().reliable();
+
+    goal_checker_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+      topic_name_, qos, std::bind(&GoalCheckerSelector::callbackGoalCheckerSelect, this, _1));
+  }
+
   rclcpp::spin_some(node_);
 
   // This behavior always use the last selected goal checker received from the topic input.
diff --git a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp
@@ -30,30 +30,35 @@ using std::placeholders::_1;
 PlannerSelector::PlannerSelector(
   const std::string & name,
   const BT::NodeConfiguration & conf)
-: BT::SyncActionNode(name, conf)
+: BT::SyncActionNode(name, conf),
+  first_time(true)
 {
   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());
-
-  getInput("topic_name", topic_name_);
-
-  rclcpp::QoS qos(rclcpp::KeepLast(1));
-  qos.transient_local().reliable();
-
-  rclcpp::SubscriptionOptions sub_option;
-  sub_option.callback_group = callback_group_;
-  planner_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
-    topic_name_,
-    qos,
-    std::bind(&PlannerSelector::callbackPlannerSelect, this, _1),
-    sub_option);
 }
 
 BT::NodeStatus PlannerSelector::tick()
 {
+  if (first_time) {
+    first_time = false;
+    callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      false);
+    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+
+    getInput("topic_name", topic_name_);
+
+    rclcpp::QoS qos(rclcpp::KeepLast(1));
+    qos.transient_local().reliable();
+
+    rclcpp::SubscriptionOptions sub_option;
+    sub_option.callback_group = callback_group_;
+    planner_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+      topic_name_,
+      qos,
+      std::bind(&PlannerSelector::callbackPlannerSelect, this, _1),
+      sub_option);
+  }
+
   callback_group_executor_.spin_some();
 
   // This behavior always use the last selected planner received from the topic input.
diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp
@@ -29,24 +29,29 @@ RemovePassedGoals::RemovePassedGoals(
   const std::string & name,
   const BT::NodeConfiguration & conf)
 : BT::ActionNodeBase(name, conf),
-  viapoint_achieved_radius_(0.5)
+  viapoint_achieved_radius_(0.5),
+  first_time(true)
 {
-  getInput("radius", viapoint_achieved_radius_);
-
-  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_);
-
-  global_frame_ = BT::deconflictPortAndParamFrame<std::string, RemovePassedGoals>(
-    node, "global_frame", this);
-  robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string, RemovePassedGoals>(
-    node, "robot_base_frame", this);
 }
 
 inline BT::NodeStatus RemovePassedGoals::tick()
 {
   setStatus(BT::NodeStatus::RUNNING);
 
+  if (first_time) {
+    first_time = false;
+    getInput("radius", viapoint_achieved_radius_);
+
+    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_);
+
+    global_frame_ = BT::deconflictPortAndParamFrame<std::string, RemovePassedGoals>(
+      node, "global_frame", this);
+    robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string, RemovePassedGoals>(
+      node, "robot_base_frame", this);
+  }
+
   Goals goal_poses;
   getInput("input_goals", goal_poses);
 
diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp
@@ -31,30 +31,34 @@ using std::placeholders::_1;
 SmootherSelector::SmootherSelector(
   const std::string & name,
   const BT::NodeConfiguration & conf)
-: BT::SyncActionNode(name, conf)
+: BT::SyncActionNode(name, conf),
+  first_time(true)
 {
   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());
-
-  getInput("topic_name", topic_name_);
-
-  rclcpp::QoS qos(rclcpp::KeepLast(1));
-  qos.transient_local().reliable();
-
-  rclcpp::SubscriptionOptions sub_option;
-  sub_option.callback_group = callback_group_;
-  smoother_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
-    topic_name_,
-    qos,
-    std::bind(&SmootherSelector::callbackSmootherSelect, this, _1),
-    sub_option);
 }
 
 BT::NodeStatus SmootherSelector::tick()
 {
+  if (first_time) {
+    first_time = false;
+    getInput("topic_name", topic_name_);
+
+    rclcpp::QoS qos(rclcpp::KeepLast(1));
+    qos.transient_local().reliable();
+
+    rclcpp::SubscriptionOptions sub_option;
+    sub_option.callback_group = callback_group_;
+    smoother_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+      topic_name_,
+      qos,
+      std::bind(&SmootherSelector::callbackSmootherSelect, this, _1),
+      sub_option);
+  }
+
   callback_group_executor_.spin_some();
 
   // This behavior always use the last selected smoother received from the topic input.
diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp
@@ -36,7 +36,7 @@ void SpinAction::on_tick()
   goal_.target_yaw = dist;
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
   getInput("is_recovery", is_recovery_);
-  
+
   if (is_recovery_) {
     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
@@ -32,8 +32,6 @@ DistanceTraveledCondition::DistanceTraveledCondition(
   distance_(1.0),
   transform_tolerance_(0.1)
 {
-  getInput("distance", distance_);
-
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
   node_->get_parameter("transform_tolerance", transform_tolerance_);
@@ -46,6 +44,7 @@ DistanceTraveledCondition::DistanceTraveledCondition(
 
 BT::NodeStatus DistanceTraveledCondition::tick()
 {
+  getInput("distance", distance_);
   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/is_battery_charging_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
@@ -26,7 +26,6 @@ IsBatteryChargingCondition::IsBatteryChargingCondition(
   battery_topic_("/battery_status"),
   is_battery_charging_(false)
 {
-  getInput("battery_topic", battery_topic_);
   auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   callback_group_ = node->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive,
@@ -44,6 +43,7 @@ IsBatteryChargingCondition::IsBatteryChargingCondition(
 
 BT::NodeStatus IsBatteryChargingCondition::tick()
 {
+  getInput("battery_topic", battery_topic_);
   callback_group_executor_.spin_some();
   if (is_battery_charging_) {
     return BT::NodeStatus::SUCCESS;
diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp
@@ -29,9 +29,6 @@ IsBatteryLowCondition::IsBatteryLowCondition(
   is_voltage_(false),
   is_battery_low_(false)
 {
-  getInput("min_battery", min_battery_);
-  getInput("battery_topic", battery_topic_);
-  getInput("is_voltage", is_voltage_);
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   callback_group_ = node_->create_callback_group(
     rclcpp::CallbackGroupType::MutuallyExclusive,
@@ -49,6 +46,10 @@ IsBatteryLowCondition::IsBatteryLowCondition(
 
 BT::NodeStatus IsBatteryLowCondition::tick()
 {
+  getInput("min_battery", min_battery_);
+  getInput("battery_topic", battery_topic_);
+  getInput("is_voltage", is_voltage_);
+
   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
@@ -29,12 +29,12 @@ IsPathValidCondition::IsPathValidCondition(
   client_ = node_->create_client<nav2_msgs::srv::IsPathValid>("is_path_valid");
 
   server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
-  getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
 }
 
 BT::NodeStatus IsPathValidCondition::tick()
 {
   nav_msgs::msg::Path path;
+  getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
   getInput("path", path);
 
   auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>();
diff --git a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp
@@ -29,12 +29,13 @@ PathExpiringTimerCondition::PathExpiringTimerCondition(
   period_(1.0),
   first_time_(true)
 {
-  getInput("seconds", period_);
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 }
 
 BT::NodeStatus PathExpiringTimerCondition::tick()
 {
+  getInput("seconds", period_);
+
   if (first_time_) {
     getInput("path", prev_path_);
     first_time_ = false;
diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp
@@ -29,13 +29,14 @@ TimeExpiredCondition::TimeExpiredCondition(
 : BT::ConditionNode(condition_name, conf),
   period_(1.0)
 {
-  getInput("seconds", period_);
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   start_ = node_->now();
 }
 
 BT::NodeStatus TimeExpiredCondition::tick()
 {
+  getInput("seconds", period_);
+
   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
@@ -32,6 +32,15 @@ TransformAvailableCondition::TransformAvailableCondition(
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
 
+}
+
+TransformAvailableCondition::~TransformAvailableCondition()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "Shutting down TransformAvailableCondition BT node");
+}
+
+BT::NodeStatus TransformAvailableCondition::tick()
+{
   getInput("child", child_frame_);
   getInput("parent", parent_frame_);
 
@@ -43,15 +52,7 @@ TransformAvailableCondition::TransformAvailableCondition(
   }
 
   RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node");
-}
-
-TransformAvailableCondition::~TransformAvailableCondition()
-{
-  RCLCPP_DEBUG(node_->get_logger(), "Shutting down TransformAvailableCondition BT node");
-}
 
-BT::NodeStatus TransformAvailableCondition::tick()
-{
   if (was_found_) {
     return BT::NodeStatus::SUCCESS;
   }
diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp
diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp
diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp

Original file line number	Diff line number	Diff line change
`@@ -36,7 +36,7 @@ void AssistedTeleopAction::on_tick()`
`36`	`36`
`37`	`37`	`// Populate the input message`
`38`	`38`	`goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);`
`39`		`-`
	`39`	`+`
`40`	`40`	`if (is_recovery_) {`
`41`	`41`	`increment_recovery_count();`
`42`	`42`	`}`
Original file line number	Diff line number	Diff line change
`@@ -43,7 +43,7 @@ void BackUpAction::on_tick()`
`43`	`43`	`goal_.target.z = 0.0;`
`44`	`44`	`goal_.speed = speed;`
`45`	`45`	`goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);`
`46`		`-`
	`46`	`+`
`47`	`47`	`increment_recovery_count();`
`48`	`48`	`}`
`49`	`49`