Add IsBatteryChargingCondition

nav2_behavior_tree/CMakeLists.txt

@@ -129,6 +129,9 @@ list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node)
 add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp)
 list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node)
 
+add_library(nav2_is_battery_charging_condition_bt_node SHARED plugins/condition/is_battery_charging_condition.cpp)
+list(APPEND plugin_libs nav2_is_battery_charging_condition_bt_node)
+
 add_library(nav2_is_battery_low_condition_bt_node SHARED plugins/condition/is_battery_low_condition.cpp)
 list(APPEND plugin_libs nav2_is_battery_low_condition_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp

@@ -0,0 +1,81 @@
+// Copyright (c) 2023 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_
+
+#include <string>
+#include <memory>
+#include <mutex>
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/battery_state.hpp"
+#include "behaviortree_cpp_v3/condition_node.h"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A BT::ConditionNode that listens to a battery topic and
+ * returns SUCCESS when battery is charging and FAILURE otherwise
+ */
+class IsBatteryChargingCondition : public BT::ConditionNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::IsBatteryChargingCondition
+   * @param condition_name Name for the XML tag for this node
+   * @param conf BT node configuration
+   */
+  IsBatteryChargingCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  IsBatteryChargingCondition() = delete;
+
+  /**
+   * @brief The main override required by a BT action
+   * @return BT::NodeStatus Status of tick execution
+   */
+  BT::NodeStatus tick() override;
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<std::string>(
+        "battery_topic", std::string("/battery_status"), "Battery topic")
+    };
+  }
+
+private:
+  /**
+   * @brief Callback function for battery topic
+   * @param msg Shared pointer to sensor_msgs::msg::BatteryState message
+   */
+  void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg);
+
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+  rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_;
+  std::string battery_topic_;
+  bool is_battery_charging_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -223,6 +223,10 @@
       <input_port name="is_voltage">Bool if check based on voltage or total %</input_port>
     </Condition>
 
+    <Condition ID="IsBatteryCharging">
+      <input_port name="battery_topic">Topic for battery info</input_port>
+    </Condition>
+
     <Condition ID="DistanceTraveled">
       <input_port name="distance">Distance to check if passed</input_port>
       <input_port name="global_frame">reference frame to check in</input_port>

nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp

@@ -0,0 +1,66 @@
+// Copyright (c) 2023 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+
+#include "nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+IsBatteryChargingCondition::IsBatteryChargingCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  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,
+    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(&IsBatteryChargingCondition::batteryCallback, this, std::placeholders::_1),
+    sub_option);
+}
+
+BT::NodeStatus IsBatteryChargingCondition::tick()
+{
+  callback_group_executor_.spin_some();
+  if (is_battery_charging_) {
+    return BT::NodeStatus::SUCCESS;
+  }
+  return BT::NodeStatus::FAILURE;
+}
+
+void IsBatteryChargingCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg)
+{
+  is_battery_charging_ =
+    (msg->power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING);
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsBatteryChargingCondition>("IsBatteryCharging");
+}

nav2_behavior_tree/test/plugins/condition/CMakeLists.txt

@@ -34,6 +34,10 @@ ament_add_gtest(test_condition_is_stuck test_is_stuck.cpp)
 target_link_libraries(test_condition_is_stuck nav2_is_stuck_condition_bt_node)
 ament_target_dependencies(test_condition_is_stuck ${dependencies})
 
+ament_add_gtest(test_condition_is_battery_charging test_is_battery_charging.cpp)
+target_link_libraries(test_condition_is_battery_charging nav2_is_battery_charging_condition_bt_node)
+ament_target_dependencies(test_condition_is_battery_charging ${dependencies})
+
 ament_add_gtest(test_condition_is_battery_low test_is_battery_low.cpp)
 target_link_libraries(test_condition_is_battery_low nav2_is_battery_low_condition_bt_node)
 ament_target_dependencies(test_condition_is_battery_low ${dependencies})

nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp

@@ -0,0 +1,130 @@
+// Copyright (c) 2023 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <set>
+#include <string>
+#include <chrono>
+
+#include "sensor_msgs/msg/battery_state.hpp"
+
+#include "utils/test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp"
+
+class IsBatteryChargingConditionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("test_is_battery_charging");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    config_ = new BT::NodeConfiguration();
+
+    // Create the blackboard that will be shared by all of the nodes in the tree
+    config_->blackboard = BT::Blackboard::create();
+    // Put items on the blackboard
+    config_->blackboard->set<rclcpp::Node::SharedPtr>(
+      "node",
+      node_);
+
+    factory_->registerNodeType<nav2_behavior_tree::IsBatteryChargingCondition>("IsBatteryCharging");
+
+    battery_pub_ = node_->create_publisher<sensor_msgs::msg::BatteryState>(
+      "/battery_status",
+      rclcpp::SystemDefaultsQoS());
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    battery_pub_.reset();
+    node_.reset();
+    factory_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_pub_;
+};
+
+rclcpp::Node::SharedPtr IsBatteryChargingConditionTestFixture::node_ = nullptr;
+BT::NodeConfiguration * IsBatteryChargingConditionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> IsBatteryChargingConditionTestFixture::factory_ = nullptr;
+rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr
+IsBatteryChargingConditionTestFixture::battery_pub_ = nullptr;
+
+TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <IsBatteryCharging battery_topic="/battery_status"/>
+        </BehaviorTree>
+      </root>)";
+
+  auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
+
+  sensor_msgs::msg::BatteryState battery_msg;
+  battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+  battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS);
+
+  battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+  battery_msg.power_supply_status =
+    sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+  battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL;
+  battery_pub_->publish(battery_msg);
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  rclcpp::spin_some(node_);
+  EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

nav2_bringup/params/nav2_multirobot_params_1.yaml

@@ -105,6 +105,7 @@ bt_navigator:
       - nav2_back_up_cancel_bt_node
       - nav2_assisted_teleop_cancel_bt_node
       - nav2_drive_on_heading_cancel_bt_node
+      - nav2_is_battery_charging_condition_bt_node
     error_code_names:
       - compute_path_error_code
       - follow_path_error_code

nav2_bringup/params/nav2_multirobot_params_2.yaml

@@ -105,6 +105,7 @@ bt_navigator:
       - nav2_back_up_cancel_bt_node
       - nav2_assisted_teleop_cancel_bt_node
       - nav2_drive_on_heading_cancel_bt_node
+      - nav2_is_battery_charging_condition_bt_node
     error_code_names:
       - compute_path_error_code
       - follow_path_error_code

nav2_bringup/params/nav2_params.yaml

@@ -105,6 +105,7 @@ bt_navigator:
       - nav2_back_up_cancel_bt_node
       - nav2_assisted_teleop_cancel_bt_node
       - nav2_drive_on_heading_cancel_bt_node
+      - nav2_is_battery_charging_condition_bt_node
     error_code_names:
       - compute_path_error_code
       - follow_path_error_code

nav2_bt_navigator/src/bt_navigator.cpp

@@ -85,7 +85,8 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options)
     "nav2_spin_cancel_bt_node",
     "nav2_assisted_teleop_cancel_bt_node",
     "nav2_back_up_cancel_bt_node",
-    "nav2_drive_on_heading_cancel_bt_node"
+    "nav2_drive_on_heading_cancel_bt_node",
+    "nav2_is_battery_charging_condition_bt_node"
   };
 
   declare_parameter("plugin_lib_names", plugin_libs);