Merge pull request #11 from youtalk/import-update

feat: import updates from `autoware_common`

autoware_lanelet2_extension/docs/lanelet2_format_extension.md

@@ -409,7 +409,7 @@ _An example:_
   </relation>
 ```
 
-For more details about the `no_drivable_lane` concept and design, please refer to the [**_no-drivable-lane-design_**](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_velocity_no_drivable_lane_module/README.md) document.
+For more details about the `no_drivable_lane` concept and design, please refer to the [**_no-drivable-lane-design_**](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/README.md) document.
 
 ### Localization Landmarks
 
@@ -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 `overwriteLaneletsCenterlineWithWaypoints` was implemented in addition to `overwriteLaneletsCenterline` where the centerline in all the lanes is calculated.
+
+- `overwriteLaneletsCenterlineWithWaypoints`
+  - 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.
+- `overwriteLaneletsCenterline`
+  - 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.

autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/utilities.hpp

@@ -59,6 +59,16 @@ void overwriteLaneletsCenterline(
   lanelet::LaneletMapPtr lanelet_map, const double resolution = 5.0,
   const bool force_overwrite = false);
 
+/**
+ * @brief  Apply another patch for centerline because the overwriteLaneletsCenterline
+ * has several limitations. See the following document in detail.
+ * https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#centerline
+ * // NOLINT
+ */
+void overwriteLaneletsCenterlineWithWaypoints(
+  lanelet::LaneletMapPtr lanelet_map, const double resolution = 5.0,
+  const bool force_overwrite = false);
+
 lanelet::ConstLanelets getConflictingLanelets(
   const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet);
 

autoware_lanelet2_extension/lib/utilities.cpp

@@ -511,6 +511,25 @@ void overwriteLaneletsCenterline(
   }
 }
 
+void overwriteLaneletsCenterlineWithWaypoints(
+  lanelet::LaneletMapPtr lanelet_map, const double resolution, const bool force_overwrite)
+{
+  for (auto & lanelet_obj : lanelet_map->laneletLayer) {
+    if (force_overwrite) {
+      const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution);
+      lanelet_obj.setCenterline(fine_center_line);
+    } else {
+      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);
+    }
+  }
+}
+
 lanelet::ConstLanelets getConflictingLanelets(
   const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet)
 {

autoware_lanelet2_extension/lib/visualization.cpp

@@ -40,7 +40,7 @@ namespace
 template <typename T>
 bool exists(const std::unordered_set<T> & set, const T & element)
 {
-  return std::find(set.begin(), set.end(), element) != set.end();
+  return set.find(element) != set.end();
 }
 
 void adjacentPoints(

autoware_lanelet2_extension/test/src/test_regulatory_elements.cpp

@@ -16,6 +16,8 @@
 
 #include "autoware_lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp"
 
+#include <boost/optional/optional_io.hpp>
+
 #include <gtest/gtest.h>
 #include <lanelet2_core/Attribute.h>
 #include <lanelet2_core/LaneletMap.h>

autoware_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,47 @@ class TestSuite : public ::testing::Test  // NOLINT for gtest
 private:
 };
 
+TEST_F(TestSuite, OverwriteLaneletsCenterlineWithWaypoints)  // NOLINT for gtest
+{
+  double resolution = 5.0;
+  bool force_overwrite = false;
+
+  // 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::overwriteLaneletsCenterlineWithWaypoints(
+    sample_map_ptr, resolution, 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, OverwriteLaneletsCenterline)  // NOLINT for gtest
 {
   double resolution = 5.0;
   bool force_overwrite = false;
   lanelet::utils::overwriteLaneletsCenterline(sample_map_ptr, resolution, 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";
   }