feat(mpc_lateral_controller): visualize/correct the longitudinal and lateral deviations for pid and mpc

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -91,6 +91,7 @@ struct Output
   bool is_out_of_lane{};
   bool will_cross_boundary{};
   PoseDeviation trajectory_deviation{};
+  double deviation_longitudinal_vel{0.0};
   lanelet::ConstLanelets candidate_lanelets{};
   TrajectoryPoints resampled_trajectory{};
   std::vector<LinearRing2d> vehicle_footprints{};
@@ -138,6 +139,10 @@ class LaneDepartureChecker
   Param param_;
   std::shared_ptr<vehicle_info_util::VehicleInfo> vehicle_info_ptr_;
 
+  static double calcLongitudinalDeviationDerivatives(
+    const Trajectory & trajectory, const double dist_threshold, const double yaw_threshold,
+    const Input & input);
+
   static PoseDeviation calcTrajectoryDeviation(
     const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose,
     const double dist_threshold, const double yaw_threshold);

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -104,6 +104,10 @@
 
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch;
 
+  output.deviation_longitudinal_vel = calcLongitudinalDeviationDerivatives(
+    *input.predicted_trajectory, param_.ego_nearest_dist_threshold,
+    param_.ego_nearest_yaw_threshold, input);
+
   output.trajectory_deviation = calcTrajectoryDeviation(
     *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold,
     param_.ego_nearest_yaw_threshold);
@@ -165,6 +169,23 @@
   return willLeaveLane(candidate_lanelets, vehicle_footprints);
 }
 
+double LaneDepartureChecker::calcLongitudinalDeviationDerivatives(
+  const Trajectory & trajectory, const double dist_threshold, const double yaw_threshold,
+  const Input & input)
+{
+  const geometry_msgs::msg::Pose & pose = input.current_odom->pose.pose;
+  const auto nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    trajectory.points, pose, dist_threshold, yaw_threshold);
+  double deviation_longitudinal_vel;
+  const auto & ref_point = trajectory.points.at(nearest_idx);
+
+  const auto current_vel = input.current_odom->twist.twist.linear.x;
+  const double ref_vel = ref_point.longitudinal_velocity_mps;
+  deviation_longitudinal_vel = current_vel - ref_vel;
+
+  return deviation_longitudinal_vel;
+}
+
 PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation(
   const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold,
   const double yaw_threshold)

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -358,11 +358,16 @@
 
   {
     const auto & deviation = output_.trajectory_deviation;
+    const double deviation_vel_longitudinal = output_.deviation_longitudinal_vel;
     debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
       "deviation/lateral", deviation.lateral);
     debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>("deviation/yaw", deviation.yaw);
     debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
       "deviation/yaw_deg", rad2deg(deviation.yaw));
+    debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "deviation/longitudinal", deviation.longitudinal);
+    debug_publisher_.publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "deviation/longitudinal_velocity", deviation_vel_longitudinal);
   }
   processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true);
 

control/mpc_lateral_controller/README.md

@@ -202,6 +202,32 @@ Defined in the `steering_offset` namespace. This logic is designed as simple as
 | cf            | double | front cornering power [N/rad]               | 155494.663    |
 | cr            | double | rear cornering power [N/rad]                | 155494.663    |
 
+### Visualizations of the control results (debug information)
+
+Some signals are published as topics to evaluate the quality of the control results. A control researcher can visualize these information using the plotting tool such as PlotJuggler. Note that the 'deviation' and the 'state error' are defined as the difference between the state and the reference. That is state - reference. This definition is different from the conventional sign of the 'state error' defined in the control theory.
+
+`/control/trajectory_follower/lane_departure_checker_node/debug/deviation/longitudinal`:
+This topic represents the longitudinal deviation of the position at the current time.
+
+`/control/trajectory_follower/lane_departure_checker_node/debug/deviation/longitudinal_velocity`:
+This topic represents the longitudinal deviation of the velocity at the current time.
+
+`/control/trajectory_follower/lateral/diagnostic`:
+The signals in this topic evaluate the quality of MPC.
+
+Some commonly used signals are listed below:
+
+| The signal                                                     | Number |
+| :------------------------------------------------------------- | :----- |
+| final steering command (MPC + Clamp + LPF)                     | [0]    |
+| MPC result                                                     | [1]    |
+| MPC after Clamp (MPC + Clamp)                                  | [21]   |
+| feedforward term (reference steering angle)                    | [2]    |
+| 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

@@ -377,9 +377,10 @@ class MPC
    * @return The generated diagnostic data.
    */
   Float32MultiArrayStamped generateDiagData(
-    const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
-    const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
-    const Odometry & current_kinematics) const;
+    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, bool success_data_traj_raw) const;
 
   /**
    * @brief calculate steering rate limit along with the target trajectory
@@ -406,6 +407,7 @@ class MPC
 
 public:
   MPCTrajectory m_reference_trajectory;  // Reference trajectory to be followed.
+  MPCTrajectory m_mpc_traj_raw;          // The raw trajectory from the planner.
   MPCParam m_param;                      // MPC design parameters.
   std::deque<double> m_input_buffer;     // MPC output buffer for delay time compensation.
   double m_raw_steer_cmd_prev = 0.0;     // Previous MPC raw output.

control/mpc_lateral_controller/src/mpc.cpp

@@ -1,4 +1,4 @@
 // Copyright 2018-2021 The Autoware Foundation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -73,6 +73,10 @@
     return fail_warn_throttle("trajectory resampling failed. 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);
+
   // generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex
   const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);
 
@@ -109,16 +113,17 @@
     calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt);
 
   // prepare diagnostic message
-  diagnostic =
-    generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics);
+  diagnostic = generateDiagData(
+    reference_trajectory, mpc_data_traj_raw, mpc_data, mpc_matrix, ctrl_cmd, Uex,
+    current_kinematics, success_data_traj_raw);
 
   return true;
 }
 
 Float32MultiArrayStamped MPC::generateDiagData(
-  const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
-  const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
-  const Odometry & current_kinematics) const
+  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, bool success_data_traj_raw) const
 {
   Float32MultiArrayStamped diagnostic;
 
@@ -143,7 +148,7 @@
   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(mpc_data.lateral_err);  // [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
@@ -159,6 +164,12 @@
   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(
+    success_data_traj_raw ? mpc_data_traj_raw.lateral_err
+                          : -1.0);  // [22] lateral error from raw trajectory
 
   return diagnostic;
 }
@@ -173,11 +184,11 @@
   const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment(
     trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position);
 
-  const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg);
+  m_mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg);
 
   // resampling
   const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance(
-    mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
+    m_mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
   if (!success_resample) {
     warn_throttle("[setReferenceTrajectory] spline error when resampling by distance");
     return;
@@ -214,7 +225,7 @@
    */
   if (param.extend_trajectory_for_end_yaw_control) {
     MPCUtils::extendTrajectoryInYawDirection(
-      mpc_traj_raw.yaw.back(), param.traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
+      m_mpc_traj_raw.yaw.back(), param.traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
   }
 
   // calculate yaw angle