feat: add getArcCoordinatesUsingWaypoints to enable custom waypoint usage

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>

Author: mehmetdogru

autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/utilities.hpp

@@ -86,6 +86,14 @@ double getLaneletLength3d(const lanelet::ConstLanelets & lanelet_sequence);
 lanelet::ArcCoordinates getArcCoordinates(
   const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose);
 
+/**
+ * @brief  This function uses custom centerline for the lanelet if exists defined with the attribute
+ * "waypoints" instead of centerline associated to the lanelet
+ */
+lanelet::ArcCoordinates getArcCoordinatesConsideringWaypoints(
+  const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose,
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
 lanelet::ConstLineString3d getClosestSegment(
   const lanelet::BasicPoint2d & search_pt, const lanelet::ConstLineString3d & linestring);
 

autoware_lanelet2_extension/lib/utilities.cpp

@@ -675,6 +675,36 @@ lanelet::ArcCoordinates getArcCoordinates(
   return arc_coordinates;
 }
 
+lanelet::ArcCoordinates getArcCoordinatesConsideringWaypoints(
+  const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose,
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLanelet closest_lanelet;
+  lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet);
+
+  double length = 0;
+  lanelet::ArcCoordinates arc_coordinates;
+  for (const auto & llt : lanelet_sequence) {
+    ConstLineString2d centerline_2d;
+    if (llt.hasAttribute("waypoints")) {
+      const auto waypoints_id = llt.attribute("waypoints").asId().value();
+      centerline_2d = lanelet::utils::to2D(lanelet_map_ptr->lineStringLayer.get(waypoints_id));
+    } else {
+      centerline_2d = lanelet::utils::to2D(llt.centerline());
+    }
+
+    if (llt == closest_lanelet) {
+      const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position);
+      arc_coordinates = lanelet::geometry::toArcCoordinates(
+        centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
+      arc_coordinates.length += length;
+      break;
+    }
+    length += static_cast<double>(boost::geometry::length(centerline_2d));
+  }
+  return arc_coordinates;
+}
+
 lanelet::ConstLineString3d getClosestSegment(
   const lanelet::BasicPoint2d & search_pt, const lanelet::ConstLineString3d & linestring)
 {