set the default error output and update the readme

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

Author: HansOersted

control/mpc_lateral_controller/README.md

@@ -226,6 +226,8 @@ Some commonly used signals are listed below:
 | lateral error w.r.t the modified reference, feeding MPC module | [5]    |
 | lateral error w.r.t the original reference, feeding mpc.cpp    | [22]   |
 
+Note: The default error output for each signal is -1.
+
 ### How to tune MPC parameters
 
 #### Set kinematics information

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp

@@ -380,7 +380,8 @@ class MPC
     const MPCTrajectory & reference_trajectory, const MPCData & mpc_data_traj_raw,
     const MPCData & mpc_data, const MPCMatrix & mpc_matrix,
     const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
-    const Odometry & current_kinematics) const;
+    const Odometry & current_kinematics,
+    bool success_data_for_diagnostic , bool success_data_traj_raw) const;
 
   /**
    * @brief calculate steering rate limit along with the target trajectory

control/mpc_lateral_controller/src/mpc.cpp

@@ -76,16 +76,10 @@ bool MPC::calculateMPC(
   // get the diagnostic data
   const auto [success_data_for_diagnostic, mpc_data_for_diagnostic] =
     getData(mpc_resampled_ref_trajectory, current_steer, current_kinematics);
-  if (!success_data_for_diagnostic) {
-    return fail_warn_throttle("fail to get MPC Data for the diagnostic. Stop MPC.");
-  }
 
   // get the diagnostic data w.r.t. the original trajectory
   const auto [success_data_traj_raw, mpc_data_traj_raw] =
     getData(m_mpc_traj_raw, current_steer, current_kinematics);
-  if (!success_data_traj_raw) {
-    return fail_warn_throttle("fail to get MPC Data for the raw trajectory. Stop MPC.");
-  }
 
   // generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex
   const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);
@@ -125,15 +119,15 @@ bool MPC::calculateMPC(
   // prepare diagnostic message
   diagnostic = generateDiagData(
     mpc_resampled_ref_trajectory, mpc_data_traj_raw, mpc_data_for_diagnostic, mpc_matrix, ctrl_cmd,
-    Uex, current_kinematics);
+    Uex, current_kinematics, success_data_for_diagnostic, success_data_traj_raw);
 
   return true;
 }
 
 Float32MultiArrayStamped MPC::generateDiagData(
   const MPCTrajectory & reference_trajectory, const MPCData & mpc_data_traj_raw,
   const MPCData & mpc_data, const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd,
-  const VectorXd & Uex, const Odometry & current_kinematics) const
+  const VectorXd & Uex, const Odometry & current_kinematics, bool success_data_for_diagnostic , bool success_data_traj_raw) const
 {
   Float32MultiArrayStamped diagnostic;
 
@@ -156,28 +150,28 @@ Float32MultiArrayStamped MPC::generateDiagData(
   append_diag(ctrl_cmd.steering_tire_angle);      // [0] final steering command (MPC + LPF)
   append_diag(Uex(0));                            // [1] mpc calculation result
   append_diag(mpc_matrix.Uref_ex(0));             // [2] feed-forward steering value
-  append_diag(std::atan(nearest_smooth_k * wb));  // [3] feed-forward steering value raw
-  append_diag(mpc_data.steer);                    // [4] current steering angle
-  append_diag(mpc_data.lateral_err);              // [5] lateral error
+  append_diag(success_data_for_diagnostic ? std::atan(nearest_smooth_k * wb) : -1.0);  // [3] feed-forward steering value raw
+  append_diag(success_data_for_diagnostic ? mpc_data.steer : -1.0);                    // [4] current steering angle
+  append_diag(success_data_for_diagnostic ? mpc_data.lateral_err : -1.0);              // [5] lateral error (the actual error used for MPC)
   append_diag(tf2::getYaw(current_kinematics.pose.pose.orientation));  // [6] current_pose yaw
-  append_diag(tf2::getYaw(mpc_data.nearest_pose.orientation));         // [7] nearest_pose yaw
-  append_diag(mpc_data.yaw_err);                                       // [8] yaw error
-  append_diag(reference_trajectory.vx.at(mpc_data.nearest_idx));       // [9] reference velocity
+  append_diag(success_data_for_diagnostic ? tf2::getYaw(mpc_data.nearest_pose.orientation) : -1.0);         // [7] nearest_pose yaw
+  append_diag(success_data_for_diagnostic ? mpc_data.yaw_err : -1.0);                                       // [8] yaw error
+  append_diag(success_data_for_diagnostic ? reference_trajectory.vx.at(mpc_data.nearest_idx) : -1.0);       // [9] reference velocity
   append_diag(current_velocity);                                       // [10] measured velocity
   append_diag(wz_command);                           // [11] angular velocity from steer command
-  append_diag(wz_measured);                          // [12] angular velocity from measured steer
-  append_diag(current_velocity * nearest_smooth_k);  // [13] angular velocity from path curvature
-  append_diag(nearest_smooth_k);          // [14] nearest path curvature (used for feed-forward)
-  append_diag(nearest_k);                 // [15] nearest path curvature (not smoothed)
-  append_diag(mpc_data.predicted_steer);  // [16] predicted steer
-  append_diag(wz_predicted);              // [17] angular velocity from predicted steer
+  append_diag(success_data_for_diagnostic ? wz_measured : -1.0);                          // [12] angular velocity from measured steer
+  append_diag(success_data_for_diagnostic ? current_velocity * nearest_smooth_k : -1.0);  // [13] angular velocity from path curvature
+  append_diag(success_data_for_diagnostic ? nearest_smooth_k : -1.0);          // [14] nearest path curvature (used for feed-forward)
+  append_diag(success_data_for_diagnostic ? nearest_k : -1.0);                 // [15] nearest path curvature (not smoothed)
+  append_diag(success_data_for_diagnostic ? mpc_data.predicted_steer : -1.0);  // [16] predicted steer
+  append_diag(success_data_for_diagnostic ? wz_predicted : -1.0);              // [17] angular velocity from predicted steer
   append_diag(iteration_num);             // [18] iteration number
   append_diag(runtime);                   // [19] runtime of the latest problem solved
   append_diag(objective_value);           // [20] objective value of the latest problem solved
   append_diag(std::clamp(
     Uex(0), -m_steer_lim,
     m_steer_lim));  // [21] control signal after the saturation constraint (clamp)
-  append_diag(mpc_data_traj_raw.lateral_err);  // [22] lateral error from raw trajectory
+  append_diag(success_data_traj_raw ? mpc_data_traj_raw.lateral_err : -1.0);  // [22] lateral error from raw trajectory
 
   return diagnostic;
 }