From d844b622a397ccd0c74fdcb43e1d1763dbeaef79 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Thu, 6 Mar 2025 15:42:42 +0900 Subject: [PATCH] fix(autoware_mpc_lateral_controller): replace Eigen::VectorXd with Eigen::Vector3d for state representation Signed-off-by: kyoichi-sugahara --- .../vehicle_model_bicycle_kinematics.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index c0052d4ff1b3e..6e5c2b60a61c5 100644 --- a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -82,30 +82,30 @@ MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordin const auto & t = reference_trajectory; // create initial state in the world coordinate - Eigen::VectorXd state_w = [&]() { - Eigen::VectorXd state = Eigen::VectorXd::Zero(3); + Eigen::Vector3d state_w = [&]() { + Eigen::Vector3d state = Eigen::Vector3d::Zero(); 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(0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x + state(1) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y + state(2) = t.yaw.at(0) + yaw_error_0; // world-yaw return state; }(); // update state in the world coordinate const auto updateState = [&]( - const Eigen::VectorXd & state_w, const double & input, const double dt, + const Eigen::Vector3d & state_w, const double & input, const double dt, const double velocity) { const auto yaw = state_w(2); - Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4); + Eigen::Vector3d dstate = Eigen::Vector3d::Zero(); dstate(0) = velocity * std::cos(yaw); dstate(1) = velocity * std::sin(yaw); 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. - const Eigen::VectorXd next_state = state_w + dstate * dt; + const Eigen::Vector3d next_state = state_w + dstate * dt; return next_state; };