feat(lanelet2_extension): deprecate lanelet2_extension utilities functions (final) (#103)

Signed-off-by: Sarun Mukdapitak <sarun.mukda@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: sarun-hub

autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/utilities.hpp

@@ -37,6 +37,7 @@ namespace lanelet::utils
 [[deprecated("please use autoware::lanelet2_utils::combine_lanelets_shape instead")]]
 lanelet::ConstLanelet combineLaneletsShape(const lanelet::ConstLanelets & lanelets);
 
+[[deprecated("please use autoware::lanelet2_utils::get_fine_centerline instead")]]
 lanelet::LineString3d generateFineCenterline(
   const lanelet::ConstLanelet & lanelet_obj, const double resolution = 5.0);
 [[deprecated("please use autoware::lanelet2_utils::get_centerline_with_offset instead")]]
@@ -109,6 +110,7 @@ lanelet::ArcCoordinates getArcCoordinates(
  * - when the `use_waypoints` in the autoware_map_loader is false,
  *   - the centerline in the lanelet2::LaneletMapPtr is used.
  */
+[[deprecated("please use autoware::lanelet2_utils::get_arc_coordinates_on_ego_centerline instead")]]
 lanelet::ArcCoordinates getArcCoordinatesOnEgoCenterline(
   const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose,
   const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
@@ -118,11 +120,13 @@ lanelet::ArcCoordinates getArcCoordinatesOnEgoCenterline(
 getClosestSegment(
   const lanelet::BasicPoint2d & search_pt, const lanelet::ConstLineString3d & linestring);
 
+[[deprecated("please use autoware::lanelet2_utils::get_polygon_from_arc_length instead")]]
 lanelet::CompoundPolygon3d getPolygonFromArcLength(
   const lanelet::ConstLanelets & lanelets, const double s1, const double s2);
 [[deprecated("please use autoware::lanelet2_utils::get_lanelet_angle instead")]] double
 getLaneletAngle(
   const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point);
+[[deprecated("please use autoware::lanelet2_utils::is_in_lanelet instead")]]
 bool isInLanelet(
   const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet,
   const double radius = 0.0);

autoware_lanelet2_extension/lib/deprecated.cpp

@@ -20,16 +20,17 @@
 
 #include "deprecated.hpp"
 
-// TODO(sarun-hub): lanelet2_extension will be removed when toLaneletPoint is deprecated.
-#include <autoware_lanelet2_extension/utility/message_conversion.hpp>  // For toLaneletPoint
 #include <tf2/utils.hpp>
 
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <lanelet2_core/geometry/Lanelet.h>
 
+#include <algorithm>
 #include <iostream>
 #include <limits>
+#include <utility>
+#include <vector>
 
 using lanelet::utils::to2D;
 
@@ -63,6 +64,96 @@ void copyZ(const T1 & from, T2 & to)
     to[i_to].z() = from[i_from - 1].z() + ratio * (from[i_from].z() - from[i_from - 1].z());
   }
 }
+
+std::vector<double> calculateSegmentDistances(const lanelet::ConstLineString3d & line_string)
+{
+  std::vector<double> segment_distances;
+  segment_distances.reserve(line_string.size() - 1);
+
+  for (size_t i = 1; i < line_string.size(); ++i) {
+    const auto distance = lanelet::geometry::distance(line_string[i], line_string[i - 1]);
+    segment_distances.push_back(distance);
+  }
+
+  return segment_distances;
+}
+
+std::vector<double> calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string)
+{
+  const auto segment_distances = calculateSegmentDistances(line_string);
+
+  std::vector<double> accumulated_lengths{0};
+  accumulated_lengths.reserve(segment_distances.size() + 1);
+  std::partial_sum(
+    std::begin(segment_distances), std::end(segment_distances),
+    std::back_inserter(accumulated_lengths));
+
+  return accumulated_lengths;
+}
+
+std::pair<size_t, size_t> findNearestIndexPair(
+  const std::vector<double> & accumulated_lengths, const double target_length)
+{
+  // List size
+  const auto N = accumulated_lengths.size();
+
+  // Front
+  if (target_length < accumulated_lengths.at(1)) {
+    return std::make_pair(0, 1);
+  }
+
+  // Back
+  if (target_length > accumulated_lengths.at(N - 2)) {
+    return std::make_pair(N - 2, N - 1);
+  }
+
+  // Middle
+  for (std::size_t i = 1; i < N; ++i) {
+    if (
+      accumulated_lengths.at(i - 1) <= target_length &&
+      target_length <= accumulated_lengths.at(i)) {
+      return std::make_pair(i - 1, i);
+    }
+  }
+
+  // Throw an exception because this never happens
+  throw std::runtime_error("No nearest point found.");
+}
+
+std::vector<lanelet::BasicPoint3d> resamplePoints(
+  const lanelet::ConstLineString3d & line_string, const int num_segments)
+{
+  // Calculate length
+  const auto line_length = static_cast<double>(lanelet::geometry::length(line_string));
+
+  // Calculate accumulated lengths
+  const auto accumulated_lengths = calculateAccumulatedLengths(line_string);
+  if (accumulated_lengths.size() < 2) return {};
+
+  // Create each segment
+  std::vector<lanelet::BasicPoint3d> resampled_points;
+  for (auto i = 0; i <= num_segments; ++i) {
+    // Find two nearest points
+    const auto target_length = (static_cast<double>(i) / num_segments) * line_length;
+    const auto index_pair = findNearestIndexPair(accumulated_lengths, target_length);
+
+    // Apply linear interpolation
+    const lanelet::BasicPoint3d back_point = line_string[index_pair.first];
+    const lanelet::BasicPoint3d front_point = line_string[index_pair.second];
+    const auto direction_vector = (front_point - back_point);
+
+    const auto back_length = accumulated_lengths.at(index_pair.first);
+    const auto front_length = accumulated_lengths.at(index_pair.second);
+    const auto segment_length = front_length - back_length;
+    const auto target_point =
+      back_point + (direction_vector * (target_length - back_length) / segment_length);
+
+    // Add to list
+    resampled_points.emplace_back(target_point);
+  }
+
+  return resampled_points;
+}
 }  // namespace detail
 
 lanelet::ConstLineString3d getClosestSegment(
@@ -240,15 +331,6 @@ bool getClosestLanelet(
   return found;
 }
 
-bool isInLanelet(
-  const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet,
-  const double radius)
-{
-  constexpr double eps = 1.0e-9;
-  const lanelet::BasicPoint2d p(current_pose.position.x, current_pose.position.y);
-  return boost::geometry::distance(p, lanelet.polygon2d().basicPolygon()) < radius + eps;
-}
-
 double getLateralDistanceToCenterline(
   const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Pose & pose)
 {
@@ -432,4 +514,32 @@ lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src)
   toLaneletPoint(src, &dst);
   return dst;
 }
