feat(lanelet2_extension): overwriteLaneletsCenterline supports "waypoints" (#252)

* feat(lanelet2_extension): centerline is converted to waypoints

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix lanelet2_extension_python

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update README

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* early return

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix clang-tidy

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* Update tmp/lanelet2_extension/lib/utilities.cpp

Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>

* style(pre-commit): autofix

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: 3 people

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 `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.

tmp/lanelet2_extension/include/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);
 

tmp/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)
 {

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,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";
   }

tmp/lanelet2_extension_python/src/utility.cpp

@@ -45,9 +45,8 @@ lanelet::Optional<lanelet::ConstPolygon3d> lineStringWithWidthToPolygon(
   lanelet::ConstPolygon3d poly{};
   if (lanelet::utils::lineStringWithWidthToPolygon(linestring, &poly)) {
     return poly;
-  } else {
-    return {};
   }
+  return {};
 }
 
 lanelet::Optional<lanelet::ConstPolygon3d> lineStringToPolygon(
@@ -56,9 +55,8 @@ lanelet::Optional<lanelet::ConstPolygon3d> lineStringToPolygon(
   lanelet::ConstPolygon3d poly{};
   if (lanelet::utils::lineStringToPolygon(linestring, &poly)) {
     return poly;
-  } else {
-    return {};
   }
+  return {};
 }
 
 lanelet::ArcCoordinates getArcCoordinates(
@@ -69,7 +67,9 @@ lanelet::ArcCoordinates getArcCoordinates(
   serialized_msg.reserve(message_header_length + pose_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
   for (size_t i = 0; i < pose_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Pose pose;
   static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
@@ -84,7 +84,9 @@ double getLaneletAngle(const lanelet::ConstLanelet & lanelet, const std::string
   serialized_msg.reserve(message_header_length + point_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size();
   for (size_t i = 0; i < point_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Point point;
   static rclcpp::Serialization<geometry_msgs::msg::Point> serializer;
@@ -100,7 +102,9 @@ bool isInLanelet(
   serialized_msg.reserve(message_header_length + pose_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
   for (size_t i = 0; i < pose_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Pose pose;
   static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
@@ -116,7 +120,9 @@ std::vector<double> getClosestCenterPose(
   serialized_point_msg.reserve(message_header_length + search_point_byte.size());
   serialized_point_msg.get_rcl_serialized_message().buffer_length = search_point_byte.size();
   for (size_t i = 0; i < search_point_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_point_msg.get_rcl_serialized_message().buffer[i] = search_point_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Point search_point;
   static rclcpp::Serialization<geometry_msgs::msg::Point> serializer_point;
@@ -137,7 +143,9 @@ double getLateralDistanceToCenterline(
   serialized_msg.reserve(message_header_length + pose_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
   for (size_t i = 0; i < pose_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Pose pose;
   static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
@@ -153,7 +161,9 @@ double getLateralDistanceToClosestLanelet(
   serialized_msg.reserve(message_header_length + pose_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
   for (size_t i = 0; i < pose_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Pose pose;
   static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
@@ -180,9 +190,8 @@ lanelet::Optional<lanelet::ConstLanelet> getLinkedLanelet(
   if (lanelet::utils::query::getLinkedLanelet(
         parking_space, all_road_lanelets, all_parking_lots, &linked_lanelet)) {
     return linked_lanelet;
-  } else {
-    return {};
   }
+  return {};
 }
 
 lanelet::Optional<lanelet::ConstLanelet> getLinkedLanelet(
@@ -192,9 +201,8 @@ lanelet::Optional<lanelet::ConstLanelet> getLinkedLanelet(
   lanelet::ConstLanelet linked_lanelet;
   if (lanelet::utils::query::getLinkedLanelet(parking_space, lanelet_map_ptr, &linked_lanelet)) {
     return linked_lanelet;
-  } else {
-    return {};
   }
+  return {};
 }
 
 lanelet::Optional<lanelet::ConstPolygon3d> getLinkedParkingLot(
@@ -203,9 +211,8 @@ lanelet::Optional<lanelet::ConstPolygon3d> getLinkedParkingLot(
   lanelet::ConstPolygon3d linked_parking_lot;
   if (lanelet::utils::query::getLinkedParkingLot(lanelet, all_parking_lots, &linked_parking_lot)) {
     return linked_parking_lot;
-  } else {
-    return {};
   }
+  return {};
 }
 
 lanelet::Optional<lanelet::ConstPolygon3d> getLinkedParkingLot(
@@ -215,9 +222,8 @@ lanelet::Optional<lanelet::ConstPolygon3d> getLinkedParkingLot(
   if (lanelet::utils::query::getLinkedParkingLot(
         current_position, all_parking_lots, &linked_parking_lot)) {
     return linked_parking_lot;
-  } else {
-    return {};
   }
+  return {};
 }
 
 lanelet::Optional<lanelet::ConstPolygon3d> getLinkedParkingLot(
@@ -228,9 +234,8 @@ lanelet::Optional<lanelet::ConstPolygon3d> getLinkedParkingLot(
   if (lanelet::utils::query::getLinkedParkingLot(
         parking_space, all_parking_lots, &linked_parking_lot)) {
     return linked_parking_lot;
-  } else {
-    return {};
   }
+  return {};
 }
 
 lanelet::ConstLanelets getLaneletsWithinRange_point(
@@ -241,7 +246,9 @@ lanelet::ConstLanelets getLaneletsWithinRange_point(
   serialized_msg.reserve(message_header_length + point_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size();
   for (size_t i = 0; i < point_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Point point;
   static rclcpp::Serialization<geometry_msgs::msg::Point> serializer;
@@ -258,7 +265,9 @@ lanelet::ConstLanelets getLaneChangeableNeighbors_point(
   serialized_msg.reserve(message_header_length + point_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size();
   for (size_t i = 0; i < point_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Point point;
   static rclcpp::Serialization<geometry_msgs::msg::Point> serializer;
@@ -275,7 +284,9 @@ lanelet::ConstLanelets getAllNeighbors_point(
   serialized_msg.reserve(message_header_length + point_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size();
   for (size_t i = 0; i < point_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Point point;
   static rclcpp::Serialization<geometry_msgs::msg::Point> serializer;
@@ -291,17 +302,18 @@ lanelet::Optional<lanelet::ConstLanelet> getClosestLanelet(
   serialized_msg.reserve(message_header_length + pose_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
   for (size_t i = 0; i < pose_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Pose pose;
   static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
   serializer.deserialize_message(&serialized_msg, &pose);
   lanelet::ConstLanelet closest_lanelet{};
   if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) {
     return closest_lanelet;
-  } else {
-    return {};
   }
+  return {};
 }
 
 lanelet::Optional<lanelet::ConstLanelet> getClosestLaneletWithConstrains(
@@ -314,7 +326,9 @@ lanelet::Optional<lanelet::ConstLanelet> getClosestLaneletWithConstrains(
   serialized_msg.reserve(message_header_length + pose_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
   for (size_t i = 0; i < pose_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Pose pose;
   static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
@@ -323,9 +337,8 @@ lanelet::Optional<lanelet::ConstLanelet> getClosestLaneletWithConstrains(
   if (lanelet::utils::query::getClosestLaneletWithConstrains(
         lanelets, pose, &closest_lanelet, dist_threshold, yaw_threshold)) {
     return closest_lanelet;
-  } else {
-    return {};
   }
+  return {};
 }
 
 lanelet::ConstLanelets getCurrentLanelets_point(
@@ -336,7 +349,9 @@ lanelet::ConstLanelets getCurrentLanelets_point(
   serialized_msg.reserve(message_header_length + point_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = point_byte.size();
   for (size_t i = 0; i < point_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = point_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Point point;
   static rclcpp::Serialization<geometry_msgs::msg::Point> serializer;
@@ -354,7 +369,9 @@ lanelet::ConstLanelets getCurrentLanelets_pose(
   serialized_msg.reserve(message_header_length + pose_byte.size());
   serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();
   for (size_t i = 0; i < pose_byte.size(); ++i) {
+    // NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic)
     serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
+    // NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic)
   }
   geometry_msgs::msg::Pose pose;
   static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
@@ -368,6 +385,7 @@ lanelet::ConstLanelets getCurrentLanelets_pose(
 
 // for handling functions with default arguments
 /// utilities.cpp
+// NOLINTBEGIN
 BOOST_PYTHON_FUNCTION_OVERLOADS(
   generateFineCenterline_overload, lanelet::utils::generateFineCenterline, 1, 2)
 BOOST_PYTHON_FUNCTION_OVERLOADS(
@@ -387,6 +405,7 @@ BOOST_PYTHON_FUNCTION_OVERLOADS(
   getClosestLaneletWithConstrains_overload, ::getClosestLaneletWithConstrains, 2, 4)
 BOOST_PYTHON_FUNCTION_OVERLOADS(
   getPrecedingLaneletSequences_overload, lanelet::utils::query::getPrecedingLaneletSequences, 3, 4)
+// NOLINTEND
 
 BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility)
 {