diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 30955681deb..3aaec265445 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -95,6 +95,9 @@ list(APPEND plugin_libs nav2_clear_costmap_service_bt_node) add_library(nav2_is_stuck_condition_bt_node SHARED plugins/condition/is_stuck_condition.cpp) list(APPEND plugin_libs nav2_is_stuck_condition_bt_node) +add_library(nav2_is_stopped_condition_bt_node SHARED plugins/condition/is_stopped_condition.cpp) +list(APPEND plugin_libs nav2_is_stopped_condition_bt_node) + add_library(nav2_transform_available_condition_bt_node SHARED plugins/condition/transform_available_condition.cpp) list(APPEND plugin_libs nav2_transform_available_condition_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp new file mode 100644 index 00000000000..40197939d1f --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -0,0 +1,90 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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_STOPPED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/condition_node.h" +#include "nav_msgs/msg/odometry.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" + + +using namespace std::chrono_literals; // NOLINT + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that tracks robot odometry and returns SUCCESS + * if robot is considered stopped for long enough, RUNNING if stopped but not for long enough and FAILURE otherwise + */ +class IsStoppedCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::IsStoppedCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + IsStoppedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsStoppedCondition() = delete; + + /** + * @brief A destructor for nav2_behavior_tree::IsStoppedCondition + */ + ~IsStoppedCondition() override; + + /** + * @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("velocity_threshold", 0.01, + "Velocity threshold below which robot is considered stopped"), + BT::InputPort("duration_stopped", 1000ms, + "Duration (ms) the velocity must remain below the threshold"), + }; + } + +private: + rclcpp::Node::SharedPtr node_; + + double velocity_threshold_; + std::chrono::milliseconds duration_stopped_; + rclcpp::Time stopped_stamp_; + + std::shared_ptr odom_smoother_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 71c20ee4ecc..67059def0eb 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -227,6 +227,11 @@ + + Velocity threshold below which robot is considered stopped + Duration (ms) the velocity must remain below the threshold + + Child frame for transform Parent frame for transform diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp new file mode 100644 index 00000000000..f0b32d4c1c0 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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 "nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp" + +namespace nav2_behavior_tree +{ + +IsStoppedCondition::IsStoppedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + velocity_threshold_(0.01), + duration_stopped_(1000ms), + stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) +{ + node_ = config().blackboard->get("node"); + odom_smoother_ = config().blackboard->get>( + "odom_smoother"); +} + +IsStoppedCondition::~IsStoppedCondition() +{ + RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStoppedCondition BT node"); +} + +BT::NodeStatus IsStoppedCondition::tick() +{ + getInput("velocity_threshold", velocity_threshold_); + getInput("duration_stopped", duration_stopped_); + + auto twist = odom_smoother_->getRawTwistStamped(); + + // if there is no timestamp, set it to now + if (twist.header.stamp.sec == 0 && twist.header.stamp.nanosec == 0) { + twist.header.stamp = node_->get_clock()->now(); + } + + if (abs(twist.twist.linear.x) < velocity_threshold_ && + abs(twist.twist.linear.y) < velocity_threshold_ && + abs(twist.twist.angular.z) < velocity_threshold_) + { + if (stopped_stamp_ == rclcpp::Time(0, 0, RCL_ROS_TIME)) { + stopped_stamp_ = rclcpp::Time(twist.header.stamp); + } + + if (node_->get_clock()->now() - stopped_stamp_ > rclcpp::Duration(duration_stopped_)) { + stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + return BT::NodeStatus::SUCCESS; + } else { + return BT::NodeStatus::RUNNING; + } + + } else { + stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + return BT::NodeStatus::FAILURE; + } +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsStopped"); +} diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index 87c9eb5bf16..f5b4eb3bd86 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -50,8 +50,6 @@ SpeedController::SpeedController( d_rate_ = max_rate_ - min_rate_; d_speed_ = max_speed_ - min_speed_; - std::string odom_topic; - node_->get_parameter_or("odom_topic", odom_topic, std::string("odom")); odom_smoother_ = config().blackboard->get>( "odom_smoother"); } diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index de380c769af..106c4a96f27 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -16,6 +16,8 @@ plugin_add_test(test_condition_transform_available test_transform_available.cpp plugin_add_test(test_condition_is_stuck test_is_stuck.cpp nav2_is_stuck_condition_bt_node) +plugin_add_test(test_condition_is_stopped test_is_stopped.cpp nav2_is_stopped_condition_bt_node) + plugin_add_test(test_condition_is_battery_charging test_is_battery_charging.cpp nav2_is_battery_charging_condition_bt_node) plugin_add_test(test_condition_is_battery_low test_is_battery_low.cpp nav2_is_battery_low_condition_bt_node) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp new file mode 100644 index 00000000000..26ae239a180 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp @@ -0,0 +1,123 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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 "rclcpp/rclcpp.hpp" +#include "nav2_util/odometry_utils.hpp" + +#include "utils/test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class IsStoppedTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + odom_smoother_ = std::make_shared(node_); + config_->blackboard->set( + "odom_smoother", odom_smoother_); // NOLINT + // shorten duration_stopped from default to make the test faster + std::chrono::milliseconds duration = 100ms; + config_->input_ports["duration_stopped"] = std::to_string(duration.count()) + "ms"; + bt_node_ = std::make_shared("is_stopped", *config_); + } + + void TearDown() + { + bt_node_.reset(); + odom_smoother_.reset(); + } + +protected: + static std::shared_ptr bt_node_; + static std::shared_ptr odom_smoother_; +}; + +std::shared_ptr +IsStoppedTestFixture::bt_node_ = nullptr; +std::shared_ptr +IsStoppedTestFixture::odom_smoother_ = nullptr; + + +TEST_F(IsStoppedTestFixture, test_behavior) +{ + auto odom_pub = node_->create_publisher("odom", + rclcpp::SystemDefaultsQoS()); + nav_msgs::msg::Odometry odom_msg; + + // Test FAILURE when robot is moving + auto time = node_->now(); + odom_msg.header.stamp = time; + odom_msg.twist.twist.linear.x = 1.0; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + // Test RUNNING when robot is stopped but not long enough + odom_msg.header.stamp = node_->now(); + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(90ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + // Test SUCCESS when robot is stopped for long enough + std::this_thread::sleep_for(20ms); // 20ms + 90ms = 110ms + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + + // Test FAILURE immediately after robot starts moving + odom_msg.header.stamp = node_->now(); + odom_msg.twist.twist.angular.z = 0.1; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + // Test SUCCESS when robot is stopped for long with a back-dated timestamp + odom_msg.header.stamp = node_->now() - rclcpp::Duration(2, 0); + odom_msg.twist.twist.angular.z = 0; + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); // wait just enough for the message to be received + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + + // Test SUCCESS when robot is stopped for long enough without a stamp for odometry + odom_msg.header.stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); + // In the first tick, the timestamp is set + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + std::this_thread::sleep_for(110ms); + EXPECT_EQ(bt_node_->executeTick(), 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; +} diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp index 185a86d1471..ab9132b5ffe 100644 --- a/nav2_util/include/nav2_util/odometry_utils.hpp +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -68,13 +68,67 @@ class OdomSmoother * @brief Get twist msg from smoother * @return twist Twist msg */ - inline geometry_msgs::msg::Twist getTwist() {return vel_smooth_.twist;} + inline geometry_msgs::msg::Twist getTwist() + { + std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::Twist twist; + return twist; + } + return vel_smooth_.twist; + } /** * @brief Get twist stamped msg from smoother * @return twist TwistStamped msg */ - inline geometry_msgs::msg::TwistStamped getTwistStamped() {return vel_smooth_;} + inline geometry_msgs::msg::TwistStamped getTwistStamped() + { + std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::TwistStamped twist_stamped; + return twist_stamped; + } + return vel_smooth_; + } + + /** + * @brief Get raw twist msg from smoother (without smoothing) + * @return twist Twist msg + */ + inline geometry_msgs::msg::Twist getRawTwist() + { + std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::Twist twist; + return twist; + } + return odom_history_.back().twist.twist; + } + + /** + * @brief Get raw twist stamped msg from smoother (without smoothing) + * @return twist TwistStamped msg + */ + inline geometry_msgs::msg::TwistStamped getRawTwistStamped() + { + std::lock_guard lock(odom_mutex_); + geometry_msgs::msg::TwistStamped twist_stamped; + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + return twist_stamped; + } + twist_stamped.header = odom_history_.back().header; + twist_stamped.twist = odom_history_.back().twist.twist; + return twist_stamped; + } protected: /** @@ -88,6 +142,7 @@ class OdomSmoother */ void updateState(); + bool received_odom_; rclcpp::Subscription::SharedPtr odom_sub_; nav_msgs::msg::Odometry odom_cumulate_; geometry_msgs::msg::TwistStamped vel_smooth_; diff --git a/nav2_util/src/odometry_utils.cpp b/nav2_util/src/odometry_utils.cpp index 9bf585bdfe2..6b50c332bcc 100644 --- a/nav2_util/src/odometry_utils.cpp +++ b/nav2_util/src/odometry_utils.cpp @@ -27,7 +27,7 @@ OdomSmoother::OdomSmoother( const rclcpp::Node::WeakPtr & parent, double filter_duration, const std::string & odom_topic) -: odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) +: received_odom_(false), odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) { auto node = parent.lock(); odom_sub_ = node->create_subscription( @@ -66,6 +66,7 @@ OdomSmoother::OdomSmoother( void OdomSmoother::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { std::lock_guard lock(odom_mutex_); + received_odom_ = true; // update cumulated odom only if history is not empty if (!odom_history_.empty()) { diff --git a/nav2_util/test/test_odometry_utils.cpp b/nav2_util/test/test_odometry_utils.cpp index 7d5b8bf9fe7..6871949303e 100644 --- a/nav2_util/test/test_odometry_utils.cpp +++ b/nav2_util/test/test_odometry_utils.cpp @@ -32,6 +32,34 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; +TEST(OdometryUtils, test_uninitialized) +{ + auto node = std::make_shared("test_node"); + nav2_util::OdomSmoother odom_smoother(node, 0.3, "odom"); + geometry_msgs::msg::Twist twist_msg; + geometry_msgs::msg::TwistStamped twist_stamped_msg; + + twist_msg = odom_smoother.getTwist(); + EXPECT_EQ(twist_msg.linear.x, 0.0); + EXPECT_EQ(twist_msg.linear.y, 0.0); + EXPECT_EQ(twist_msg.angular.z, 0.0); + + twist_msg = odom_smoother.getRawTwist(); + EXPECT_EQ(twist_msg.linear.x, 0.0); + EXPECT_EQ(twist_msg.linear.y, 0.0); + EXPECT_EQ(twist_msg.angular.z, 0.0); + + twist_stamped_msg = odom_smoother.getTwistStamped(); + EXPECT_EQ(twist_stamped_msg.twist.linear.x, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.linear.y, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.angular.z, 0.0); + + twist_stamped_msg = odom_smoother.getRawTwistStamped(); + EXPECT_EQ(twist_stamped_msg.twist.linear.x, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.linear.y, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.angular.z, 0.0); +} + TEST(OdometryUtils, test_smoothed_velocity) { auto node = std::make_shared("test_node"); @@ -41,6 +69,7 @@ TEST(OdometryUtils, test_smoothed_velocity) nav_msgs::msg::Odometry odom_msg; geometry_msgs::msg::Twist twist_msg; + geometry_msgs::msg::Twist twist_raw_msg; auto time = node->now(); @@ -67,9 +96,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 1.5); EXPECT_EQ(twist_msg.linear.y, 1.5); EXPECT_EQ(twist_msg.angular.z, 1.5); + EXPECT_EQ(twist_raw_msg.linear.x, 2.0); + EXPECT_EQ(twist_raw_msg.linear.y, 2.0); + EXPECT_EQ(twist_raw_msg.angular.z, 2.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.2); odom_msg.twist.twist.linear.x = 3.0; @@ -81,9 +114,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 2.0); EXPECT_EQ(twist_msg.linear.y, 2.0); EXPECT_EQ(twist_msg.angular.z, 2.0); + EXPECT_EQ(twist_raw_msg.linear.x, 3.0); + EXPECT_EQ(twist_raw_msg.linear.y, 3.0); + EXPECT_EQ(twist_raw_msg.angular.z, 3.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.45); odom_msg.twist.twist.linear.x = 4.0; @@ -95,9 +132,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 3.5); EXPECT_EQ(twist_msg.linear.y, 3.5); EXPECT_EQ(twist_msg.angular.z, 3.5); + EXPECT_EQ(twist_raw_msg.linear.x, 4.0); + EXPECT_EQ(twist_raw_msg.linear.y, 4.0); + EXPECT_EQ(twist_raw_msg.angular.z, 4.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(1.0); odom_msg.twist.twist.linear.x = 5.0; @@ -109,7 +150,11 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 5.0); EXPECT_EQ(twist_msg.linear.y, 5.0); EXPECT_EQ(twist_msg.angular.z, 5.0); + EXPECT_EQ(twist_raw_msg.linear.x, 5.0); + EXPECT_EQ(twist_raw_msg.linear.y, 5.0); + EXPECT_EQ(twist_raw_msg.angular.z, 5.0); }