autoware_lanelet2_utils#

Nomenclature#

This package aims to strictly define the meaning of several words to clarify the documentation and API's scope. In the table below, codespace words are given specific meanings when used in the API and API description. italic words are emphasized to indicate that it refers to social common sense which often comes with ambiguity. To help disambiguate the meaning, illustration is provided. "Lanelet" refers to the entity of alanelet::ConstLanelet object in order to distinguish with the word "lane" used in social customs. A and B stands for some Lanelets objects.

Word	Meaning	Illustration
`driving`	The vehicle position belongs to the designated Lanelet.	In each map, green Lanelet are the `driving` lanes of the vehicle. Open
`boundary`, `entry`, `exit`	The `boundary` of a Lanelet refers to the left or right Linestring.	Open
`adjacent`	If A is `adjacent` to B, A and B share a common `boundary` with same direction either on the left or right side.	In each map, orange Lanelet is `adjacent` to green Lanelet. Open
`same_direction`	Lanelet A and Lanelet B are `same_direction` if A and B are directly or indirectly `adjacent` to each other.	In each map, orange Lanelets are `same_direction` as green Lanelet. Open
`bundle`	A `bundle` refers to a transitive closure set of Lanelets which are `same_direction` to each other.	The enclosed sets of Lanelets are `bundle`s. Open
`opposite`	If A is `opposite` to B, A and B share a common `boundary` with opposite direction.	In the first map, green Lanelet and orange Lanelet are `opposite` to each other. In the second map, two red Lanelets are not `opposite` relation because they do not share a common LineString. Open
`opposite_direction`	If A and B are `opposite_direction`, the `bundle` of A and B are directly `opposite` to each other.	In the each map, green Lanelet and orange Lanelet are `opposite_direction` because their `bundle`s(enclosed in dotted line) are `opposite` relation. Open
`connected`	A is `connected` to(from) B if and only if the `exit`(`entry`) of A is identical to the `entry`(`exit`) of B.	A is connected to B, and B is connected from A. Open
`following`	The `following` Lanelets of A is the list of Lanelets to which A is `connected`.	In each map, orange Lanelets are the `following` of green Lanelet. Open
`previous`	The `previous` Lanelets of A is the list of Lanelets from which A is `connected`.	In each map, orange Lanelets are the `previous` of green Lanelet. Open
`conflicting`	A is `conflicting` with B if A and B are geometrically intersecting.
`merging`	A is said to be `merging` Lanelet of B if and only if A is `conflicting` with B and both A and B are connected to a common Lanelet.	In each map, one of the orange Lanelet is a `merging` Lanelet of the other orange Lanelet. Open
`sibling`	The designated Lanelets are referred to as `sibling` if all of them are `connected` from a common Lanelet.	In each map, orange Lanelets are `sibling`s. Open
`oncoming`	TBD	TBD
`upcoming`	TBD	TBD
`sequence`	`sequence` is a list of Lanelets whose each element is `connected from` or `adjacent to` the previous element.	Open
`current_route_lanelet`	`current_route_lanelet` is one of the lanelet within the route which serves as the reference for ego position.

API description#

`<autoware/lanelet2_utils/geometry.hpp>`#

Function	Description	Illustration
`extrapolate_point`	Linearly extrapolate a point (`ConstPoint3d`) beyond a segment defined by two points (`first` and `second`) with given distance.	Open
`interpolate_point`	Linearly interpolates a point along a segment defined by two points.	Open
`interpolate_lanelet`	Find an interpolated point from a lanelet centerline at a given distance.	Open
`interpolate_lanelet_sequence`	Find the first interpolated point from a centerline of the lanelet sequence at a given distance.	Open
`concatenate_center_line`	Concatenate all center line of input lanelet sequence (several `ConstLanelets`)	Open
`get_linestring_from_arc_length`	Extract a sub-linestring between two arc-length positions along an input linestring.	Open
`get_pose_from_2d_arc_length`	Compute the 2D pose (position and heading) at a given arc-length along a sequence of lanelets.	Open
`get_closest_segment`	Find the closest segment of the `ConstLineString3d` to the search point (`BasicPoint3d`).	Open
`get_lanelet_angle`	Find the angle of center line segment of the lanelet that is closest to search point (`BasicPoint3d`). The angle is defined with the x-axis as the reference (0 radians). Positive angles are measured counterclockwise, while negative angles are measured clockwise within range of −π to π radians.	Open

Example Usage of `geometry`#

Extrapolate point for distance of 5.

./examples/example_geometry.cpp:51:55

  lanelet::ConstPoint3d p1(1, 0.0, 0.0, 0.0);
  lanelet::ConstPoint3d p2(2, 10.0, 0.0, 0.0);
  double distance = 5.0;

  auto extrapolated_point = lanelet2_utils::extrapolate_point(p1, p2, distance);

Interpolate point at half of the segment.

./examples/example_geometry.cpp:69:74

    lanelet::ConstPoint3d p1(1, 1.0, 2.0, 3.0);
    lanelet::ConstPoint3d p2(2, 4.0, 5.0, 6.0);
    double half_of_segment_length = std::hypot(4.0 - 1.0, 5.0 - 2.0, 6.0 - 3.0) / 2;

    auto opt = lanelet2_utils::interpolate_point(p1, p2, half_of_segment_length);
    auto interpolated_pt = *opt;

Interpolate lanelet at distance of 3.

./examples/example_geometry.cpp:82:84

    const auto ll = lanelet_map_ptr_->laneletLayer.get(2287);
    auto opt_pt = lanelet2_utils::interpolate_lanelet(ll, 3.0);
    auto interpolated_pt = *opt_pt;

Interpolate lanelet sequence at distance of 3.

./examples/example_geometry.cpp:92:98

    lanelet::ConstLanelets lanelets;
    lanelets.reserve(3);
    for (const auto & id : {2287, 2288, 2289}) {
      lanelets.push_back(lanelet_map_ptr_->laneletLayer.get(id));
    }
    auto opt_pt = lanelet2_utils::interpolate_lanelet_sequence(lanelets, 3.0);
    auto interpolated_pt = *opt_pt;

Concatenate several lanelet centerline.

./examples/example_geometry.cpp:110:118

  lanelet::ConstLanelets lanelets;
  lanelets.reserve(3);
  for (auto id : {2287, 2288, 2289}) {
    lanelets.push_back(lanelet_map_ptr_->laneletLayer.get(id));
  }

  auto opt_ls = lanelet2_utils::concatenate_center_line(lanelets);

  const auto & ls = *opt_ls;

Get linestring from arc-length range (of 0.5 to 1.5).

./examples/example_geometry.cpp:142:150

    std::vector<lanelet::Point3d> pts = {
      lanelet::Point3d{lanelet::ConstPoint3d(1, 0.0, 0.0, 0.0)},
      lanelet::Point3d{lanelet::ConstPoint3d(1, 1.0, 0.0, 0.0)},
      lanelet::Point3d{lanelet::ConstPoint3d(1, 1.7, 0.0, 0.0)},
      lanelet::Point3d{lanelet::ConstPoint3d(1, 2.0, 0.0, 0.0)}};
    lanelet::ConstLineString3d line{lanelet::InvalId, pts};
    auto opt =
      autoware::experimental::lanelet2_utils::get_linestring_from_arc_length(line, 0.5, 1.5);
    const auto & out = *opt;

