From 964ccabec064c11f5fd41d582653eeea54d5dba1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 14 Nov 2024 20:53:30 +0900 Subject: [PATCH] refactor: modify member function name of RoutingGraphs class --- .../hdmap_utils/hdmap_utils.hpp | 6 +- .../src/hdmap_utils/hdmap_utils.cpp | 92 +++++++++++-------- 2 files changed, 56 insertions(+), 42 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 643db3faf54..42c588c427b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -351,10 +351,10 @@ class HdMapUtils RouteCache route_cache; }; - [[nodiscard]] lanelet::routing::RoutingGraphPtr get( + [[nodiscard]] lanelet::routing::RoutingGraphPtr routing_graph( const traffic_simulator::RoutingGraphType type) const; - [[nodiscard]] lanelet::traffic_rules::TrafficRulesPtr getRules( + [[nodiscard]] lanelet::traffic_rules::TrafficRulesPtr traffic_rule( const traffic_simulator::RoutingGraphType type) const; [[nodiscard]] auto getRoute( @@ -363,7 +363,7 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type) -> lanelet::Ids; private: - [[nodiscard]] RouteCache & getRouteCache(const traffic_simulator::RoutingGraphType type); + [[nodiscard]] RouteCache & route_cache(const traffic_simulator::RoutingGraphType type); RuleWithGraph vehicle; diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index f48dece7148..002b69dde3c 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -78,8 +78,10 @@ HdMapUtils::HdMapUtils( } overwriteLaneletsCenterline(); std::vector all_graphs; - all_graphs.push_back(routing_graphs_->get(traffic_simulator::RoutingGraphType::VEHICLE)); - all_graphs.push_back(routing_graphs_->get(traffic_simulator::RoutingGraphType::PEDESTRIAN)); + all_graphs.push_back( + routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE)); + all_graphs.push_back( + routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::PEDESTRIAN)); shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(lanelet::utils::query::laneletLayer(lanelet_map_ptr_)); } @@ -397,7 +399,7 @@ auto HdMapUtils::getConflictingLaneIds(const lanelet::Ids & lanelet_ids) const - for (const auto & lanelet_id : lanelet_ids) { const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); const auto conflicting_lanelets = lanelet::utils::getConflictingLanelets( - routing_graphs_->get(traffic_simulator::RoutingGraphType::VEHICLE), lanelet); + routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE), lanelet); for (const auto & conflicting_lanelet : conflicting_lanelets) { ids.emplace_back(conflicting_lanelet.id()); } @@ -409,8 +411,9 @@ auto HdMapUtils::getConflictingCrosswalkIds(const lanelet::Ids & lanelet_ids) co { lanelet::Ids ids; std::vector graphs; - graphs.emplace_back(routing_graphs_->get(traffic_simulator::RoutingGraphType::VEHICLE)); - graphs.emplace_back(routing_graphs_->get(traffic_simulator::RoutingGraphType::PEDESTRIAN)); + graphs.emplace_back(routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE)); + graphs.emplace_back( + routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::PEDESTRIAN)); lanelet::routing::RoutingGraphContainer container(graphs); for (const auto & lanelet_id : lanelet_ids) { const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); @@ -546,7 +549,7 @@ auto HdMapUtils::matchToLane( lanelet::matching::getDeterministicMatches(*lanelet_map_ptr_, obj, matching_distance); if (!include_crosswalk) { matches = lanelet::matching::removeNonRuleCompliantMatches( - matches, routing_graphs_->getRules(traffic_simulator::RoutingGraphType::VEHICLE)); + matches, routing_graphs_->traffic_rule(traffic_simulator::RoutingGraphType::VEHICLE)); } if (matches.empty()) { return std::nullopt; @@ -730,8 +733,8 @@ auto HdMapUtils::getSpeedLimit(const lanelet::Ids & lanelet_ids) const -> double } for (auto itr = lanelet_ids.begin(); itr != lanelet_ids.end(); itr++) { const auto lanelet = lanelet_map_ptr_->laneletLayer.get(*itr); - const auto limit = - routing_graphs_->getRules(traffic_simulator::RoutingGraphType::VEHICLE)->speedLimit(lanelet); + const auto limit = routing_graphs_->traffic_rule(traffic_simulator::RoutingGraphType::VEHICLE) + ->speedLimit(lanelet); limits.push_back(lanelet::units::KmHQuantity(limit.speedLimit).value() / 3.6); } return *std::min_element(limits.begin(), limits.end()); @@ -772,15 +775,19 @@ auto HdMapUtils::getLaneChangeableLaneletId( target = lanelet.id(); break; case traffic_simulator::lane_change::Direction::LEFT: - if (routing_graphs_->get(traffic_simulator::RoutingGraphType::VEHICLE)->left(lanelet)) { - target = - routing_graphs_->get(traffic_simulator::RoutingGraphType::VEHICLE)->left(lanelet)->id(); + if (routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) + ->left(lanelet)) { + target = routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) + ->left(lanelet) + ->id(); } break; case traffic_simulator::lane_change::Direction::RIGHT: - if (routing_graphs_->get(traffic_simulator::RoutingGraphType::VEHICLE)->right(lanelet)) { - target = - routing_graphs_->get(traffic_simulator::RoutingGraphType::VEHICLE)->right(lanelet)->id(); + if (routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) + ->right(lanelet)) { + target = routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) + ->right(lanelet) + ->id(); } break; } @@ -992,7 +999,8 @@ auto HdMapUtils::getPreviousLaneletIds(const lanelet::Id lanelet_id) const -> la lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); for (const auto & llt : - routing_graphs_->get(traffic_simulator::RoutingGraphType::VEHICLE)->previous(lanelet)) { + routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) + ->previous(lanelet)) { ids.push_back(llt.id()); } for (const auto & id : getPreviousRoadShoulderLanelet(lanelet_id)) { @@ -1017,7 +1025,7 @@ auto HdMapUtils::getPreviousLaneletIds( { lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : routing_graphs_->get(type)->previous(lanelet)) { + for (const auto & llt : routing_graphs_->routing_graph(type)->previous(lanelet)) { if (llt.attributeOr("turn_direction", "else") == turn_direction) { ids.push_back(llt.id()); } @@ -1054,7 +1062,8 @@ auto HdMapUtils::getNextLaneletIds(const lanelet::Id lanelet_id) const -> lanele lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); for (const auto & llt : - routing_graphs_->get(traffic_simulator::RoutingGraphType::VEHICLE)->following(lanelet)) { + routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) + ->following(lanelet)) { ids.push_back(llt.id()); } for (const auto & id : getNextRoadShoulderLanelet(lanelet_id)) { @@ -1079,7 +1088,7 @@ auto HdMapUtils::getNextLaneletIds( { lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : routing_graphs_->get(type)->following(lanelet)) { + for (const auto & llt : routing_graphs_->routing_graph(type)->following(lanelet)) { if (llt.attributeOr("turn_direction", "else") == turn_direction) { ids.push_back(llt.id()); } @@ -1209,10 +1218,10 @@ auto HdMapUtils::getLeftLaneletIds( { if (include_opposite_direction) { return getLaneletIds( - routing_graphs_->get(type)->lefts(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); + routing_graphs_->routing_graph(type)->lefts(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); } else { - return getLaneletIds( - routing_graphs_->get(type)->adjacentLefts(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); + return getLaneletIds(routing_graphs_->routing_graph(type)->adjacentLefts( + lanelet_map_ptr_->laneletLayer.get(lanelet_id))); } } @@ -1222,10 +1231,10 @@ auto HdMapUtils::getRightLaneletIds( { if (include_opposite_direction) { return getLaneletIds( - routing_graphs_->get(type)->rights(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); + routing_graphs_->routing_graph(type)->rights(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); } else { - return getLaneletIds( - routing_graphs_->get(type)->adjacentRights(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); + return getLaneletIds(routing_graphs_->routing_graph(type)->adjacentRights( + lanelet_map_ptr_->laneletLayer.get(lanelet_id))); } } @@ -1434,7 +1443,7 @@ auto HdMapUtils::canChangeLane( { const auto from_lanelet = lanelet_map_ptr_->laneletLayer.get(from_lanelet_id); const auto to_lanelet = lanelet_map_ptr_->laneletLayer.get(to_lanelet_id); - return routing_graphs_->getRules(traffic_simulator::RoutingGraphType::VEHICLE) + return routing_graphs_->traffic_rule(traffic_simulator::RoutingGraphType::VEHICLE) ->canChangeLane(from_lanelet, to_lanelet); } @@ -2195,7 +2204,7 @@ HdMapUtils::RoutingGraphs::RoutingGraphs(const lanelet::LaneletMapPtr & lanelet_ pedestrian.graph = lanelet::routing::RoutingGraph::build(*lanelet_map, *pedestrian.rules); } -lanelet::routing::RoutingGraphPtr HdMapUtils::RoutingGraphs::get( +lanelet::routing::RoutingGraphPtr HdMapUtils::RoutingGraphs::routing_graph( const traffic_simulator::RoutingGraphType type) const { switch (type) { @@ -2204,11 +2213,13 @@ lanelet::routing::RoutingGraphPtr HdMapUtils::RoutingGraphs::get( case traffic_simulator::RoutingGraphType::PEDESTRIAN: return pedestrian.graph; default: - throw std::runtime_error("Invalid routing graph type"); + std::stringstream what; + what << "Invalid routing graph type: " << static_cast(type); + throw common::Error(what.str()); } } -lanelet::traffic_rules::TrafficRulesPtr HdMapUtils::RoutingGraphs::getRules( +lanelet::traffic_rules::TrafficRulesPtr HdMapUtils::RoutingGraphs::traffic_rule( const traffic_simulator::RoutingGraphType type) const { switch (type) { @@ -2217,12 +2228,13 @@ lanelet::traffic_rules::TrafficRulesPtr HdMapUtils::RoutingGraphs::getRules( case traffic_simulator::RoutingGraphType::PEDESTRIAN: return pedestrian.rules; default: - throw std::runtime_error("Invalid routing graph type"); + std::stringstream what; + what << "Invalid routing graph type: " << static_cast(type); + throw common::Error(what.str()); } } -RouteCache & HdMapUtils::RoutingGraphs::getRouteCache( - const traffic_simulator::RoutingGraphType type) +RouteCache & HdMapUtils::RoutingGraphs::route_cache(const traffic_simulator::RoutingGraphType type) { switch (type) { case traffic_simulator::RoutingGraphType::VEHICLE: @@ -2230,7 +2242,9 @@ RouteCache & HdMapUtils::RoutingGraphs::getRouteCache( case traffic_simulator::RoutingGraphType::PEDESTRIAN: return pedestrian.route_cache; default: - throw std::runtime_error("Invalid routing graph type"); + std::stringstream what; + what << "Invalid routing graph type: " << static_cast(type); + throw common::Error(what.str()); } } @@ -2239,28 +2253,28 @@ auto HdMapUtils::RoutingGraphs::getRoute( lanelet::LaneletMapPtr lanelet_map_ptr, bool allow_lane_change, const traffic_simulator::RoutingGraphType type) -> lanelet::Ids { - auto & route_cache = getRouteCache(type); - if (route_cache.exists(from_lanelet_id, to_lanelet_id, allow_lane_change)) { - return route_cache.getRoute(from_lanelet_id, to_lanelet_id, allow_lane_change); + auto & cache = route_cache(type); + if (cache.exists(from_lanelet_id, to_lanelet_id, allow_lane_change)) { + return cache.getRoute(from_lanelet_id, to_lanelet_id, allow_lane_change); } lanelet::Ids ids; const auto lanelet = lanelet_map_ptr->laneletLayer.get(from_lanelet_id); const auto to_lanelet = lanelet_map_ptr->laneletLayer.get(to_lanelet_id); lanelet::Optional route = - get(type)->getRoute(lanelet, to_lanelet, 0, allow_lane_change); + routing_graph(type)->getRoute(lanelet, to_lanelet, 0, allow_lane_change); if (!route) { - route_cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); + cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); return ids; } lanelet::routing::LaneletPath shortest_path = route->shortestPath(); if (shortest_path.empty()) { - route_cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); + cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); return ids; } for (auto lane_itr = shortest_path.begin(); lane_itr != shortest_path.end(); lane_itr++) { ids.push_back(lane_itr->id()); } - route_cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); + cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); return ids; }