-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Foxy sync 4 - SMAC planner, amongst others (#2072)
* initialize variables in inflation layer (#1970) * Fix zero waypoints crash (#1978) * return if the number of waypoints is zero. * terminate the action. * Succeed action instead of terminating. * 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 * Initialize inflate_cone_ variable. (#1988) * Initialize inflate_cone_ variable. * initialize inflate_cone_ based on parameter. * Increase the sleep time in the tests makes the costmap test always succeed on my machine. * Add timeouts to all spin_until_future_complete calls (#1998) * Add timeouts to all spin_until_future_complete calls Signed-off-by: Sarthak Mittal <[email protected]> * Update default timeout value Signed-off-by: Sarthak Mittal <[email protected]> * Controllers should not be influenced by time jumps or slew (#2012) * Controllers should not be influenced by time jumps Therefore use rclcpp::GenericRate<std::chrono::steady_clock> instead of rclcpp::Rate Signed-off-by: Martijn Buijs <[email protected]> * Change to using `rclcpp::WallRate` for better readability Signed-off-by: Martijn Buijs <[email protected]> * Fix max path cycles for case where map has larger Y dimension than X dimension (#2017) * Fix max path cycles for case where map has larger Y dimension than X dimension * Improve readability * fix ament_cpplint and ament_uncrustify issues * fix minor cherry pick conflict mistake * bump version to 0.4.4 Co-authored-by: Michael Ferguson <[email protected]> Co-authored-by: Wilco Bonestroo <[email protected]> Co-authored-by: Sarthak Mittal <[email protected]> Co-authored-by: Martijn Buijs <[email protected]> Co-authored-by: justinIRBT <[email protected]>
- Loading branch information
6 people
committed
Nov 4, 2020
1 parent
603150b
commit 96b4713
Showing
51 changed files
with
434 additions
and
74 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
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -2,7 +2,7 @@ | |
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>nav2_behavior_tree</name> | ||
| <version>0.4.3</version> | ||
| <version>0.4.4</version> | ||
| <description>TODO</description> | ||
| <maintainer email="[email protected]">Michael Jeronimo</maintainer> | ||
| <maintainer email="[email protected]">Carlos Orduno</maintainer> | ||
|
|
@@ -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> | ||
|
|
||
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.