Skip to content

Commit

Permalink
ref(traffic_simulator): apply sonar required changes
Browse files Browse the repository at this point in the history
  • Loading branch information
dmoszynski committed Jan 7, 2025
1 parent 4b8616d commit 2645a65
Show file tree
Hide file tree
Showing 9 changed files with 57 additions and 61 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ class RouteCache
public:
auto exists(lanelet::Id from, lanelet::Id to, bool allow_lane_change)
{
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
std::tuple<lanelet::Id, lanelet::Id, bool> key = {from, to, allow_lane_change};
return data_.find(key) != data_.end();
}
Expand All @@ -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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_.at({from, to, allow_lane_change});
}
}
Expand All @@ -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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
data_[{from, to, allow_lane_change}] = route;
}

Expand All @@ -68,7 +68,7 @@ class CenterPointsCache
public:
auto exists(lanelet::Id lanelet_id) -> bool
{
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_.find(lanelet_id) != data_.end();
}

Expand All @@ -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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_.at(lanelet_id);
}

Expand All @@ -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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return splines_[lanelet_id];
}

auto appendData(lanelet::Id lanelet_id, const std::vector<geometry_msgs::msg::Point> & route)
{
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
data_[lanelet_id] = route;
splines_[lanelet_id] = std::make_shared<math::geometry::CatmullRomSpline>(route);
}
Expand All @@ -110,7 +110,7 @@ class LaneletLengthCache
public:
auto exists(lanelet::Id lanelet_id)
{
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_.find(lanelet_id) != data_.end();
}

Expand All @@ -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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_[lanelet_id];
}

auto appendData(lanelet::Id lanelet_id, double length)
{
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
data_[lanelet_id] = length;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ class RouteCache
shortest_path_ids);
}
}
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_.at({from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change});
}

Expand All @@ -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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_.at({from, to, allow_lane_change});
}
}
Expand All @@ -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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
std::tuple<lanelet::Id, lanelet::Id, bool> key = {from, to, allow_lane_change};
return data_.find(key) != data_.end();
}
Expand All @@ -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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
data_[{from, to, allow_lane_change}] = route;
}
};
Expand All @@ -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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_.at(lanelet_id);
}

Expand All @@ -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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return splines_.at(lanelet_id);
}

Expand All @@ -157,7 +157,7 @@ class CenterPointsCache
if (!exists(lanelet_id)) {
appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map));
}
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_.at(lanelet_id);
}

Expand All @@ -168,7 +168,7 @@ class CenterPointsCache
if (!exists(lanelet_id)) {
appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map));
}
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return splines_.at(lanelet_id);
}

Expand All @@ -178,13 +178,13 @@ class CenterPointsCache

auto exists(const lanelet::Id lanelet_id) -> bool
{
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_.find(lanelet_id) != data_.end();
}

auto appendData(const lanelet::Id lanelet_id, const std::vector<Point> & route) -> void
{
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
data_[lanelet_id] = route;
splines_[lanelet_id] = std::make_shared<Spline>(route);
}
Expand Down Expand Up @@ -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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_.at(lanelet_id);
}

Expand All @@ -230,7 +230,7 @@ class LaneletLengthCache
appendData(
lanelet_id, lanelet::utils::getLaneletLength2d(lanelet_map->laneletLayer.get(lanelet_id)));
}
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
return data_.at(lanelet_id);
}

Expand All @@ -239,13 +239,13 @@ class LaneletLengthCache

auto exists(const lanelet::Id lanelet_id) -> bool
{
std::lock_guard<std::mutex> 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<std::mutex> lock(mutex_);
std::lock_guard lock(mutex_);
data_[lanelet_id] = length;
}
};
Expand All @@ -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 &;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -40,7 +41,7 @@ class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanV
{
using lanelet::AttributeValueString;
using lanelet::Participants;
const static std::map<std::string, std::vector<std::string>> participants_map{
const static std::map<std::string, std::vector<std::string>, std::less<>> participants_map{
// clang-format off
{"", {Participants::Vehicle}},
{AttributeValueString::Road, {Participants::Vehicle, Participants::Bicycle}},
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -988,7 +988,7 @@ auto HdMapUtils::getLongitudinalDistance(
const traffic_simulator::RoutingConfiguration & routing_configuration) const
-> std::optional<double>
{
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 =
Expand Down
28 changes: 14 additions & 14 deletions simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -72,17 +72,17 @@ auto nearbyLaneletIds(
auto excludeSubtypeLanelets =
[](
const std::vector<std::pair<double, lanelet::Lanelet>> & pair_distance_lanelet,
const char subtype[]) -> std::vector<std::pair<double, lanelet::Lanelet>> {
std::vector<std::pair<double, lanelet::Lanelet>> 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<std::pair<double, lanelet::Lanelet>> 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),
Expand Down Expand Up @@ -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;
Expand All @@ -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<lanelet::Id> next_lanelet_ids_set;
Expand Down Expand Up @@ -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;
Expand All @@ -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<lanelet::Id> previous_lanelet_ids_set;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::mutex> 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:
Expand All @@ -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:
Expand Down Expand Up @@ -105,7 +102,7 @@ LaneletWrapper::LaneletWrapper(const std::filesystem::path & lanelet_map_path)

LaneletWrapper & LaneletWrapper::getInstance()
{
std::lock_guard<std::mutex> 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
Expand Down
Loading

0 comments on commit 2645a65

Please sign in to comment.