diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index efeb33c6402..eb0a8be4301 100644 --- 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("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( - 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("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( + battery_topic_, + rclcpp::SystemDefaultsQoS(), + std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), + sub_option); + } } BT::NodeStatus IsBatteryLowCondition::tick()