Possible code refactoring related to uage of lanelet2 routing API

[felixf4xu]

Code of Conduct

• I have read CODE OF CONDUCT and Support Guidelines before creating this Discussion post.

Contents

Hi,

I'm reading this function: https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_route_handler/src/route_handler.cpp#L257C1-L287C4

this part of code uses getAllNeighbors:

void RouteHandler::setRouteLanelets(const lanelet::ConstLanelets & path_lanelets)
{
  if (!path_lanelets.empty()) {
    const auto & first_lanelet = path_lanelets.front();
    start_lanelets_ = lanelet::utils::query::getAllNeighbors(routing_graph_ptr_, first_lanelet);
    const auto & last_lanelet = path_lanelets.back();
    goal_lanelets_ = lanelet::utils::query::getAllNeighbors(routing_graph_ptr_, last_lanelet);
  }

then this part of code is using rightRelations (from lanelet2 routing API) directly

  // set route lanelets
  std::unordered_set<lanelet::Id> route_lanelets_id;
  std::unordered_set<lanelet::Id> candidate_lanes_id;
  for (const auto & lane : path_lanelets) {
    route_lanelets_id.insert(lane.id());
    const auto right_relations = routing_graph_ptr_->rightRelations(lane);
    for (const auto & right_relation : right_relations) {
      if (right_relation.relationType == lanelet::routing::RelationType::Right) {
        route_lanelets_id.insert(right_relation.lanelet.id());
      } else if (right_relation.relationType == lanelet::routing::RelationType::AdjacentRight) {
        candidate_lanes_id.insert(right_relation.lanelet.id());
      }
    }
    const auto left_relations = routing_graph_ptr_->leftRelations(lane);
    for (const auto & left_relation : left_relations) {
      if (left_relation.relationType == lanelet::routing::RelationType::Left) {
        route_lanelets_id.insert(left_relation.lanelet.id());
      } else if (left_relation.relationType == lanelet::routing::RelationType::AdjacentLeft) {
        candidate_lanes_id.insert(left_relation.lanelet.id());
      }
    }
  }

I would like to verify if the second part of code is actually doing the same thing as the function of lanelet::utils::query::getAllNeighbors does. If so, could it be refactored to use getAllNeighbors instead? so that the code can be simplified and uniformed

[maxime-clem]

getAllNeighbors() gets lanelets with both the relation left (or right) and adjacentLeft (or adjacentRight).
In the // set route lanelets part of the code, we need to distinguish between the relation type to split the lanelets into a "route" and a "candidate" group.
Therefore I do not think we can directly use getAllNeighbors().
But the function can probably be refactored and simplified so more investigation is needed.