Get pose from arc-length (of 3.0).

./examples/example_geometry.cpp:162:168

    lanelet::ConstLanelets lanelets;
    for (auto id : {2287, 2288, 2289}) {
      lanelets.push_back(lanelet_map_ptr_->laneletLayer.get(id));
    }
    auto opt_pose =
      autoware::experimental::lanelet2_utils::get_pose_from_2d_arc_length(lanelets, 3.0);
    const auto & p = *opt_pose;

Get closest segment from the ConstLineString3d.

./examples/example_geometry.cpp:179:186

void closest_segment()
{
  std::vector<lanelet::Point3d> pts = {
    lanelet::Point3d{lanelet::ConstPoint3d(1, 0.0, 0.0, 0.0)},
    lanelet::Point3d{lanelet::ConstPoint3d(1, 1.0, 0.0, 0.0)},
    lanelet::Point3d{lanelet::ConstPoint3d(1, 2.0, 0.0, 0.0)}};
  lanelet::ConstLineString3d line{lanelet::InvalId, pts};

Get lanelet angle from the lanelet centerline.

./examples/example_geometry.cpp:202:204

  const auto ll = lanelet_map_ptr_->laneletLayer.get(2258);
  lanelet::BasicPoint3d p(106.71, 149.3, 100);
  auto out = autoware::experimental::lanelet2_utils::get_lanelet_angle(ll, p);

Get closest center pose from ConstLanelet.

./examples/example_geometry.cpp:211:221

  auto p1 = lanelet::BasicPoint3d(0.0, 0.0, 0.0);
  auto p2 = lanelet::BasicPoint3d(0.0, 3.0, 0.0);
  auto p3 = lanelet::BasicPoint3d(2.0, 0.0, 0.0);
  auto p4 = lanelet::BasicPoint3d(2.0, 3.0, 0.0);

  std::vector<lanelet::BasicPoint3d> left_points = {p1, p2};
  std::vector<lanelet::BasicPoint3d> right_points = {p3, p4};
  auto ll = create_safe_lanelet(left_points, right_points);

  auto search_pt = lanelet::BasicPoint3d(1.2, 1.0, 0.0);
  auto p = autoware::experimental::lanelet2_utils::get_closest_center_pose(*ll, search_pt);

`<autoware/lanelet2_utils/conversion.hpp>`#

Function	Description	Average Computational Complexity	Illustration
`load_mgrs_coordinate_map(path, centerline_resolution)`	Instantiate a LaneletMap object from given `path` to `.osm` file. Also it sets more dense centerline(at the interval of `centerline_resolution`) than default `Lanelet2` library, to help improve Planning accuracy.
`instantiate_routing_graph_and_traffic_rules`	This function creates a `RoutingGraph` and `TrafficRules` object only from "road" lanes for `Vehicle` participant, which means "road_shoulder","bicycle_lane", "crosswalk", etc. Lanelets are inaccessible from left/right adjacency.
`from_autoware_map_msgs(...)` `to_autoware_map_msgs(...)`	Convert LaneletMap object from/to `autoware_mapping_msgs::LaneletMapBin` message
`from_ros(...)` `to_ros(...)`	Convert lanelet point (`lanelet::BasicPoint3d` or `lanelet::ConstPoint3d`) from/to `geometry_msgs::msg::Point` or `geometry_msgs::msg::Pose`
`create_safe_linestring`	Construct LineString from vector of lanelet points (`BasicLineString3d` - `BasicPoint3d` and `ConstLineString3d` - `ConstPoint3d`)
`create_safe_lanelet`	Construct `ConstLanelet` from two vectors of lanelet points (left and right) - `BasicPoint3d` or `ConstPoint3d`

Example Usage of `conversion`#

Load coordinate map.

./examples/example_conversion.cpp:36:44

  // setup path
  const auto sample_map_dir =
    fs::path(ament_index_cpp::get_package_share_directory("autoware_lanelet2_utils")) /
    "sample_map";
  const auto intersection_crossing_map_path = sample_map_dir / "vm_03/left_hand/lanelet2_map.osm";

  // load map
  lanelet::LaneletMapConstPtr lanelet_map_ptr_ =
    lanelet2_utils::load_mgrs_coordinate_map(intersection_crossing_map_path.string(), 5.0);

Load routing graph and traffic rules.

./examples/example_conversion.cpp:48:59

  // load routing graph and traffic rules
  lanelet::routing::RoutingGraphConstPtr routing_graph_{nullptr};
  lanelet::traffic_rules::TrafficRulesPtr traffic_rules_{nullptr};
  std::tie(routing_graph_, traffic_rules_) =
    lanelet2_utils::instantiate_routing_graph_and_traffic_rules(lanelet_map_ptr_);

  // Or get only routing graph or traffic rules
  routing_graph_ =
    lanelet2_utils::instantiate_routing_graph_and_traffic_rules(lanelet_map_ptr_).first;

  traffic_rules_ =
    lanelet2_utils::instantiate_routing_graph_and_traffic_rules(lanelet_map_ptr_).second;

Conversion between LaneletMapBin and LaneletMapConstPtr.

./examples/example_conversion.cpp:63:71

  // convert to LaneletMapBin
  autoware_map_msgs::msg::LaneletMapBin map_msg_ =
    lanelet2_utils::to_autoware_map_msgs(lanelet_map_ptr_);
  std::cout << "Convert LaneletMapConstPtr to LaneletMapBin!" << std::endl;

  // convert to LaneletMapConstPtr
  lanelet::LaneletMapConstPtr lanelet_map_ptr_converted =
    lanelet2_utils::from_autoware_map_msgs(map_msg_);
  std::cout << "Convert LaneletMapBin to LaneletMapConstPtr !" << std::endl;

Message conversion from BasicPoint3d to Point.

./examples/example_conversion.cpp:78:80

    auto basicpoint3d = lanelet::BasicPoint3d(1.0, 2.0, 3.0);

    auto point_from_basicpoint3d = lanelet2_utils::to_ros(basicpoint3d);

Message conversion between ConstPoint3d and Point.

./examples/example_conversion.cpp:87:93

    auto constpoint3d = lanelet::ConstPoint3d(lanelet::Point3d(lanelet::InvalId, 1.0, 2.0, 3.0));

    auto point_from_constpoint3d = lanelet2_utils::to_ros(constpoint3d);
    std::cout << "Convert from lanelet::ConstPoint3d to geometry_msgs::msg::Point." << std::endl;

    auto converted_constpoint3d_from_point = lanelet2_utils::from_ros(point_from_constpoint3d);
    std::cout << "Convert from geometry_msgs::msg::Point to lanelet::ConstPoint3d." << std::endl;

Message conversion from Pose to ConstPoint3d.

./examples/example_conversion.cpp:98:104

    geometry_msgs::msg::Pose pose;
    pose.position.x = 1.0;
    pose.position.y = 2.0;
    pose.position.z = 3.0;

    auto const_pt = lanelet2_utils::from_ros(pose);
    std::cout << "Convert from geometry_msgs::msg::Pose to lanelet::ConstPoint3d." << std::endl;

Message conversion from BasicPoint2d to Point.

./examples/example_conversion.cpp:109:112

    auto basicpoint2d = lanelet::BasicPoint2d(1.0, 2.0);

    auto point_from_basicpoint2d = lanelet2_utils::to_ros(basicpoint2d, 3.0);
    std::cout << "Convert from lanelet::BasicPoint2d to geometry_msgs::msg::Point." << std::endl;

Message conversion from ConstPoint2d to Point.

./examples/example_conversion.cpp:118:127

    auto constpoint2d = lanelet::ConstPoint2d(lanelet::Point2d(lanelet::InvalId, 1.0, 2.0));

    auto point_from_constpoint2d = lanelet2_utils::to_ros(constpoint2d, 3.0);
    std::cout << "Convert from lanelet::ConstPoint2d to geometry_msgs::msg::Point." << std::endl;

    auto constpoint3d_from_point = lanelet2_utils::from_ros(point_from_constpoint2d);
    std::cout << "Convert from geometry_msgs::msg::Point to lanelet::ConstPoint3d." << std::endl;

    auto constpoint2d_from_point = lanelet::utils::to2D(constpoint3d_from_point);
    std::cout << "Convert from  lanelet::ConstPoint3d to lanelet::ConstPoint2d." << std::endl;

`<autoware/lanelet2_utils/kind.hpp>`#

Function	Description	Average Computational Complexity
`is_road_lane`	This function returns `true` if the input Lanelet is `road` subtype.	\(O(1)\)
`is_shoulder_lane`	This function returns `true` if the input Lanelet is `road_shoulder` subtype.	\(O(1)\)
`is_bicycle_lane`	This function returns `true` if the input Lanelet is `bicycle_lane` subtype.	\(O(1)\)

Example Usage of `kind`#

Check type of lanelet.

./examples/example_kind.cpp:44:66

  auto lanelet_map_ptr_ = set_up_lanelet_map_ptr();
  const auto ll = lanelet_map_ptr_->laneletLayer.get(46);
  bool check_road_lane = autoware::experimental::lanelet2_utils::is_road_lane(ll);
  bool check_shoulder_lane = autoware::experimental::lanelet2_utils::is_shoulder_lane(ll);
  bool check_bicycle_lane = autoware::experimental::lanelet2_utils::is_bicycle_lane(ll);

  if (check_road_lane) {
    std::cout << "This lanelet is road lane." << std::endl;
  } else {
    std::cout << "This lanelet is NOT road lane." << std::endl;
  }

  if (check_shoulder_lane) {
    std::cout << "This lanelet is shoulder lane." << std::endl;
  } else {
    std::cout << "This lanelet is NOT shoulder lane." << std::endl;
  }

  if (check_bicycle_lane) {
    std::cout << "This lanelet is bicycle lane." << std::endl;
  } else {
    std::cout << "This lanelet is NOT bicycle lane." << std::endl;
  }

`<autoware/lanelet2_utils/hatched_road_markings.hpp>`#

Function	Description	Average Computational Complexity	Illustration
`get_adjacent_hatched_road_markings`	Returns polygons with type `hatched_road_markings` that touch the left/right bounds of the given lanelet sequence. Polygons are grouped by side and duplicates removed.	\(O(V)\) where \(V\) is the number of boundary vertices inspected

Example Usage of `hatched_road_markings`#

./examples/example_hatched_road_markings.cpp:51:75

  const auto adjacent = autoware::experimental::lanelet2_utils::get_adjacent_hatched_road_markings(
    lanelets, lanelet_map_ptr_);

  // Collect unique polygon IDs from the result.
  std::unordered_set<lanelet::Id> left_ids;
  std::unordered_set<lanelet::Id> right_ids;

  for (const auto & poly : adjacent.left) {
    left_ids.insert(poly.id());
  }
  for (const auto & poly : adjacent.right) {
    right_ids.insert(poly.id());
  }

  std::cout << "Left ID size is: " << left_ids.size() << std::endl;
  std::cout << "Left ID is: " << std::endl;
  for (const auto & left : left_ids) {
    std::cout << left << std::endl;
  }

  std::cout << "Right ID size is: " << right_ids.size() << std::endl;
  std::cout << "Right ID is: " << std::endl;
  for (const auto & right : right_ids) {
    std::cout << right << std::endl;
  }

`<autoware/lanelet2_utils/topology.hpp>`#

Function	Description	Average Computational Complexity	Illustration
`left_opposite_lanelet`	same as below `right_opposite_lanelet`	\(O(1)\) see `findUsage` for detail
`right_opposite_lanelet`	This functions returns the right `opposite` Lanelet of the input Lanelet if available, otherwise returns null.	\(O(1)\) see `findUsage` for detail	In the first and second map, the green Lanelet is the `right_opposite_lanelet` of the orange Lanelet. In the third map, the `right_opposite_lanelet` of the orange Lanelet is `null`. Open
`following_lanelets`	This function returns the `following` Lanelets of the input Lanelet. The order is not defined.	\(O(E)\) where \(E\) is the number of Lanelets to which the input is connected to.
`previous_lanelets`	This function returns the `previous` Lanelets of the input Lanelet. The order is not defined.	\(O(E)\) where \(E\) is the number of Lanelets from which the input is connected from.
`sibling_lanelets`	This function returns the `sibling` Lanelets of the input Lanelet excluding itself. The order is not defined.	\(O(E)\) where \(E\) is the number of sibling Lanelets
`from_ids`	This function returns Lanelet objects in the same order as the input IDs.	\(O(n)\)
`get_conflicting_lanelets`	This function returns the `conflicting` Lanelets of the input Lanelet. The order is not defined.	\(O(E)\) where \(E\) is the number of conflicting Lanelets

Example Usage of `topology`#

Get left opposite lanelet.

./examples/example_topology.cpp:60:63

    const auto opt = lanelet2_utils::left_opposite_lanelet(
      lanelet_map_ptr_->laneletLayer.get(2311), lanelet_map_ptr_);
    const auto lane = *opt;
    std::cout << "The left opposite lanelet id is : " << lane.id() << std::endl;

Get right opposite lanelet.

./examples/example_topology.cpp:71:74

    const auto opt = lanelet2_utils::right_opposite_lanelet(
      lanelet_map_ptr_->laneletLayer.get(2288), lanelet_map_ptr_);
    const auto lane = *opt;
    std::cout << "The right opposite lanelet id is : " << lane.id() << std::endl;

Get following lanelets.

./examples/example_topology.cpp:84:89

    const auto following = lanelet2_utils::following_lanelets(
      lanelet_map_ptr_->laneletLayer.get(2244), routing_graph_ptr_);
    std::cout << "The following lanelets id are :" << std::endl;
    for (const auto & ll : following) {
      std::cout << ll.id() << std::endl;
    }

Get previous lanelets.

./examples/example_topology.cpp:93:98

    const auto previous = lanelet2_utils::previous_lanelets(
      lanelet_map_ptr_->laneletLayer.get(2249), routing_graph_ptr_);
    std::cout << "The previous lanelets id are :" << std::endl;
    for (const auto & ll : previous) {
      std::cout << ll.id() << std::endl;
    }

Get sibling lanelets.

./examples/example_topology.cpp:102:107

    const auto siblings = lanelet2_utils::sibling_lanelets(
      lanelet_map_ptr_->laneletLayer.get(2273), routing_graph_ptr_);
    std::cout << "The sibling lanelets id are :" << std::endl;
    for (const auto & ll : siblings) {
      std::cout << ll.id() << std::endl;
    }

Get conflicting lanelets.

./examples/example_topology.cpp:111:116

    const auto conflicting_lanelets = lanelet2_utils::get_conflicting_lanelets(
      lanelet_map_ptr_->laneletLayer.get(2270), routing_graph_ptr_);
    std::cout << "The conflicting lanelets id are :" << std::endl;
    for (const auto & ll : conflicting_lanelets) {
      std::cout << ll.id() << std::endl;
    }

Get ConstLanelets from ids.

./examples/example_topology.cpp:120:122

    const auto lanelets =
      lanelet2_utils::from_ids(lanelet_map_ptr_, std::vector<lanelet::Id>({2296, 2286, 2270}));
    std::cout << "Get ConstLanelets of size: " << lanelets.size() << std::endl;

`<autoware/lanelet2_utils/intersection.hpp>`#

Function	Description	Average Computational Complexity
`is_intersection_lanelet`	This function returns `true` if and only if the input Lanelet has `turn_direction` attribute.	\(O(1)\)
`is_straight_lanelet` `is_left_lanelet` `is_right_lanelet`	This function returns `true` if and only if the input Lanelet has `turn_direction` attribute and its value is `straight`/`left`/`right`.	\(O(1)\)
`get_turn_direction`	This function returns the turn direction (`straight`/`left`/`right`) at the intersection if and only if the input Lanelet has `turn_direction` attribute.	\(O(1)\)

Example Usage of `intersection`#

Check if the lanelet is intersection.

./examples/example_intersection.cpp:47:49

    bool check = lanelet2_utils::is_intersection_lanelet(lanelet_map_ptr_->laneletLayer.get(2274));
    std::cout << (check ? "This lanelet is intersection!" : "This lanelet is not intersection.")
              << std::endl;

Check if the lanelet is straight direction.

./examples/example_intersection.cpp:52:54

    bool check = lanelet2_utils::is_straight_direction(lanelet_map_ptr_->laneletLayer.get(2278));
    std::cout << (check ? "This lanelet is straight direction!"
                        : "This lanelet is not straight direction.")

Check if the lanelet is left direction.

./examples/example_intersection.cpp:57:59

  {
    bool check = lanelet2_utils::is_left_direction(lanelet_map_ptr_->laneletLayer.get(2274));
    std::cout << (check ? "This lanelet is left direction!" : "This lanelet is not left direction.")

Check if the lanelet is right direction.

./examples/example_intersection.cpp:62:65

  {
    bool check = lanelet2_utils::is_right_direction(lanelet_map_ptr_->laneletLayer.get(2277));
    std::cout << (check ? "This lanelet is right direction!"
                        : "This lanelet is not right direction.")

Get Turn direction at the intersection.

./examples/example_intersection.cpp:71:104

    {
      // not intersection
      auto opt = lanelet2_utils::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2257));
      bool check = opt.has_value();
      std::cout << (check ? "This lanelet has turn_direction attribute!"
                          : "This lanelet has no turn_direction attribute.")
                << std::endl;
    }
    // straight
    {
      const auto opt = lanelet2_utils::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2278));
      if (opt.has_value()) {
        auto direction = *opt;
        std::cout << "Straight: " << direction << std::endl;
      }
    }

    // left
    {
      const auto opt = lanelet2_utils::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2274));
      if (opt.has_value()) {
        auto direction = *opt;
        std::cout << "Left: " << direction << std::endl;
      }
    }

    // right
    {
      const auto opt = lanelet2_utils::get_turn_direction(lanelet_map_ptr_->laneletLayer.get(2277));
      if (opt.has_value()) {
        auto direction = *opt;
        std::cout << "Right: " << direction << std::endl;
      }
    }

