Merge branch 'main' into fix/autoware_ekf_localizer

Author: youtalk

planning/autoware_path_generator/include/autoware/path_generator/utils.hpp

@@ -113,42 +113,69 @@ std::vector<std::pair<lanelet::ConstPoints3d, std::pair<double, double>>> get_wa
   const double group_separation_threshold, const double interval_margin_ratio);
 
 /**
- * @brief get position of first self-intersection (point where return
- * path intersects outward path) of lanelet sequence in arc length
- * @param lanelet_sequence target lanelet sequence (both left and right bound are
- * considered)
- * @param s_start longitudinal distance of point to start searching for self-intersections
+ * @brief get position of first intersection (including self-intersection) in lanelet sequence in
+ * arc length
+ * @param lanelet_sequence target lanelet sequence
+ * @param s_start longitudinal distance of point to start searching for intersections
  * @param s_end longitudinal distance of point to end search
- * @return longitudinal distance of self-intersecting point (std::nullopt if no
- * self-intersection)
+ * @param vehicle_length vehicle length
+ * @return longitudinal distance of intersecting point (std::nullopt if no intersection)
  */
-std::optional<double> get_first_self_intersection_arc_length(
-  const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end);
+std::optional<double> get_first_intersection_arc_length(
+  const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end,
+  const double vehicle_length);
 
 /**
- * @brief get position of first self-intersection of line string in arc length
+ * @brief get position of first self-intersection (point where return
+ * path intersects outward path) of line string in arc length
  * @param line_string target line string
- * @param s_start longitudinal distance of point to start searching for self-intersections
- * @param s_end longitudinal distance of point to end search
  * @return longitudinal distance of self-intersecting point (std::nullopt if no
  * self-intersection)
  */
 std::optional<double> get_first_self_intersection_arc_length(
-  const lanelet::BasicLineString2d & line_string, const double s_start, const double s_end);
+  const lanelet::BasicLineString2d & line_string);
+
+/**
+ * @brief get path bounds for PathWithLaneId cropped within specified range
+ * @param lanelet_sequence lanelet sequence
+ * @param s_start longitudinal distance of start of bound on centerline
+ * @param s_end longitudinal distance of end of bound on centerline
+ * @return cropped bounds (left / right)
+ */
+std::array<std::vector<geometry_msgs::msg::Point>, 2> get_path_bounds(
+  const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end);
 
 /**
- * @brief get path bound for PathWithLaneId cropped within specified range
- * @param lanelet_bound original bound of lanelet
- * @param lanelet_centerline centerline of lanelet
- * @param s_start longitudinal distance of start of bound
- * @param s_end longitudinal distance of end of bound
- * @return cropped bound
+ * @brief crop line string
+ * @param line_string line string
+ * @param s_start longitudinal distance to crop from
+ * @param s_end longitudinal distance to crop to
+ * @return cropped line string
  */
-std::vector<geometry_msgs::msg::Point> get_path_bound(
-  const lanelet::CompoundLineString3d & lanelet_bound,
-  const lanelet::CompoundLineString2d & lanelet_centerline, const double s_start,
+std::vector<geometry_msgs::msg::Point> crop_line_string(
+  const std::vector<geometry_msgs::msg::Point> & line_string, const double s_start,
   const double s_end);
 
+/**
+ * @brief get positions of given point on centerline projected to left / right bound in arc length
+ * @param lanelet_sequence lanelet sequence
+ * @param s_centerline longitudinal distance of point on centerline
+ * @return longitudinal distance of projected point (left / right)
+ */
+std::array<double, 2> get_arc_length_on_bounds(
+  const lanelet::LaneletSequence & lanelet_sequence, const double s_centerline);
+
+/**
+ * @brief get positions of given point on left / right bound projected to centerline in arc length
+ * @param lanelet_sequence lanelet sequence
+ * @param s_left_bound longitudinal distance of point on left bound
+ * @param s_right_bound longitudinal distance of point on left bound
+ * @return longitudinal distance of projected point (left / right)
+ */
+std::array<std::optional<double>, 2> get_arc_length_on_centerline(
+  const lanelet::LaneletSequence & lanelet_sequence, const std::optional<double> & s_left_bound,
+  const std::optional<double> & s_right_bound);
+
 /**
  * @brief Recreate the goal pose to prevent the goal point being too far from the lanelet, which
  *  causes the path to twist near the goal.

planning/autoware_path_generator/src/node.cpp

@@ -272,9 +272,11 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
     }
 
     if (
-      const auto s_intersection =
-        utils::get_first_self_intersection_arc_length(lanelet_sequence, s_start, s_end)) {
-      s_end = std::clamp(s_end, 0.0, *s_intersection - vehicle_info_.max_longitudinal_offset_m);
+      const auto s_intersection = utils::get_first_intersection_arc_length(
+        lanelet_sequence, std::max(0., s_start - vehicle_info_.max_longitudinal_offset_m),
+        s_end + vehicle_info_.max_longitudinal_offset_m, vehicle_info_.vehicle_length_m)) {
+      s_end =
+        std::min(s_end, std::max(0., *s_intersection - vehicle_info_.max_longitudinal_offset_m));
     }
 
     return s_end;
@@ -443,14 +445,11 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
   finalized_path_with_lane_id.header.frame_id = planner_data_.route_frame_id;
   finalized_path_with_lane_id.header.stamp = now();
 
-  const auto s_bound_start_offset = std::max(0., s_offset + s_bound_start);
-  const auto s_bound_end_offset = std::max(0., s_offset + s_bound_end);
-  finalized_path_with_lane_id.left_bound = utils::get_path_bound(
-    extended_lanelet_sequence.leftBound(), extended_lanelet_sequence.centerline2d(),
-    s_bound_start_offset, s_bound_end_offset);
-  finalized_path_with_lane_id.right_bound = utils::get_path_bound(
-    extended_lanelet_sequence.rightBound(), extended_lanelet_sequence.centerline2d(),
-    s_bound_start_offset, s_bound_end_offset);
+  const auto [left_bound, right_bound] = utils::get_path_bounds(
+    extended_lanelet_sequence, std::max(0., s_offset + s_bound_start),
+    std::max(0., s_offset + s_bound_end));
+  finalized_path_with_lane_id.left_bound = left_bound;
+  finalized_path_with_lane_id.right_bound = right_bound;
 
   return finalized_path_with_lane_id;
 }