feat(autoware_lanelet2_extension): replace remaining lanelet2_extension utilities functions - bpp packages

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -23,7 +23,7 @@
 #include <autoware/behavior_path_lane_change_module/utils/calculation.hpp>
 #include <autoware/behavior_path_lane_change_module/utils/utils.hpp>
 #include <autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp>
-#include <autoware_lanelet2_extension/utility/utilities.hpp>
+#include <autoware/lanelet2_utils/geometry.hpp>
 #include <rclcpp/logging.hpp>
 
 #include <boost/geometry/algorithms/centroid.hpp>
@@ -252,7 +252,8 @@ std::optional<ObjectData> AvoidanceByLaneChange::createObjectData(
   ObjectData object_data{};
   // Calc lateral deviation from path to target object.
   object_data.to_centerline =
-    lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance;
+    autoware::experimental::lanelet2_utils::get_arc_coordinates(data.current_lanelets, object_pose)
+      .distance;
 
   if (
     std::abs(object_data.to_centerline) <

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp

@@ -25,6 +25,7 @@
 #include <autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp>
 #include <autoware/behavior_path_planner_common/utils/path_utils.hpp>
 #include <autoware/lanelet2_utils/conversion.hpp>
+#include <autoware/lanelet2_utils/geometry.hpp>
 #include <autoware/route_handler/route_handler.hpp>
 #include <autoware_lanelet2_extension/io/autoware_osm_parser.hpp>
 #include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
@@ -321,7 +322,8 @@
     // get road lanes which is at least backward_length[m] behind the goal
     const auto road_lanes = getExtendedCurrentLanesFromPath(
       upstream_module_output.path, planner_data, backward_length, 0.0, false);
-    const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length;
+    const auto goal_pose_length =
+      autoware::experimental::lanelet2_utils::get_arc_coordinates(road_lanes, goal_pose).length;
     return planner_data->route_handler->getCenterLinePath(
       road_lanes, std::max(0.0, goal_pose_length - backward_length),
       goal_pose_length + parameters.forward_goal_search_length);
@@ -470,7 +472,8 @@
   const auto departure_check_lane =
     autoware::behavior_path_planner::goal_planner_utils::createDepartureCheckLanelet(
       pull_over_lanes, *route_handler, true);
-  const auto goal_arc_coords = lanelet::utils::getArcCoordinates(pull_over_lanes, refined_goal);
+  const auto goal_arc_coords =
+    autoware::experimental::lanelet2_utils::get_arc_coordinates(pull_over_lanes, refined_goal);
   const double s_start = std::max(0.0, goal_arc_coords.length - backward_length);
   const double s_end = goal_arc_coords.length + forward_length;
   const double longitudinal_interval = use_bus_stop_area

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp

@@ -25,6 +25,7 @@
 #include <autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp>
 #include <autoware/behavior_path_planner_common/utils/path_utils.hpp>
 #include <autoware/lanelet2_utils/conversion.hpp>
+#include <autoware/lanelet2_utils/geometry.hpp>
 #include <autoware/route_handler/route_handler.hpp>
 #include <autoware_lanelet2_extension/io/autoware_osm_parser.hpp>
 #include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
@@ -320,7 +321,8 @@
     // get road lanes which is at least backward_length[m] behind the goal
     const auto road_lanes = getExtendedCurrentLanesFromPath(
       upstream_module_output.path, planner_data, backward_length, 0.0, false);
-    const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length;
+    const auto goal_pose_length =
+      autoware::experimental::lanelet2_utils::get_arc_coordinates(road_lanes, goal_pose).length;
     return planner_data->route_handler->getCenterLinePath(
       road_lanes, std::max(0.0, goal_pose_length - backward_length),
       goal_pose_length + parameters.forward_goal_search_length);
@@ -480,7 +482,8 @@
   const auto departure_check_lane =
     autoware::behavior_path_planner::goal_planner_utils::createDepartureCheckLanelet(
       pull_over_lanes, *route_handler, true);
-  const auto goal_arc_coords = lanelet::utils::getArcCoordinates(pull_over_lanes, refined_goal);
+  const auto goal_arc_coords =
+    autoware::experimental::lanelet2_utils::get_arc_coordinates(pull_over_lanes, refined_goal);
   const double s_start = std::max(0.0, goal_arc_coords.length - backward_length);
   const double s_end = goal_arc_coords.length + forward_length;
   const double longitudinal_interval = use_bus_stop_area

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -32,7 +32,6 @@
 #include <autoware/lanelet2_utils/geometry.hpp>
 #include <autoware/lanelet2_utils/nn_search.hpp>
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
-#include <autoware_lanelet2_extension/utility/utilities.hpp>
 #include <autoware_utils/math/normalization.hpp>
 #include <autoware_utils/system/stop_watch.hpp>
 #include <magic_enum.hpp>
@@ -389,8 +388,9 @@
       // priority
       path_candidates.push_back(*pull_over_path);
       // calculate closest pull over start pose for stop path
-      const double start_arc_length =
-        lanelet::utils::getArcCoordinates(current_lanelets, pull_over_path->start_pose()).length;
+      const double start_arc_length = autoware::experimental::lanelet2_utils::get_arc_coordinates(
+                                        current_lanelets, pull_over_path->start_pose())
+                                        .length;
       if (start_arc_length < min_start_arc_length) {
         min_start_arc_length = start_arc_length;
         // closest start pose is stop point when not finding safe path
@@ -511,9 +511,9 @@
 
   double min_start_arc_length = std::numeric_limits<double>::infinity();
   for (const auto & bezier_pull_over_path : path_candidates) {
-    const double start_arc_length =
-      lanelet::utils::getArcCoordinates(current_lanelets, bezier_pull_over_path.start_pose())
-        .length;
+    const double start_arc_length = autoware::experimental::lanelet2_utils::get_arc_coordinates(
+                                      current_lanelets, bezier_pull_over_path.start_pose())
+                                      .length;
     if (start_arc_length < min_start_arc_length) {
       min_start_arc_length = start_arc_length;
       closest_start_pose = bezier_pull_over_path.start_pose();
@@ -1296,7 +1296,8 @@
     // get road lanes which is at least backward_length[m] behind the goal
     const auto road_lanes = utils::getExtendedCurrentLanesFromPath(
       prev_module_output_path, planner_data_, backward_length, 0.0, false);
-    const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length;
+    const auto goal_pose_length =
+      autoware::experimental::lanelet2_utils::get_arc_coordinates(road_lanes, goal_pose).length;
     return planner_data_->route_handler->getCenterLinePath(
       road_lanes, std::max(0.0, goal_pose_length - backward_length),
       goal_pose_length + parameters_.forward_goal_search_length);
@@ -1898,15 +1899,18 @@
   // otherwise, generate path to the goal_pose.
   const auto & pull_over_path_opt = context_data.pull_over_path_opt;
   const auto reference_path = std::invoke([&]() -> PathWithLaneId {
-    const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length;
+    const auto s_current =
+      autoware::experimental::lanelet2_utils::get_arc_coordinates(current_lanes, current_pose)
+        .length;
     const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length);
     const bool is_arc_backward =
       pull_over_path_opt.has_value() &&
       pull_over_path_opt.value().type() == PullOverPlannerType::ARC_BACKWARD;
     const Pose path_end_pose =
       is_arc_backward ? pull_over_path_opt.value().start_pose() : route_handler->getGoalPose();
     const double s_end = std::clamp(
-      lanelet::utils::getArcCoordinates(current_lanes, path_end_pose).length,
+      autoware::experimental::lanelet2_utils::get_arc_coordinates(current_lanes, path_end_pose)
+        .length,
       s_current + std::numeric_limits<double>::epsilon(),
       s_current + common_parameters.forward_path_length);
     return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true);
@@ -2170,7 +2174,8 @@
   }
 
   const double current_shift_length =
-    lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance;
+    autoware::experimental::lanelet2_utils::get_arc_coordinates(current_lanes, current_pose)
+      .distance;
 
   constexpr bool egos_lane_is_shifted = true;
   constexpr bool is_driving_forward = true;
@@ -2629,8 +2634,13 @@
       pull_over_lanes, left_side_parking_, ego_pose_for_expand,
       planner_data->parameters.vehicle_info, parameters_.outer_road_detection_offset,
       parameters_.inner_road_detection_offset);
-  const auto merged_expanded_pull_over_lanes =
-    lanelet::utils::combineLaneletsShape(expanded_pull_over_lanes_between_ego);
+  const auto merged_expanded_pull_over_lanes_opt =
+    autoware::experimental::lanelet2_utils::combine_lanelets_shape(
+      expanded_pull_over_lanes_between_ego);
+  lanelet::ConstLanelet merged_expanded_pull_over_lanes{};
+  if (merged_expanded_pull_over_lanes_opt.has_value()) {
+    merged_expanded_pull_over_lanes = merged_expanded_pull_over_lanes_opt.value();
+  }
   debug_data_.expanded_pull_over_lane_between_ego = merged_expanded_pull_over_lanes;
 
   const auto filtered_objects = filterObjectsByWithinPolicy(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp

@@ -24,9 +24,9 @@
 #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp"
 #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp"
 #include "autoware_lanelet2_extension/utility/query.hpp"
-#include "autoware_lanelet2_extension/utility/utilities.hpp"
 #include "autoware_utils/geometry/boost_polygon_utils.hpp"
 
+#include <autoware/lanelet2_utils/geometry.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info.hpp>
 
 #include <boost/geometry/algorithms/union.hpp>
@@ -321,7 +321,12 @@
   const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet(
     pull_over_lanes_, *route_handler, left_side_parking_);
 
-  const auto pull_over_lanelet = lanelet::utils::combineLaneletsShape(pull_over_lanes_);
+  const auto pull_over_lanelet_opt =
+    autoware::experimental::lanelet2_utils::combine_lanelets_shape(pull_over_lanes_);
+  if (!pull_over_lanelet_opt.has_value()) {
+    return goal_candidates;
+  }
+  const auto & pull_over_lanelet = pull_over_lanelet_opt.value();
   const auto boundary =
     left_side_parking_ ? pull_over_lanelet.leftBound() : pull_over_lanelet.rightBound();
 
@@ -421,8 +426,8 @@
 
   // calculate search start/end pose in pull over lanes
   const auto search_start_end_poses = std::invoke([&]() -> std::pair<Pose, Pose> {
-    const auto goal_arc_coords =
-      lanelet::utils::getArcCoordinates(pull_over_lanes_, reference_goal_pose);
+    const auto goal_arc_coords = autoware::experimental::lanelet2_utils::get_arc_coordinates(
+      pull_over_lanes_, reference_goal_pose);
     const double s_start = std::max(0.0, goal_arc_coords.length - backward_length);
     const double s_end = goal_arc_coords.length + forward_length;
     const auto center_line_path = utils::resamplePathWithSpline(
@@ -440,8 +445,11 @@
     /*forward_only_in_route*/ false);
   const auto current_center_line_path = std::invoke([&]() -> PathWithLaneId {
     const double s_start =
-      lanelet::utils::getArcCoordinates(current_lanes, search_start_pose).length;
-    const double s_end = lanelet::utils::getArcCoordinates(current_lanes, search_end_pose).length;
+      autoware::experimental::lanelet2_utils::get_arc_coordinates(current_lanes, search_start_pose)
+        .length;
+    const double s_end =
+      autoware::experimental::lanelet2_utils::get_arc_coordinates(current_lanes, search_end_pose)
+        .length;
     return utils::resamplePathWithSpline(
       route_handler->getCenterLinePath(current_lanes, s_start, s_end), 1.0);
   });
@@ -462,10 +470,14 @@
         continue;
       }
       const Pose & object_pose = object.kinematics.initial_pose_with_covariance.pose;
-      const double s_object = lanelet::utils::getArcCoordinates(current_lanes, object_pose).length;
+      const double s_object =
+        autoware::experimental::lanelet2_utils::get_arc_coordinates(current_lanes, object_pose)
+          .length;
       for (auto & goal_candidate : goal_candidates) {
         const Pose & goal_pose = goal_candidate.goal_pose;
-        const double s_goal = lanelet::utils::getArcCoordinates(current_lanes, goal_pose).length;
+        const double s_goal =
+          autoware::experimental::lanelet2_utils::get_arc_coordinates(current_lanes, goal_pose)
+            .length;
         if (s_object < s_goal) {
           goal_candidate.num_objects_to_avoid++;
         }
@@ -689,7 +701,9 @@
 
   // Define a lambda function to compute the arc length for a given goal candidate.
   auto getGoalArcLength = [&current_lanes](const auto & candidate) {
-    return lanelet::utils::getArcCoordinates(current_lanes, candidate.goal_pose).length;
+    return autoware::experimental::lanelet2_utils::get_arc_coordinates(
+             current_lanes, candidate.goal_pose)
+      .length;
   };
 
   // Find the closest goal candidate by comparing the arc lengths of each candidate.

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp

@@ -19,6 +19,7 @@
 #include "autoware/behavior_path_planner_common/utils/path_utils.hpp"
 #include "autoware/behavior_path_planner_common/utils/utils.hpp"
 
+#include <autoware/lanelet2_utils/geometry.hpp>
 #include <autoware/lanelet2_utils/nn_search.hpp>
 #include <autoware/motion_utils/trajectory/path_shift.hpp>
 #include <autoware_bezier_sampler/bezier_sampling.hpp>
@@ -152,8 +153,10 @@
   // case2) crop path if shift end pose is ahead of previous module path terminal pose
   const auto processed_prev_module_path = std::invoke([&]() -> std::optional<PathWithLaneId> {
     const bool extend_previous_module_path =
-      lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length >
-      lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length;
+      autoware::experimental::lanelet2_utils::get_arc_coordinates(road_lanes, shift_end_pose)
+        .length > autoware::experimental::lanelet2_utils::get_arc_coordinates(
+                    road_lanes, prev_module_path_terminal_pose)
+                    .length;
     if (extend_previous_module_path) {  // case1
       // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it
       // by setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is
@@ -374,17 +377,20 @@
   const double pull_over_velocity = parameters_.pull_over_velocity;
   const double deceleration_interval = parameters_.deceleration_interval;
 
-  const auto current_road_arc_coords = lanelet::utils::getArcCoordinates(road_lanes, current_pose);
+  const auto current_road_arc_coords =
+    autoware::experimental::lanelet2_utils::get_arc_coordinates(road_lanes, current_pose);
   const double s_start = current_road_arc_coords.length - backward_path_length;
   const double s_end = std::max(
-    lanelet::utils::getArcCoordinates(road_lanes, end_pose).length,
+    autoware::experimental::lanelet2_utils::get_arc_coordinates(road_lanes, end_pose).length,
     s_start + std::numeric_limits<double>::epsilon());
   auto road_lane_reference_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end);
 
   // decelerate velocity linearly to minimum pull over velocity
   // (or keep original velocity if it is lower than pull over velocity)
   for (auto & point : road_lane_reference_path.points) {
-    const auto arclength = lanelet::utils::getArcCoordinates(road_lanes, point.point.pose).length;
+    const auto arclength =
+      autoware::experimental::lanelet2_utils::get_arc_coordinates(road_lanes, point.point.pose)
+        .length;
     const double distance_to_pull_over_start =
       std::clamp(s_end - arclength, 0.0, deceleration_interval);
     const auto decelerated_velocity = static_cast<float>(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp

@@ -18,10 +18,10 @@
 #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
 #include "autoware/behavior_path_planner_common/utils/path_utils.hpp"
 
+#include <autoware/lanelet2_utils/geometry.hpp>
 #include <autoware/lanelet2_utils/nn_search.hpp>
 #include <autoware/motion_utils/trajectory/path_shift.hpp>
 #include <autoware_lanelet2_extension/utility/query.hpp>
-#include <autoware_lanelet2_extension/utility/utilities.hpp>
 
 #include <algorithm>
 #include <limits>
@@ -88,17 +88,20 @@
   const double pull_over_velocity = parameters_.pull_over_velocity;
   const double deceleration_interval = parameters_.deceleration_interval;
 
-  const auto current_road_arc_coords = lanelet::utils::getArcCoordinates(road_lanes, current_pose);
+  const auto current_road_arc_coords =
+    autoware::experimental::lanelet2_utils::get_arc_coordinates(road_lanes, current_pose);
   const double s_start = current_road_arc_coords.length - backward_path_length;
   const double s_end = std::max(
-    lanelet::utils::getArcCoordinates(road_lanes, end_pose).length,
+    autoware::experimental::lanelet2_utils::get_arc_coordinates(road_lanes, end_pose).length,
     s_start + std::numeric_limits<double>::epsilon());
   auto road_lane_reference_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end);
 
   // decelerate velocity linearly to minimum pull over velocity
   // (or keep original velocity if it is lower than pull over velocity)
   for (auto & point : road_lane_reference_path.points) {
-    const auto arclength = lanelet::utils::getArcCoordinates(road_lanes, point.point.pose).length;
+    const auto arclength =
+      autoware::experimental::lanelet2_utils::get_arc_coordinates(road_lanes, point.point.pose)
+        .length;
     const double distance_to_pull_over_start =
       std::clamp(s_end - arclength, 0.0, deceleration_interval);
     const auto decelerated_velocity = static_cast<float>(
@@ -139,8 +142,10 @@
   // case2) crop path if shift end pose is ahead of previous module path terminal pose
   const auto processed_prev_module_path = std::invoke([&]() -> std::optional<PathWithLaneId> {
     const bool extend_previous_module_path =
-      lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length >
-      lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length;
+      autoware::experimental::lanelet2_utils::get_arc_coordinates(road_lanes, shift_end_pose)
+        .length > autoware::experimental::lanelet2_utils::get_arc_coordinates(
+                    road_lanes, prev_module_path_terminal_pose)
+                    .length;
     if (extend_previous_module_path) {  // case1
       // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it
       // by setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is