Change detection of front entity on path of NPC to intersection of sp…

…line polygon
RobotecAI · Dec 9, 2024 · fad059a · fad059a
1 parent 3dd43ab
commit fad059a
Show file tree

Hide file tree

Showing 5 changed files with 30 additions and 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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(