fix(controller): revival of dry steering

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

@@ -271,7 +271,7 @@ class MPC
    */
   std::pair<bool, VectorXd> executeOptimization(
     const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt,
-    const MPCTrajectory & trajectory);
+    const MPCTrajectory & trajectory, const double current_velocity);
 
   /**
    * @brief Resample the trajectory with the MPC resampling time.
@@ -386,7 +386,8 @@ class MPC
    * @param reference_trajectory The reference trajectory.
    * @param current_velocity current velocity of ego.
    */
-  VectorXd calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const;
+  VectorXd calcSteerRateLimitOnTrajectory(
+    const MPCTrajectory & trajectory, const double current_velocity) const;
 
   //!< @brief logging with warn and return false
   template <typename... Args>

control/autoware_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.
@@ -76,8 +76,9 @@
   const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);
 
   // solve Optimization problem
-  const auto [success_opt, Uex] =
-    executeOptimization(mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory);
+  const auto [success_opt, Uex] = executeOptimization(
+    mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
+    current_kinematics.twist.twist.linear.x);
   if (!success_opt) {
     return fail_warn_throttle("optimization failed. Stop MPC.");
   }
@@ -543,40 +544,41 @@
  * [    -au_lim * dt    ] < [uN-uN-1] < [     au_lim * dt    ] (*N... DIM_U)
  */
 std::pair<bool, VectorXd> MPC::executeOptimization(
-  const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj)
+  const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj,
+  const double current_velocity)
 {
   VectorXd Uex;
 
   if (!isValid(m)) {
     warn_throttle("model matrix is invalid. stop MPC.");
     return {false, {}};
   }
 
   const int DIM_U_N = m_param.prediction_horizon * m_vehicle_model_ptr->getDimU();
 
   // cost function: 1/2 * Uex' * H * Uex + f' * Uex,  H = B' * C' * Q * C * B + R
   const MatrixXd CB = m.Cex * m.Bex;
   const MatrixXd QCB = m.Qex * CB;
   // MatrixXd H = CB.transpose() * QCB + m.R1ex + m.R2ex; // This calculation is heavy. looking for
   // a good way.  //NOLINT
   MatrixXd H = MatrixXd::Zero(DIM_U_N, DIM_U_N);
   H.triangularView<Eigen::Upper>() = CB.transpose() * QCB;
   H.triangularView<Eigen::Upper>() += m.R1ex + m.R2ex;
   H.triangularView<Eigen::Lower>() = H.transpose();
   MatrixXd f = (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Uref_ex.transpose() * m.R1ex;
   addSteerWeightF(prediction_dt, f);
 
   MatrixXd A = MatrixXd::Identity(DIM_U_N, DIM_U_N);
   for (int i = 1; i < DIM_U_N; i++) {
     A(i, i - 1) = -1.0;
   }
 
   // steering angle limit
   VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim);  // min steering angle
   VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim);   // max steering angle
 
   // steering angle rate limit
-  VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj);
+  VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity);
   VectorXd ubA = steer_rate_limits * prediction_dt;
   VectorXd lbA = -steer_rate_limits * prediction_dt;
   ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period;
@@ -728,40 +730,48 @@
   return steer_rate;
 }
 
-VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const
+VectorXd MPC::calcSteerRateLimitOnTrajectory(
+  const MPCTrajectory & trajectory, const double current_velocity) const
 {
   const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) {
     std::vector<double> reference, limits;
     for (const auto & p : steer_rate_limit_map) {
       reference.push_back(p.first);
       limits.push_back(p.second);
     }
 
     // If the speed is out of range of the reference, apply zero-order hold.
     if (current <= reference.front()) {
       return limits.front();
     }
     if (current >= reference.back()) {
       return limits.back();
     }
 
     // Apply linear interpolation
     for (size_t i = 0; i < reference.size() - 1; ++i) {
       if (reference.at(i) <= current && current <= reference.at(i + 1)) {
         auto ratio =
           (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5);
         ratio = std::clamp(ratio, 0.0, 1.0);
         const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i));
         return interp;
       }
     }
 
     std::cerr << "MPC::calcSteerRateLimitOnTrajectory() interpolation logic is broken. Command "
                  "filter is not working. Please check the code."
               << std::endl;
     return reference.back();
   };
 
