Add IsStoppedBTNode (ros-navigation#4764)

* Add IsStoppedBTNode Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add topic name + reformat Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix comment Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix abs Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * remove log Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add getter functions for raw twist Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * remove unused code Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use odomsmoother Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * update groot Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * reset at success Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * FIX velocity_threshold_ Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix stopped Node Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add tests to odometry_utils Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix linting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
quasi-robotics · Dec 4, 2024 · 75fd150 · 75fd150
1 parent 5d82c7e
commit 75fd150
Show file tree

Hide file tree

Showing 10 changed files with 409 additions and 5 deletions.
diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
@@ -102,6 +102,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
@@ -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 <string>
+#include <atomic>
+#include <deque>
+#include <memory>
+
+#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<double>("velocity_threshold", 0.01,
+          "Velocity threshold below which robot is considered stopped"),
+      BT::InputPort<std::chrono::milliseconds>("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<nav2_util::OdomSmoother> 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
@@ -219,6 +219,11 @@
 
     <Condition ID="IsStuck"/>
 
+    <Condition ID="IsStopped">
+      <input_port name="velocity_threshold">Velocity threshold below which robot is considered stopped</input_port>
+      <input_port name="duration_stopped">Duration (ms) the velocity must remain below the threshold</input_port>
+    </Condition>
+
     <Condition ID="TransformAvailable">
       <input_port name="child">Child frame for transform</input_port>
       <input_port name="parent">Parent frame for transform</input_port>

diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp 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 <string>
+#include <chrono>
+
+#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<rclcpp::Node::SharedPtr>("node");
+  odom_smoother_ = config().blackboard->get<std::shared_ptr<nav2_util::OdomSmoother>>(
+    "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<nav2_behavior_tree::IsStoppedCondition>("IsStopped");
+}
diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp
@@ -51,8 +51,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<std::shared_ptr<nav2_util::OdomSmoother>>(
     "odom_smoother");
 }

diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt
@@ -34,6 +34,10 @@ 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_stopped test_is_stopped.cpp)
+target_link_libraries(test_condition_is_stopped nav2_is_stopped_condition_bt_node)
+ament_target_dependencies(test_condition_is_stopped ${dependencies})
+
 ament_add_gtest(test_condition_is_battery_charging test_is_battery_charging.cpp)
 target_link_libraries(test_condition_is_battery_charging nav2_is_battery_charging_condition_bt_node)
 ament_target_dependencies(test_condition_is_battery_charging ${dependencies})

diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp 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 <gtest/gtest.h>
+#include <chrono>
+#include <memory>
+#include <set>
+
+#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<nav2_util::OdomSmoother>(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<nav2_behavior_tree::IsStoppedCondition>("is_stopped", *config_);
+  }
+
+  void TearDown()
+  {
+    bt_node_.reset();
+    odom_smoother_.reset();
+  }
+
+protected:
+  static std::shared_ptr<nav2_behavior_tree::IsStoppedCondition> bt_node_;
+  static std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
+};
+
+std::shared_ptr<nav2_behavior_tree::IsStoppedCondition>
+IsStoppedTestFixture::bt_node_ = nullptr;
+std::shared_ptr<nav2_util::OdomSmoother>
+IsStoppedTestFixture::odom_smoother_ = nullptr;
+
+
+TEST_F(IsStoppedTestFixture, test_behavior)
+{
+  auto odom_pub = node_->create_publisher<nav_msgs::msg::Odometry>("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;
+}