fix(autoware_mpc_lateral_controller): replace Eigen::VectorXd with Eigen::Vector3d for state representation

[kyoichi-sugahara]

Description

Replaced all Eigen::VectorXd instances with Eigen::Vector3d in the calculatePredictedTrajectoryInWorldCoordinate method

issue

When building with --cmake-args -DCMAKE_BUILD_TYPE=Debug option, identified a critical bug in the KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate method.
Debug logs showed that state_w had size 3 while dstate had size 4.
This dimension mismatch caused an assertion failure in Eigen when performing the operation state_w + dstate * dt, resulting in the following error:

Assertion `aLhs.rows() == aRhs.rows() && aLhs.cols() == aRhs.cols()' failed.

This issue did not manifest in release builds as assertion checks are disabled in that mode.

How was this PR tested?

Inserted debug print before the next_state calculation as follows:

    std::cerr << "-----" << std::endl;
    std::cerr << "state_w size: " << state_w.size() << std::endl;
    std::cerr << "dstate size: " << dstate.size() << std::endl;

With the original implementation, I got the following error message:

[component_container_mt-40] -----
[component_container_mt-40] state_w size: 3
[component_container_mt-40] dstate size: 4
[component_container_mt-40] component_container_mt: /usr/include/eigen3/Eigen/src/Core/CwiseBinaryOp.h:116: Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>::CwiseBinaryOp(const Lhs&, const Rhs&, const BinaryOp&) [with BinaryOp = Eigen::internal::scalar_sum_op<double, double>; LhsType = const Eigen::Matrix<double, -1, 1>; RhsType = const Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, const Eigen::Matrix<double, -1, 1>, const Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, const Eigen::Matrix<double, -1, 1> > >; Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>::Lhs = Eigen::Matrix<double, -1, 1>; Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>::Rhs = Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, const Eigen::Matrix<double, -1, 1>, const Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, const Eigen::Matrix<double, -1, 1> > >]: Assertion `aLhs.rows() == aRhs.rows() && aLhs.cols() == aRhs.cols()' failed.
[component_container_mt-40] *** Aborted at 1741242719 (unix time) try "date -d @1741242719" if you are using GNU date ***

With the updated version, there is no error and both vectors have the same size:

[component_container_mt-40] -----
[component_container_mt-40] state_w size: 3
[component_container_mt-40] dstate size: 3

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[TakaHoribe]

@kyoichi-sugahara Please modify the comments to align with the code for consistency.

  // World coordinate x = [x, y, yaw, steer]

Additionally, it does not match the model used in the control model. It would be helpful to include a note stating:

"Ideally, the first-order delay of the steering should be considered, as in the control model. However, significant accuracy degradation was observed when discretizing with a long dt, so it has been ignored here. If the accuracy of the discretization improves, an appropriate model should be considered."

[kyoichi-sugahara]

@TakaHoribe
Addressed in a5bd7e7

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 26.34%. Comparing base (28308e8) to head (8f2e13b).
Report is 2 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #10235      +/-   ##
==========================================
+ Coverage   26.24%   26.34%   +0.09%     
==========================================
  Files        1378     1384       +6     
  Lines      107445   107444       -1     
  Branches    41428    41350      -78     
==========================================
+ Hits        28200    28303     +103     
+ Misses      76426    76321     -105     
- Partials     2819     2820       +1     

Flag	Coverage Δ		*Carryforward flag
differential	44.29% <100.00%> (?)
total	26.34% <ø> (+0.09%)	⬆️	Carriedforward from 41158cd

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.