`<autoware/lanelet2_utils/lane_sequence.hpp`#

Function	Description	Average Computational Complexity	Illustration
`class LaneSequence`	This class internally holds `lanelet::ConstLanelets` such that they are consecutive on the routing graph.
class invariance	`.as_lanelets()` return Lanelets that are consecutive on the routing graph
`create(lanelets, routing_graph)`	Return an optional of `LaneSequence` class that satisfies the invariance
`.as_lanelets()`	Return the underlying `lanelet::ConstLanelets`

Example Usage of `lane_sequence`#

Create LaneSequence using constructor.

./examples/example_lane_sequence.cpp:61:67

  auto lane2 = lanelet_map_ptr_->laneletLayer.get(2261);  // lane2 -> lane_other is not connected
  auto lane_other = lanelet_map_ptr_->laneletLayer.get(2312);
  {
    const auto seq = lanelet2_utils::LaneSequence(lane1);
    std::cout << "LaneSequence size is: " << seq.as_lanelets().size() << std::endl;
  }
  {

Create LaneSequence using create.

./examples/example_lane_sequence.cpp:68:80

    const auto opt =
      lanelet2_utils::LaneSequence::create({lane1, lane2, lane_other}, routing_graph_ptr_);
    std::cout << (opt.has_value()
                    ? "opt has value"
                    : "opt doesn't have value because lane_other is not connecting to lane2")
              << std::endl;
  }
  {
    const auto opt = lanelet2_utils::LaneSequence::create({lane1, lane2}, routing_graph_ptr_);
    const auto seq = *opt;
    std::cout << "Created LaneSequence size is: " << seq.as_lanelets().size() << std::endl;
  }
}

