Skip to content

Commit

Permalink
refactor: modify member function name of RoutingGraphs class
Browse files Browse the repository at this point in the history
  • Loading branch information
@HansRobo
HansRobo committed Nov 14, 2024
1 parent 0ae8d1d commit 964ccab
Show file tree
Hide file tree
Showing 2 changed files with 56 additions and 42 deletions.
6 changes: 3 additions & 3 deletions simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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;

Expand Down
92 changes: 53 additions & 39 deletions simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,8 +78,10 @@ HdMapUtils::HdMapUtils(
}
overwriteLaneletsCenterline();
std::vector<lanelet::routing::RoutingGraphConstPtr> 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_));
}
Expand Down Expand Up @@ -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());
}
Expand All @@ -409,8 +411,9 @@ auto HdMapUtils::getConflictingCrosswalkIds(const lanelet::Ids & lanelet_ids) co
{
lanelet::Ids ids;
std::vector<lanelet::routing::RoutingGraphConstPtr> 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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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)) {
Expand All @@ -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());
}
Expand Down Expand Up @@ -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)) {
Expand All @@ -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());
}
Expand Down Expand Up @@ -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)));
}
}

Expand All @@ -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)));
}
}

Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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) {
Expand All @@ -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<int>(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) {
Expand All @@ -2217,20 +2228,23 @@ 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<int>(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:
return vehicle.route_cache;
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<int>(type);
throw common::Error(what.str());
}
}

Expand All @@ -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<lanelet::routing::Route> 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;
}

Expand Down

0 comments on commit 964ccab

Please sign in to comment.