+  // When the vehicle is stopped, a large steer rate limit is used for the dry steering.
+  constexpr double steer_rate_lim = 100.0;
+  const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
+  if (is_vehicle_stopped) {
+    return steer_rate_lim * VectorXd::Ones(m_param.prediction_horizon);
+  }
+
   // calculate steering rate limit
   VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon);
   for (int i = 0; i < m_param.prediction_horizon; ++i) {

control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp

@@ -234,6 +234,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   // debug values
   DebugValues m_debug_values;
 
+  std::optional<bool> m_prev_keep_stopped_condition{std::nullopt};
+
   std::shared_ptr<rclcpp::Time> m_last_running_time{std::make_shared<rclcpp::Time>(clock_->now())};
 
   // Diagnostic

control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -598,89 +598,77 @@
 
   // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity
   static constexpr double vel_epsilon = 1e-3;
 
-  // Let vehicle start after the steering is converged for control accuracy
-  const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon &&
-                                      m_enable_keep_stopped_until_steer_convergence &&
-                                      !lateral_sync_data_.is_steer_converged;
-  if (keep_stopped_condition) {
-    auto marker = createDefaultMarker(
-      "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING,
-      createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));
-    marker.pose = autoware::universe_utils::calcOffsetPose(
-      m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang,
-      m_vehicle_width / 2 + 2.0, 1.5);
-    marker.text = "steering not\nconverged";
-    m_pub_stop_reason_marker->publish(marker);
-  }
-
   const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
 
   const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel &&
                           std::abs(current_acc) < p.stopped_state_entry_acc;
   // Case where the ego slips in the opposite direction of the gear due to e.g. a slope is also
   // considered as a stop
   const bool is_not_running = [&]() {
     if (control_data.shift == Shift::Forward) {
       if (is_stopped || current_vel < 0.0) {
         // NOTE: Stopped or moving backward
         return true;
       }
     } else {
       if (is_stopped || 0.0 < current_vel) {
         // NOTE: Stopped or moving forward
         return true;
       }
     }
     return false;
   }();
   if (!is_not_running) {
     m_last_running_time = std::make_shared<rclcpp::Time>(clock_->now());
   }
   const bool stopped_condition =
     m_last_running_time
       ? (clock_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time
       : false;
 
   // ==========================================================================================
   // NOTE: due to removeOverlapPoints() in getControlData() m_trajectory and
   // control_data.interpolated_traj have different size.
   // ==========================================================================================
   const double current_vel_cmd = std::fabs(
     control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps);
   const bool emergency_condition = m_enable_overshoot_emergency &&
                                    stop_dist < -p.emergency_state_overshoot_stop_dist &&
                                    current_vel_cmd < vel_epsilon;
   const bool has_nonzero_target_vel = std::abs(current_vel_cmd) > 1.0e-5;
 
   const auto changeState = [this](const auto s) {
     if (s != m_control_state) {
       RCLCPP_DEBUG_STREAM(
         logger_, "controller state changed: " << toStr(m_control_state) << " -> " << toStr(s));
     }
     m_control_state = s;
     return;
   };
 
   const auto debug_msg_once = [this](const auto & s) { RCLCPP_DEBUG_ONCE(logger_, "%s", s); };
 
   const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
                                 m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
 
   if (is_under_control != m_prev_vehicle_is_under_control) {
     m_prev_vehicle_is_under_control = is_under_control;
     m_under_control_starting_time =
       is_under_control ? std::make_shared<rclcpp::Time>(clock_->now()) : nullptr;
   }
+
+  if (m_control_state != ControlState::STOPPED) {
+    m_prev_keep_stopped_condition = std::nullopt;
+  }
+
   // transit state
   // in DRIVE state
   if (m_control_state == ControlState::DRIVE) {
     if (emergency_condition) {
       return changeState(ControlState::EMERGENCY);
     }
-    if (!is_under_control && stopped_condition && keep_stopped_condition) {
-      // NOTE: When the ego is stopped on manual driving, since the driving state may transit to
-      //       autonomous, keep_stopped_condition should be checked.
+    if (!is_under_control && stopped_condition) {
       return changeState(ControlState::STOPPED);
     }
 
@@ -723,19 +711,42 @@
 
   // in STOPPED state
   if (m_control_state == ControlState::STOPPED) {
-    // -- debug print --
+    // debug print
     if (has_nonzero_target_vel && !departure_condition_from_stopped) {
       debug_msg_once("target speed > 0, but departure condition is not met. Keep STOPPED.");
     }
-    if (has_nonzero_target_vel && keep_stopped_condition) {
-      debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED.");
-    }
-    // ---------------
 
-    if (keep_stopped_condition) {
-      return changeState(ControlState::STOPPED);
-    }
     if (departure_condition_from_stopped) {
+      // Let vehicle start after the steering is converged for dry steering
+      const bool current_keep_stopped_condition =
+        std::fabs(current_vel) < vel_epsilon && !lateral_sync_data_.is_steer_converged;
+      // NOTE: Dry steering is considered unnecessary when the steering is converged twice in a
+      //       row. This is because lateral_sync_data_.is_steer_converged is not the current but
+      //       the previous value due to the order controllers' run and sync functions.
+      const bool keep_stopped_condition =
+        !m_prev_keep_stopped_condition ||
+        (current_keep_stopped_condition || *m_prev_keep_stopped_condition);
+      m_prev_keep_stopped_condition = current_keep_stopped_condition;
+      if (m_enable_keep_stopped_until_steer_convergence && keep_stopped_condition) {
+        // debug print
+        if (has_nonzero_target_vel) {
+          debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED.");
+        }
+
+        // publish debug marker
+        auto marker = createDefaultMarker(
+          "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING,
+          createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));
+        marker.pose = autoware::universe_utils::calcOffsetPose(
+          m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang,
+          m_vehicle_width / 2 + 2.0, 1.5);
+        marker.text = "steering not\nconverged";
+        m_pub_stop_reason_marker->publish(marker);
+
+        // keep STOPPED
+        return;
+      }
+
       m_pid_vel.reset();
       m_lpf_vel_error->reset(0.0);
       // prevent the car from taking a long time to start to move

control/autoware_trajectory_follower_node/test/test_controller_node.cpp

@@ -1,4 +1,4 @@
 // Copyright 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.
@@ -589,11 +589,28 @@
   traj.points.push_back(make_traj_point(0.0, 0.0, 1.0f));
   traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f));
   traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f));
-  tester.traj_pub->publish(traj);
 
-  test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+  {  // Check if the ego can keep stopped when the steering is not converged.
+    tester.traj_pub->publish(traj);
+    test_utils::waitForMessage(tester.node, this, tester.received_control_command);
 
-  ASSERT_TRUE(tester.received_control_command);
-  // Keep stopped state when the lateral control is not converged.
-  EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f);
+    ASSERT_TRUE(tester.received_control_command);
+    // Keep stopped state when the lateral control is not converged.
+    EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f);
+  }
+
+  {  // Check if the ego can keep stopped after the following sequence
+    // 1. not converged -> 2. converged -> 3. not converged
+    tester.publish_steer_angle(0.0);
+    tester.traj_pub->publish(traj);
+    test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+
+    tester.publish_steer_angle(steering_tire_angle);
+    tester.traj_pub->publish(traj);
+    test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+
+    ASSERT_TRUE(tester.received_control_command);
+    // Keep stopped state when the lateral control is not converged.
+    EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f);
+  }
 }