Add IsBatteryLow condition node (#1974)

* Add IsBatteryLow condition node

* Update default battery topic and switch to battery %

* Fix test

* Switch to sensor_msgs/BatteryState

* Add option to use voltage by default or switch to percentage

* Add sensor_msgs dependency in package.xml

* Make percentage default over voltage

* Update parameter list

doc/parameters/param_list.md

@@ -667,6 +667,14 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 
 ## Conditions
 
+### BT Node DistanceTraveled
+
+| Input Port | Default | Description |
+| ---------- | ------- | ----------- |
+| distance | 1.0 | Distance in meters after which the node should return success |
+| global_frame | "map" | Reference frame |
+| robot_base_frame | "base_link" | Robot base frame |
+
 ### BT Node GoalReached
 
 | Input Port | Default | Description |
@@ -675,7 +683,21 @@ When `recovery_plugins` parameter is not overridden, the following default plugi
 | global_frame | "map" | Reference frame |
 | robot_base_frame | "base_link" | Robot base frame |
 
-### BT Node TransformAvailable (condition)
+### BT Node IsBatteryLow
+
+| Input Port | Default | Description |
+| ---------- | ------- | ----------- |
+| min_battery | N/A | Minimum battery percentage/voltage |
+| battery_topic | "/battery_status" | Battery topic |
+| is_voltage | false | If true voltage will be used to check for low battery |
+
+### BT Node TimeExpired
+
+| Input Port | Default | Description |
+| ---------- | ------- | ----------- |
+| seconds | 1.0 | Number of seconds after which node returns success |
+
+### BT Node TransformAvailable
 
 | Input Port | Default | Description |
 | ---------- | ------- | ----------- |

nav2_behavior_tree/CMakeLists.txt

@@ -6,9 +6,9 @@ find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
-find_package(std_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(behaviortree_cpp_v3 REQUIRED)
@@ -31,6 +31,7 @@ set(dependencies
   rclcpp_action
   rclcpp_lifecycle
   geometry_msgs
+  sensor_msgs
   nav2_msgs
   nav_msgs
   behaviortree_cpp_v3
@@ -89,6 +90,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_low_condition_bt_node SHARED plugins/condition/is_battery_low_condition.cpp)
+list(APPEND plugin_libs nav2_is_battery_low_condition_bt_node)
+
 add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp)
 list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node)
 

nav2_behavior_tree/README.md

@@ -72,6 +72,7 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 | IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. |
 | TransformAvailable | Condition | Checks if a TF transform is available. Returns failure if it cannot be found. Once found, it will always return success. Useful for initial condition checks. |
 | GoalUpdated | Condition | Checks if the global navigation goal has changed in the blackboard. Returns failure if the goal is the same, if it changes, it returns success. |
+| IsBatteryLow | Condition | Checks if battery is low by subscribing to a sensor_msgs/BatteryState topic and checking if battery voltage/percentage is below a specified minimum value. |
 | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. |
 | RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. |
 | DistanceController | Decorator | A node that controls the tick rate for its child based on the distance traveled. The distance to be traveled before replanning can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `DistanceController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. |

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp

+// Copyright (c) 2020 Sarthak Mittal
+// Copyright (c) 2019 Intel Corporation
+//
+// 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_LOW_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_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
+{
+
+class IsBatteryLowCondition : public BT::ConditionNode
+{
+public:
+  IsBatteryLowCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  IsBatteryLowCondition() = delete;
+
+  BT::NodeStatus tick() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<double>("min_battery", "Minimum battery percentage/voltage"),
+      BT::InputPort<std::string>(
+        "battery_topic", std::string("/battery_status"), "Battery topic"),
+      BT::InputPort<bool>(
+        "is_voltage", false, "If true voltage will be used to check for low battery"),
+    };
+  }
+
+private:
+  void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg);
+
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_;
+  std::string battery_topic_;
+  double min_battery_;
+  bool is_voltage_;
+  bool is_battery_low_;
+  std::mutex mutex_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_

nav2_behavior_tree/package.xml

@@ -17,10 +17,10 @@
   <build_depend>rclcpp</build_depend>
   <build_depend>rclcpp_action</build_depend>
   <build_depend>rclcpp_lifecycle</build_depend>
-  <build_depend>std_msgs</build_depend>
   <build_depend>behaviortree_cpp_v3</build_depend>
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
   <build_depend>nav2_msgs</build_depend>
   <build_depend>nav_msgs</build_depend>
   <build_depend>tf2</build_depend>
@@ -39,6 +39,7 @@
   <exec_depend>behaviortree_cpp_v3</exec_depend>
   <exec_depend>builtin_interfaces</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>tf2</exec_depend>

nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp

+// Copyright (c) 2020 Sarthak Mittal
+// Copyright (c) 2019 Intel Corporation
+//
+// 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_low_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+IsBatteryLowCondition::IsBatteryLowCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  battery_topic_("/battery_status"),
+  min_battery_(0.0),
+  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");
+  battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
+    battery_topic_,
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1));
+}
+
+BT::NodeStatus IsBatteryLowCondition::tick()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (is_battery_low_) {
+    return BT::NodeStatus::SUCCESS;
+  }
+  return BT::NodeStatus::FAILURE;
+}
+
+void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (is_voltage_) {
+    is_battery_low_ = msg->voltage <= min_battery_;
+  } else {
+    is_battery_low_ = msg->percentage <= min_battery_;
+  }
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsBatteryLowCondition>("IsBatteryLow");
+}

nav2_behavior_tree/test/plugins/condition/CMakeLists.txt

@@ -25,3 +25,7 @@ ament_target_dependencies(test_condition_transform_available ${dependencies})
 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_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_low.cpp

+// Copyright (c) 2020 Sarthak Mittal
+// Copyright (c) 2019 Intel Corporation
+//
+// 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 "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp"
+
+class IsBatteryLowConditionTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("test_is_battery_low");
+    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::IsBatteryLowCondition>("IsBatteryLow");
+
+    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 IsBatteryLowConditionTestFixture::node_ = nullptr;
+BT::NodeConfiguration * IsBatteryLowConditionTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> IsBatteryLowConditionTestFixture::factory_ = nullptr;
+rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr
+IsBatteryLowConditionTestFixture::battery_pub_ = nullptr;
+
+TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <IsBatteryLow min_battery="0.5" battery_topic="/battery_status"/>
+        </BehaviorTree>
+      </root>)";
+
+  auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
+
+  sensor_msgs::msg::BatteryState battery_msg;
+  battery_msg.percentage = 1.0;
+  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.percentage = 0.49;
+  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.percentage = 0.51;
+  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.percentage = 0.0;
+  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);
+}
+
+TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <IsBatteryLow min_battery="5.0" battery_topic="/battery_status" is_voltage="true"/>
+        </BehaviorTree>
+      </root>)";
+
+  auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
+
+  sensor_msgs::msg::BatteryState battery_msg;
+  battery_msg.voltage = 10.0;
+  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.voltage = 4.9;
+  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.voltage = 5.1;
+  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.voltage = 0.0;
+  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);
+}
+
+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;
+}