From 11c13af1db3dbbb09c116509eb5f1c2c2620d1ca Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 3 Sep 2020 06:39:38 +0530 Subject: [PATCH 1/8] Add IsBatteryLow condition node --- nav2_behavior_tree/CMakeLists.txt | 3 + nav2_behavior_tree/README.md | 1 + .../condition/is_battery_low_condition.hpp | 61 +++++++++ .../condition/is_battery_low_condition.cpp | 59 +++++++++ .../test/plugins/condition/CMakeLists.txt | 4 + .../plugins/condition/test_is_battery_low.cpp | 119 ++++++++++++++++++ 6 files changed, 247 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp create mode 100644 nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp create mode 100644 nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index b062c777973..b8ef079e9e3 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -89,6 +89,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) diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 2917bed7639..5c949cd2b61 100644 --- a/nav2_behavior_tree/README.md +++ b/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 battery voltage topic and checking if voltage 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. | diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp new file mode 100644 index 00000000000..a5c5305b8d3 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -0,0 +1,61 @@ +// 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64.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("min_battery", "Minimum battery voltage"), + BT::InputPort( + "battery_topic", std::string("/battery"), "Battery topic") + }; + } + +private: + void batteryCallback(std_msgs::msg::Float64::SharedPtr msg); + + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr battery_sub_; + std::string battery_topic_; + double min_battery_; + bool is_battery_low_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_ diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp new file mode 100644 index 00000000000..abd9b194b1f --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -0,0 +1,59 @@ +// 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 + +#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"), + min_battery_(0.0), + is_battery_low_(false) +{ + getInput("min_battery", min_battery_); + getInput("battery_topic", battery_topic_); + node_ = config().blackboard->get("node"); + battery_sub_ = node_->create_subscription( + battery_topic_, + rclcpp::SystemDefaultsQoS(), + std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1)); +} + +BT::NodeStatus IsBatteryLowCondition::tick() +{ + if (is_battery_low_) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + +void IsBatteryLowCondition::batteryCallback(std_msgs::msg::Float64::SharedPtr msg) +{ + is_battery_low_ = msg->data <= min_battery_; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsBatteryLow"); +} diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index e8d9d8d3d51..b1350f74537 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/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}) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp new file mode 100644 index 00000000000..ff2aa7f028b --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -0,0 +1,119 @@ +// 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 +#include +#include +#include + +#include "std_msgs/msg/float64.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("test_is_battery_low"); + factory_ = std::make_shared(); + + 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( + "node", + node_); + + factory_->registerNodeType("IsBatteryLow"); + + battery_pub_ = node_->create_publisher( + "/battery", + 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 factory_; + static rclcpp::Publisher::SharedPtr battery_pub_; +}; + +rclcpp::Node::SharedPtr IsBatteryLowConditionTestFixture::node_ = nullptr; +BT::NodeConfiguration * IsBatteryLowConditionTestFixture::config_ = nullptr; +std::shared_ptr IsBatteryLowConditionTestFixture::factory_ = nullptr; +rclcpp::Publisher::SharedPtr +IsBatteryLowConditionTestFixture::battery_pub_ = nullptr; + +TEST_F(IsBatteryLowConditionTestFixture, test_behavior) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + std_msgs::msg::Float64 battery_msg; + battery_msg.data = 5.0; + battery_pub_->publish(battery_msg); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + + battery_msg.data = 3.0; + battery_pub_->publish(battery_msg); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + + battery_msg.data = 2.0; + battery_pub_->publish(battery_msg); + rclcpp::spin_some(node_); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + + battery_msg.data = 4.0; + battery_pub_->publish(battery_msg); + 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; +} From c76a338f99843054cfaa4d7519f38902544b4e8b Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 3 Sep 2020 07:17:21 +0530 Subject: [PATCH 2/8] Update default battery topic and switch to battery % --- nav2_behavior_tree/README.md | 2 +- .../plugins/condition/is_battery_low_condition.hpp | 4 ++-- .../plugins/condition/is_battery_low_condition.cpp | 2 +- .../test/plugins/condition/test_is_battery_low.cpp | 12 ++++++------ 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 5c949cd2b61..b4de51ffdd2 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -72,7 +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 battery voltage topic and checking if voltage is below a specified minimum value. | +| IsBatteryLow | Condition | Checks if battery is low by subscribing to battery status topic and checking if battery 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. | diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index a5c5305b8d3..66810199d9e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -40,9 +40,9 @@ class IsBatteryLowCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("min_battery", "Minimum battery voltage"), + BT::InputPort("min_battery", "Minimum battery percentage"), BT::InputPort( - "battery_topic", std::string("/battery"), "Battery topic") + "battery_topic", std::string("/battery_status"), "Battery topic") }; } 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 abd9b194b1f..9253c3f331d 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -24,7 +24,7 @@ IsBatteryLowCondition::IsBatteryLowCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - battery_topic_("/battery"), + battery_topic_("/battery_status"), min_battery_(0.0), is_battery_low_(false) { diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index ff2aa7f028b..15f91d18a8a 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -43,7 +43,7 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test factory_->registerNodeType("IsBatteryLow"); battery_pub_ = node_->create_publisher( - "/battery", + "/battery_status", rclcpp::SystemDefaultsQoS()); } @@ -75,29 +75,29 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior) R"( - + )"; auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); std_msgs::msg::Float64 battery_msg; - battery_msg.data = 5.0; + battery_msg.data = 100.0; battery_pub_->publish(battery_msg); rclcpp::spin_some(node_); EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); - battery_msg.data = 3.0; + battery_msg.data = 50.0; battery_pub_->publish(battery_msg); rclcpp::spin_some(node_); EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); - battery_msg.data = 2.0; + battery_msg.data = 51.0; battery_pub_->publish(battery_msg); rclcpp::spin_some(node_); EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); - battery_msg.data = 4.0; + battery_msg.data = 0.0; battery_pub_->publish(battery_msg); rclcpp::spin_some(node_); EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); From d26ab10106e38aca7ab068e18b274cd7dd212c15 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 3 Sep 2020 07:28:13 +0530 Subject: [PATCH 3/8] Fix test --- .../test/plugins/condition/test_is_battery_low.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index 15f91d18a8a..0976f4da31d 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -95,12 +95,12 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior) battery_msg.data = 51.0; battery_pub_->publish(battery_msg); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); battery_msg.data = 0.0; battery_pub_->publish(battery_msg); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) From c7fd70427c7822aa377c5ebf98caa1cbabb82bb7 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 3 Sep 2020 08:13:47 +0530 Subject: [PATCH 4/8] Switch to sensor_msgs/BatteryState --- nav2_behavior_tree/CMakeLists.txt | 3 ++- nav2_behavior_tree/README.md | 2 +- .../condition/is_battery_low_condition.hpp | 10 +++++--- .../condition/is_battery_low_condition.cpp | 8 +++--- .../plugins/condition/test_is_battery_low.cpp | 25 +++++++++++-------- 5 files changed, 29 insertions(+), 19 deletions(-) diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index b8ef079e9e3..4d030a55616 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -9,12 +9,12 @@ 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) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(nav2_util REQUIRED) @@ -31,6 +31,7 @@ set(dependencies rclcpp_action rclcpp_lifecycle geometry_msgs + sensor_msgs nav2_msgs nav_msgs behaviortree_cpp_v3 diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index b4de51ffdd2..48a8bf550ee 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -72,7 +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 battery status topic and checking if battery percentage is below a specified minimum value. | +| IsBatteryLow | Condition | Checks if battery is low by subscribing to a sensor_msgs/BatteryState topic and checking if battery 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. | diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 66810199d9e..9865509bd28 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -18,9 +18,10 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/float64.hpp" +#include "sensor_msgs/msg/battery_state.hpp" #include "behaviortree_cpp_v3/condition_node.h" namespace nav2_behavior_tree @@ -40,20 +41,21 @@ class IsBatteryLowCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("min_battery", "Minimum battery percentage"), + BT::InputPort("min_battery", "Minimum battery percentage on 0 to 1 range"), BT::InputPort( "battery_topic", std::string("/battery_status"), "Battery topic") }; } private: - void batteryCallback(std_msgs::msg::Float64::SharedPtr msg); + void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg); rclcpp::Node::SharedPtr node_; - rclcpp::Subscription::SharedPtr battery_sub_; + rclcpp::Subscription::SharedPtr battery_sub_; std::string battery_topic_; double min_battery_; bool is_battery_low_; + std::mutex mutex_; }; } // namespace nav2_behavior_tree 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 9253c3f331d..d37c6e82186 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -31,7 +31,7 @@ IsBatteryLowCondition::IsBatteryLowCondition( getInput("min_battery", min_battery_); getInput("battery_topic", battery_topic_); node_ = config().blackboard->get("node"); - battery_sub_ = node_->create_subscription( + battery_sub_ = node_->create_subscription( battery_topic_, rclcpp::SystemDefaultsQoS(), std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1)); @@ -39,15 +39,17 @@ IsBatteryLowCondition::IsBatteryLowCondition( BT::NodeStatus IsBatteryLowCondition::tick() { + std::lock_guard lock(mutex_); if (is_battery_low_) { return BT::NodeStatus::SUCCESS; } return BT::NodeStatus::FAILURE; } -void IsBatteryLowCondition::batteryCallback(std_msgs::msg::Float64::SharedPtr msg) +void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg) { - is_battery_low_ = msg->data <= min_battery_; + std::lock_guard lock(mutex_); + is_battery_low_ = msg->percentage <= min_battery_; } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index 0976f4da31d..f4c27cf7ea2 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -17,8 +17,9 @@ #include #include #include +#include -#include "std_msgs/msg/float64.hpp" +#include "sensor_msgs/msg/battery_state.hpp" #include "../../test_behavior_tree_fixture.hpp" #include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp" @@ -42,7 +43,7 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test factory_->registerNodeType("IsBatteryLow"); - battery_pub_ = node_->create_publisher( + battery_pub_ = node_->create_publisher( "/battery_status", rclcpp::SystemDefaultsQoS()); } @@ -60,13 +61,13 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test static rclcpp::Node::SharedPtr node_; static BT::NodeConfiguration * config_; static std::shared_ptr factory_; - static rclcpp::Publisher::SharedPtr battery_pub_; + static rclcpp::Publisher::SharedPtr battery_pub_; }; rclcpp::Node::SharedPtr IsBatteryLowConditionTestFixture::node_ = nullptr; BT::NodeConfiguration * IsBatteryLowConditionTestFixture::config_ = nullptr; std::shared_ptr IsBatteryLowConditionTestFixture::factory_ = nullptr; -rclcpp::Publisher::SharedPtr +rclcpp::Publisher::SharedPtr IsBatteryLowConditionTestFixture::battery_pub_ = nullptr; TEST_F(IsBatteryLowConditionTestFixture, test_behavior) @@ -75,30 +76,34 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior) R"( - + )"; auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); - std_msgs::msg::Float64 battery_msg; - battery_msg.data = 100.0; + 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.data = 50.0; + 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.data = 51.0; + 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.data = 0.0; + 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); } From 384bd5e34f006b31078e1772d5a8661ad9d52e5d Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 3 Sep 2020 08:31:25 +0530 Subject: [PATCH 5/8] Add option to use voltage by default or switch to percentage --- nav2_behavior_tree/README.md | 2 +- .../condition/is_battery_low_condition.hpp | 7 +++- .../condition/is_battery_low_condition.cpp | 8 +++- .../plugins/condition/test_is_battery_low.cpp | 40 ++++++++++++++++++- 4 files changed, 52 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 48a8bf550ee..2a9cfac472b 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -72,7 +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 percentage is below a specified minimum value. | +| 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. | diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 9865509bd28..4aa2cf79f76 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -41,9 +41,11 @@ class IsBatteryLowCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("min_battery", "Minimum battery percentage on 0 to 1 range"), + BT::InputPort("min_battery", "Minimum battery voltage/percentage"), BT::InputPort( - "battery_topic", std::string("/battery_status"), "Battery topic") + "battery_topic", std::string("/battery_status"), "Battery topic"), + BT::InputPort( + "is_percentage", false, "If specified minimum battery value is a percentage"), }; } @@ -54,6 +56,7 @@ class IsBatteryLowCondition : public BT::ConditionNode rclcpp::Subscription::SharedPtr battery_sub_; std::string battery_topic_; double min_battery_; + bool is_percentage_; bool is_battery_low_; std::mutex mutex_; }; 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 d37c6e82186..815f9fd9bd9 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -26,10 +26,12 @@ IsBatteryLowCondition::IsBatteryLowCondition( : BT::ConditionNode(condition_name, conf), battery_topic_("/battery_status"), min_battery_(0.0), + is_percentage_(false), is_battery_low_(false) { getInput("min_battery", min_battery_); getInput("battery_topic", battery_topic_); + getInput("is_percentage", is_percentage_); node_ = config().blackboard->get("node"); battery_sub_ = node_->create_subscription( battery_topic_, @@ -49,7 +51,11 @@ BT::NodeStatus IsBatteryLowCondition::tick() void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg) { std::lock_guard lock(mutex_); - is_battery_low_ = msg->percentage <= min_battery_; + if (is_percentage_) { + is_battery_low_ = msg->percentage <= min_battery_; + } else { + is_battery_low_ = msg->voltage <= min_battery_; + } } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index f4c27cf7ea2..14f926773a7 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -70,7 +70,7 @@ std::shared_ptr IsBatteryLowConditionTestFixture::facto rclcpp::Publisher::SharedPtr IsBatteryLowConditionTestFixture::battery_pub_ = nullptr; -TEST_F(IsBatteryLowConditionTestFixture, test_behavior) +TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) { std::string xml_txt = R"( @@ -82,6 +82,44 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior) auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + sensor_msgs::msg::BatteryState battery_msg; + battery_msg.voltage = 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.voltage = 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.voltage = 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.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); +} + +TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + sensor_msgs::msg::BatteryState battery_msg; battery_msg.percentage = 1.0; battery_pub_->publish(battery_msg); From 636f2917ba5c45918032d101e75e52316ab91024 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Thu, 3 Sep 2020 10:05:48 +0530 Subject: [PATCH 6/8] Add sensor_msgs dependency in package.xml --- nav2_behavior_tree/CMakeLists.txt | 2 +- nav2_behavior_tree/package.xml | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 4d030a55616..c92b06228fb 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -6,7 +6,6 @@ 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) @@ -15,6 +14,7 @@ find_package(nav_msgs REQUIRED) find_package(behaviortree_cpp_v3 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(nav2_util REQUIRED) diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index a012a86e20e..e3600947cc8 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -17,10 +17,10 @@ rclcpp rclcpp_action rclcpp_lifecycle - std_msgs behaviortree_cpp_v3 builtin_interfaces geometry_msgs + sensor_msgs nav2_msgs nav_msgs tf2 @@ -39,6 +39,7 @@ behaviortree_cpp_v3 builtin_interfaces geometry_msgs + sensor_msgs nav2_msgs nav_msgs tf2 From 7c103802f31a9498251e5b9a781b15a8369bdf6f Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Fri, 4 Sep 2020 05:36:09 +0530 Subject: [PATCH 7/8] Make percentage default over voltage --- .../condition/is_battery_low_condition.hpp | 6 ++--- .../condition/is_battery_low_condition.cpp | 10 ++++----- .../plugins/condition/test_is_battery_low.cpp | 22 +++++++++---------- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 4aa2cf79f76..0bc30cd5635 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -41,11 +41,11 @@ class IsBatteryLowCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("min_battery", "Minimum battery voltage/percentage"), + BT::InputPort("min_battery", "Minimum battery percentage/voltage"), BT::InputPort( "battery_topic", std::string("/battery_status"), "Battery topic"), BT::InputPort( - "is_percentage", false, "If specified minimum battery value is a percentage"), + "is_voltage", false, "If true voltage will be used to check for low battery"), }; } @@ -56,7 +56,7 @@ class IsBatteryLowCondition : public BT::ConditionNode rclcpp::Subscription::SharedPtr battery_sub_; std::string battery_topic_; double min_battery_; - bool is_percentage_; + bool is_voltage_; bool is_battery_low_; std::mutex mutex_; }; 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 815f9fd9bd9..4aad6d16769 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -26,12 +26,12 @@ IsBatteryLowCondition::IsBatteryLowCondition( : BT::ConditionNode(condition_name, conf), battery_topic_("/battery_status"), min_battery_(0.0), - is_percentage_(false), + is_voltage_(false), is_battery_low_(false) { getInput("min_battery", min_battery_); getInput("battery_topic", battery_topic_); - getInput("is_percentage", is_percentage_); + getInput("is_voltage", is_voltage_); node_ = config().blackboard->get("node"); battery_sub_ = node_->create_subscription( battery_topic_, @@ -51,10 +51,10 @@ BT::NodeStatus IsBatteryLowCondition::tick() void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg) { std::lock_guard lock(mutex_); - if (is_percentage_) { - is_battery_low_ = msg->percentage <= min_battery_; - } else { + if (is_voltage_) { is_battery_low_ = msg->voltage <= min_battery_; + } else { + is_battery_low_ = msg->percentage <= min_battery_; } } diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index 14f926773a7..992c50297de 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -70,7 +70,7 @@ std::shared_ptr IsBatteryLowConditionTestFixture::facto rclcpp::Publisher::SharedPtr IsBatteryLowConditionTestFixture::battery_pub_ = nullptr; -TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) +TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) { std::string xml_txt = R"( @@ -83,63 +83,63 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); sensor_msgs::msg::BatteryState battery_msg; - battery_msg.voltage = 1.0; + 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.voltage = 0.49; + 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.voltage = 0.51; + 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.voltage = 0.0; + 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_percentage) +TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) { std::string xml_txt = R"( - + )"; auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); sensor_msgs::msg::BatteryState battery_msg; - battery_msg.percentage = 1.0; + 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.percentage = 0.49; + 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.percentage = 0.51; + 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.percentage = 0.0; + battery_msg.voltage = 0.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); From d71ae1709e0f69f55fcfdf1185fca4a232109f7c Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 5 Sep 2020 12:28:33 +0530 Subject: [PATCH 8/8] Update parameter list --- doc/parameters/param_list.md | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 4fa9a78c97b..b0dfd7957c7 100644 --- a/doc/parameters/param_list.md +++ b/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 | | ---------- | ------- | ----------- |