feat(mpc_lateral_controller): add delayed initial state and debug tra…

…jectory publishing flags (#1721)

feat(mpc_lateral_controller): add flags for delayed initial state and debug trajectory publishing

Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>

control/autoware_mpc_lateral_controller/README.md

@@ -95,6 +95,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 | use_steer_prediction      | boolean | flag for using steer prediction (do not use steer measurement)              | false         |
 | admissible_position_error | double  | stop vehicle when following position error is larger than this value [m]    | 5.0           |
 | admissible_yaw_error_rad  | double  | stop vehicle when following yaw angle error is larger than this value [rad] | 1.57          |
+| use_delayed_initial_state | boolean | flag to use x0_delayed as initial state for predicted trajectory            | true          |
 
 #### Path Smoothing
 
@@ -202,6 +203,12 @@ 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    |
 
+#### Debug
+
+| Name                       | Type    | Description                                                                       | Default value |
+| :------------------------- | :------ | :-------------------------------------------------------------------------------- | :------------ |
+| publish_debug_trajectories | boolean | publish predicted trajectory and resampled reference trajectory for debug purpose | true          |
+
 ### How to tune MPC parameters
 
 #### Set kinematics information

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

@@ -222,6 +222,7 @@ class MPC
   double m_min_prediction_length = 5.0;  // Minimum prediction distance.
 
   rclcpp::Publisher<Trajectory>::SharedPtr m_debug_frenet_predicted_trajectory_pub;
+  rclcpp::Publisher<Trajectory>::SharedPtr m_debug_resampled_reference_trajectory_pub;
   /**
    * @brief Get variables for MPC calculation.
    * @param trajectory The reference trajectory.
@@ -341,11 +342,14 @@ class MPC
    * @param Uex optimized input.
    * @param mpc_resampled_ref_traj reference trajectory resampled in the mpc time-step
    * @param dt delta time used in the mpc problem.
+   * @param coordinate String specifying the coordinate system ("world" or "frenet", default is
+   * "world")
    * @return predicted path
    */
   Trajectory calculatePredictedTrajectory(
     const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
-    const MPCTrajectory & mpc_resampled_ref_traj, const double dt) const;
+    const MPCTrajectory & reference_trajectory, const double dt,
+    const std::string & coordinate = "world") const;
 
   /**
    * @brief Check if the MPC matrix has any invalid values.
@@ -426,7 +430,11 @@ class MPC
   double ego_nearest_dist_threshold = 3.0;  // Threshold for nearest index search based on distance.
   double ego_nearest_yaw_threshold = M_PI_2;  // Threshold for nearest index search based on yaw.
 
-  bool m_debug_publish_predicted_trajectory = false;  // Flag to publish debug predicted trajectory
+  bool m_use_delayed_initial_state =
+    true;  // Flag to use x0_delayed as initial state for predicted trajectory
+
+  bool m_publish_debug_trajectories = false;  // Flag to publish predicted trajectory and
+                                              // resampled reference trajectory for debug purpose
 
   //!< Constructor.
   explicit MPC(rclcpp::Node & node);

control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml

@@ -5,6 +5,7 @@
     use_steer_prediction: false     # flag for using steer prediction (do not use steer measurement)
     admissible_position_error: 5.0  # stop mpc calculation when error is larger than the following value
     admissible_yaw_error_rad: 1.57  # stop mpc calculation when error is larger than the following value
+    use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory
 
     # -- path smoothing --
     enable_path_smoothing: false   # flag for path smoothing
@@ -84,4 +85,4 @@
     cf: 155494.663
     cr: 155494.663
 
-    debug_publish_predicted_trajectory: false  # publish debug predicted trajectory in Frenet coordinate
+    publish_debug_trajectories: true # flag to publish predicted trajectory and resampled reference trajectory for debug purpose

control/autoware_mpc_lateral_controller/src/mpc.cpp

@@ -33,6 +33,8 @@ MPC::MPC(rclcpp::Node & node)
 {
   m_debug_frenet_predicted_trajectory_pub = node.create_publisher<Trajectory>(
     "~/debug/predicted_trajectory_in_frenet_coordinate", rclcpp::QoS(1));
+  m_debug_resampled_reference_trajectory_pub =
+    node.create_publisher<Trajectory>("~/debug/resampled_reference_trajectory", rclcpp::QoS(1));
 }
 
 bool MPC::calculateMPC(
@@ -104,8 +106,19 @@ bool MPC::calculateMPC(
   m_raw_steer_cmd_prev = Uex(0);
 
   /* calculate predicted trajectory */