`<autoware/lanelet2_utils/nn_search.hpp>`#

Function	Description	Average Computational Complexity
`get_closest_lanelet(lanelets, pose)`	This function retrieves the lanelet which gives the smallest distance to given `pose`(if it is within a lanelet, it gives zero distance) and whose centerline is closest to the given orientation among them	\(O(N)\) where \(N\) is the number of input lanelets "Native R-tree API" and `LaneletRTree` is much more efficient
`get_closest_lanelet_within_constraint(lanelets, pose, dist_thresh, yaw_thresh)`	In addition to `get_closest_lanelet`, it filters `lanelets` whose distance to `pose` is \(\leq\) `dist_thresh` and yaw angle difference is \(\leq\) `yaw_thresh`	\(O(N)\) where \(N\) is the number of input lanelets "Native R-tree API" and `LaneletRTree` is much more efficient
`get_road_lanelets_at(lanelet_map, x, y)`	Retrieve all "road" Lanelets at given position	R-tree
`get_shoulder_lanelets_at(lanelet_map, x, y)`	Retrieve all "road_shoulder" Lanelets at given position	R-tree
`class LaneletRTree`	`class LaneletRTree` constructs R-tree structure from given Lanelets and provides more efficient operations.
`.get_closest_lanelet(pose)`	Efficient version of `get_closest_lanelet`	R-tree
`.get_closest_lanelet_within_constraint(pose, dist_thresh, yaw_thresh)`	Efficient version of `get_closest_lanelet_within_constraint`	R-tree

Example Usage of `nn_search`#

Create R-tree

