Add IsStoppedBTNode

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)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp

@@ -0,0 +1,102 @@
+// 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 "rclcpp/rclcpp.hpp"
+#include "behaviortree_cpp/condition_node.h"
+#include "nav_msgs/msg/odometry.hpp"
+
+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 Callback function for odom topic
+   * @param msg Shared pointer to nav_msgs::msg::Odometry::SharedPtr message
+   */
+  void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg);
+
+  /**
+   * @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.1,
+          "Velocity threshold below which robot is considered stopped"),
+      BT::InputPort<std::chrono::milliseconds>("time_stopped_threshold", 1000,
+          "Time threshold for which the velocity needs to be below the threshold to consider the robot stopped"),
+      BT::InputPort<std::string>(
+        "topic_name",
+        "odom",
+        "the odometry topic name"),
+    };
+  }
+
+private:
+  // The node that will be used for any ROS operations
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+  std::thread callback_group_executor_thread;
+
+  std::string topic_name_;
+  std::atomic<bool> is_stopped_;
+  double velocity_threshold_;
+  std::chrono::milliseconds time_stopped_threshold_;
+  rclcpp::Time stopped_stamp_;
+  std::mutex mutex_;
+
+  // Listen to odometry
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_

nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp

@@ -0,0 +1,103 @@
+// 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"
+
+using namespace std::chrono_literals; // NOLINT
+
+namespace nav2_behavior_tree
+{
+
+IsStoppedCondition::IsStoppedCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  is_stopped_(false),
+  velocity_threshold_(0.1),
+  time_stopped_threshold_(1000),
+  stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME))
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  getInput("topic_name", topic_name_);
+
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+  callback_group_executor_thread = std::thread([this]() {callback_group_executor_.spin();});
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
+    topic_name_,
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&IsStoppedCondition::onOdomReceived, this, std::placeholders::_1),
+    sub_option);
+}
+
+IsStoppedCondition::~IsStoppedCondition()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStoppedCondition BT node");
+  callback_group_executor_.cancel();
+  callback_group_executor_thread.join();
+}
+
+void IsStoppedCondition::onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg)
+{
+  getInput("velocity_threshold", velocity_threshold_);
+  getInput("time_stopped_threshold", time_stopped_threshold_);
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  // Check if the robot is stopped for a certain amount of time
+  if (abs(msg->twist.twist.linear.x) < velocity_threshold_ &&
+    abs(msg->twist.twist.linear.y) < velocity_threshold_ &&
+    abs(msg->twist.twist.angular.z) < velocity_threshold_)
+  {
+    if (stopped_stamp_ == rclcpp::Time(0, 0, RCL_ROS_TIME)) {
+      stopped_stamp_ = rclcpp::Time(msg->header.stamp);
+    } else if (rclcpp::Time(msg->header.stamp) - stopped_stamp_ >
+      rclcpp::Duration(time_stopped_threshold_))
+    {
+      is_stopped_ = true;
+    }
+  } else {
+    stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
+    is_stopped_ = false;
+  }
+}
+
+BT::NodeStatus IsStoppedCondition::tick()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (is_stopped_) {
+    return BT::NodeStatus::SUCCESS;
+  } else if (stopped_stamp_ != rclcpp::Time(0, 0, RCL_ROS_TIME)) {
+    // Robot was stopped but not for long enough
+    return BT::NodeStatus::RUNNING;
+  } else {
+    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");
+}