fix get_first_self_intersection_arc_length idx

kosuke55 · kosuke55 · commit 6765458e0107 · 2025-03-21T20:44:52.000+09:00
Signed-off-by: kosuke55 &lt;kosuke.tnp@gmail.com&gt;
diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp
@@ -399,7 +399,7 @@ std::optional<double> get_first_self_intersection_arc_length(
   double s = 0.;
 
   for (size_t i = 1; i < line_string.size() - 1; ++i) {
-    if (first_self_intersection_long && i == first_self_intersection_long->idx) {
+    if (first_self_intersection_long && i == first_self_intersection_long->idx + 1) {
       return s + first_self_intersection_long->s;
     }
     s += lanelet::geometry::distance2d(line_string.at(i - 1), line_string.at(i));

Original file line number	Diff line number	Diff line change
`@@ -399,7 +399,7 @@ std::optional<double> get_first_self_intersection_arc_length(`
`399`	`399`	`double s = 0.;`
`400`	`400`
`401`	`401`	`for (size_t i = 1; i < line_string.size() - 1; ++i) {`
`402`		`- if (first_self_intersection_long && i == first_self_intersection_long->idx) {`
	`402`	`+ if (first_self_intersection_long && i == first_self_intersection_long->idx + 1) {`
`403`	`403`	`return s + first_self_intersection_long->s;`
`404`	`404`	`}`
`405`	`405`	`s += lanelet::geometry::distance2d(line_string.at(i - 1), line_string.at(i));`