mpc fail checker diagnostic added

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

Author: HansOersted

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -91,6 +91,12 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   // trajectory buffer for detecting new trajectory
   std::deque<Trajectory> m_trajectory_buffer;
 
+  Updater diag_updater_{this}; // Diagnostic updater for publishing diagnostic data.
+
+  void setStatus(DiagnosticStatusWrapper & stat, const bool & m_MPC_failed);
+
+  void setupDiag();
+
   std::unique_ptr<MPC> m_mpc;  // MPC object for trajectory following.
 
   // Check is mpc output converged

control/mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -152,6 +152,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node)
 
   m_mpc->setLogger(logger_);
   m_mpc->setClock(clock_);
+
+  setupDiag();
 }
 
 MpcLateralController::~MpcLateralController()
@@ -227,6 +229,28 @@ std::shared_ptr<SteeringOffsetEstimator> MpcLateralController::createSteerOffset
   return steering_offset_;
 }
 
+void MpcLateralController::setStatus(
+  DiagnosticStatusWrapper & stat, const bool & is_mpc_solved)
+{
+  if (is_mpc_solved) {
+    stat.summary(DiagnosticStatus::OK, "MPC succeeded.");
+  } else {
+    const std::string & error_msg = "The MPC solver failed. Call MRM to stop the car.";
+    stat.summary(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, is_mpc_solved);
+  });
+}
+
 trajectory_follower::LateralOutput MpcLateralController::run(
   trajectory_follower::InputData const & input_data)
 {
@@ -255,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);
 
+  diag_updater_.force_update();
+
   // reset previous MPC result
   // Note: When a large deviation from the trajectory occurs, the optimization stops and
   // the vehicle will return to the path by re-planning the trajectory or external operation.