fix(autoware_mpc_lateral_controller): fix calculation method of predi…

…cted trajectory (autowarefoundation#9048) * fix(vehicle_model): fix calculation method of predicted trajectory Signed-off-by: Kyoichi Sugahara <[email protected]> --------- Signed-off-by: Kyoichi Sugahara <[email protected]>
leo-drive · Oct 7, 2024 · c2438fd · c2438fd
1 parent d4d40cd
commit c2438fd
Showing 1 changed file with 5 additions and 9 deletions.
diff --git a/...ol/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/...ol/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
@@ -83,29 +83,25 @@ MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordin
 
   // create initial state in the world coordinate
   Eigen::VectorXd state_w = [&]() {
-    Eigen::VectorXd state = Eigen::VectorXd::Zero(4);
+    Eigen::VectorXd state = Eigen::VectorXd::Zero(3);
     const auto lateral_error_0 = x0(0);
     const auto yaw_error_0 = x0(1);
     state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0;  // world-x
     state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0;  // world-y
     state(2, 0) = t.yaw.at(0) + yaw_error_0;                            // world-yaw
-    state(3, 0) = x0(2);                                                // steering
     return state;
   }();
 
   // update state in the world coordinate
   const auto updateState = [&](
-                             const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input,
-                             const double dt, const double velocity) {
+                             const Eigen::VectorXd & state_w, const double & input, const double dt,
+                             const double velocity) {
     const auto yaw = state_w(2);
-    const auto steer = state_w(3);
-    const auto desired_steer = input(0);
 
     Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4);
     dstate(0) = velocity * std::cos(yaw);
     dstate(1) = velocity * std::sin(yaw);
-    dstate(2) = velocity * std::tan(steer) / m_wheelbase;
-    dstate(3) = -(steer - desired_steer) / m_steer_tau;
+    dstate(2) = velocity * std::tan(input) / m_wheelbase;
 
     // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation
     // in Eigen.
@@ -117,7 +113,7 @@ MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordin
   const auto DIM_U = getDimU();
 
   for (size_t i = 0; i < reference_trajectory.size(); ++i) {
-    state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i));
+    state_w = updateState(state_w, Uex(i * DIM_U, 0), dt, t.vx.at(i));
     mpc_predicted_trajectory.push_back(
       state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i),
       t.relative_time.at(i));