AMRNAV-6916 Goal checker: Check for x and y separately

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp

@@ -17,6 +17,7 @@
 
 #include <string>
 #include <memory>
+#include <limits>
 
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp_v3/condition_node.h"
@@ -74,7 +75,9 @@ class GoalReachedCondition : public BT::ConditionNode
     return {
       BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination"),
       BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
-      BT::InputPort<double>("xy_goal_tolerance", 0.1, "xy goal tolerance"),
+      BT::InputPort<double>("x_goal_tolerance", std::numeric_limits<double>::infinity(), "x goal tolerance"),
+      BT::InputPort<double>("y_goal_tolerance", std::numeric_limits<double>::infinity(), "y goal tolerance"),
+      BT::InputPort<double>("xy_goal_tolerance", std::numeric_limits<double>::infinity(), "xy goal tolerance"),
       BT::InputPort<double>("yaw_goal_tolerance", 0.1, "yaw goal tolerance"),
       BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
     };
@@ -93,6 +96,8 @@ class GoalReachedCondition : public BT::ConditionNode
 
   bool initialized_;
   double goal_reached_tol_;
+  double goal_reached_tol_x_;
+  double goal_reached_tol_y_ ;
   double goal_reached_tol_yaw_;
   std::string global_frame_;
   std::string robot_base_frame_;

nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

@@ -65,6 +65,8 @@ void GoalReachedCondition::initialize()
   getInput("global_frame", global_frame_);
   getInput("robot_base_frame", robot_base_frame_);
   getInput("xy_goal_tolerance", goal_reached_tol_);
+  getInput("x_goal_tolerance", goal_reached_tol_x_);
+  getInput("y_goal_tolerance", goal_reached_tol_y_);
   getInput("yaw_goal_tolerance", goal_reached_tol_yaw_);
 
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
@@ -83,18 +85,33 @@ bool GoalReachedCondition::isGoalReached()
     RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
     return false;
   }
-
   geometry_msgs::msg::PoseStamped goal;
   getInput("goal", goal);
 
+  // Transform to goal frame
+  tf2::Transform goal_frame_transform;
+  goal_frame_transform.setOrigin(tf2::Vector3(goal.pose.position.x, goal.pose.position.y, 0.0));
+  goal_frame_transform.setRotation(tf2::Quaternion(goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w));
+
+  tf2::Transform current_pose_in_goal_frame = goal_frame_transform.inverse() * tf2::Transform(tf2::Quaternion(current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w), 
+  tf2::Vector3(current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z));
+
+  double x_in_goal_frame = fabs(current_pose_in_goal_frame.getOrigin().x());
+  double y_in_goal_frame = fabs(current_pose_in_goal_frame.getOrigin().y());
+
+  RCLCPP_INFO(node_->get_logger(), "current_pose_in_goal_frame x: %f, y: %f", x_in_goal_frame, y_in_goal_frame);
+
   double current_yaw = tf2::getYaw(current_pose.pose.orientation);
   double goal_yaw = tf2::getYaw(goal.pose.orientation);
   double dangle = fabs(angles::shortest_angular_distance(goal_yaw, current_yaw));
-  double dx = goal.pose.position.x - current_pose.pose.position.x;
-  double dy = goal.pose.position.y - current_pose.pose.position.y;
 
-  return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_) &&
-         dangle <= goal_reached_tol_yaw_;
+  // Check conditions for x, y, and xy tolerances
+  bool within_xy_tolerance = (x_in_goal_frame * x_in_goal_frame + y_in_goal_frame * y_in_goal_frame) <= (goal_reached_tol_ * goal_reached_tol_);  
+  bool within_x_tolerance = x_in_goal_frame <= goal_reached_tol_x_;
+  bool within_y_tolerance = y_in_goal_frame <= goal_reached_tol_y_;
+  bool within_yaw_tolerance = dangle <= goal_reached_tol_yaw_;
+
+  return within_xy_tolerance && within_x_tolerance && within_y_tolerance && within_yaw_tolerance;
 }
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp

@@ -30,26 +30,24 @@ using namespace std::chrono_literals;  // NOLINT
 class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
 {
 public:
-  void SetUp()
+  void SetUp(const std::string &xml_txt, const double &goal_x, const double &goal_y)
   {
-    node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue{0.1});
+    if (!node_->has_parameter("transform_tolerance")) {
+      node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue{0.1});
+    }
 
     geometry_msgs::msg::PoseStamped goal;
     goal.header.stamp = node_->now();
     goal.header.frame_id = "map";
-    goal.pose.position.x = 1.0;
-    goal.pose.position.y = 1.0;
+    goal.pose.position.x = goal_x;
+    goal.pose.position.y = goal_y; 
     config_->blackboard->set("goal", goal);
-
-    std::string xml_txt =
-      R"(
-      <root main_tree_to_execute = "MainTree" >
-        <BehaviorTree ID="MainTree">
-            <GoalReached goal="{goal}" />
-        </BehaviorTree>
-      </root>)";
-
-    factory_->registerNodeType<nav2_behavior_tree::GoalReachedCondition>("GoalReached");
+
+    if(!registered_type_) {
+      registered_type_ = true;
+      factory_->registerNodeType<nav2_behavior_tree::GoalReachedCondition>("GoalReached");
+    }
+
     tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
   }
 
