From 90b34466aa30cb4f5f66d92ae8bbebaab2e47364 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 22 Jan 2025 18:02:03 +0100 Subject: [PATCH 1/3] Utilize other overloads and add named constexpr parameters Signed-off-by: Mateusz Palczuk --- .../traffic_simulator/src/lanelet_wrapper/distance.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp index dddd02c8334..5bdfde8f1d0 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp @@ -248,17 +248,16 @@ auto distanceToCrosswalk(const std::vector & 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 { + 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 From f3b37827b6450215d82774c7b1eb49132dbbee84 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Wed, 22 Jan 2025 18:03:33 +0100 Subject: [PATCH 2/3] Use vector instead of set when many values are inserted and search is performed only once Signed-off-by: Mateusz Palczuk --- simulation/traffic_simulator/src/utils/distance.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 8f6dc364914..91711a03fb7 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -94,7 +94,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; @@ -296,7 +296,7 @@ auto distanceToYieldStop( return ret; }; - std::set distances; + std::vector 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) { @@ -309,14 +309,14 @@ auto distanceToYieldStop( helper::constructLaneletPose(lanelet_id, 0.0, 0.0), static_cast(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; From 1649df06ab880b6e8a16a810c2167b0e2246903a Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 23 Jan 2025 10:34:37 +0100 Subject: [PATCH 3/3] 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 --- .../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 + { + std::lock_guard lock(mutex_); + return data_.at(lanelet_id); + } + + auto readDataSpline(const lanelet::Id lanelet_id) -> std::shared_ptr + { + std::lock_guard lock(mutex_); + return splines_.at(lanelet_id); + } + auto appendData(const lanelet::Id lanelet_id, const std::vector & 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_);