Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' into …

…ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-distance
tier4 · Jan 23, 2025 · a642982 · a642982
2 parents f3b3782 + 1649df0
commit a642982
Showing 1 changed file with 33 additions and 16 deletions.
diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp
@@ -97,8 +97,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)
@@ -109,8 +108,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);
     }
   }
 
@@ -122,6 +120,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
@@ -142,17 +147,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)
@@ -161,8 +164,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(
@@ -172,8 +174,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:
@@ -183,6 +184,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_);
@@ -225,8 +238,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
@@ -235,8 +247,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:
@@ -246,6 +257,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_);