Fix battery sub creation bug

ros-navigation · Nov 11, 2024 · a14aad1 · a14aad1
1 parent ff526cb
commit a14aad1
Showing 1 changed file with 20 additions and 13 deletions.
diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp
@@ -34,21 +34,28 @@ IsBatteryLowCondition::IsBatteryLowCondition(
 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);
+  // 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()