member attribute added.

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

Author: HansOersted

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -89,12 +89,15 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   // Flag indicating whether to keep the steering control until it converges.
   bool m_keep_steer_control_until_converged;
 
+  // MPC colver checker.
+  bool m_is_mpc_solved{true};
+
   // trajectory buffer for detecting new trajectory
   std::deque<Trajectory> m_trajectory_buffer;
 
   diagnostic_updater::Updater diag_updater_{this}; // Diagnostic updater for publishing diagnostic data.
 
-  void setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & m_MPC_failed);
+  void setStatus(diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & m_is_mpc_solved);
 
   void setupDiag();
 

control/mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -230,9 +230,9 @@ std::shared_ptr<SteeringOffsetEstimator> MpcLateralController::createSteerOffset
 }
 
 void MpcLateralController::setStatus(
-  diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & is_mpc_solved)
+  diagnostic_updater::DiagnosticStatusWrapper & stat, const bool & m_is_mpc_solved)
 {
-  if (is_mpc_solved) {
+  if (m_is_mpc_solved) {
     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.";
@@ -247,7 +247,7 @@ void MpcLateralController::setupDiag()
 
   d.add("MPC_solve_checker", [&](auto & stat) {
     setStatus(
-      stat, is_mpc_solved);
+      stat, m_is_mpc_solved);
   });
 }
 
@@ -279,6 +279,8 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   const bool is_mpc_solved = m_mpc->calculateMPC(
     m_current_steering, m_current_kinematic_state, ctrl_cmd, predicted_traj, debug_values);
 
+  m_is_mpc_solved = is_mpc_solved; // for diagnostic updater
+
   diag_updater_.force_update();
 
   // reset previous MPC result