feat(mpc_lateral_controller): remove trans/rot deviation validation s…

…ince the control_validator has the same feature (#9684)

Signed-off-by: Takayuki Murooka <[email protected]>

control/autoware_mpc_lateral_controller/README.md

@@ -95,13 +95,11 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 
 #### System
 
-| Name                      | Type    | Description                                                                 | Default value |
-| :------------------------ | :------ | :-------------------------------------------------------------------------- | :------------ |
-| traj_resample_dist        | double  | distance of waypoints in resampling [m]                                     | 0.1           |
-| 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          |
+| Name                      | Type    | Description                                                      | Default value |
+| :------------------------ | :------ | :--------------------------------------------------------------- | :------------ |
+| traj_resample_dist        | double  | distance of waypoints in resampling [m]                          | 0.1           |
+| use_steer_prediction      | boolean | flag for using steer prediction (do not use steer measurement)   | false         |
+| use_delayed_initial_state | boolean | flag to use x0_delayed as initial state for predicted trajectory | true          |
 
 #### Path Smoothing
 

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

@@ -421,10 +421,8 @@ class MPC
   double m_raw_steer_cmd_prev = 0.0;     // Previous MPC raw output.
 
   /* Parameters for control */
-  double m_admissible_position_error;  // Threshold for lateral error to trigger stop command [m].
-  double m_admissible_yaw_error_rad;   // Threshold for yaw error to trigger stop command [rad].
-  double m_steer_lim;                  // Steering command limit [rad].
-  double m_ctrl_period;                // Control frequency [s].
+  double m_steer_lim;    // Steering command limit [rad].
+  double m_ctrl_period;  // Control frequency [s].
 
   //!< @brief steering rate limit list depending on curvature [/m], [rad/s]
   std::vector<std::pair<double, double>> m_steer_rate_lim_map_by_curvature{};

control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml

@@ -3,8 +3,6 @@
     # -- system --
     traj_resample_dist: 0.1         # path resampling interval [m]
     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 --

control/autoware_mpc_lateral_controller/src/mpc.cpp

@@ -307,17 +307,6 @@ std::pair<ResultWithReason, MPCData> MPC::getData(
   // get predicted steer
   data.predicted_steer = m_steering_predictor->calcSteerPrediction();
 
-  // check error limit
-  const double dist_err = calcDistance2d(current_pose, data.nearest_pose);
-  if (dist_err > m_admissible_position_error) {
-    return {ResultWithReason{false, "too large position error"}, MPCData{}};
-  }
-
-  // check yaw error limit
-  if (std::fabs(data.yaw_err) > m_admissible_yaw_error_rad) {
-    return {ResultWithReason{false, "too large yaw error"}, MPCData{}};
-  }
-
   // check trajectory time length
   const double max_prediction_time =
     m_param.min_prediction_length / static_cast<double>(m_param.prediction_horizon - 1);

control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -57,8 +57,6 @@ MpcLateralController::MpcLateralController(
   p_filt.traj_resample_dist = dp_double("traj_resample_dist");
   p_filt.extend_trajectory_for_end_yaw_control = dp_bool("extend_trajectory_for_end_yaw_control");
 
-  m_mpc->m_admissible_position_error = dp_double("admissible_position_error");
-  m_mpc->m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad");
   m_mpc->m_use_steer_prediction = dp_bool("use_steer_prediction");
   m_mpc->m_param.steer_tau = dp_double("vehicle_model_steer_tau");
 

control/autoware_mpc_lateral_controller/test/test_mpc.cpp

@@ -96,8 +96,6 @@ class MPCTest : public ::testing::Test
   double error_deriv_lpf_cutoff_hz = 5.0;
 
   // Test Parameters
-  double admissible_position_error = 5.0;
-  double admissible_yaw_error_rad = M_PI_2;
   double steer_lim = 0.610865;      // 35 degrees
   double steer_rate_lim = 2.61799;  // 150 degrees
   double ctrl_period = 0.03;
@@ -162,8 +160,6 @@ class MPCTest : public ::testing::Test
   void initializeMPC(mpc_lateral_controller::MPC & mpc)
   {
     mpc.m_param = param;
-    mpc.m_admissible_position_error = admissible_position_error;
-    mpc.m_admissible_yaw_error_rad = admissible_yaw_error_rad;
     mpc.m_steer_lim = steer_lim;
     mpc.m_steer_rate_lim_map_by_curvature.emplace_back(0.0, steer_rate_lim);
     mpc.m_steer_rate_lim_map_by_velocity.emplace_back(0.0, steer_rate_lim);
@@ -480,37 +476,4 @@ TEST_F(MPCTest, MultiSolveWithBuffer)
   EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f);
   EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f);
 }
-
-TEST_F(MPCTest, FailureCases)
-{
-  auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{});
-  auto mpc = std::make_unique<MPC>(node);
-  std::shared_ptr<VehicleModelInterface> vehicle_model_ptr =
-    std::make_shared<KinematicsBicycleModel>(wheelbase, steer_limit, steer_tau);
-  mpc->setVehicleModel(vehicle_model_ptr);
-  std::shared_ptr<QPSolverInterface> qpsolver_ptr = std::make_shared<QPSolverEigenLeastSquareLLT>();
-  mpc->setQPSolver(qpsolver_ptr);
-
-  // Init parameters and reference trajectory
-  initializeMPC(*mpc);
-
-  // Calculate MPC with a pose too far from the trajectory
-  Pose pose_far;
-  pose_far.position.x = pose_zero.position.x - admissible_position_error - 1.0;
-  pose_far.position.y = pose_zero.position.y - admissible_position_error - 1.0;
-  Lateral ctrl_cmd;
-  Trajectory pred_traj;
-  Float32MultiArrayStamped diag;
-  LateralHorizon ctrl_cmd_horizon;
-  const auto odom = makeOdometry(pose_far, default_velocity);
-  EXPECT_FALSE(
-    mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon).result);
-
-  // Calculate MPC with a fast velocity to make the prediction go further than the reference path
-  EXPECT_FALSE(mpc
-                 ->calculateMPC(
-                   neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd,
-                   pred_traj, diag, ctrl_cmd_horizon)
-                 .result);
-}
 }  // namespace autoware::motion::control::mpc_lateral_controller