Adding left, right side errors

Signed-off-by: silanus23 <[email protected]>

Author: silanus23

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

@@ -52,7 +52,8 @@ class IsWithinPathTrackingBoundsCondition : public BT::ConditionNode
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<double>("max_error", "Maximum allowed tracking error")
+      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")
     };
   }
 
@@ -62,7 +63,8 @@ class IsWithinPathTrackingBoundsCondition : public BT::ConditionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
   rclcpp::Subscription<nav2_msgs::msg::TrackingFeedback>::SharedPtr tracking_feedback_sub_;
   double last_error_{0.0};
-  double max_error_{1.5};
+  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);

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -368,7 +368,8 @@
     </Condition>
 
     <Decorator ID="IsWithinPathTrackingBounds">
-      <input_port name="max_error">Maximum distance robot can go out in meters</input_port>
+      <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">

nav2_behavior_tree/plugins/condition/is_within_path_tracking_bounds_condition.cpp

@@ -52,7 +52,8 @@ void IsWithinPathTrackingBoundsCondition::trackingFeedbackCallback(
 
 void IsWithinPathTrackingBoundsCondition::initialize()
 {
-  getInput("max_error", max_error_);
+  getInput("max_error_left", max_error_left_);
+  getInput("max_error_right", max_error_right_);
 }
 
 BT::NodeStatus IsWithinPathTrackingBoundsCondition::tick()
@@ -63,24 +64,44 @@ BT::NodeStatus IsWithinPathTrackingBoundsCondition::tick()
 
   callback_group_executor_.spin_all(bt_loop_duration_);
 
-  if (!getInput("max_error", max_error_)) {
+  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 parameter not provided");
     return BT::NodeStatus::FAILURE;
   }
 
-  if (max_error_ < 0.0) {
-    RCLCPP_WARN(logger_, "max_error should be positive, using absolute value");
-    max_error_ = std::abs(max_error_);
+  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_ <= max_error_) {
-    return BT::NodeStatus::SUCCESS;
+  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;
+    }
   }
-  return BT::NodeStatus::FAILURE;
 }
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/test/plugins/condition/test_is_within_path_tracking_bounds.cpp

@@ -38,9 +38,7 @@ class IsWithinPathTrackingBoundsConditionTestFixture : public ::testing::Test
     // Create the blackboard that will be shared by all of the nodes in the tree
     config_->blackboard = BT::Blackboard::create();
     // Put items on the blackboard
-    config_->blackboard->set(
-      "node",
-      node_);
+    config_->blackboard->set("node", node_);
     config_->blackboard->set<std::chrono::milliseconds>(
       "bt_loop_duration",
       std::chrono::milliseconds(10));
@@ -49,7 +47,7 @@ class IsWithinPathTrackingBoundsConditionTestFixture : public ::testing::Test
       "IsWithinPathTrackingBounds");
 
     tracking_feedback_pub_ = node_->create_publisher<nav2_msgs::msg::TrackingFeedback>(
-      "/tracking_feedback",
+      "tracking_feedback",
       rclcpp::SystemDefaultsQoS());
     tracking_feedback_pub_->on_activate();
   }
@@ -64,6 +62,15 @@ class IsWithinPathTrackingBoundsConditionTestFixture : public ::testing::Test
     executor_.reset();
   }
 
+  void publishAndSpin(float error_value)
+  {
+    nav2_msgs::msg::TrackingFeedback msg;
+    msg.tracking_error = error_value;
+    tracking_feedback_pub_->publish(msg);
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    executor_->spin_some();
+  }
+
 protected:
   static nav2::LifecycleNode::SharedPtr node_;
   static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
@@ -81,129 +88,175 @@ std::shared_ptr<BT::BehaviorTreeFactory> IsWithinPathTrackingBoundsConditionTest
 nav2::Publisher<nav2_msgs::msg::TrackingFeedback>::SharedPtr
 IsWithinPathTrackingBoundsConditionTestFixture::tracking_feedback_pub_ = nullptr;
 
-TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_within_bounds)
+TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_symmetric_bounds)
 {
+  // Test with equal bounds for left and right sides
   std::string xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <IsWithinPathTrackingBounds max_error="1.0"/>
+            <IsWithinPathTrackingBounds max_error_left="1.0" max_error_right="1.0"/>
         </BehaviorTree>
       </root>)";
 
   auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
 
-  // Test case 1: Error is within bounds (should return SUCCESS)
-  nav2_msgs::msg::TrackingFeedback tracking_feedback_msg;
-  tracking_feedback_msg.tracking_error = 0.5;
-  tracking_feedback_pub_->publish(tracking_feedback_msg);
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  executor_->spin_some();
+  publishAndSpin(0.5);
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
-
-  // Test case 2: Error is exactly at the boundary (should return SUCCESS)
-  tracking_feedback_msg.tracking_error = 1.0;
-  tracking_feedback_pub_->publish(tracking_feedback_msg);
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  executor_->spin_some();
+  publishAndSpin(-0.5);
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
-
-  // Test case 3: Error exceeds bounds (should return FAILURE)
-  tracking_feedback_msg.tracking_error = 1.5;
-  tracking_feedback_pub_->publish(tracking_feedback_msg);
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  executor_->spin_some();
+  publishAndSpin(1.0);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(-1.0);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(1.5);
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
+  publishAndSpin(-1.5);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
+  publishAndSpin(0.0);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+}
 
