feat(lanelet2_extension): overwriteLaneletsCenterline supports "waypoints"

tmp/lanelet2_extension/docs/lanelet2_format_extension.md

@@ -479,3 +479,23 @@ _An example:_
 ...
 
 ```
+
+### Centerline
+
+Note that the following explanation is not related to the Lanelet2 map format but how the Autoware handles the centerline in the Lanelet2 map.
+
+Centerline is defined in Lanelet2 to guide the vehicle. By explicitly setting the centerline in the Lanelet2 map, the ego's planned path follows the centerline.
+However, based on the current Autoware's usage of the centerline, there are several limitations.
+
+- The object's predicted path also follows the centerline.
+  - This may adversely affect the decision making of planning modules since the centerline is supposed to be used only by the ego's path planning.
+- The coordinate transformation on the lane's frenet frame cannot be calculated correctly.
+  - For example, when the lateral distance between the actual road's centerline and a parked vehicle is calculated, actually the result will be the lateral distance between the (explicit) centerline and the vehicle.
+
+To solve above limitations, the `overwriteLaneletsCenterline` has a `use_waypoints` flag where the centerline in all the lanes is calculated.
+
+- `use_waypoints` is True
+  - The (explicit) centerline in the Lanelet2 map is converted to the new `waypoints` tag. This `waypoints` is only applied to the ego's path planning.
+  - Therefore, the above limitations can be solved, but the Autoware's usage of the centerline may be hard to understand.
+- `use_waypoints` is False
+  - The (explicit) centerline in the Lanelet2 map is used as it is. Easy to understand the Autoware's usage of the centerline, but we still have above limitations.

tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp

@@ -56,7 +56,7 @@ lanelet::ConstLanelets getExpandedLanelets(
  * doesn't have enough quality
  */
 void overwriteLaneletsCenterline(
-  lanelet::LaneletMapPtr lanelet_map, const double resolution = 5.0,
+  lanelet::LaneletMapPtr lanelet_map, const double resolution, const bool use_waypoints,
   const bool force_overwrite = false);
 
 lanelet::ConstLanelets getConflictingLanelets(

tmp/lanelet2_extension/lib/utilities.cpp

@@ -501,12 +501,28 @@ lanelet::ConstLanelets getExpandedLanelets(
 }
 
 void overwriteLaneletsCenterline(
-  lanelet::LaneletMapPtr lanelet_map, const double resolution, const bool force_overwrite)
+  lanelet::LaneletMapPtr lanelet_map, const double resolution, const bool use_waypoints,
+  const bool force_overwrite)
 {
   for (auto & lanelet_obj : lanelet_map->laneletLayer) {
-    if (force_overwrite || !lanelet_obj.hasCustomCenterline()) {
+    if (force_overwrite) {
       const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution);
       lanelet_obj.setCenterline(fine_center_line);
+    } else {
+      if (use_waypoints) {
+        if (lanelet_obj.hasCustomCenterline()) {
+          const auto & centerline = lanelet_obj.centerline();
+          lanelet_obj.setAttribute("waypoints", centerline.id());
+        }
+
+        const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution);
+        lanelet_obj.setCenterline(fine_center_line);
+      } else {
+        if (!lanelet_obj.hasCustomCenterline()) {
+          const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution);
+          lanelet_obj.setCenterline(fine_center_line);
+        }
+      }
     }
   }
 }

tmp/lanelet2_extension/test/src/test_utilities.cpp

@@ -87,6 +87,14 @@ class TestSuite : public ::testing::Test  // NOLINT for gtest
     merging_lanelet.attributes()[lanelet::AttributeName::Subtype] =
       lanelet::AttributeValueString::Road;
 
+    // create sample lanelets
+    Point3d cp1 = Point3d(getId(), 0.5, 0., 0.);
+    Point3d cp2 = Point3d(getId(), 0.5, 0.5, 0.);
+    Point3d cp3 = Point3d(getId(), 0.5, 1., 0.);
+
+    LineString3d ls_centerline(getId(), {cp1, cp2, cp3});
+    road_lanelet.setCenterline(ls_centerline);
+
     sample_map_ptr->add(road_lanelet);
     sample_map_ptr->add(next_lanelet);
     sample_map_ptr->add(next_lanelet2);
@@ -104,12 +112,50 @@ class TestSuite : public ::testing::Test  // NOLINT for gtest
 private:
 };
 
-TEST_F(TestSuite, OverwriteLaneletsCenterline)  // NOLINT for gtest
+TEST_F(TestSuite, OverwriteLaneletsCenterlineWithWaypoints)  // NOLINT for gtest
+{
+  double resolution = 5.0;
+  bool force_overwrite = false;
+  bool use_waypoints = true;
+
+  // memorize the original information of the centerline
+  std::unordered_map<lanelet::Id, lanelet::Id> lanelet_centerline_map{};
+  for (const auto & lanelet : sample_map_ptr->laneletLayer) {
+    if (lanelet.hasCustomCenterline()) {
+      lanelet_centerline_map.insert({lanelet.id(), lanelet.centerline().id()});
+    }
+  }
+
+  // convert centerline to waypoints
+  lanelet::utils::overwriteLaneletsCenterline(
+    sample_map_ptr, resolution, use_waypoints, force_overwrite);
+
+  for (const auto & lanelet : sample_map_ptr->laneletLayer) {
+    if (lanelet_centerline_map.find(lanelet.id()) != lanelet_centerline_map.end()) {
+      // check if the lanelet has waypoints.
+      ASSERT_TRUE(lanelet.hasAttribute("waypoints")) << "The lanelet does not have a waypoints.";
+      // check if the waypoints points to the linestring of the centerline.
+      ASSERT_TRUE(
+        lanelet.attribute("waypoints").asId().value() == lanelet_centerline_map.at(lanelet.id()))
+        << "The waypoint's ID is invalid.";
+    }
+  }
+
+  // check if all the lanelets have a centerline
+  for (const auto & lanelet : sample_map_ptr->laneletLayer) {
+    ASSERT_TRUE(lanelet.hasCustomCenterline()) << "failed to calculate fine centerline";
+  }
+}
+
+TEST_F(TestSuite, OverwriteLaneletsCenterlineWithoutWaypoints)  // NOLINT for gtest
 {
   double resolution = 5.0;
   bool force_overwrite = false;
-  lanelet::utils::overwriteLaneletsCenterline(sample_map_ptr, resolution, force_overwrite);
+  bool use_waypoints = false;
+  lanelet::utils::overwriteLaneletsCenterline(
+    sample_map_ptr, resolution, use_waypoints, force_overwrite);
 
+  // check if all the lanelets have a centerline
   for (const auto & lanelet : sample_map_ptr->laneletLayer) {
     ASSERT_TRUE(lanelet.hasCustomCenterline()) << "failed to calculate fine centerline";
   }

tmp/lanelet2_extension_python/src/utility.cpp

@@ -377,7 +377,7 @@ BOOST_PYTHON_FUNCTION_OVERLOADS(
 BOOST_PYTHON_FUNCTION_OVERLOADS(
   getLeftBoundWithOffset_overload, lanelet::utils::getLeftBoundWithOffset, 2, 3)
 BOOST_PYTHON_FUNCTION_OVERLOADS(
-  overwriteLaneletsCenterline_overload, lanelet::utils::overwriteLaneletsCenterline, 1, 3)
+  overwriteLaneletsCenterline_overload, lanelet::utils::overwriteLaneletsCenterline, 3, 4)
 BOOST_PYTHON_FUNCTION_OVERLOADS(isInLanelet_overload, ::isInLanelet, 2, 3)
 
 /// query.cpp