From 1649df06ab880b6e8a16a810c2167b0e2246903a Mon Sep 17 00:00:00 2001
From: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
Date: Thu, 23 Jan 2025 10:34:37 +0100
Subject: [PATCH] Add reader functions to cache classes to mitigate direct data
 members access and having to lock the mutex in different places

Signed-off-by: Mateusz Palczuk <mateusz.palczuk@robotec.ai>
---
 .../lanelet_wrapper/lanelet_wrapper.hpp       | 49 +++++++++++++------
 1 file changed, 33 insertions(+), 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
index aa0db059390..b0bfca88a94 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp
@@ -95,8 +95,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)
@@ -107,8 +106,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);
     }
   }
 
@@ -120,6 +118,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
@@ -140,8 +145,7 @@ 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)
@@ -149,8 +153,7 @@ 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 splines_.at(lanelet_id);
+    return readDataSpline(lanelet_id);
   }
 
   auto getCenterPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map)
@@ -159,8 +162,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(
@@ -170,8 +172,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:
@@ -181,6 +182,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_);
@@ -223,8 +236,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
@@ -233,8 +245,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:
@@ -244,6 +255,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_);