-  // Test case 4: Zero error (should return SUCCESS)
-  tracking_feedback_msg.tracking_error = 0.0;
-  tracking_feedback_pub_->publish(tracking_feedback_msg);
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  executor_->spin_some();
+TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_asymmetric_bounds)
+{
+  // Test with different bounds for left and right sides
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <IsWithinPathTrackingBounds max_error_left="2.0" max_error_right="0.5"/>
+        </BehaviorTree>
+      </root>)";
+
+  auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
+  publishAndSpin(1.5);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(-0.3);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(2.0);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(-0.5);
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(2.5);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
+  publishAndSpin(-0.8);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
 }
 
-TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_different_max_error)
+TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_left_side_only)
 {
+  // Test with very restrictive right bound
   std::string xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <IsWithinPathTrackingBounds max_error="0.2"/>
+            <IsWithinPathTrackingBounds max_error_left="5.0" max_error_right="0.0"/>
         </BehaviorTree>
       </root>)";
 
   auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
 
-  // Test case 1: Error is within smaller bounds (should return SUCCESS)
-  nav2_msgs::msg::TrackingFeedback tracking_feedback_msg;
-  tracking_feedback_msg.tracking_error = 0.1;
-  tracking_feedback_pub_->publish(tracking_feedback_msg);
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  executor_->spin_some();
+  publishAndSpin(4.9);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(5.0);
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(5.1);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
+  publishAndSpin(-0.001);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
+  publishAndSpin(0.0);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+}
 
-  // Test case 2: Error exceeds smaller bounds (should return FAILURE)
-  tracking_feedback_msg.tracking_error = 0.3;
-  tracking_feedback_pub_->publish(tracking_feedback_msg);
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  executor_->spin_some();
+TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_right_side_only)
+{
+  // Test with very restrictive left bound
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <IsWithinPathTrackingBounds max_error_left="0.0" max_error_right="5.0"/>
+        </BehaviorTree>
+      </root>)";
+
+  auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
+
+  publishAndSpin(-4.9);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(-5.0);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(-5.1);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
+  publishAndSpin(0.001);
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
+  publishAndSpin(0.0);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
 }
 
-TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_large_error_values)
+TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_very_tight_bounds)
 {
+  // Test with very small bounds
   std::string xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <IsWithinPathTrackingBounds max_error="5.0"/>
+            <IsWithinPathTrackingBounds max_error_left="0.1" max_error_right="0.1"/>
         </BehaviorTree>
       </root>)";
 
   auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
 
-  // Test case 1: Large error within bounds (should return SUCCESS)
-  nav2_msgs::msg::TrackingFeedback tracking_feedback_msg;
-  tracking_feedback_msg.tracking_error = 4.9;
-  tracking_feedback_pub_->publish(tracking_feedback_msg);
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  executor_->spin_some();
+  publishAndSpin(0.05);
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(-0.05);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(0.15);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
+  publishAndSpin(-0.15);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
+}
+
+TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_no_feedback_received)
+{
+  // Test behavior when no feedback has been received yet
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <IsWithinPathTrackingBounds max_error_left="1.0" max_error_right="1.0"/>
+        </BehaviorTree>
+      </root>)";
 
-  // Test case 2: Very large error exceeding bounds (should return FAILURE)
-  tracking_feedback_msg.tracking_error = 10.0;
-  tracking_feedback_pub_->publish(tracking_feedback_msg);
+  // Create a fresh tree without publishing any feedback
+  auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
+
+  // Should return FAILURE when no feedback received
+  // Note: This test creates a new condition node, so last_error_ will be at max()
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
   executor_->spin_some();
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
 }
 
-TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_behavior_edge_cases)
+TEST_F(IsWithinPathTrackingBoundsConditionTestFixture, test_sign_convention)
 {
+  // Verify sign convention: positive = left, negative = right
   std::string xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <IsWithinPathTrackingBounds max_error="0.0"/>
+            <IsWithinPathTrackingBounds max_error_left="1.0" max_error_right="2.0"/>
         </BehaviorTree>
       </root>)";
 
   auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
 
-  // Test case 1: Zero max_error with zero tracking error (should return SUCCESS)
-  nav2_msgs::msg::TrackingFeedback tracking_feedback_msg;
-  tracking_feedback_msg.tracking_error = 0.0;
-  tracking_feedback_pub_->publish(tracking_feedback_msg);
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  executor_->spin_some();
+  publishAndSpin(0.9);
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
-
-  // Test case 2: Zero max_error with any positive tracking error (should return FAILURE)
-  tracking_feedback_msg.tracking_error = 0.001;
-  tracking_feedback_pub_->publish(tracking_feedback_msg);
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  executor_->spin_some();
+  publishAndSpin(1.5);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
+  publishAndSpin(-1.5);
+  EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS);
+  publishAndSpin(-2.5);
   EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE);
 }