feat(path_generator): add path cut feature (#216)

* feat(path_generator): add path cut feature

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>

* ignore makeIndexedSegmenTree from spell check

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* delete comments from cspell for pre-commit

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

---------

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: mitukou1109 <mitukou1109@gmail.com>

.cspell.json

@@ -2,5 +2,5 @@
   "_comment": "configuration file for spell-check workflow",
   "ignorePaths": [],
   "ignoreRegExpList": [],
-  "words": []
+  "words": ["makeIndexedSegmenTree"]
 }

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

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>

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;
   }();
 

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)