fix(lane_change): fix terminal path not working in some case (autowarefoundation#6722)

zulfaqar-azmi-t4 · zulfaqar-azmi-t4 · commit 90ff601e8a22 · 2024-04-03T18:25:26.000+09:00
Signed-off-by: Zulfaqar Azmi &lt;zulfaqar.azmi@tier4.jp&gt;
diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
@@ -439,7 +439,8 @@ PathWithLaneId getReferencePathFromTargetLane(
       .get_child("getReferencePathFromTargetLane"),
     "start: %f, end: %f", s_start, s_end);
 
-  if (s_end - s_start < lane_changing_length) {
+  constexpr double epsilon = 1e-4;
+  if (s_end - s_start + epsilon < lane_changing_length) {
     return PathWithLaneId();
   }
 

Original file line number	Diff line number	Diff line change
`@@ -439,7 +439,8 @@ PathWithLaneId getReferencePathFromTargetLane(`
`439`	`439`	`.get_child("getReferencePathFromTargetLane"),`
`440`	`440`	`"start: %f, end: %f", s_start, s_end);`
`441`	`441`
`442`		`- if (s_end - s_start < lane_changing_length) {`
	`442`	`+ constexpr double epsilon = 1e-4;`
	`443`	`+ if (s_end - s_start + epsilon < lane_changing_length) {`
`443`	`444`	`return PathWithLaneId();`
`444`	`445`	`}`
`445`	`446`