fix

Signed-off-by: Arjun Jagdish Ram <arjun.ram@tier4.jp>

Author: PanConChicharron

planning/autoware_path_optimizer/config/path_optimizer.param.yaml

@@ -35,7 +35,7 @@
 
     # mpt param
     mpt:
-      use_acados: false
+      use_acados: true
       option:
         # TODO(murooka) enable the following. Currently enabling the flag makes QP so heavy
         steer_limit_constraint: false

planning/autoware_path_optimizer/src/mpt_optimizer.cpp

@@ -763,8 +763,8 @@ std::array<double, NP> MPTOptimizer::buildParameters(
       }
     }
 
-    double last_k = 0;
-    for (size_t j = 0; j < n_missing-1; ++j) {
+    double last_k = 0.0;
+    for (size_t j = 0; j < n_missing; ++j) {
       curvatures.push_back(last_k);
     }
 
@@ -780,7 +780,17 @@ std::array<double, NP> MPTOptimizer::buildParameters(
     knots.resize(target_n_knots);
     x_coeffs_flat.resize(4 * target_segments);
     y_coeffs_flat.resize(4 * target_segments);
-    curvatures.resize(target_segments);
+    curvatures.resize(target_n_knots);
+  }
+  
+  // Ensure curvatures has same size as knots for cubic spline interpolation
+  if (curvatures.size() < knots.size()) {
+    double last_k = 0.0;
+    while (curvatures.size() < knots.size()) {
+      curvatures.push_back(last_k);
+    }
+  } else if (curvatures.size() > knots.size()) {
+    curvatures.resize(knots.size());
   }
 
   RCLCPP_ERROR(logger_, "sizes: knots=%zu x_coeffs=%zu y_coeffs=%zu curvatures=%zu body_points=%zu",
@@ -838,7 +848,23 @@ std::array<double, NP> MPTOptimizer::buildParameters(
   // 7. Compute cubic spline coefficients from curvatures
   // Python uses: ClothoidSpline -> CubicSpline(knots, curvatures) -> coeffs (4×(N-1))
   // We need to fit a cubic spline to (knots, curvatures) and extract the 4×(N-1) coefficients
+  
+  // Debug: Check curvature values
+  std::cerr << "Curvatures size: " << curvatures.size() << ", first 10: ";
+  for (size_t i = 0; i < std::min(size_t(10), curvatures.size()); ++i) {
+    std::cerr << curvatures[i] << " ";
+  }
+  std::cerr << std::endl;
+  std::cerr << "Knots size: " << knots.size() << std::endl;
+  
   const std::vector<double> clothoid_coeffs_flat = computeCubicSplineCoeffs(knots, curvatures);
+  std::cerr << "Clothoid coeffs size: " << clothoid_coeffs_flat.size() << std::endl;
+  std::cerr << "Clothoid coeffs (first 20): ";
+  for (size_t i = 0; i < std::min(size_t(20), clothoid_coeffs_flat.size()); ++i) {
+    std::cerr << clothoid_coeffs_flat[i] << " ";
+  }
+  std::cerr << std::endl;
+  
   for (double v : clothoid_coeffs_flat) {
     parameters[idx++] = v;
   }
@@ -857,11 +883,17 @@ std::array<double, NP> MPTOptimizer::buildParameters(
 
   // set x0: initial state vector
   idx = 0;
-  x0[idx++] = 0.0;  // s_interp
-  x0[idx++] = 0.0;
+  x0[idx++] = 0.0;  // eY (lateral error)
+  x0[idx++] = 0.0;  // eψ (yaw error)
   for (const auto& body_point_curvilinear : body_points_curvilinear) {
     x0[idx++] = body_point_curvilinear;
   }
+  
+  std::cerr << "Initial state x0: ";
+  for (size_t i = 0; i < NX; ++i) {
+    std::cerr << x0[i] << " ";
+  }
+  std::cerr << std::endl;
 
   // store reference path length for per-stage s_interp construction
   if (!knots.empty()) sref_ = knots.back();