+
+lanelet::LineString3d generateFineCenterline(
+  const lanelet::ConstLanelet & lanelet_obj, const double resolution)
+{
+  // Get length of longer border
+  const double left_length =
+    static_cast<double>(lanelet::geometry::length(lanelet_obj.leftBound()));
+  const double right_length =
+    static_cast<double>(lanelet::geometry::length(lanelet_obj.rightBound()));
+  const double longer_distance = (left_length > right_length) ? left_length : right_length;
+  const int num_segments = std::max(static_cast<int>(ceil(longer_distance / resolution)), 1);
+
+  // Resample points
+  const auto left_points = detail::resamplePoints(lanelet_obj.leftBound(), num_segments);
+  const auto right_points = detail::resamplePoints(lanelet_obj.rightBound(), num_segments);
+
+  // Create centerline
+  lanelet::LineString3d centerline(lanelet::utils::getId());
+  for (int i = 0; i < num_segments + 1; i++) {
+    // Add ID for the average point of left and right
+    const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2;
+    const lanelet::Point3d center_point(
+      lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(),
+      center_basic_point.z());
+    centerline.push_back(center_point);
+  }
+  return centerline;
+}
 }  // namespace deprecated

autoware_lanelet2_extension/lib/deprecated.hpp

@@ -73,10 +73,6 @@ bool getClosestLanelet(
   const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose,
   lanelet::ConstLanelet * closest_lanelet_ptr);
 
-bool isInLanelet(
-  const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet,
-  const double radius);
-
 double getLateralDistanceToCenterline(
   const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Pose & pose);
 
@@ -98,6 +94,9 @@ void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * ds
 void toLaneletPoint(const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst);
 
 lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src);
+
+lanelet::LineString3d generateFineCenterline(
+  const lanelet::ConstLanelet & lanelet_obj, const double resolution = 5.0);
 }  // namespace deprecated
 // NOLINTEND(readability-identifier-naming)
 

autoware_lanelet2_extension/lib/utilities.cpp

@@ -516,7 +516,7 @@ void overwriteLaneletsCenterline(
 {
   for (auto & lanelet_obj : lanelet_map->laneletLayer) {
     if (force_overwrite || !lanelet_obj.hasCustomCenterline()) {
-      const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution);
+      const auto fine_center_line = deprecated::generateFineCenterline(lanelet_obj, resolution);
       lanelet_obj.setCenterline(fine_center_line);
     }
   }
@@ -527,15 +527,15 @@ void overwriteLaneletsCenterlineWithWaypoints(
 {
   for (auto & lanelet_obj : lanelet_map->laneletLayer) {
     if (force_overwrite) {
-      const auto fine_center_line = generateFineCenterline(lanelet_obj, resolution);
+      const auto fine_center_line = deprecated::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);
+      const auto fine_center_line = deprecated::generateFineCenterline(lanelet_obj, resolution);
       lanelet_obj.setCenterline(fine_center_line);
     }
   }