forked from ros-navigation/navigation2
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add IsBatteryLow condition node (ros-navigation#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
- Loading branch information
Showing
8 changed files
with
330 additions
and
3 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
66 changes: 66 additions & 0 deletions
66
nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,66 @@ | ||
| // 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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
67 changes: 67 additions & 0 deletions
67
nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,67 @@ | ||
| // 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"); | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.