@@ -59,13 +57,24 @@ class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT
   }
 
 protected:
+  static bool registered_type_; 
   static std::shared_ptr<BT::Tree> tree_;
 };
 
 std::shared_ptr<BT::Tree> GoalReachedConditionTestFixture::tree_ = nullptr;
+bool GoalReachedConditionTestFixture::registered_type_ = false;
 
 TEST_F(GoalReachedConditionTestFixture, test_behavior)
 {
+  std::string xml_txt =
+    R"(
+    <root main_tree_to_execute = "MainTree" >
+      <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" y_goal_tolerance="0.1"/>
+      </BehaviorTree>
+    </root>)";
+
+  SetUp(xml_txt, 1.0, 1.0);
   EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);
 
   geometry_msgs::msg::Pose pose;
@@ -94,6 +103,182 @@ TEST_F(GoalReachedConditionTestFixture, test_behavior)
   EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS);
 }
 
+TEST_F(GoalReachedConditionTestFixture, test_tolerance_cases)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = 1.0;
+  pose.position.y = 0.0;
+  {
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" x_goal_tolerance="0.3" y_goal_tolerance="2.0" />
+        </BehaviorTree>
+      </root>)";
+    SetUp(xml_txt, 0.0, 0.0);
+    transform_handler_->updateRobotPose(pose);
+    std::this_thread::sleep_for(500ms);
+    EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);
+  }
+
+  {
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" x_goal_tolerance="1.3" y_goal_tolerance="0.5" />
+        </BehaviorTree>
+      </root>)";
+    SetUp(xml_txt, 0.0, 0.0);
+    transform_handler_->updateRobotPose(pose);
+    std::this_thread::sleep_for(500ms);
+    EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS);
+  }
+
+  {
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" x_goal_tolerance="0.3" y_goal_tolerance="0.3" xy_goal_tolerance="0.3" />
+        </BehaviorTree>
+      </root>)";
+    SetUp(xml_txt, 0.0, 0.0);
+    transform_handler_->updateRobotPose(pose);
+    std::this_thread::sleep_for(500ms);
+    EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);
+  }
+
+  {
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" y_goal_tolerance="0.3" xy_goal_tolerance="1.3" />
+        </BehaviorTree>
+      </root>)";
+    SetUp(xml_txt, 0.0, 0.0);
+    transform_handler_->updateRobotPose(pose);
+    std::this_thread::sleep_for(500ms);
+    EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS);
+  }
+
+  {
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" xy_goal_tolerance="1.3" />
+        </BehaviorTree>
+      </root>)";
+    SetUp(xml_txt, 0.0, 0.0);
+    transform_handler_->updateRobotPose(pose);
+    std::this_thread::sleep_for(500ms);
+    EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS);
+  }
+
+  {
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" xy_goal_tolerance="0.3" />
+        </BehaviorTree>
+      </root>)";
+    SetUp(xml_txt, 0.0, 0.0);
+    transform_handler_->updateRobotPose(pose);
+    std::this_thread::sleep_for(500ms);
+    EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);
+  }
+
+  {
+    pose.position.x = 0.4;
+    pose.position.y = 0.0;
+
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" x_goal_tolerance="0.3" y_goal_tolerance="2.0" />
+        </BehaviorTree>
+      </root>)";
+    SetUp(xml_txt, 0.0, 0.0);
+    transform_handler_->updateRobotPose(pose);
+    std::this_thread::sleep_for(500ms);
+    EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); 
+  } 
+
+  {
+    pose.position.x = 0.2;
+    pose.position.y = 0.0;
+
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" x_goal_tolerance="0.3" y_goal_tolerance="2.0" />
+        </BehaviorTree>
+      </root>)";
+    SetUp(xml_txt, 0.0, 0.0);
+    transform_handler_->updateRobotPose(pose);
+    std::this_thread::sleep_for(500ms);
+    EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS); 
+  }
+
+  {
+    pose.position.x = 0.0;
+    pose.position.y = 0.0;
+
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" x_goal_tolerance="0.3" y_goal_tolerance="0.3" />
+        </BehaviorTree>
+      </root>)";
+    SetUp(xml_txt, 0.0, 0.0);
+    transform_handler_->updateRobotPose(pose);
+    std::this_thread::sleep_for(500ms);
+    EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS);  
+  }
+
+  {
+    pose.position.x = 1.5;
+    pose.position.y = 1.5;  
+
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" xy_goal_tolerance="2.25" />
+        </BehaviorTree>
+      </root>)";
+    SetUp(xml_txt, 0.0, 0.0);
+    transform_handler_->updateRobotPose(pose);
+    std::this_thread::sleep_for(500ms);
+    EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS);  
+  }
+
+  {
+    pose.position.x = 1.5;
+    pose.position.y = 1.5;  
+
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalReached goal="{goal}" xy_goal_tolerance="1.25" />
+        </BehaviorTree>
+      </root>)";
+    SetUp(xml_txt, 0.0, 0.0);
+    transform_handler_->updateRobotPose(pose);
+    std::this_thread::sleep_for(500ms);
+    EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE);  
+  }
+
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);
@@ -107,4 +292,4 @@ int main(int argc, char ** argv)
   rclcpp::shutdown();
 
   return all_successful;
-}
+}