From 2645a653162bf9cc7ed7e998f64c22e5387a67ee Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Tue, 7 Jan 2025 12:57:19 +0100 Subject: [PATCH] ref(traffic_simulator): apply sonar required changes --- .../traffic_simulator/hdmap_utils/cache.hpp | 20 +++++------ .../lanelet_wrapper/lanelet_map.hpp | 8 ++--- .../lanelet_wrapper/lanelet_wrapper.hpp | 34 +++++++++---------- .../lanelet_wrapper/traffic_rules.hpp | 3 +- .../src/hdmap_utils/hdmap_utils.cpp | 2 +- .../src/lanelet_wrapper/lanelet_map.cpp | 28 +++++++-------- .../src/lanelet_wrapper/lanelet_wrapper.cpp | 13 +++---- .../src/lanelet_wrapper/pose.cpp | 8 ++--- .../src/utils/lanelet_map.cpp | 2 +- 9 files changed, 57 insertions(+), 61 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp index f8895a0a3c2..fa20308d0cc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp @@ -31,7 +31,7 @@ class RouteCache public: auto exists(lanelet::Id from, lanelet::Id to, bool allow_lane_change) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); std::tuple key = {from, to, allow_lane_change}; return data_.find(key) != data_.end(); } @@ -44,7 +44,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_); + std::lock_guard lock(mutex_); return data_.at({from, to, allow_lane_change}); } } @@ -53,7 +53,7 @@ class RouteCache lanelet::Id from, lanelet::Id to, const bool allow_lane_change, const lanelet::Ids & route) -> void { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[{from, to, allow_lane_change}] = route; } @@ -68,7 +68,7 @@ class CenterPointsCache public: auto exists(lanelet::Id lanelet_id) -> bool { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } @@ -77,7 +77,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_); + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -86,13 +86,13 @@ 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_); + std::lock_guard lock(mutex_); return splines_[lanelet_id]; } auto appendData(lanelet::Id lanelet_id, const std::vector & route) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[lanelet_id] = route; splines_[lanelet_id] = std::make_shared(route); } @@ -110,7 +110,7 @@ class LaneletLengthCache public: auto exists(lanelet::Id lanelet_id) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } @@ -119,13 +119,13 @@ class LaneletLengthCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_[lanelet_id]; } auto appendData(lanelet::Id lanelet_id, double length) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[lanelet_id] = length; } diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp index b0969574350..ebda8439df7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp @@ -67,11 +67,11 @@ auto nextLaneletIds( const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; auto nextLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, + const lanelet::Id lanelet_id, std::string_view turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; auto nextLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; auto previousLaneletIds( @@ -83,11 +83,11 @@ auto previousLaneletIds( const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; auto previousLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, + const lanelet::Id lanelet_id, std::string_view turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; auto previousLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; } // namespace lanelet_map } // namespace lanelet_wrapper 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 765e73079eb..7d01b2a7dee 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 @@ -94,7 +94,7 @@ class RouteCache shortest_path_ids); } } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at({from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change}); } @@ -106,7 +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_); + std::lock_guard lock(mutex_); return data_.at({from, to, allow_lane_change}); } } @@ -116,7 +116,7 @@ class RouteCache auto exists(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) -> bool { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); std::tuple key = {from, to, allow_lane_change}; return data_.find(key) != data_.end(); } @@ -125,7 +125,7 @@ class RouteCache const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change, const lanelet::Ids & route) -> void { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[{from, to, allow_lane_change}] = route; } }; @@ -138,7 +138,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_); + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -147,7 +147,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_); + std::lock_guard lock(mutex_); return splines_.at(lanelet_id); } @@ -157,7 +157,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -168,7 +168,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return splines_.at(lanelet_id); } @@ -178,13 +178,13 @@ class CenterPointsCache auto exists(const lanelet::Id lanelet_id) -> bool { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } auto appendData(const lanelet::Id lanelet_id, const std::vector & route) -> void { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[lanelet_id] = route; splines_[lanelet_id] = std::make_shared(route); } @@ -220,7 +220,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_); + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -230,7 +230,7 @@ class LaneletLengthCache appendData( lanelet_id, lanelet::utils::getLaneletLength2d(lanelet_map->laneletLayer.get(lanelet_id))); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -239,13 +239,13 @@ class LaneletLengthCache auto exists(const lanelet::Id lanelet_id) -> bool { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } auto appendData(const lanelet::Id lanelet_id, double length) -> void { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[lanelet_id] = length; } }; @@ -270,11 +270,11 @@ class LaneletWrapper public: static auto activate(const std::string & lanelet_map_path) -> void; - [[nodiscard]] static auto map() -> const lanelet::LaneletMapPtr; + [[nodiscard]] static auto map() -> lanelet::LaneletMapPtr; [[nodiscard]] static auto routingGraph(const RoutingGraphType type) - -> const lanelet::routing::RoutingGraphConstPtr; + -> lanelet::routing::RoutingGraphConstPtr; [[nodiscard]] static auto trafficRules(const RoutingGraphType type) - -> const lanelet::traffic_rules::TrafficRulesPtr; + -> lanelet::traffic_rules::TrafficRulesPtr; [[nodiscard]] static auto routeCache(const RoutingGraphType type) -> RouteCache &; [[nodiscard]] static auto centerPointsCache() -> CenterPointsCache &; diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp index cbeda304ca0..f96a3486a75 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp @@ -31,6 +31,7 @@ struct Locations class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanVehicle { public: + using lanelet::traffic_rules::GenericTrafficRules::canPass; using lanelet::traffic_rules::GermanVehicle::GermanVehicle; protected: @@ -40,7 +41,7 @@ class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanV { using lanelet::AttributeValueString; using lanelet::Participants; - const static std::map> participants_map{ + const static std::map, std::less<>> participants_map{ // clang-format off {"", {Participants::Vehicle}}, {AttributeValueString::Road, {Participants::Vehicle, Participants::Bicycle}}, diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 90d2ed5ef55..af964f22341 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -988,7 +988,7 @@ auto HdMapUtils::getLongitudinalDistance( const traffic_simulator::RoutingConfiguration & routing_configuration) const -> std::optional { - const auto is_lane_change_required = [this, routing_configuration]( + const auto is_lane_change_required = [routing_configuration]( const lanelet::Id current_lanelet, const lanelet::Id next_lanelet) -> bool { const auto next_lanelet_ids = diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp index 4cf778efeb2..3bf62e09fb0 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -72,17 +72,17 @@ auto nearbyLaneletIds( auto excludeSubtypeLanelets = []( const std::vector> & pair_distance_lanelet, - const char subtype[]) -> std::vector> { - std::vector> filtered_lanelets; - for (const auto & pair : pair_distance_lanelet) { - if ( - pair.second.hasAttribute(lanelet::AttributeName::Subtype) && - pair.second.attribute(lanelet::AttributeName::Subtype).value() != subtype) { - filtered_lanelets.push_back(pair); + const char subtype[]) { + std::vector> filtered_lanelets; + for (const auto & pair : pair_distance_lanelet) { + if ( + pair.second.hasAttribute(lanelet::AttributeName::Subtype) && + pair.second.attribute(lanelet::AttributeName::Subtype).value() != subtype) { + filtered_lanelets.push_back(pair); + } } - } - return filtered_lanelets; - }; + return filtered_lanelets; + }; auto nearest_lanelets = lanelet::geometry::findNearest( LaneletWrapper::map()->laneletLayer, lanelet::BasicPoint2d(point.x, point.y), @@ -158,7 +158,7 @@ auto nextLaneletIds(const lanelet::Ids & lanelet_ids, const RoutingGraphType typ } auto nextLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, const RoutingGraphType type) + const lanelet::Id lanelet_id, std::string_view turn_direction, const RoutingGraphType type) -> lanelet::Ids { lanelet::Ids next_lanelet_ids; @@ -173,7 +173,7 @@ auto nextLaneletIds( } auto nextLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, const RoutingGraphType type) + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type) -> lanelet::Ids { std::set next_lanelet_ids_set; @@ -206,7 +206,7 @@ auto previousLaneletIds(const lanelet::Ids & lanelet_ids, const RoutingGraphType } auto previousLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, const RoutingGraphType type) + const lanelet::Id lanelet_id, std::string_view turn_direction, const RoutingGraphType type) -> lanelet::Ids { lanelet::Ids previous_lanelet_ids; @@ -221,7 +221,7 @@ auto previousLaneletIds( } auto previousLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, const RoutingGraphType type) + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type) -> lanelet::Ids { std::set previous_lanelet_ids_set; diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp index 11dfaabf7d5..afb8248cbd7 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp @@ -24,18 +24,15 @@ auto LaneletWrapper::activate(const std::string & lanelet_map_path) -> void { lanelet_map_path_ = lanelet_map_path; if (instance) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); instance.reset(); } } -auto LaneletWrapper::map() -> const lanelet::LaneletMapPtr -{ - return getInstance().lanelet_map_ptr_; -} +auto LaneletWrapper::map() -> lanelet::LaneletMapPtr { return getInstance().lanelet_map_ptr_; } auto LaneletWrapper::routingGraph(const RoutingGraphType type) - -> const lanelet::routing::RoutingGraphConstPtr + -> lanelet::routing::RoutingGraphConstPtr { switch (type) { case RoutingGraphType::VEHICLE: @@ -52,7 +49,7 @@ auto LaneletWrapper::routingGraph(const RoutingGraphType type) } auto LaneletWrapper::trafficRules(const RoutingGraphType type) - -> const lanelet::traffic_rules::TrafficRulesPtr + -> lanelet::traffic_rules::TrafficRulesPtr { switch (type) { case RoutingGraphType::VEHICLE: @@ -105,7 +102,7 @@ LaneletWrapper::LaneletWrapper(const std::filesystem::path & lanelet_map_path) LaneletWrapper & LaneletWrapper::getInstance() { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); if (!instance) { if (!lanelet_map_path_.empty()) { /// `new` is intentionally used here instead of `make_unique` since the LaneletWrapper constructor is private diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 662842960c8..d620aa825df 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -205,8 +205,7 @@ auto toLaneletPoses( auto alternativeLaneletPoses(const LaneletPose & reference_lanelet_pose) -> std::vector { - const auto alternativesInPreviousLanelet = - [](const auto & lanelet_pose) -> std::vector { + const auto alternativesInPreviousLanelet = [](const auto & lanelet_pose) { std::vector lanelet_poses_in_previous_lanelet; for (const auto & previous_lanelet_id : lanelet_map::previousLaneletIds(lanelet_pose.lanelet_id)) { @@ -226,7 +225,7 @@ auto alternativeLaneletPoses(const LaneletPose & reference_lanelet_pose) -> std: return lanelet_poses_in_previous_lanelet; }; - const auto alternativesInNextLanelet = [](const auto & lanelet_pose) -> std::vector { + const auto alternativesInNextLanelet = [](const auto & lanelet_pose) { std::vector lanelet_poses_in_next_lanelet; for (const auto & next_lanelet_id : lanelet_map::nextLaneletIds(lanelet_pose.lanelet_id)) { const auto lanelet_pose_in_next_lanelet = helper::constructLaneletPose( @@ -393,8 +392,7 @@ auto matchToLane( const double matching_distance, const double reduction_ratio, const RoutingGraphType type) -> std::optional { - const auto absoluteHullPolygon = [&reduction_ratio, - &bounding_box](const auto & pose) -> lanelet::BasicPolygon2d { + const auto absoluteHullPolygon = [&reduction_ratio, &bounding_box](const auto & pose) { auto relative_hull = lanelet::matching::Hull2d{ lanelet::BasicPoint2d{ bounding_box.center.x + bounding_box.dimensions.x * 0.5 * reduction_ratio, diff --git a/simulation/traffic_simulator/src/utils/lanelet_map.cpp b/simulation/traffic_simulator/src/utils/lanelet_map.cpp index c3eea682c97..81566a28cc1 100644 --- a/simulation/traffic_simulator/src/utils/lanelet_map.cpp +++ b/simulation/traffic_simulator/src/utils/lanelet_map.cpp @@ -31,7 +31,7 @@ auto borderlinePoses() -> std::vector> LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lanelet_id; lanelet_pose.s = lanelet_map::laneletLength(lanelet_id); - borderline_poses.push_back({lanelet_id, pose::toMapPose(lanelet_pose)}); + borderline_poses.emplace_back(lanelet_id, pose::toMapPose(lanelet_pose)); } } return borderline_poses;