add path_generator package

Signed-off-by: mitukou1109 <mitukou1109@gmail.com> fix spell check error Signed-off-by: mitukou1109 <mitukou1109@gmail.com> include necessary headers Signed-off-by: mitukou1109 <mitukou1109@gmail.com> change package version to 0.0.0 Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> Signed-off-by: mitukou1109 <mitukou1109@gmail.com> fix include guard name Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> Signed-off-by: mitukou1109 <mitukou1109@gmail.com> replace flowchart uml with pre-generated image Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> Signed-off-by: mitukou1109 <mitukou1109@gmail.com> style(pre-commit): autofix Signed-off-by: mitukou1109 <mitukou1109@gmail.com> replace tier4_planning_msgs with autoware_internal_planning_msgs Signed-off-by: mitukou1109 <mitukou1109@gmail.com> style(pre-commit): autofix Signed-off-by: mitukou1109 <mitukou1109@gmail.com> use LaneletSequence instead of ConstLanelets Signed-off-by: mitukou1109 <mitukou1109@gmail.com> set orientation to path points Signed-off-by: mitukou1109 <mitukou1109@gmail.com> crop path bound to fit trajectory Signed-off-by: mitukou1109 <mitukou1109@gmail.com> offset path bound Signed-off-by: mitukou1109 <mitukou1109@gmail.com> no need to make return value optional Signed-off-by: mitukou1109 <mitukou1109@gmail.com> address deprecation warning Signed-off-by: mitukou1109 <mitukou1109@gmail.com> add doxygen comments Signed-off-by: mitukou1109 <mitukou1109@gmail.com> support multiple previous/next lanelets Signed-off-by: mitukou1109 <mitukou1109@gmail.com> fix path bound cut issue Signed-off-by: mitukou1109 <mitukou1109@gmail.com> group parameters Signed-off-by: mitukou1109 <mitukou1109@gmail.com> add path cut feature Signed-off-by: mitukou1109 <mitukou1109@gmail.com> ensure s_end is not negative Signed-off-by: mitukou1109 <mitukou1109@gmail.com> simplify return value selection Signed-off-by: mitukou1109 <mitukou1109@gmail.com> add doxygen comments Signed-off-by: mitukou1109 <mitukou1109@gmail.com>
autowarefoundation · Feb 25, 2025 · b9dd1c4 · b9dd1c4
1 parent b27283b
commit b9dd1c4
Show file tree

Hide file tree

Showing 4 changed files with 77 additions and 0 deletions.
diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp
@@ -101,6 +101,30 @@ std::vector<std::pair<lanelet::ConstPoints3d, std::pair<double, double>>> get_wa
   const lanelet::LaneletSequence & lanelet_sequence, const lanelet::LaneletMap & lanelet_map,
   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
+ * @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::LaneletSequence & lanelet_sequence, const double s_start, const double s_end);
+
+/**
+ * @brief get position of first self-intersection 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);
+
 /**
  * @brief get bound of path cropped within specified range
  * @param lanelet_bound original bound of lanelet

diff --git a/planning/autoware_path_generator/package.xml b/planning/autoware_path_generator/package.xml
@@ -7,11 +7,13 @@
   <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <maintainer email="mitsuhiro.sakamoto@tier4.jp">Mitsuhiro Sakamoto</maintainer>
+  <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="satoshi.ota@tier4.jp">Satoshi Ota</author>
   <author email="takayuki.murooka@tier4.jp">Takayuki Murooka</author>
   <author email="mitsuhiro.sakamoto@tier4.jp">Mitsuhiro Sakamoto</author>
+  <author email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</author>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>

diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp
@@ -245,6 +245,12 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
       s_end = std::min(s_end, goal_arc_coordinates.length);
     }
 
+    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);
+    }
+
     return s_end;
   }();
 

diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp
@@ -20,6 +20,7 @@
 
 #include <lanelet2_core/geometry/Lanelet.h>
 
+#include <algorithm>
 #include <utility>
 #include <vector>
 
@@ -166,6 +167,50 @@ std::optional<lanelet::ConstLanelet> get_next_lanelet_within_route(
   return *next_lanelet_itr;
 }
 
+std::optional<double> get_first_self_intersection_arc_length(
+  const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end)
+{
+  const auto s_left = get_first_self_intersection_arc_length(
+    lanelet_sequence.leftBound2d().basicLineString(), s_start, s_end);
+  const auto s_right = get_first_self_intersection_arc_length(
+    lanelet_sequence.rightBound2d().basicLineString(), s_start, s_end);
+
+  if (s_left && s_right) {
+    return std::min(*s_left, *s_right);
+  }
+  return s_left ? s_left : s_right;
+}
+
+std::optional<double> get_first_self_intersection_arc_length(
+  const lanelet::BasicLineString2d & line_string, const double s_start, const double s_end)
+{
+  const auto tree = lanelet::geometry::internal::makeIndexedSegmenTree(line_string);
+  std::optional<std::pair<size_t, double>> last_intersection = std::nullopt;
+  auto s = 0.;
+
+  for (size_t i = 1; i < line_string.size() - 1; ++i) {
+    if (last_intersection && i == last_intersection->first) {
+      return s + last_intersection->second;
+    }
+    s += lanelet::geometry::distance2d(line_string.at(i - 1), line_string.at(i));
+    if (s < s_start) {
+      continue;
+    }
+    if (s > s_end) {
+      break;
+    }
+    const auto self_intersections = lanelet::geometry::internal::getSelfIntersectionsAt(
+      tree, 0, lanelet::BasicSegment2d{line_string.at(i - 1), line_string.at(i)});
+    if (self_intersections.empty()) {
+      continue;
+    }
+    last_intersection = {
+      self_intersections.front().lastSegment.idx, self_intersections.front().lastSegment.s};
+  }
+
+  return std::nullopt;
+}
+
 std::vector<std::pair<lanelet::ConstPoints3d, std::pair<double, double>>> get_waypoint_groups(
   const lanelet::LaneletSequence & lanelet_sequence, const lanelet::LaneletMap & lanelet_map,
   const double group_separation_threshold, const double interval_margin_ratio)