From fad059ad87400c405fba12eadfb687a1704ee721 Mon Sep 17 00:00:00 2001
From: Grzegorz Maj <grzegorz.maj@robotec.ai>
Date: Mon, 9 Dec 2024 01:36:01 +0100
Subject: [PATCH] Change detection of front entity on path of NPC to
 intersection of spline polygon

---
 .../geometry/spline/catmull_rom_spline.hpp    |  4 ++--
 .../spline/catmull_rom_spline_interface.hpp   |  2 ++
 .../geometry/spline/catmull_rom_subspline.hpp |  3 +++
 .../src/spline/catmull_rom_subspline.cpp      |  6 ++++++
 .../behavior_tree_plugin/src/action_node.cpp  | 19 +++++++++++++++++--
 5 files changed, 30 insertions(+), 4 deletions(-)

diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp
index 9018f938855..4c0be55eba1 100644
--- a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp
+++ b/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp
@@ -58,8 +58,8 @@ class CatmullRomSpline : public CatmullRomSplineInterface
   auto getCollisionPointIn2D(
     const std::vector<geometry_msgs::msg::Point> & polygon,
     const bool search_backward = false) const -> std::optional<double> override;
-  auto getPolygon(const double width, const size_t num_points = 30, const double z_offset = 0)
-    -> std::vector<geometry_msgs::msg::Point>;
+  auto getPolygon(const double width, const size_t num_points = 30, const double z_offset = 0) const
+    -> std::vector<geometry_msgs::msg::Point> override;
   const std::vector<geometry_msgs::msg::Point> control_points;
   virtual ~CatmullRomSpline() = default;
 
diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp
index 8969a818308..4e4c6ddc974 100644
--- a/common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp
+++ b/common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp
@@ -34,6 +34,8 @@ class CatmullRomSplineInterface
   virtual std::optional<double> getCollisionPointIn2D(
     const std::vector<geometry_msgs::msg::Point> & polygon,
     const bool search_backward = false) const = 0;
+  virtual std::vector<geometry_msgs::msg::Point> getPolygon(
+    const double width, const size_t num_points = 30, const double z_offset = 0) const = 0;
 };
 }  // namespace geometry
 }  // namespace math
diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp
index 22827657f2f..20de2277088 100644
--- a/common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp
+++ b/common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp
@@ -46,6 +46,9 @@ class CatmullRomSubspline : public CatmullRomSplineInterface
     const std::vector<geometry_msgs::msg::Point> & polygon,
     const bool search_backward = false) const override;
 
+  std::vector<geometry_msgs::msg::Point> getPolygon(
+    const double width, const size_t num_points = 30, const double z_offset = 0) const override;
+
 private:
   std::shared_ptr<math::geometry::CatmullRomSpline> spline_;
   double start_s_;
diff --git a/common/math/geometry/src/spline/catmull_rom_subspline.cpp b/common/math/geometry/src/spline/catmull_rom_subspline.cpp
index 8cfadb4ad24..eb2a5da7e9c 100644
--- a/common/math/geometry/src/spline/catmull_rom_subspline.cpp
+++ b/common/math/geometry/src/spline/catmull_rom_subspline.cpp
@@ -61,5 +61,11 @@ std::optional<double> CatmullRomSubspline::getCollisionPointIn2D(
   }
   return *begin - start_s_;
 }
+
+std::vector<geometry_msgs::msg::Point> CatmullRomSubspline::getPolygon(
+  const double width, const size_t num_points, const double z_offset) const
+{
+  return spline_->getPolygon(width, num_points, z_offset);
+}
 }  // namespace geometry
 }  // namespace math
diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index cbe86b1c649..eaecf7d4dda 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -15,6 +15,7 @@
 #include <algorithm>
 #include <behavior_tree_plugin/action_node.hpp>
 #include <geometry/bounding_box.hpp>
+#include <geometry/intersection/collision.hpp>
 #include <geometry/quaternion/euler_to_quaternion.hpp>
 #include <geometry/quaternion/get_rotation.hpp>
 #include <geometry/quaternion/get_rotation_matrix.hpp>
@@ -247,6 +248,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf
 {
   std::vector<double> distances;
   std::vector<std::string> entities;
+
   for (const auto & each : other_entity_status) {
     const auto distance = getDistanceToTargetEntityPolygon(spline, each.first);
     const auto quat = math::geometry::getRotation(
@@ -316,14 +318,27 @@ auto ActionNode::getDistanceToTargetEntityPolygon(
   double width_extension_left, double length_extension_front, double length_extension_rear) const
   -> std::optional<double>
 {
+  std::optional<double> distance{std::nullopt};
   if (status.laneMatchingSucceed()) {
     const auto polygon = math::geometry::transformPoints(
       status.getMapPose(), math::geometry::getPointsFromBbox(
                              status.getBoundingBox(), width_extension_right, width_extension_left,
                              length_extension_front, length_extension_rear));
-    return spline.getCollisionPointIn2D(polygon, false);
+    const auto splinePolygon =
+      spline.getPolygon(canonicalized_entity_status->getBoundingBox().dimensions.y);
+    for (const auto & point : polygon) {
+      if (math::geometry::contains(splinePolygon, point)) {
+        const auto dst = traffic_simulator::pose::boundingBoxRelativePose(
+          canonicalized_entity_status->getMapPose(), canonicalized_entity_status->getBoundingBox(),
+          status.getMapPose(), traffic_simulator_msgs::msg::BoundingBox());
+        if (dst and (distance.value() >= 0)) {
+          distance.emplace(dst.value().position.x);
+          break;
+        }
+      }
+    }
   }
-  return std::nullopt;
+  return distance;
 }
 
 auto ActionNode::getDistanceToConflictingEntity(