Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-distance' i…

…nto ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-distance-step-2

simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp

@@ -100,8 +100,7 @@ class RouteCache
           shortest_path_ids);
       }
     }
-    std::lock_guard lock(mutex_);
-    return data_.at({from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change});
+    return readData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change);
   }
 
   auto getRoute(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change)
@@ -112,8 +111,7 @@ class RouteCache
         "route from : ", from, " to : ", to, (allow_lane_change ? " with" : " without"),
         " lane change does not exists on route cache.");
     } else {
-      std::lock_guard lock(mutex_);
-      return data_.at({from, to, allow_lane_change});
+      return readData(from, to, allow_lane_change);
     }
   }
 
@@ -125,6 +123,13 @@ class RouteCache
     return data_.find(key) != data_.end();
   }
 
+  auto readData(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change)
+    -> lanelet::Ids
+  {
+    std::lock_guard lock(mutex_);
+    return data_.at({from, to, allow_lane_change});
+  }
+
   auto appendData(
     const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change,
     const lanelet::Ids & route) -> void
@@ -145,17 +150,15 @@ class CenterPointsCache
     if (!exists(lanelet_id)) {
       THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache.");
     }
-    std::lock_guard lock(mutex_);
-    return data_.at(lanelet_id);
+    return readData(lanelet_id);
   }
 
   auto centerPointsSpline(lanelet::Id lanelet_id) -> decltype(auto)
   {
     if (!exists(lanelet_id)) {
       THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache.");
     }
-    std::lock_guard lock(mutex_);
-    return splines_.at(lanelet_id);
+    return readDataSpline(lanelet_id);
   }
 
   auto getCenterPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map)
@@ -164,8 +167,7 @@ class CenterPointsCache
     if (!exists(lanelet_id)) {
       appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map));
     }
-    std::lock_guard lock(mutex_);
-    return data_.at(lanelet_id);
+    return readData(lanelet_id);
   }
 
   auto getCenterPointsSpline(
@@ -175,8 +177,7 @@ class CenterPointsCache
     if (!exists(lanelet_id)) {
       appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map));
     }
-    std::lock_guard lock(mutex_);
-    return splines_.at(lanelet_id);
+    return readDataSpline(lanelet_id);
   }
 
 private:
@@ -186,6 +187,18 @@ class CenterPointsCache
     return data_.find(lanelet_id) != data_.end();
   }
 
+  auto readData(const lanelet::Id lanelet_id) -> std::vector<Point>
+  {
+    std::lock_guard lock(mutex_);
+    return data_.at(lanelet_id);
+  }
+
+  auto readDataSpline(const lanelet::Id lanelet_id) -> std::shared_ptr<Spline>
+  {
+    std::lock_guard lock(mutex_);
+    return splines_.at(lanelet_id);
+  }
+
   auto appendData(const lanelet::Id lanelet_id, const std::vector<Point> & route) -> void
   {
     std::lock_guard lock(mutex_);
@@ -228,8 +241,7 @@ class LaneletLengthCache
     if (!exists(lanelet_id)) {
       THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache.");
     }
-    std::lock_guard lock(mutex_);
-    return data_.at(lanelet_id);
+    return readData(lanelet_id);
   }
 
   auto getLength(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> double
@@ -238,8 +250,7 @@ class LaneletLengthCache
       appendData(
         lanelet_id, lanelet::utils::getLaneletLength2d(lanelet_map->laneletLayer.get(lanelet_id)));
     }
-    std::lock_guard lock(mutex_);
-    return data_.at(lanelet_id);
+    return readData(lanelet_id);
   }
 
 private:
@@ -249,6 +260,12 @@ class LaneletLengthCache
     return data_.find(lanelet_id) != data_.end();
   }
 
+  auto readData(const lanelet::Id lanelet_id) -> double
+  {
+    std::lock_guard lock(mutex_);
+    return data_.at(lanelet_id);
+  }
+
   auto appendData(const lanelet::Id lanelet_id, double length) -> void
   {
     std::lock_guard lock(mutex_);

simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp

@@ -248,17 +248,16 @@ auto distanceToCrosswalk(const std::vector<Point> & route_waypoints, const lanel
   if (route_waypoints.empty()) {
     return std::nullopt;
   } else {
-    const Spline route_spline(route_waypoints);
-    return route_spline.getCollisionPointIn2D(
-      lanelet_wrapper::lanelet_map::laneletPolygon(crosswalk_id));
+    return distanceToCrosswalk(Spline{route_waypoints}, crosswalk_id);
   }
 }
 
 auto distanceToCrosswalk(const SplineInterface & route_spline, const lanelet::Id crosswalk_id)
   -> std::optional<double>
 {
+  constexpr bool search_in_backward_direction{false};
   return route_spline.getCollisionPointIn2D(
-    lanelet_wrapper::lanelet_map::laneletPolygon(crosswalk_id), false);
+    lanelet_wrapper::lanelet_map::laneletPolygon(crosswalk_id), search_in_backward_direction);
 }
 }  // namespace distance
 }  // namespace lanelet_wrapper

simulation/traffic_simulator/src/utils/distance.cpp

@@ -97,7 +97,7 @@ auto longitudinalDistance(
     /**
      * @brief A matching distance of about 1.5*lane widths is given as the matching distance to match the
      * Entity present on the adjacent Lanelet.
-     * The length of the horizontal bar must intersect with the adjacent lanelet, 
+     * The length of the horizontal bar must intersect with the adjacent lanelet,
      * so it is always 10m regardless of the entity type.
      */
     constexpr double matching_distance = 5.0;
@@ -319,7 +319,7 @@ auto distanceToYieldStop(
     return ret;
   };
 
-  std::set<double> distances;
+  std::vector<double> distances;
   for (const auto & lanelet_id : following_lanelets) {
     const auto right_of_way_ids = lanelet_wrapper::lanelet_map::rightOfWayLaneletIds(lanelet_id);
     for (const auto right_of_way_id : right_of_way_ids) {
@@ -332,14 +332,14 @@ auto distanceToYieldStop(
           helper::constructLaneletPose(lanelet_id, 0.0, 0.0),
           static_cast<LaneletPose>(reference_pose), RoutingConfiguration());
         if (distance_forward) {
-          distances.insert(distance_forward.value());
+          distances.push_back(distance_forward.value());
         } else if (distance_backward) {
-          distances.insert(-distance_backward.value());
+          distances.push_back(-distance_backward.value());
         }
       }
     }
-    if (distances.size() != 0) {
-      return *distances.begin();
+    if (!distances.empty()) {
+      return *std::min_element(distances.begin(), distances.end());
     }
   }
   return std::nullopt;