fix(lane_change): prevent node from dying for trimmed shifted path (#…

…5271) Signed-off-by: Zulfaqar Azmi <[email protected]>
autowarefoundation · Oct 13, 2023 · df83621 · df83621
1 parent 69560ad
commit df83621
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 1 deletion.
diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
@@ -301,6 +301,15 @@ std::optional<LaneChangePath> constructCandidatePath(
       "failed to generate shifted path.");
   }
 
+  // TODO(Zulfaqar Azmi): have to think of a more feasible solution for points being remove by path
+  // shifter.
+  if (shifted_path.path.points.size() < shift_line.end_idx + 1) {
+    RCLCPP_DEBUG(
+      rclcpp::get_logger("behavior_path_planner").get_child("utils").get_child(__func__),
+      "path points are removed by PathShifter.");
+    return std::nullopt;
+  }
+
   const auto & prepare_length = lane_change_length.prepare;
   const auto & lane_changing_length = lane_change_length.lane_changing;
 

diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp
@@ -226,7 +226,7 @@ std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
   turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
   const double max_time = std::numeric_limits<double>::max();
   const double max_distance = std::numeric_limits<double>::max();
-  if (path.shift_length.empty()) {
+  if (path.shift_length.size() < shift_line.end_idx + 1) {
     return std::make_pair(turn_signal, max_distance);
   }
   const auto base_link2front = common_parameter.base_link2front;