RJD-1509 Add using only part of spline for getCollisionPointIn2D

Author: gmajrobotec

common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp

@@ -50,14 +50,15 @@ class CatmullRomSpline : public CatmullRomSplineInterface
   auto getSquaredDistanceVector(const geometry_msgs::msg::Point & point, const double s) const
     -> geometry_msgs::msg::Vector3;
   auto getCollisionPointsIn2D(
-    const std::vector<geometry_msgs::msg::Point> & polygon,
-    const bool search_backward = false) const -> std::set<double>;
+    const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward = false,
+    std::optional<std::pair<double, double>> s_part = std::nullopt) const -> std::set<double>;
   auto getCollisionPointIn2D(
     const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1,
     const bool search_backward = false) const -> std::optional<double>;
   auto getCollisionPointIn2D(
-    const std::vector<geometry_msgs::msg::Point> & polygon,
-    const bool search_backward = false) const -> std::optional<double> override;
+    const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward = false,
+    std::optional<std::pair<double, double>> s_part = std::nullopt) 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>;
   const std::vector<geometry_msgs::msg::Point> control_points;

common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp

@@ -32,8 +32,8 @@ class CatmullRomSplineInterface
   virtual ~CatmullRomSplineInterface() = default;
   virtual double getLength() const = 0;
   virtual std::optional<double> getCollisionPointIn2D(
-    const std::vector<geometry_msgs::msg::Point> & polygon,
-    const bool search_backward = false) const = 0;
+    const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward = false,
+    std::optional<std::pair<double, double>> s_part = std::nullopt) const = 0;
   virtual double getSquaredDistanceIn2D(
     const geometry_msgs::msg::Point & point, const double s) const = 0;
 };

common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp

@@ -43,8 +43,8 @@ class CatmullRomSubspline : public CatmullRomSplineInterface
   double getLength() const override;
 
   std::optional<double> getCollisionPointIn2D(
-    const std::vector<geometry_msgs::msg::Point> & polygon,
-    const bool search_backward = false) const override;
+    const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward = false,
+    std::optional<std::pair<double, double>> s_part = std::nullopt) const override;
 
   auto getSquaredDistanceIn2D(const geometry_msgs::msg::Point & point, const double s) const
     -> double override;

common/math/geometry/src/spline/catmull_rom_spline.cpp

@@ -279,8 +279,8 @@ auto CatmullRomSpline::getSInSplineCurve(const size_t curve_index, const double
 }
 
 auto CatmullRomSpline::getCollisionPointsIn2D(
-  const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward) const
-  -> std::set<double>
+  const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward,
+  std::optional<std::pair<double, double>> s_part) const -> std::set<double>
 {
   if (polygon.size() <= 1) {
     THROW_SIMULATION_ERROR(
@@ -291,15 +291,19 @@ auto CatmullRomSpline::getCollisionPointsIn2D(
   }
   /// @note If the spline has three or more control points.
   const auto get_collision_point_2d_with_curve =
-    [this](const auto & local_polygon, const auto local_search_backward) {
+    [this, s_part](const auto & local_polygon, const auto local_search_backward) {
       std::set<double> s_value_candidates;
+      double s_all = 0;
       for (size_t i = 0; i < curves_.size(); ++i) {
-        /// @note The local_polygon is assumed to be closed
-        const auto s =
-          curves_[i].getCollisionPointsIn2D(local_polygon, local_search_backward, true, true);
-        std::for_each(s.begin(), s.end(), [&s_value_candidates, i, this](const auto & s) {
-          s_value_candidates.insert(getSInSplineCurve(i, s));
-        });
+        if (s_part == std::nullopt || (s_all >= s_part->first && s_all <= s_part->second)) {
+          /// @note The local_polygon is assumed to be closed
+          const auto s =
+            curves_[i].getCollisionPointsIn2D(local_polygon, local_search_backward, true, true);
+          std::for_each(s.begin(), s.end(), [&s_value_candidates, i, this](const auto & s) {
+            s_value_candidates.insert(getSInSplineCurve(i, s));
+          });
+        }
+        s_all += curves_[i].getLength();
       }
       return s_value_candidates;
     };
@@ -358,10 +362,10 @@ auto CatmullRomSpline::getCollisionPointsIn2D(
  * @return std::optional<double> Denormalized s values along the frenet coordinate of the spline curve.
  */
 auto CatmullRomSpline::getCollisionPointIn2D(
-  const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward) const
-  -> std::optional<double>
+  const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward,
+  std::optional<std::pair<double, double>> s_part) const -> std::optional<double>
 {
-  std::set<double> s_value_candidates = getCollisionPointsIn2D(polygon, search_backward);
+  std::set<double> s_value_candidates = getCollisionPointsIn2D(polygon, search_backward, s_part);
   if (s_value_candidates.empty()) {
     return std::nullopt;
   }

common/math/geometry/src/spline/catmull_rom_subspline.cpp

@@ -24,7 +24,8 @@ namespace geometry
 double CatmullRomSubspline::getLength() const { return end_s_ - start_s_; }
 
 std::optional<double> CatmullRomSubspline::getCollisionPointIn2D(
-  const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward) const
+  const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward,
+  std::optional<std::pair<double, double>> s_part) const
 {
   /// @note Make sure end is greater than start, otherwise the spline is invalid
   if (end_s_ < start_s_) {
@@ -37,7 +38,7 @@ std::optional<double> CatmullRomSubspline::getCollisionPointIn2D(
       "contact the developer of traffic_simulator.");
   }
 
-  std::set<double> s_value_candidates = spline_->getCollisionPointsIn2D(polygon);
+  std::set<double> s_value_candidates = spline_->getCollisionPointsIn2D(polygon, false, s_part);
 
   if (s_value_candidates.empty()) {
     return std::nullopt;

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -331,11 +331,14 @@ auto ActionNode::getDistanceToTargetEntity(
       target_lanelet_pose) {
     const auto & from_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose();
     const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox();
-    if (const auto bounding_box_distances =
-          traffic_simulator::distance::laneLongitudinalDistances(
-            *from_lanelet_pose, from_bounding_box, *target_lanelet_pose, target_bounding_box,
-            include_adjacent_lanelet, include_opposite_direction, routing_configuration,
-            hdmap_utils);
+    const auto bounding_box_map_points = math::geometry::transformPoints(
+      static_cast<geometry_msgs::msg::Pose>(*target_lanelet_pose),
+      math::geometry::getPointsFromBbox(target_bounding_box));
+    const auto bounding_box_diagonal_length =
+      math::geometry::getDistance(bounding_box_map_points[0], bounding_box_map_points[2]);
+    if (const auto bounding_box_distances = traffic_simulator::distance::laneLongitudinalDistances(
+          *from_lanelet_pose, from_bounding_box, *target_lanelet_pose, target_bounding_box,
+          include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils);
         !bounding_box_distances || bounding_box_distances.value().first < 0.0) {
       return std::nullopt;
     } else {
@@ -352,7 +355,11 @@ auto ActionNode::getDistanceToTargetEntity(
       /// @note if the distance of the target entity to the spline cannot be calculated because a collision occurs
       else if (const auto target_polygon = math::geometry::transformPoints(
                  status.getMapPose(), math::geometry::getPointsFromBbox(target_bounding_box));
-               spline.getCollisionPointIn2D(target_polygon, search_backward)) {
+               spline.getCollisionPointIn2D(
+                 target_polygon, search_backward,
+                 std::make_pair(
+                   bounding_box_distances.value().first,
+                   bounding_box_distances.value().first + bounding_box_diagonal_length))) {
         return target_bounding_box_distance;
       }
     }