-  predicted_trajectory =
-    calculatePredictedTrajectory(mpc_matrix, x0, Uex, mpc_resampled_ref_trajectory, prediction_dt);
+  Eigen::VectorXd initial_state = m_use_delayed_initial_state ? x0_delayed : x0;
+  predicted_trajectory = calculatePredictedTrajectory(
+    mpc_matrix, initial_state, Uex, mpc_resampled_ref_trajectory, prediction_dt, "world");
+
+  // Publish predicted trajectories in different coordinates for debugging purposes
+  if (m_publish_debug_trajectories) {
+    // Calculate and publish predicted trajectory in Frenet coordinate
+    auto predicted_trajectory_frenet = calculatePredictedTrajectory(
+      mpc_matrix, initial_state, Uex, mpc_resampled_ref_trajectory, prediction_dt, "frenet");
+    predicted_trajectory_frenet.header.stamp = m_clock->now();
+    predicted_trajectory_frenet.header.frame_id = "map";
+    m_debug_frenet_predicted_trajectory_pub->publish(predicted_trajectory_frenet);
+  }
 
   // prepare diagnostic message
   diagnostic =
@@ -310,6 +323,13 @@ std::pair<bool, MPCTrajectory> MPC::resampleMPCTrajectoryByTime(
     warn_throttle("calculateMPC: mpc resample error. stop mpc calculation. check code!");
     return {false, {}};
   }
+  // Publish resampled reference trajectory for debug purpose.
+  if (m_publish_debug_trajectories) {
+    auto converted_output = MPCUtils::convertToAutowareTrajectory(output);
+    converted_output.header.stamp = m_clock->now();
+    converted_output.header.frame_id = "map";
+    m_debug_resampled_reference_trajectory_pub->publish(converted_output);
+  }
   return {true, output};
 }
 
@@ -785,12 +805,21 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory(
 
 Trajectory MPC::calculatePredictedTrajectory(
   const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
-  const MPCTrajectory & reference_trajectory, const double dt) const
+  const MPCTrajectory & reference_trajectory, const double dt, const std::string & coordinate) const
 {
-  const auto predicted_mpc_trajectory =
-    m_vehicle_model_ptr->calculatePredictedTrajectoryInWorldCoordinate(
+  MPCTrajectory predicted_mpc_trajectory;
+
+  if (coordinate == "world") {
+    predicted_mpc_trajectory = m_vehicle_model_ptr->calculatePredictedTrajectoryInWorldCoordinate(
+      mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory,
+      dt);
+  } else if (coordinate == "frenet") {
+    predicted_mpc_trajectory = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate(
       mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory,
       dt);
+  } else {
+    throw std::invalid_argument("Invalid coordinate system specified. Use 'world' or 'frenet'.");
+  }
 
   // do not over the reference trajectory
   const auto predicted_length = MPCUtils::calcMPCTrajectoryArcLength(reference_trajectory);
@@ -799,18 +828,6 @@ Trajectory MPC::calculatePredictedTrajectory(
 
   const auto predicted_trajectory = MPCUtils::convertToAutowareTrajectory(clipped_trajectory);
 
-  // Publish trajectory in relative coordinate for debug purpose.
-  if (m_debug_publish_predicted_trajectory) {
-    const auto frenet = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate(
-      mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory,
-      dt);
-    auto frenet_clipped = MPCUtils::convertToAutowareTrajectory(
-      MPCUtils::clipTrajectoryByLength(frenet, predicted_length));
-    frenet_clipped.header.stamp = m_clock->now();
-    frenet_clipped.header.frame_id = "map";
-    m_debug_frenet_predicted_trajectory_pub->publish(frenet_clipped);
-  }
-
   return predicted_trajectory;
 }
 

control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -134,7 +134,9 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node)
   m_mpc->ego_nearest_dist_threshold = m_ego_nearest_dist_threshold;
   m_mpc->ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold;
 
-  m_mpc->m_debug_publish_predicted_trajectory = dp_bool("debug_publish_predicted_trajectory");
+  m_mpc->m_use_delayed_initial_state = dp_bool("use_delayed_initial_state");
+
+  m_mpc->m_publish_debug_trajectories = dp_bool("publish_debug_trajectories");
 
   m_pub_predicted_traj = node.create_publisher<Trajectory>("~/output/predicted_trajectory", 1);
   m_pub_debug_values =

control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml

@@ -5,6 +5,7 @@
     use_steer_prediction: false     # flag for using steer prediction (do not use steer measurement)
     admissible_position_error: 5.0  # stop mpc calculation when error is larger than the following value
     admissible_yaw_error_rad: 1.57  # stop mpc calculation when error is larger than the following value
+    use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory
 
     # -- path smoothing --
     enable_path_smoothing: false   # flag for path smoothing
@@ -75,4 +76,4 @@
       average_num: 1000
       steering_offset_limit: 0.02
 
-    debug_publish_predicted_trajectory: false  # publish debug predicted trajectory in Frenet coordinate
+    publish_debug_trajectories: true # flag to publish predicted trajectory and resampled reference trajectory for debug purpose