change mpc, not complete.

src/decition/mpccontroller/mpc.cpp

@@ -21,6 +21,7 @@
 
 #include <algorithm>
 #include <limits>
+#include <chrono>
 
 namespace autoware::motion::control::mpc_lateral_controller
 {

src/decition/mpccontroller/mpc_utils.cpp

@@ -91,7 +91,7 @@ inline static double ADCnormalizeDegree(const double deg, const double min_deg =
   return value - std::copysign(360.0, value);
 }
 
-inline  double ADCnormalizeRadian(const double rad, const double min_rad = -M_PI)
+inline  double ADCnormalizeRadian(const double rad, const double min_rad)
 {
   const auto max_rad = min_rad + 2 * M_PI;
 
@@ -205,7 +205,7 @@ size_t findNearestIndex(const MPCTrajectory & currenttraj, const iv::ADCPoint &
  * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose
  * @return index of nearest point (index or none if not found)
  */
-static size_t findFirstNearestIndexWithSoftConstraints(
+size_t findFirstNearestIndexWithSoftConstraints(
     const MPCTrajectory & currenttraj, const iv::ADCPose & pose,
     const double dist_threshold = std::numeric_limits<double>::max(),
     const double yaw_threshold = std::numeric_limits<double>::max())
@@ -282,6 +282,106 @@ static size_t findFirstNearestIndexWithSoftConstraints(
 }
 
 
+//MPCTrajectory removeOverlapPoints(const MPCTrajectory & points, const size_t start_idx = 0)
+//{
+//  if (points.size() < start_idx + 1) {
+//      return points;
+//  }
+
+//  MPCTrajectory dst;
+
+//  for (size_t i = 0; i <= start_idx; ++i) {
+//      dst.push_back(points.x[i],points.y[i],points.z[i],points.yaw[i],points.vx[i],points.k[i],points.smooth_k[i],points.relative_time[i]);
+//  }
+
+//  constexpr double eps = 1.0E-08;
+//  for (size_t i = start_idx + 1; i < points.size(); ++i) {
+//      const auto prev_p = tier4_autoware_utils::getPoint(dst.back());
+//      const auto curr_p = tier4_autoware_utils::getPoint(points.at(i));
+//      const double dist =    tier4_autoware_utils::calcDistance2d(prev_p, curr_p);
+//      if (dist < eps) {
+//          continue;
+//      }
+//      dst.push_back(points.at(i));
+//  }
+
+//  return dst;
+//}
+
+//double calcLongitudinalOffsetToSegment(
+//    MPCTrajectory & currenttraj, const size_t seg_idx, const iv::ADCPoint & p_target,
+//    const bool throw_exception = false)
+//{
+//  if (seg_idx >= currenttraj.size() - 1) {
+//      const std::out_of_range e("Segment index is invalid.");
+// //     tier4_autoware_utils::print_backtrace();
+//      std::cout<<" seg_idx > trajsize."<<std::endl;
+//      if (throw_exception) {
+//          throw e;
+//      }
+//      std::cerr << e.what() << std::endl;
+//      return std::nan("");
+//  }
+
+//  const auto overlap_removed_points = removeOverlapPoints(points, seg_idx);
+
+//  if (throw_exception) {
+////      validateNonEmpty(overlap_removed_points);
+//  } else {
+//      try {
+//  //        validateNonEmpty(overlap_removed_points);
+//      } catch (const std::exception & e) {
+//          std::cerr << e.what() << std::endl;
+//          return std::nan("");
+//      }
+//  }
+
+//  if (seg_idx >= overlap_removed_points.size() - 1) {
+//      const std::runtime_error e("Same points are given.");
+//  //    tier4_autoware_utils::print_backtrace();
+//      if (throw_exception) {
+//          throw e;
+//      }
+//      std::cerr << e.what() << std::endl;
+//      return std::nan("");
+//  }
+
+//  const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx));
+//  const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1));
+
+//  const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0};
+//  const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0};
+
+//  return segment_vec.dot(target_vec) / segment_vec.norm();
+//}
+
+//size_t findFirstNearestSegmentIndexWithSoftConstraints(
+//    const MPCTrajectory & currenttraj, const iv::ADCPose & pose,
+//    const double dist_threshold = std::numeric_limits<double>::max(),
+//    const double yaw_threshold = std::numeric_limits<double>::max())
+//{
+//  // find first nearest index with soft constraints (not segment index)
+//  const size_t nearest_idx =
+//      findFirstNearestIndexWithSoftConstraints(currenttraj, pose, dist_threshold, yaw_threshold);
+
+//  // calculate segment index
+//  if (nearest_idx == 0) {
+//      return 0;
+//  }
+//  if (nearest_idx == currenttraj.size() - 1) {
+//      return currenttraj.size() - 2;
+//  }
+
+//  const double signed_length = calcLongitudinalOffsetToSegment(currenttraj, nearest_idx, pose.position);
+
+//  if (signed_length <= 0) {
+//      return nearest_idx - 1;
+//  }
+
+//  return nearest_idx;
+//}
+
+
 double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2)
 {
   const double dx = trajectory.x.at(idx1) - trajectory.x.at(idx2);