diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 0102265d534dc..462ebbb9eca20 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1025,6 +1025,19 @@ Trajectories ObstacleAvoidancePlanner::optimizeTrajectory( return getPrevTrajs(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.