fix(avoidance): guard invalid shift point (autowarefoundation#1924) (a…

…utowarefoundation#135)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2212,6 +2212,11 @@ boost::optional<AvoidPointArray> AvoidanceModule::findNewShiftPoint(
       throw std::logic_error("prev_reference_ and prev_linear_shift_path_ must have same size.");
     }
 
+    // new shift points must exist in front of Ego
+    if (candidate.start_longitudinal < 0.0) {
+      continue;
+    }
+
     // TODO(Horibe): this code prohibits the changes on ego pose. Think later.
     // if (candidate.start_idx < avoidance_data_.ego_closest_path_index) {
     //   DEBUG_PRINT("%s, start_idx is behind ego. skip.", pfx);