Review changes

Author: gmajrobotec

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

@@ -51,13 +51,14 @@ class CatmullRomSpline : public CatmullRomSplineInterface
     -> geometry_msgs::msg::Vector3;
   auto getCollisionPointsIn2D(
     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>;
+    const std::optional<std::pair<double, double>> & s_range = 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,
-    std::optional<std::pair<double, double>> s_part = std::nullopt) const
+    const std::optional<std::pair<double, double>> & s_range = 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>;

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

@@ -33,7 +33,7 @@ class CatmullRomSplineInterface
   virtual double getLength() const = 0;
   virtual std::optional<double> getCollisionPointIn2D(
     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;
+    const std::optional<std::pair<double, double>> & s_range = 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

@@ -44,7 +44,7 @@ class CatmullRomSubspline : public CatmullRomSplineInterface
 
   std::optional<double> getCollisionPointIn2D(
     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;
+    const std::optional<std::pair<double, double>> & s_range = 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

@@ -280,7 +280,7 @@ 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,
-  std::optional<std::pair<double, double>> s_part) const -> std::set<double>
+  const std::optional<std::pair<double, double>> & s_range) const -> std::set<double>
 {
   if (polygon.size() <= 1) {
     THROW_SIMULATION_ERROR(
@@ -290,23 +290,27 @@ auto CatmullRomSpline::getCollisionPointsIn2D(
       "developer of traffic_simulator.");
   }
   /// @note If the spline has three or more control points.
-  const auto get_collision_point_2d_with_curve =
-    [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) {
-        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));
+  const auto get_collision_point_2d_with_curve = [this, &s_range](
+                                                   const auto & local_polygon,
+                                                   const auto local_search_backward) {
+    std::set<double> s_value_candidates;
+    auto current_curve_start_s = 0.0;
+    for (size_t i = 0; i < curves_.size(); ++i) {
+      if (
+        s_range == std::nullopt ||
+        (current_curve_start_s >= s_range->first && current_curve_start_s <= s_range->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, &current_curve_start_s, this](const auto & s) {
+            s_value_candidates.insert(current_curve_start_s + s);
           });
-        }
-        s_all += curves_[i].getLength();
       }
-      return s_value_candidates;
-    };
+      current_curve_start_s += curves_[i].getLength();
+    }
+    return s_value_candidates;
+  };
   /// @note If the spline has two control points. (Same as single line segment.)
   const auto get_collision_point_2d_with_line = [this](const auto & local_polygon) {
     std::set<double> s_value_candidates;
@@ -363,9 +367,9 @@ auto CatmullRomSpline::getCollisionPointsIn2D(
  */
 auto CatmullRomSpline::getCollisionPointIn2D(
   const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward,
-  std::optional<std::pair<double, double>> s_part) const -> std::optional<double>
+  const std::optional<std::pair<double, double>> & s_range) const -> std::optional<double>
 {
-  std::set<double> s_value_candidates = getCollisionPointsIn2D(polygon, search_backward, s_part);
+  std::set<double> s_value_candidates = getCollisionPointsIn2D(polygon, search_backward, s_range);
   if (s_value_candidates.empty()) {
     return std::nullopt;
   }

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

@@ -25,7 +25,7 @@ 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,
-  std::optional<std::pair<double, double>> s_part) const
+  const std::optional<std::pair<double, double>> & s_range) const
 {
   /// @note Make sure end is greater than start, otherwise the spline is invalid
   if (end_s_ < start_s_) {
@@ -38,7 +38,8 @@ std::optional<double> CatmullRomSubspline::getCollisionPointIn2D(
       "contact the developer of traffic_simulator.");
   }
 
-  std::set<double> s_value_candidates = spline_->getCollisionPointsIn2D(polygon, false, s_part);
+  std::set<double> s_value_candidates =
+    spline_->getCollisionPointsIn2D(polygon, search_backward, s_range);
 
   if (s_value_candidates.empty()) {
     return std::nullopt;

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -336,19 +336,26 @@ auto ActionNode::getDistanceToTargetEntity(
       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) {
+    if (const auto position_distance = traffic_simulator::distance::longitudinalDistance(
+          *from_lanelet_pose, *target_lanelet_pose, include_adjacent_lanelet,
+          include_opposite_direction, routing_configuration, hdmap_utils);
+        !position_distance) {
+      return std::nullopt;
+    } else if (const auto bounding_box_distance =
+                 traffic_simulator::distance::boundingBoxLaneLongitudinalDistance(
+                   position_distance, from_bounding_box, target_bounding_box);
+               !bounding_box_distance || bounding_box_distance.value() < 0.0) {
       return std::nullopt;
     } else {
+      // TODO rotation of NPC is not taken into account, same as in boundingBoxLaneLongitudinalDistance
+      // this should be considered to be changed in separate task in the future
       const auto target_bounding_box_distance =
-        bounding_box_distances.value().first + from_bounding_box.dimensions.x / 2.0;
+        bounding_box_distance.value() + from_bounding_box.dimensions.x / 2.0;
 
       /// @note if the distance of the target entity to the spline is smaller than the width of the reference entity
       if (const auto target_to_spline_distance = traffic_simulator::distance::distanceToSpline(
             static_cast<geometry_msgs::msg::Pose>(*target_lanelet_pose), target_bounding_box,
-            spline, bounding_box_distances.value().second);
+            spline, position_distance.value());
           target_to_spline_distance <= from_bounding_box.dimensions.y / 2.0) {
         return target_bounding_box_distance;
       }
@@ -358,8 +365,8 @@ auto ActionNode::getDistanceToTargetEntity(
                spline.getCollisionPointIn2D(
                  target_polygon, search_backward,
                  std::make_pair(
-                   bounding_box_distances.value().first,
-                   bounding_box_distances.value().first + bounding_box_diagonal_length))) {
+                   bounding_box_distance.value(),
+                   bounding_box_distance.value() + bounding_box_diagonal_length))) {
         return target_bounding_box_distance;
       }
     }

simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp

@@ -71,14 +71,10 @@ auto boundingBoxLaneLongitudinalDistance(
   const traffic_simulator::RoutingConfiguration & routing_configuration,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
 
-auto laneLongitudinalDistances(
-  const CanonicalizedLaneletPose & from,
+auto boundingBoxLaneLongitudinalDistance(
+  const std::optional<double> & longitudinal_distance,
   const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box,
-  const CanonicalizedLaneletPose & to,
-  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool include_adjacent_lanelet,
-  bool include_opposite_direction,
-  const traffic_simulator::RoutingConfiguration & routing_configuration,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<std::pair<double,double>>;
+  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box) -> std::optional<double>;
 
 // Bounds
 auto distanceToLaneBound(

simulation/traffic_simulator/src/utils/distance.cpp

@@ -197,19 +197,12 @@ auto boundingBoxLaneLongitudinalDistance(
   return std::nullopt;
 }
 
-auto laneLongitudinalDistances(
-  const CanonicalizedLaneletPose & from,
+auto boundingBoxLaneLongitudinalDistance(
+  const std::optional<double> & longitudinal_distance,
   const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box,
-  const CanonicalizedLaneletPose & to,
-  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool include_adjacent_lanelet,
-  bool include_opposite_direction,
-  const traffic_simulator::RoutingConfiguration & routing_configuration,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<std::pair<double,double>>
+  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box) -> std::optional<double>
 {
-  if (const auto longitudinal_distance = longitudinalDistance(
-        from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration,
-        hdmap_utils_ptr);
-      longitudinal_distance) {
+  if (longitudinal_distance) {
     const auto from_bounding_box_distances =
       math::geometry::getDistancesFromCenterToEdge(from_bounding_box);
     const auto to_bounding_box_distances =
@@ -222,7 +215,7 @@ auto laneLongitudinalDistances(
       bounding_box_distance =
         +std::abs(from_bounding_box_distances.rear) + std::abs(to_bounding_box_distances.front);
     }
-    return std::make_pair(longitudinal_distance.value() + bounding_box_distance, longitudinal_distance.value());
+    return longitudinal_distance.value() + bounding_box_distance;
   }
   return std::nullopt;
 }