./examples/example_nn_search.cpp:47:48

  lanelet::ConstLanelets all_lanelets_ = lanelet_map_ptr_->laneletLayer | ranges::to<std::vector>();
  std::optional<autoware::experimental::lanelet2_utils::LaneletRTree> rtree_{all_lanelets_};

Get the closest lanelet without constraint (in both use and not use R-tree)

./examples/example_nn_search.cpp:72:83

  // get_closest_lanelet (without rtree)
  {
    auto opt = lanelet2_utils::get_closest_lanelet(all_lanelets_, P0);
    auto closest = *opt;
    std::cout << "Closest Lanelet id is: " << closest.id() << std::endl;
  }
  // get_closest_lanelet (using rtree)
  {
    auto opt = rtree_->get_closest_lanelet(P0);
    auto closest = *opt;
    std::cout << "Closest Lanelet id is: " << closest.id() << std::endl;
  }

Get the closest lanelet without constraint (in both use and not use R-tree)

./examples/example_nn_search.cpp:85:101

  static constexpr double ego_nearest_dist_threshold = 3.0;
  static constexpr double ego_nearest_yaw_threshold = 1.046;

  // get_closest_lanelet_within_constraint (without rtree)
  {
    auto opt = lanelet2_utils::get_closest_lanelet_within_constraint(
      all_lanelets_, P0, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
    auto closest = *opt;
    std::cout << "Closest Lanelet id is: " << closest.id() << std::endl;
  }
  // get_closest_lanelet_within_constraint (using rtree)
  {
    auto opt = rtree_->get_closest_lanelet_within_constraint(
      P0, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
    auto closest = *opt;
    std::cout << "Closest Lanelet id is: " << closest.id() << std::endl;
  }

Get road lanelet from the LaneletMap

./examples/example_nn_search.cpp:104:111

  {
    geometry_msgs::msg::Point p;
    p.x = 137.53617965826874;
    p.y = 189.8355248506006;
    const auto road_lanelets = lanelet2_utils::get_road_lanelets_at(lanelet_map_ptr_, p.x, p.y);
    std::cout << "There are " << road_lanelets.size() << " road lanelet(s) in this LaneletMap."
              << std::endl;
  }

Get shoulder lanelet from the LaneletMap

./examples/example_nn_search.cpp:115:121

    geometry_msgs::msg::Point p;
    p.x = 101.11975409926032;
    p.y = 143.75759863307977;
    const auto shoulder_lanelets =
      lanelet2_utils::get_shoulder_lanelets_at(lanelet_map_ptr_, p.x, p.y);
    std::cout << "There are " << shoulder_lanelets.size()
              << " shoulder lanelet(s) in this LaneletMap." << std::endl;

`<autoware/lanelet2_utils/map_handler.hpp>`#

Function	Description	Average Computational Complexity	Illustration
`class MapHandler`	`class MapHandler` provides convenient functions related to adjacency, VRU lanes, etc. for Planning.
class invariance	`lanelet_map_ptr` is not `nullptr` `routing_graph_ptr` is not `nullptr` `traffic_rules_ptr` is not `nullptr`
`MapHandler::create(...)`	A factory function to construct under invariance
`.lanelet_map_ptr()` `.routing_graph_ptr()` `.traffic_rules_ptr()`	Getter functions
`.left_lanelet(lanelet, take_sibling, extra_vru)`	This function ignores the permission of lane change. If `extra_vru` is: `RoadOnly`, it ignores `shoulder` and `bicycle` Lanelet `Shoulder`, it searches `shoulder` Lanelet additionally `BicycleLane`, it searches `bicycle_lane` Lanelet additionally `ShoulderAndBicycleLane`, it searches `shoulder` and `bicycle_lane` Lanelet additionally	\(O(1)\)	In the first map, the green Lanelet is the `left_lanelet` of the orange Lanelet. In the second and third map, the `left_lanelet` of the orange Lanelet is `null`. Open
`.right_lanelet(lanelet, take_sibling, extra_vru)`	same as above `.left_lanelet()`	\(O(1)\)
`.leftmost_lanelet(lanelet, take_sibling, extra_vru)`	This function recursively searches `.left_lanelet()` of input `lanelet`.	\(O(W)\) where \(W\) is the size of the `bundle`.	In the first and second map, the green Lanelet is the `leftmost_lanelet` of the orange Lanelet. In the third map, the `leftmost_lanelet` of the orange Lanelet is `null`. Open
`.rightmost_lanelet(lanelet, take_sibling, extra_vru)`	This function recursively searches `.right_lanelet()` of input `lanelet`.	\(O(W)\) where \(W\) is the size of the `bundle`.	In the first map, the green Lanelet is the `rightmost_lanelet` of the orange Lanelet. In the second and third map, the `rightmost_lanelet` of the orange Lanelet is `null`. Open
`.left_lanelets(lanelet, include_opposite)`	The input Lanelet is not included in the output, and output is ordered from left to right.	\(O(W)\) where \(W\) is the size of the `bundle`.	In the first map, the green Lanelets are the `left_lanelets` of the orange Lanelet. In the second and third map, `left_lanelets` of the orange Lanelet is empty. If the flag `include_opposite = true`, the left opposite Lanelet and all of its `same_direction` Lanelets area also retrieved as illustrated in the fourth and fifth maps. Open
`.right_lanelets(lanelet, include_opposite)`	The input Lanelet is not included in the output, and output is ordered from right to left.	\(O(W)\) where \(W\) is the size of the `bundle`.	In the first map, the green Lanelets are the `right_lanelets` of the orange Lanelet. In the second and third map, `right_lanelets` of the orange Lanelet is empty. If the flag `include_opposite = true`, the right opposite Lanelet and all of its `same_direction` Lanelets area also retrieved as illustrated in the fourth and fifth maps. Open
`.get_shoulder_lanelet_sequence(lanelet, forward, backward)`	This function computes (1) "road_shoulder" Lanelets behind of `lanelet` by up to `backward` and (2) "road_shoulder" Lanelets after `lanelet` by up to `forward` as a list	\(O(\textrm{total length})\)
`.left_shoulder_lanelet(lanelet)` `.right_shoulder_lanelet(lanelet)` `.left_bicycle_lanelet(lanelet)` `.right_bicycle_lanelet(lanelet)`	Retrieve each VRU Lanelet of `lanelet` if it exists

Example Usage of `map_handler`#

Create map_handler.

./examples/example_map_handler.cpp:41:49

  const lanelet::LaneletMapConstPtr lanelet_map_ptr_ =
    autoware::experimental::lanelet2_utils::load_mgrs_coordinate_map(
      intersection_crossing_map_path.string());

  // convert to laneletMapBin
  auto map_msg_ = lanelet2_utils::to_autoware_map_msgs(lanelet_map_ptr_);

  std::optional<lanelet2_utils::MapHandler> map_handler_opt_;
  map_handler_opt_.emplace(lanelet2_utils::MapHandler::create(map_msg_).value());

Try using left_lanelet with different extra_vru.

./examples/example_map_handler.cpp:59:90

  {
    const auto opt = map_handler.left_lanelet(
      lanelet_map_ptr->laneletLayer.get(2257), false, lanelet2_utils::ExtraVRU::RoadOnly);
    std::cout << (opt.has_value() ? "There is left lane with Road Only"
                                  : "There is no left lane with Road Only")
              << std::endl;
  }

  {
    const auto lane = map_handler.left_lanelet(
      lanelet_map_ptr->laneletLayer.get(2257), false, lanelet2_utils::ExtraVRU::Shoulder);
    std::cout << "Shoulder lane id is " << (*lane).id() << std::endl;
  }

  {
    const auto opt = map_handler.left_lanelet(
      lanelet_map_ptr->laneletLayer.get(2257), false, lanelet2_utils::ExtraVRU::BicycleLane);
    std::cout << (opt.has_value() ? "There is left lane that is Bicycle Lane"
                                  : "There is no left lane that is Bicycle Lane")
              << std::endl;
  }

  {
    const auto opt = map_handler.left_lanelet(
      lanelet_map_ptr->laneletLayer.get(2257), false,
      lanelet2_utils::ExtraVRU::ShoulderAndBicycleLane);
    std::cout << (opt.has_value() ? "There is left lane that is Shoulder and Bicycle Lane"
                                  : "There is no left lane that is Shoulder and Bicycle Lane")
              << std::endl;
    auto lane = *opt;
    std::cout << "Shoulder and Bicycle lane id is " << lane.id() << std::endl;
  }

Call right_lanelet.

./examples/example_map_handler.cpp:94:97

    const auto opt = map_handler.right_lanelet(
      lanelet_map_ptr->laneletLayer.get(2245), false, lanelet2_utils::ExtraVRU::RoadOnly);
    auto lane = *opt;
    std::cout << "Right Road only lane id is " << lane.id() << std::endl;

Call leftmost_lanelet.

./examples/example_map_handler.cpp:102:105

    const auto opt = map_handler.leftmost_lanelet(
      lanelet_map_ptr->laneletLayer.get(2288), false, lanelet2_utils::ExtraVRU::RoadOnly);
    auto lane = *opt;
    std::cout << "Leftmost Road only lane id is " << lane.id() << std::endl;

Call rightmost_lanelet.

./examples/example_map_handler.cpp:110:113

    const auto opt = map_handler.rightmost_lanelet(
      lanelet_map_ptr->laneletLayer.get(2286), false, lanelet2_utils::ExtraVRU::RoadOnly);
    auto lane = *opt;
    std::cout << "Rightmost Road only lane id is " << lane.id() << std::endl;

Call left_lanelets.

./examples/example_map_handler.cpp:118:123

    const auto lefts = map_handler.left_lanelets(lanelet_map_ptr->laneletLayer.get(2288));
    std::cout << "Left lanelets size is " << lefts.size() << std::endl;
    std::cout << "That has id" << std::endl;
    for (size_t i = 0ul; i < lefts.size(); ++i) {
      std::cout << lefts[i].id() << std::endl;
    }

Call right_lanelets including opposite lanelets.

./examples/example_map_handler.cpp:128:133

    const auto rights = map_handler.right_lanelets(lanelet_map_ptr->laneletLayer.get(2286), true);
    std::cout << "Right lanelets size is " << rights.size() << std::endl;
    std::cout << "That has id" << std::endl;
    for (size_t i = 0ul; i < rights.size(); ++i) {
      std::cout << rights[i].id() << std::endl;
    }

Call get_shoulder_lanelet_sequence.

./examples/example_map_handler.cpp:140:146

    const auto seq =
      map_handler002.get_shoulder_lanelet_sequence(lanelet_map_ptr002->laneletLayer.get(48));
    std::cout << "lanelet sequence size is " << seq.size() << std::endl;
    std::cout << "That has id" << std::endl;
    for (size_t i = 0ul; i < seq.size(); ++i) {
      std::cout << seq.at(i).id() << std::endl;
    }

`<autoware/lanelet2_utils/route_manager.hpp>`#

Function	Description	Average Computational Complexity	Illustration
`class RouteManager EXTENDS MapHandler`	`class RouteManager` is responsible for properly tracking `current_route_lanelet` along the given route information, considering swerving maneuver and lane change. Also it provides several functions related to `current_route_lanelet`
class invariance	`current_pose` may not be inside of any `route_lanelets` nor `preferred_route_lanelets`, because swerving and abrupt localization jump cannot be distinguished. For the same reason `current_pose` may not be inside of `current_route_lanelet` `current_route_lanelet` matches one of the `route_lanelets`
`RouteManager::create(...)`	A factory function to construct under invariance
Inherits `MapHandler`'s member functions
`.update_current_pose(new_pose)`	This function updates `current_route_lanelet` on the route based on `new_pose`. This method should be used for all the cases excluding lane change completion, and `current_route_lanelet` is updated longitudinally. It is expected to be called in every cycle of planning.	\(O(1)\)	Even if ego vehicle is in the middle swerving, `update_current_pose` decides next `current_route_lanelet` longitudinally, as illustrated in thin violet in the figure. Open
`.commit_lane_change(new_pose)`	This function updates `current_route_lanelet` on the route considering lane change. It is expected to be called only when lane change execution has succeeded	R-tree	Only when lane change has been completed, `commit_lane_change()` is expected to be called, as illustrated in the the last item of "(2) Lane Change Scenario". Open
`.current_lanelet()`	Get `current_route_lanelet` Lanelet	\(O(1)\)
`.get_lanelet_sequence_on_route(forward, backward)`	This function computes (1) Lanelets behind of `current_route_lanelet` by up to `backward` and (2) Lanelets after `current_route_lanelet` by up to `forward` as a list, only on the route without lane change. The length is measured from `current_pose` with respect to `current_route_lanelet`.	\(O(\textrm{total length})\)	From `current_route_lanelet`, the output contains the Lanelets by up to given distance in backward/forward direction. The output does not contain non-route Lanelets even if the sequence is below specified length, as illustrated in "(1)" in below figure. Open
`.get_lanelet_sequence_on_outward_route(forward, backward)`	This function computes (1) Lanelets behind of `current_route_lanelet` by up to `backward` and (2) Lanelets after `current_route_lanelet` by up to `forward` as a list, extending to non-route Lanelet if necessary, without lane change. The length is measured from `current_pose` with respect to `current_route_lanelet`.	\(O(\textrm{total length})\)	From `current_route_lanelet`, the output contains the Lanelets by up to given distance in backward/forward direction. The output extends to non-route Lanelets if the route part of the sequence is below specified length, as illustrated in "(2)" in below figure. Open
`.get_closest_preferred_route_lanelet(...)`	preferred route lanelet limited version of `get_closest_lanelet_within_constraint`	R-tree
`.get_closest_route_lanelet_within_constraints(...)`	route lanelet limited version of `get_closest_lanelet_within_constraint`	R-tree

Example Usage of `route_manager`#

Create route_manager.

./examples/example_route_manager.cpp:33:50

static autoware_planning_msgs::msg::LaneletRoute create_route_msg(
  const std::vector<std::pair<std::vector<lanelet::Id>, lanelet::Id>> & route_ids)
{
  using autoware_planning_msgs::msg::LaneletPrimitive;
  using autoware_planning_msgs::msg::LaneletSegment;
  autoware_planning_msgs::msg::LaneletRoute route_msg;
  for (const auto & route_id : route_ids) {
    const auto & [ids, preferred_id] = route_id;
    LaneletSegment segment;
    segment.preferred_primitive.id = preferred_id;
    for (const auto & id : ids) {
      auto primitive =
        autoware_planning_msgs::build<LaneletPrimitive>().id(id).primitive_type("road");
      segment.primitives.push_back(primitive);
    }
    route_msg.segments.push_back(segment);
  }
  return route_msg;

./examples/example_route_manager.cpp:72:79

  autoware_planning_msgs::msg::LaneletRoute route_msg_ = create_route_msg(
    {{{2239, 2240, 2241, 2242}, 2240},
     {{2301, 2302, 2300, 2299}, 2302},
     {{2244, 2245, 2246, 2247}, 2245},
     {{2265, 2261, 2262, 2263}, 2261}});
  autoware_map_msgs::msg::LaneletMapBin map_msg_ = load_map_msg();

  geometry_msgs::msg::Pose initial_pose_;

./examples/example_route_manager.cpp:91:95

    const auto route_manager_opt =
      lanelet2_utils::RouteManager::create(map_msg_, route_msg_, initial_pose_);
    std::cout << (route_manager_opt.has_value() ? "Route Manager created"
                                                : "Route manager create failed.")
              << std::endl;

Update current pose.

./examples/example_route_manager.cpp:124:130

      route_manager_opt =
        std::move(route_manager_opt.value())
          .update_current_pose(P1, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);

      const auto & route_manager = route_manager_opt.value();
      const auto & current_lanelet = route_manager.current_lanelet();
      std::cout << "Current lanelet (moved) id is " << current_lanelet.id() << std::endl;

Commit success lane change.

./examples/example_route_manager.cpp:144:148

      route_manager_opt = std::move(route_manager_opt.value()).commit_lane_change_success(P5);
      const auto & route_manager = route_manager_opt.value();
      const auto & lane_changed_lanelet = route_manager.current_lanelet();
      std::cout << "Current lanelet (lane changed) id is " << lane_changed_lanelet.id()
                << std::endl;

Get current_lanelet from route_manager.

./examples/example_route_manager.cpp:98:104

    const auto & route_manager = route_manager_opt.value();
    const auto initial_lanelet = route_manager.current_lanelet();
    std::cout << "Current lanelet (initial) id is " << initial_lanelet.id() << std::endl;
    // or directly get from opt
    const auto initial_lanelet_from_opt = route_manager_opt->current_lanelet();
    std::cout << "Current lanelet (initial) id from opt is " << initial_lanelet_from_opt.id()
              << std::endl;

Get lanelet sequence (on route and outward route)

./examples/example_route_manager.cpp:159:175

    {
      const auto seq = route_manager.get_lanelet_sequence_on_route(0.0, 15.0);
      std::cout << "Size of the lanelet sequence is " << seq.as_lanelets().size() << std::endl;
      std::cout << "Including id " << std::endl;
      for (auto i = 0ul; i < seq.as_lanelets().size(); ++i) {
        std::cout << seq.as_lanelets().at(i).id() << std::endl;
      }
    }
    {
      const auto seq = route_manager.get_lanelet_sequence_outward_route(0.0, 15.0);
      std::cout << "Size of the outward lanelet sequence is " << seq.as_lanelets().size()
                << std::endl;
      std::cout << "Including id " << std::endl;
      for (auto i = 0ul; i < seq.as_lanelets().size(); ++i) {
        std::cout << seq.as_lanelets().at(i).id() << std::endl;
      }
    }

Get closest preferred route lanelet (with and without constraints)

./examples/example_route_manager.cpp:178:191

    {
      const auto lane_opt = route_manager.get_closest_preferred_route_lanelet(initial_pose_);
      auto lane = *lane_opt;
      std::cout << "Closest preferred route lanelet id is " << lane.id() << std::endl;
    }

    {
      static constexpr double ego_nearest_dist_threshold = 3.0;
      static constexpr double ego_nearest_yaw_threshold = 1.046;
      const auto lane_opt = route_manager.get_closest_route_lanelet_within_constraints(
        initial_pose_, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
      auto lane = *lane_opt;
      std::cout << "Closest route lanelet (with constraint) id is " << lane.id() << std::endl;
    }

Native R-tree API#

Function	Description	Average Computational Complexity
`.laneletLayer`, `.pointLayer`, etc. have following member functions `PrimitiveLayer::nearest(point, n)` `PrimitiveLayer::nearestUntil(point, cond)`	Against the input `point`, this function approximately returns elements of given `layer` in the ascending order of distance by specified number `n`(reference). Note that they can return inaccurate distance.	R-tree
`PrimitiveLayer::search(area)` `PrimitiveLayer::searchUntil(area, cond)`	This function approximately searches for the object within the specified `area`(reference)	R-tree
`findNearest(layer, point, n)`(in `<lanelet2_core/LaneletMap.h>`)	On the given primitive `layer`, this function approximately returns `n` closest elements to given `point` in the ascending order of distance(reference)	R-tree
`findWithin2d(layer, geometry, max_dist)`(in `<lanelet2_core/geometry/LaneletMap.h>`)	On the given primitive `layer`, this function returns the elements whose distance to given `geometry` is less than `max_dist` precisely	R-tree

Notes#

About Boost.Geometry R-tree#

This slide is useful for understanding Boost.Geometry R-tree features.

Tip

To handle large size HDMaps efficiently, it is highly recommended not to do \(O(N)\) search on primitive layers.

Info

Creating the R-tree is expensive, so it is a good practice to initialize it once from a set of Lanelets
For small number of geometries using a rtree can be more costly. Construction times may be higher than the gain in query performance

Warning

For small number of geometries using a rtree can be more costly. Construction times may be higher than the gain in query performance
Do not do queries on multiple geometries at once (e.g., multiple polygons)
- like rtree.query(intersects(geometries))
- It is usually much faster to do the query for each individual geometries
  - for(const auto & geometry : geometries) rtree.query(intersects(geometry))

Complexity of `findUsage`#

The readers should be noted that following description is implementation dependent.

Lanelet map primitives(like Lanelet, Area, RegulatoryElement) are stored in several PrimitiveLayer<T> objects according to their types as shown below.

lanelet2_core/LaneletMap.h#L375-L438

class LaneletMap : public LaneletMapLayers {
 public:
  using LaneletMapLayers::LaneletMapLayers;
  <...>
};

lanelet2_core/LaneletMap.h#L313-L359

class LaneletMapLayers {
  <...>
  LaneletLayer laneletLayer;                      //!< access to the lanelets within this map
  AreaLayer areaLayer;                            //!< access to areas
  RegulatoryElementLayer regulatoryElementLayer;  //!< access to regElems
  PolygonLayer polygonLayer;                      //!< access to the polygons
  LineStringLayer lineStringLayer;                //!< access to the lineStrings
  PointLayer pointLayer;                          //!< access to the points
};

lanelet2_core/LaneletMap.h#L285-L303

class LaneletLayer : public PrimitiveLayer<Lanelet> {
 public:
  using PrimitiveLayer::findUsages;
  LaneletLayer() = default;
  ~LaneletLayer() = default;
  LaneletLayer(const LaneletLayer&) = delete;
  LaneletLayer operator=(LaneletLayer&) = delete;
  Lanelets findUsages(const RegulatoryElementConstPtr& regElem);
  ConstLanelets findUsages(const RegulatoryElementConstPtr& regElem) const;
  <...>
};

Each PrimitiveLayer owns a field named tree_ that contains a lookup table named usage of type UsageLookup,

lanelet2_core/LaneletMap.h#L38-L253

template <typename T>
class PrimitiveLayer {
 public:
 <...>
  /**
   * @brief finds usages of an owned type within this layer
   *
   * This is the non-const version to find usages of a primitive in a layer.
   */
  std::vector<PrimitiveT> findUsages(const traits::ConstPrimitiveType<traits::OwnedT<PrimitiveT>>& primitive);
  <...>
  struct Tree;
  // NOLINTNEXTLINE
  std::unique_ptr<Tree> tree_;  //!< Hides boost trees from you/the compiler

lanelet2_core/src/LaneletMap.cpp#L277-L308

template <typename T>
struct PrimitiveLayer<T>::Tree {
  using TreeNode = std::pair<BoundingBox2d, T>;
  using RTree = bgi::rtree<TreeNode, bgi::quadratic<16>>;
  static TreeNode treeNode(const T& elem) { return {geometry::boundingBox2d(to2D(elem)), elem}; }
  <...>
  RTree rTree;
  UsageLookup<T> usage;
};

and UsageLookup contains reference relation between different types as std::unordered_multimap.

lanelet2_core/src/LaneletMap.cpp#L259-L270

template <>
struct UsageLookup<Lanelet> {
  void add(Lanelet ll) {
    ownedLookup.insert(std::make_pair(ll.leftBound(), ll));
    ownedLookup.insert(std::make_pair(ll.rightBound(), ll));
    for (const auto& elem : ll.regulatoryElements()) {
      regElemLookup.insert(std::make_pair(elem, ll));
    }
  }
  std::unordered_multimap<ConstLineString3d, Lanelet> ownedLookup;
  std::unordered_multimap<RegulatoryElementConstPtr, Lanelet> regElemLookup;
};

Thus the complexity of findUsage function is equal to that of std::unordered_multimap::equal_range which is \(O(1)\).

lanelet2_core/src/LaneletMap.cpp#L419-L424

template <typename T>
std::vector<typename PrimitiveLayer<T>::ConstPrimitiveT> PrimitiveLayer<T>::findUsages(
    const traits::ConstPrimitiveType<traits::OwnedT<PrimitiveLayer<T>::PrimitiveT>>& primitive) const {
  return forEachMatchInMultiMap<traits::ConstPrimitiveType<typename PrimitiveLayer<T>::PrimitiveT>>(
      tree_->usage.ownedLookup, primitive, [](const auto& elem) { return traits::toConst(elem.second); });
}

lanelet2_core/src/LaneletMap.cpp#L165-L169

template <typename T, typename MapT, typename KeyT, typename Func>
std::vector<T> forEachMatchInMultiMap(const MapT& map, const KeyT& key, Func&& f) {
  auto range = map.equal_range(key);
  return utils::transform(range.first, range.second, f);
}

Test maps#

Test maps are structured based on the Autoware Vector map specifications.

All of the maps are in MGRS coordinate without map_projector_info.yaml. In each map, an anchor point is set to an origin point \((100.0, 100.0)\) for simplicity.

Map name	Origin point id	Image/Description
`vm_01_10-12/dense_centerline/lanelet2_map.osm`	`16`
`vm_01_10-12/straight_waypoint/lanelet2_map.osm`	`1`	TODO
`vm_01_10-12/valid_01/lanelet2_map.osm`	`16`	The start/end of centerline is exactly on the edge of Lanelet border, and consecutive center lines are connected. Speed limits are set as annotated.
`vm_01_10-12/valid_02/lanelet2_map.osm`	`16`	Consecutive center lines are not connected.
`vm_01_10-12/valid_03/lanelet2_map.osm`	`16`	The start of the center lines are off the border.
`vm_01_10-12/valid_04/lanelet2_map.osm`	`16`	The end of the center lines are off the border.
`vm_01_10-12/valid_05/lanelet2_map.osm`	`16`	The start/end of the center lines are off the border(Consecutive center lines are connected).
`vm_01_10-12/valid_06/lanelet2_map.osm`	`16`	The start/end of the center lines are off the border(Consecutive center lines are connected).
`vm_01_10-12/invalid_01/lanelet2_map.osm`	`16`	Any of centerline points except for start/end are outside of the associated Lanelet.

`vm_01_15-16/highway/lanelet2_map.osm`	`1`
`vm_01_15-16/pudo/lanelet2_map.osm`	`140`
`vm_01_15-16/loop/lanelet2_map.osm`	`23`

`vm_02/lanelet2_map.osm`	TODO

`vm_03/left_hand/lanelet2_map.osm`	`1791`
`vm_03/right_hand/lanelet2_map.osm`	TODO	TODO

`vm_06_01/lanelet2_map.osm`	`15`

How to craft test map#

On the VMB, create the map in MGRS system and save the file as <input_map.osm>. Next, select the point to set as origin, get its <ID> and run

ros2 run autoware_lanelet2_utils lanelet_anonymizer.py <input_map.osm> <output_map.osm> <ID>

Then the coordinate of the specified point is (100, 100) on the loaded map(NOTE: not exactly (0, 0) because MGRS does not any point to have negative coordinate value).

By applying lanelet_id_aligner.py, the primitive ids are aligned to start from 1 and increase one-by-one.

ros2 run autoware_lanelet2_utils lanelet_id_aligner.py <input_map.osm>

Finally, to fix lat/lon value of the map, upload the map on VMB, then export it again.

Tested case#

test_data contains test scene yaml files describing the context of unit tests. They can be visualized

ros2 run autoware_lanelet2_utils test_case_generator.py --view <file name>

File	Tested specs	Image
`test_route_manager_001.yaml`	During lane change, `current_route_lanelet` is updated longitudinally. To update `current_route_lanelet` after lane change, `RouteManager::commit_lane_change` needs to be called
`test_route_manager_002.yaml`	During swerving maneuver like parked vehicle avoidance, `RouteManager::commit_lane_change` is not expected to be called. So `current_pose` of `RouteManager` may not be on `current_route_lanelet`

autoware_lanelet2_utils#

Nomenclature#

API description#

<autoware/lanelet2_utils/geometry.hpp>#

Example Usage of geometry#

<autoware/lanelet2_utils/conversion.hpp>#

Example Usage of conversion#

<autoware/lanelet2_utils/kind.hpp>#

Example Usage of kind#

<autoware/lanelet2_utils/hatched_road_markings.hpp>#

Example Usage of hatched_road_markings#

<autoware/lanelet2_utils/topology.hpp>#

Example Usage of topology#

<autoware/lanelet2_utils/intersection.hpp>#

Example Usage of intersection#

<autoware/lanelet2_utils/lane_sequence.hpp#

Example Usage of lane_sequence#

<autoware/lanelet2_utils/nn_search.hpp>#

Example Usage of nn_search#

<autoware/lanelet2_utils/map_handler.hpp>#

Example Usage of map_handler#

<autoware/lanelet2_utils/route_manager.hpp>#

Example Usage of route_manager#