Path trackin conditon node

nav2_behavior_tree/CMakeLists.txt

@@ -156,6 +156,9 @@ list(APPEND plugin_libs nav2_would_a_smoother_recovery_help_condition_bt_node)
 add_library(nav2_would_a_route_recovery_help_condition_bt_node SHARED plugins/condition/would_a_route_recovery_help_condition.cpp)
 list(APPEND plugin_libs nav2_would_a_route_recovery_help_condition_bt_node)
 
+add_library(nav2_is_within_path_tracking_bounds_condition_bt_node SHARED plugins/condition/is_within_path_tracking_bounds_condition.cpp)
+list(APPEND plugin_libs nav2_is_within_path_tracking_bounds_condition_bt_node)
+
 add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp)
 list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp

@@ -0,0 +1,75 @@
+// Copyright (c) 2025 Berkan Tali
+//
+// 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_WITHIN_PATH_TRACKING_BOUNDS_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_WITHIN_PATH_TRACKING_BOUNDS_CONDITION_HPP_
+
+#include <string>
+#include <memory>
+#include <mutex>
+#include <limits>
+
+#include "behaviortree_cpp/condition_node.h"
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_msgs/msg/tracking_feedback.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A BT::ConditionNode that subscribes to /tracking_feedback and returns SUCCESS
+ * if the error is within the max_error input port, FAILURE otherwise
+ */
+class IsWithinPathTrackingBoundsCondition : public BT::ConditionNode
+{
+public:
+  IsWithinPathTrackingBoundsCondition(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  IsWithinPathTrackingBoundsCondition() = delete;
+  ~IsWithinPathTrackingBoundsCondition() override = default;
+
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+
+  BT::NodeStatus tick() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<double>("max_error_right", "Maximum allowed tracking error on the right side"),
+      BT::InputPort<double>("max_error_left", "Maximum allowed tracking error left side")
+    };
+  }
+
+private:
+  rclcpp::Logger logger_{rclcpp::get_logger("is_within_path_tracking_bounds_node")};
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+  rclcpp::Subscription<nav2_msgs::msg::TrackingFeedback>::SharedPtr tracking_feedback_sub_;
+  double last_error_{0.0};
+  double max_error_right_{1.5};
+  double max_error_left_{1.5};
+  std::chrono::milliseconds bt_loop_duration_;
+
+  void trackingFeedbackCallback(const nav2_msgs::msg::TrackingFeedback::SharedPtr msg);
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_WITHIN_PATH_TRACKING_BOUNDS_CONDITION_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -367,6 +367,11 @@
       <input_port name="battery_topic">Topic for battery info</input_port>
     </Condition>
 
+    <Decorator ID="IsWithinPathTrackingBounds">
+      <input_port name="max_error_left">Maximum distance robot can go out from path in meters. On left side</input_port>
+      <input_port name="max_error_right">Maximum distance robot can go out from path in meters. On right side</input_port>
+    </Decorator>
+
     <Condition ID="DistanceTraveled">
       <input_port name="distance">Distance to check if passed</input_port>
       <input_port name="global_frame">reference frame to check in</input_port>

nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp

@@ -0,0 +1,113 @@
+// Copyright (c) 2025 Berkan Tali
+//
+// 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 "nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+IsWithinPathTrackingBoundsCondition::IsWithinPathTrackingBoundsCondition(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+  last_error_(std::numeric_limits<double>::max())
+{
+  auto node = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");
+  callback_group_ = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node->get_node_base_interface());
+
+  tracking_feedback_sub_ = node->create_subscription<nav2_msgs::msg::TrackingFeedback>(
+    "tracking_feedback",
+    std::bind(&IsWithinPathTrackingBoundsCondition::trackingFeedbackCallback, this,
+      std::placeholders::_1),
+    rclcpp::SystemDefaultsQoS(),
+    callback_group_);
+
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
+
+  RCLCPP_INFO(logger_, "Initialized IsWithinPathTrackingBoundsCondition BT node");
+  initialize();
+}
+
+void IsWithinPathTrackingBoundsCondition::trackingFeedbackCallback(
+  const nav2_msgs::msg::TrackingFeedback::SharedPtr msg)
+{
+  last_error_ = msg->tracking_error;
+}
+
+void IsWithinPathTrackingBoundsCondition::initialize()
+{
+  getInput("max_error_left", max_error_left_);
+  getInput("max_error_right", max_error_right_);
+}
+
+BT::NodeStatus IsWithinPathTrackingBoundsCondition::tick()
+{
+  if (!BT::isStatusActive(status())) {
+    initialize();
+  }
+
+  callback_group_executor_.spin_all(bt_loop_duration_);
+
+  if (!getInput("max_error_left", max_error_left_)) {
+    RCLCPP_ERROR(logger_, "max_error_left parameter not provided");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  if (max_error_left_ < 0.0) {
+    RCLCPP_WARN(logger_, "max_error_left should be positive, using absolute value");
+    max_error_left_ = std::abs(max_error_left_);
+  }
+
+  if (!getInput("max_error_right", max_error_right_)) {
+    RCLCPP_ERROR(logger_, "max_error_right parameter not provided");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  if (max_error_right_ < 0.0) {
+    RCLCPP_WARN(logger_, "max_error_right should be positive, using absolute value");
+    max_error_right_ = std::abs(max_error_right_);
+  }
+
+  if (last_error_ == std::numeric_limits<double>::max()) {
+    RCLCPP_WARN(logger_, "No tracking feedback received yet.");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  if (last_error_ > 0.0) {  // Positive = left side
+    if (last_error_ > max_error_left_) {
+      return BT::NodeStatus::FAILURE;
+    } else {
+      return BT::NodeStatus::SUCCESS;
+    }
+  } else {  // Negative = right side
+    if (std::abs(last_error_) > max_error_right_) {
+      return BT::NodeStatus::FAILURE;
+    } else {
+      return BT::NodeStatus::SUCCESS;
+    }
+  }
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsWithinPathTrackingBoundsCondition>(
+    "IsWithinPathTrackingBounds");
+}

nav2_behavior_tree/test/plugins/condition/CMakeLists.txt

@@ -30,6 +30,9 @@ plugin_add_test(test_condition_is_pose_occupied test_is_pose_occupied.cpp nav2_i
 
 plugin_add_test(test_are_error_codes_present test_are_error_codes_present.cpp nav2_would_a_controller_recovery_help_condition_bt_node)
 
+plugin_add_test(test_condition_is_within_path_tracking_bounds test_is_within_path_tracking_bounds.cpp
+  nav2_is_within_path_tracking_bounds_condition_bt_node)
+
 plugin_add_test(test_would_a_controller_recovery_help
   test_would_a_controller_recovery_help.cpp
   nav2_would_a_controller_recovery_help_condition_bt_node)