feat(mpc_lateral_controller): suppress rclcpp_warning/error (#9382)

* feat(mpc_lateral_controller): suppress rclcpp_warning/error

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix test

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

control/autoware_mpc_lateral_controller/CMakeLists.txt

@@ -4,6 +4,8 @@ project(autoware_mpc_lateral_controller)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+find_package(fmt REQUIRED)
+
 ament_auto_add_library(steering_offset_lib SHARED
   src/steering_offset/steering_offset.cpp
 )

control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp

@@ -191,6 +191,12 @@ struct MPCMatrix
   MPCMatrix() = default;
 };
 
+struct ResultWithReason
+{
+  bool result{false};
+  std::string reason{""};
+};
+
 /**
  * MPC-based waypoints follower class
  * @brief calculate control command to follow reference waypoints
@@ -232,7 +238,7 @@ class MPC
    * @param current_kinematics The current vehicle kinematics.
    * @return A pair of a boolean flag indicating success and the MPC data.
    */
-  std::pair<bool, MPCData> getData(
+  std::pair<ResultWithReason, MPCData> getData(
     const MPCTrajectory & trajectory, const SteeringReport & current_steer,
     const Odometry & current_kinematics);
 
@@ -272,7 +278,7 @@ class MPC
    * @param [in] current_velocity current ego velocity
    * @return A pair of a boolean flag indicating success and the optimized input vector.
    */
-  std::pair<bool, VectorXd> executeOptimization(
+  std::pair<ResultWithReason, VectorXd> executeOptimization(
     const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt,
     const MPCTrajectory & trajectory, const double current_velocity);
 
@@ -283,7 +289,7 @@ class MPC
    * @param input The input trajectory.
    * @return A pair of a boolean flag indicating success and the resampled trajectory.
    */
-  std::pair<bool, MPCTrajectory> resampleMPCTrajectoryByTime(
+  std::pair<ResultWithReason, MPCTrajectory> resampleMPCTrajectoryByTime(
     const double start_time, const double prediction_dt, const MPCTrajectory & input) const;
 
   /**
@@ -450,7 +456,7 @@ class MPC
    * @param diagnostic Diagnostic data for debugging purposes.
    * @return True if the MPC calculation is successful, false otherwise.
    */
-  bool calculateMPC(
+  ResultWithReason calculateMPC(
     const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
     Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic,
     LateralHorizon & ctrl_cmd_horizon);

control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -96,7 +96,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   bool m_keep_steer_control_until_converged;
 
   // MPC solver checker.
-  bool m_is_mpc_solved{true};
+  ResultWithReason m_mpc_solved_status{true};
 
   // trajectory buffer for detecting new trajectory
   std::deque<Trajectory> m_trajectory_buffer;

control/autoware_mpc_lateral_controller/src/mpc.cpp

@@ -20,6 +20,8 @@
 #include "autoware/universe_utils/math/unit_conversion.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include <fmt/format.h>
+
 #include <algorithm>
 #include <limits>
 
@@ -37,7 +39,7 @@ MPC::MPC(rclcpp::Node & node)
     node.create_publisher<Trajectory>("~/debug/resampled_reference_trajectory", rclcpp::QoS(1));
 }
 
-bool MPC::calculateMPC(
+ResultWithReason MPC::calculateMPC(
   const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
   Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic,
   LateralHorizon & ctrl_cmd_horizon)
@@ -48,10 +50,10 @@ bool MPC::calculateMPC(
     applyVelocityDynamicsFilter(m_reference_trajectory, current_kinematics);
 
   // get the necessary data
-  const auto [success_data, mpc_data] =
+  const auto [get_data_result, mpc_data] =
     getData(reference_trajectory, current_steer, current_kinematics);
-  if (!success_data) {
-    return fail_warn_throttle("fail to get MPC Data. Stop MPC.");
+  if (!get_data_result.result) {
+    return ResultWithReason{false, fmt::format("getting MPC Data ({}).", get_data_result.reason)};
   }
 
   // calculate initial state of the error dynamics
@@ -61,29 +63,30 @@ bool MPC::calculateMPC(
   const auto [success_delay, x0_delayed] =
     updateStateForDelayCompensation(reference_trajectory, mpc_data.nearest_time, x0);
   if (!success_delay) {
-    return fail_warn_throttle("delay compensation failed. Stop MPC.");
+    return ResultWithReason{false, "delay compensation."};
   }
 
   // resample reference trajectory with mpc sampling time
   const double mpc_start_time = mpc_data.nearest_time + m_param.input_delay;
   const double prediction_dt =
     getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_kinematics);
 
-  const auto [success_resample, mpc_resampled_ref_trajectory] =
+  const auto [resample_result, mpc_resampled_ref_trajectory] =
     resampleMPCTrajectoryByTime(mpc_start_time, prediction_dt, reference_trajectory);
-  if (!success_resample) {
-    return fail_warn_throttle("trajectory resampling failed. Stop MPC.");
+  if (!resample_result.result) {
+    return ResultWithReason{
+      false, fmt::format("trajectory resampling ({}).", resample_result.reason)};
   }
 
   // generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex
   const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);
 
   // solve Optimization problem
-  const auto [success_opt, Uex] = executeOptimization(
+  const auto [opt_result, Uex] = executeOptimization(
     mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
     current_kinematics.twist.twist.linear.x);
-  if (!success_opt) {
-    return fail_warn_throttle("optimization failed. Stop MPC.");
+  if (!opt_result.result) {
+    return ResultWithReason{false, fmt::format("optimization failure ({}).", opt_result.reason)};
   }
 
   // apply filters for the input limitation and low pass filter
@@ -138,7 +141,7 @@ bool MPC::calculateMPC(
     ctrl_cmd_horizon.controls.push_back(lateral);
   }
 
-  return true;
+  return ResultWithReason{true};
 }
 
 Float32MultiArrayStamped MPC::generateDiagData(
@@ -278,7 +281,7 @@ void MPC::resetPrevResult(const SteeringReport & current_steer)
   m_raw_steer_cmd_pprev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f);
 }
 
-std::pair<bool, MPCData> MPC::getData(
+std::pair<ResultWithReason, MPCData> MPC::getData(
   const MPCTrajectory & traj, const SteeringReport & current_steer,
   const Odometry & current_kinematics)
 {
@@ -288,8 +291,7 @@ std::pair<bool, MPCData> MPC::getData(
   if (!MPCUtils::calcNearestPoseInterp(
         traj, current_pose, &(data.nearest_pose), &(data.nearest_idx), &(data.nearest_time),
         ego_nearest_dist_threshold, ego_nearest_yaw_threshold)) {
-    warn_throttle("calculateMPC: error in calculating nearest pose. stop mpc.");
-    return {false, MPCData{}};
+    return {ResultWithReason{false, "error in calculating nearest pose"}, MPCData{}};
   }
 
   // get data
@@ -304,28 +306,25 @@ std::pair<bool, MPCData> MPC::getData(
   // check error limit
   const double dist_err = calcDistance2d(current_pose, data.nearest_pose);
   if (dist_err > m_admissible_position_error) {
-    warn_throttle("Too large position error: %fm > %fm", dist_err, m_admissible_position_error);
-    return {false, MPCData{}};
+    return {ResultWithReason{false, "too large position error"}, MPCData{}};
   }
 
   // check yaw error limit
   if (std::fabs(data.yaw_err) > m_admissible_yaw_error_rad) {
-    warn_throttle("Too large yaw error: %f > %f", data.yaw_err, m_admissible_yaw_error_rad);
-    return {false, MPCData{}};
+    return {ResultWithReason{false, "too large yaw error"}, MPCData{}};
   }
 
   // check trajectory time length
   const double max_prediction_time =
     m_param.min_prediction_length / static_cast<double>(m_param.prediction_horizon - 1);
   auto end_time = data.nearest_time + m_param.input_delay + m_ctrl_period + max_prediction_time;
   if (end_time > traj.relative_time.back()) {
-    warn_throttle("path is too short for prediction.");
-    return {false, MPCData{}};
+    return {ResultWithReason{false, "path is too short for prediction."}, MPCData{}};
   }
-  return {true, data};
+  return {ResultWithReason{true}, data};
 }
 
-std::pair<bool, MPCTrajectory> MPC::resampleMPCTrajectoryByTime(
+std::pair<ResultWithReason, MPCTrajectory> MPC::resampleMPCTrajectoryByTime(
   const double ts, const double prediction_dt, const MPCTrajectory & input) const
 {
   MPCTrajectory output;
@@ -334,8 +333,7 @@ std::pair<bool, MPCTrajectory> MPC::resampleMPCTrajectoryByTime(
     mpc_time_v.push_back(ts + i * prediction_dt);
   }
   if (!MPCUtils::linearInterpMPCTrajectory(input.relative_time, input, mpc_time_v, output)) {
-    warn_throttle("calculateMPC: mpc resample error. stop mpc calculation. check code!");
-    return {false, {}};
+    return {ResultWithReason{false, "mpc resample error"}, {}};
   }
   // Publish resampled reference trajectory for debug purpose.
   if (m_publish_debug_trajectories) {
@@ -344,7 +342,7 @@ std::pair<bool, MPCTrajectory> MPC::resampleMPCTrajectoryByTime(
     converted_output.header.frame_id = "map";
     m_debug_resampled_reference_trajectory_pub->publish(converted_output);
   }
-  return {true, output};
+  return {ResultWithReason{true}, output};
 }
 
 VectorXd MPC::getInitialState(const MPCData & data)
@@ -577,15 +575,14 @@ MPCMatrix MPC::generateMPCMatrix(
  *                            ~~~
  * [    -au_lim * dt    ] < [uN-uN-1] < [     au_lim * dt    ] (*N... DIM_U)
  */
-std::pair<bool, VectorXd> MPC::executeOptimization(
+std::pair<ResultWithReason, VectorXd> MPC::executeOptimization(
   const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj,
   const double current_velocity)
 {
   VectorXd Uex;
 
   if (!isValid(m)) {
-    warn_throttle("model matrix is invalid. stop MPC.");
-    return {false, {}};
+    return {ResultWithReason{false, "invalid model matrix"}, {}};
   }
 
   const int DIM_U_N = m_param.prediction_horizon * m_vehicle_model_ptr->getDimU();
@@ -622,8 +619,7 @@ std::pair<bool, VectorXd> MPC::executeOptimization(
   bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex);
   auto t_end = std::chrono::system_clock::now();
   if (!solve_result) {
-    warn_throttle("qp solver error");
-    return {false, {}};
+    return {ResultWithReason{false, "qp solver error"}, {}};
   }
 
   {
@@ -632,10 +628,9 @@ std::pair<bool, VectorXd> MPC::executeOptimization(
   }
 
   if (Uex.array().isNaN().any()) {
-    warn_throttle("model Uex includes NaN, stop MPC.");
-    return {false, {}};
+    return {ResultWithReason{false, "model Uex including NaN"}, {}};
   }
-  return {true, Uex};
+  return {ResultWithReason{true}, Uex};
 }
 
 void MPC::addSteerWeightR(const double prediction_dt, MatrixXd & R) const

control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -235,20 +235,17 @@ std::shared_ptr<SteeringOffsetEstimator> MpcLateralController::createSteerOffset
 
 void MpcLateralController::setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat)
 {
-  if (m_is_mpc_solved) {
+  if (m_mpc_solved_status.result) {
     stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "MPC succeeded.");
   } else {
-    const std::string error_msg = "The MPC solver failed. Call MRM to stop the car.";
+    const std::string error_msg = "MPC failed due to " + m_mpc_solved_status.reason;
     stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, error_msg);
   }
 }
 
 void MpcLateralController::setupDiag()
 {
-  auto & d = diag_updater_;
-  d->setHardwareID("mpc_lateral_controller");
-
-  d->add("MPC_solve_checker", [&](auto & stat) { setStatus(stat); });
+  diag_updater_->add("MPC_solve_checker", [&](auto & stat) { setStatus(stat); });
 }
 
 trajectory_follower::LateralOutput MpcLateralController::run(
@@ -277,11 +274,16 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   }
 
   trajectory_follower::LateralHorizon ctrl_cmd_horizon{};
-  const bool is_mpc_solved = m_mpc->calculateMPC(
+  const auto mpc_solved_status = m_mpc->calculateMPC(
     m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values,
     ctrl_cmd_horizon);
 
-  m_is_mpc_solved = is_mpc_solved;  // for diagnostic updater
+  if (
+    (m_mpc_solved_status.result == true && mpc_solved_status.result == false) ||
+    (!mpc_solved_status.result && mpc_solved_status.reason != m_mpc_solved_status.reason)) {
+    RCLCPP_ERROR(logger_, "MPC failed due to %s", mpc_solved_status.reason.c_str());
+  }
+  m_mpc_solved_status = mpc_solved_status;  // for diagnostic updater
 
   diag_updater_->force_update();
 
@@ -290,7 +292,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   // the vehicle will return to the path by re-planning the trajectory or external operation.
   // After the recovery, the previous value of the optimization may deviate greatly from
   // the actual steer angle, and it may make the optimization result unstable.
-  if (!is_mpc_solved || !is_under_control) {
+  if (!mpc_solved_status.result || !is_under_control) {
     m_mpc->resetPrevResult(m_current_steering);
   } else {
     setSteeringToHistory(ctrl_cmd);
@@ -334,13 +336,12 @@ trajectory_follower::LateralOutput MpcLateralController::run(
     return createLateralOutput(m_ctrl_cmd_prev, false, ctrl_cmd_horizon);
   }
 
-  if (!is_mpc_solved) {
-    warn_throttle("MPC is not solved. publish 0 velocity.");
+  if (!mpc_solved_status.result) {
     ctrl_cmd = getStopControlCommand();
   }
 
   m_ctrl_cmd_prev = ctrl_cmd;
-  return createLateralOutput(ctrl_cmd, is_mpc_solved, ctrl_cmd_horizon);
+  return createLateralOutput(ctrl_cmd, mpc_solved_status.result, ctrl_cmd_horizon);
 }
 
 bool MpcLateralController::isSteerConverged(const Lateral & cmd) const