fix(obstacle_avoidance_planner): fix inf loop in mpt (autowarefoundat…

…ion#1881)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/obstacle_avoidance_planner/src/node.cpp

@@ -1141,6 +1141,19 @@ Trajectories ObstacleAvoidancePlanner::optimizeTrajectory(
     return getPrevTrajs(p.path.points);
   }
 
+  // NOTE: Elastic band sometimes diverges with status = "OSQP_SOLVED".
+  constexpr double max_path_change_diff = 1.0e4;
+  for (size_t i = 0; i < eb_traj->size(); ++i) {
+    const auto & eb_pos = eb_traj->at(i).pose.position;
+    const auto & path_pos = path.points.at(std::min(i, path.points.size() - 1)).pose.position;
+
+    const double diff_x = eb_pos.x - path_pos.x;
+    const double diff_y = eb_pos.y - path_pos.y;
+    if (max_path_change_diff < std::abs(diff_x) || max_path_change_diff < std::abs(diff_y)) {
+      return getPrevTrajs(path.points);
+    }
+  }
+
   // EB has to be solved twice before solving MPT with fixed points
   // since the result of EB is likely to change with/without fixing (1st/2nd EB)
   // that makes MPT fixing points worse.