refactor(control): Remove unused variables and functions

Signed-off-by: Berkay Karaman <[email protected]>
autowarefoundation · Nov 11, 2023 · 25e3863 · 25e3863
1 parent c6276d6
commit 25e3863
Show file tree

Hide file tree

Showing 20 changed files with 35 additions and 147 deletions.
diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md
@@ -75,7 +75,8 @@ Return LateralOutput which contains the following to the controller node
 
 - `autoware_auto_control_msgs/AckermannLateralCommand`
 - LateralSyncData
-  - steer angle convergence
+  - is mpc ready to move?
+    - if not, the vehicle should stop until the stable mpc output is obtained
 
 ### MPC class
 

diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp
@@ -73,9 +73,6 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   // Target vehicle speed threshold to enter the stop state.
   double m_stop_state_entry_target_speed;
 
-  // Convergence threshold for steering control.
-  double m_converged_steer_rad;
-
   // max mpc output change threshold for 1 sec
   double m_mpc_converged_threshold_rps;
 
@@ -85,9 +82,6 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
   // Distance threshold to check if the trajectory shape has changed.
   double m_new_traj_end_dist;
 
-  // Flag indicating whether to keep the steering control until it converges.
-  bool m_keep_steer_control_until_converged;
-
   // trajectory buffer for detecting new trajectory
   std::deque<Trajectory> m_trajectory_buffer;
 
@@ -245,13 +239,6 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
    */
   bool isTrajectoryShapeChanged() const;
 
-  /**
-   * @brief Check if the steering control is converged and stable now.
-   * @param cmd Steering control command to be checked.
-   * @return True if the steering control is converged and stable, false otherwise.
-   */
-  bool isSteerConverged(const AckermannLateralCommand & cmd) const;
-
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
 
   /**

diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml
@@ -61,8 +61,6 @@
     # stop state: steering command is kept in the previous value in the stop state.
     stop_state_entry_ego_speed: 0.001
     stop_state_entry_target_speed: 0.001
-    converged_steer_rad: 0.1
-    keep_steer_control_until_converged: true
     new_traj_duration_time: 1.0
     new_traj_end_dist: 0.3
     mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s]

diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp
@@ -60,8 +60,6 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node)
   /* stop state parameters */
   m_stop_state_entry_ego_speed = dp_double("stop_state_entry_ego_speed");
   m_stop_state_entry_target_speed = dp_double("stop_state_entry_target_speed");
-  m_converged_steer_rad = dp_double("converged_steer_rad");
-  m_keep_steer_control_until_converged = dp_bool("keep_steer_control_until_converged");
   m_new_traj_duration_time = dp_double("new_traj_duration_time");            // [s]
   m_new_traj_end_dist = dp_double("new_traj_end_dist");                      // [m]
   m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps");  // [rad/s]
@@ -271,13 +269,11 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) {
     trajectory_follower::LateralOutput output;
     output.control_cmd = createCtrlCmdMsg(cmd);
-    // To be sure current steering of the vehicle is desired steering angle, we need to check
+    // To be sure lateral controller is ready to move, we need to check
     // following conditions.
     // 1. At the last loop, mpc should be solved because command should be optimized output.
-    // 2. The mpc should be converged.
-    // 3. The steer angle should be converged.
-    output.sync_data.is_steer_converged =
-      is_mpc_solved && isMpcConverged() && isSteerConverged(cmd);
+    // 2. The steering output of the mpc should be converged.
+    output.sync_data.is_controller_ready_to_move = is_mpc_solved && isMpcConverged();
 
     return output;
   };
@@ -301,22 +297,6 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   return createLateralOutput(ctrl_cmd, is_mpc_solved);
 }
 
-bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) const
-{
-  // wait for a while to propagate the trajectory shape to the output command when the trajectory
-  // shape is changed.
-  if (!m_has_received_first_trajectory || isTrajectoryShapeChanged()) {
-    RCLCPP_DEBUG(logger_, "trajectory shaped is changed");
-    return false;
-  }
-
-  const bool is_converged =
-    std::abs(cmd.steering_tire_angle - m_current_steering.steering_tire_angle) <
-    static_cast<float>(m_converged_steer_rad);
-
-  return is_converged;
-}
-
 bool MpcLateralController::isReady(const trajectory_follower::InputData & input_data)
 {
   setTrajectory(input_data.current_trajectory, input_data.current_odometry);
@@ -407,11 +387,6 @@ bool MpcLateralController::isStoppedState() const
   const double current_vel = m_current_kinematic_state.twist.twist.linear.x;
   const double target_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps;
 
-  const auto latest_published_cmd = m_ctrl_cmd_prev;  // use prev_cmd as a latest published command
-  if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) {
-    return false;  // not stopState: keep control
-  }
-
   if (
     std::fabs(current_vel) < m_stop_state_entry_ego_speed &&
     std::fabs(target_vel) < m_stop_state_entry_target_speed) {

diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md
@@ -160,23 +160,22 @@ Then, the velocity can be calculated by providing the current error and time ste
 The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the
 AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 
-| Name                                        | Type   | Description                                                                                                                                                                             | Default value |
-| :------------------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| delay_compensation_time                     | double | delay for longitudinal control [s]                                                                                                                                                      | 0.17          |
-| enable_smooth_stop                          | bool   | flag to enable transition to STOPPING                                                                                                                                                   | true          |
-| enable_overshoot_emergency                  | bool   | flag to enable transition to EMERGENCY when the ego is over the stop line with a certain distance. See `emergency_state_overshoot_stop_dist`.                                           | true          |
-| enable_large_tracking_error_emergency       | bool   | flag to enable transition to EMERGENCY when the closest trajectory point search is failed due to a large deviation between trajectory and ego pose.                                     | true          |
-| enable_slope_compensation                   | bool   | flag to modify output acceleration for slope compensation. The source of the slope angle can be selected from ego-pose or trajectory angle. See `use_trajectory_for_pitch_calculation`. | true          |
-| enable_brake_keeping_before_stop            | bool   | flag to keep a certain acceleration during DRIVE state before the ego stops. See [Brake keeping](#brake-keeping).                                                                       | false         |
-| enable_keep_stopped_until_steer_convergence | bool   | flag to keep stopped condition until until the steer converges.                                                                                                                         | true          |
-| max_acc                                     | double | max value of output acceleration [m/s^2]                                                                                                                                                | 3.0           |
-| min_acc                                     | double | min value of output acceleration [m/s^2]                                                                                                                                                | -5.0          |
-| max_jerk                                    | double | max value of jerk of output acceleration [m/s^3]                                                                                                                                        | 2.0           |
-| min_jerk                                    | double | min value of jerk of output acceleration [m/s^3]                                                                                                                                        | -5.0          |
-| use_trajectory_for_pitch_calculation        | bool   | If true, the slope is estimated from trajectory z-level. Otherwise the pitch angle of the ego pose is used.                                                                             | false         |
-| lpf_pitch_gain                              | double | gain of low-pass filter for pitch estimation                                                                                                                                            | 0.95          |
-| max_pitch_rad                               | double | max value of estimated pitch [rad]                                                                                                                                                      | 0.1           |
-| min_pitch_rad                               | double | min value of estimated pitch [rad]                                                                                                                                                      | -0.1          |
+| Name                                  | Type   | Description                                                                                                                                                                             | Default value |
+| :------------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| delay_compensation_time               | double | delay for longitudinal control [s]                                                                                                                                                      | 0.17          |
+| enable_smooth_stop                    | bool   | flag to enable transition to STOPPING                                                                                                                                                   | true          |
+| enable_overshoot_emergency            | bool   | flag to enable transition to EMERGENCY when the ego is over the stop line with a certain distance. See `emergency_state_overshoot_stop_dist`.                                           | true          |
+| enable_large_tracking_error_emergency | bool   | flag to enable transition to EMERGENCY when the closest trajectory point search is failed due to a large deviation between trajectory and ego pose.                                     | true          |
+| enable_slope_compensation             | bool   | flag to modify output acceleration for slope compensation. The source of the slope angle can be selected from ego-pose or trajectory angle. See `use_trajectory_for_pitch_calculation`. | true          |
+| enable_brake_keeping_before_stop      | bool   | flag to keep a certain acceleration during DRIVE state before the ego stops. See [Brake keeping](#brake-keeping).                                                                       | false         |
+| max_acc                               | double | max value of output acceleration [m/s^2]                                                                                                                                                | 3.0           |
+| min_acc                               | double | min value of output acceleration [m/s^2]                                                                                                                                                | -5.0          |
+| max_jerk                              | double | max value of jerk of output acceleration [m/s^3]                                                                                                                                        | 2.0           |
+| min_jerk                              | double | min value of jerk of output acceleration [m/s^3]                                                                                                                                        | -5.0          |
+| use_trajectory_for_pitch_calculation  | bool   | If true, the slope is estimated from trajectory z-level. Otherwise the pitch angle of the ego pose is used.                                                                             | false         |
+| lpf_pitch_gain                        | double | gain of low-pass filter for pitch estimation                                                                                                                                            | 0.95          |
+| max_pitch_rad                         | double | max value of estimated pitch [rad]                                                                                                                                                      | 0.1           |
+| min_pitch_rad                         | double | min value of estimated pitch [rad]                                                                                                                                                      | -0.1          |
 
 ### State transition
 

diff --git a/...ngitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/...ngitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp
@@ -121,7 +121,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   bool m_enable_overshoot_emergency;
   bool m_enable_slope_compensation;
   bool m_enable_large_tracking_error_emergency;
-  bool m_enable_keep_stopped_until_steer_convergence;
 
   // smooth stop transition
   struct StateTransitionParams

diff --git a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml
@@ -6,7 +6,6 @@
     enable_overshoot_emergency: true
     enable_large_tracking_error_emergency: true
     enable_slope_compensation: false
-    enable_keep_stopped_until_steer_convergence: true
 
     # state transition
     drive_state_stop_dist: 0.5

diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -49,8 +49,6 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
   m_enable_large_tracking_error_emergency =
     node.declare_parameter<bool>("enable_large_tracking_error_emergency");
   m_enable_slope_compensation = node.declare_parameter<bool>("enable_slope_compensation");
-  m_enable_keep_stopped_until_steer_convergence =
-    node.declare_parameter<bool>("enable_keep_stopped_until_steer_convergence");
 
   // parameters for state transition
   {
@@ -500,11 +498,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
   // NOTE: the same velocity threshold as 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;
-
   const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
 
   const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel &&
@@ -563,7 +556,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
       return changeState(ControlState::EMERGENCY);
     }
 
-    if (!is_under_control && stopped_condition && keep_stopped_condition) {
+    if (!is_under_control && 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.
       return changeState(ControlState::STOPPED);
@@ -614,14 +607,8 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     if (has_nonzero_target_vel && !departure_condition_from_stopped) {
       info_throttle("target speed > 0, but departure condition is not met. Keep STOPPED.");
     }
-    if (has_nonzero_target_vel && keep_stopped_condition) {
-      info_throttle("target speed > 0, but keep stop condition is met. Keep STOPPED.");
-    }
     // ---------------
 
-    if (keep_stopped_condition) {
-      return changeState(ControlState::STOPPED);
-    }
     if (departure_condition_from_stopped) {
       m_pid_vel.reset();
       m_lpf_vel_error->reset(0.0);

diff --git a/control/pure_pursuit/README.md b/control/pure_pursuit/README.md
@@ -15,5 +15,5 @@ Return LateralOutput which contains the following to the controller node
 
 - `autoware_auto_control_msgs/AckermannLateralCommand`: target steering angle
 - LateralSyncData
-  - steer angle convergence
+  - controller can create predicted path successfully or not
 - `autoware_auto_planning_msgs/Trajectory`: predicted path for ego vehicle
diff --git a/control/pure_pursuit/config/pure_pursuit.param.yaml b/control/pure_pursuit/config/pure_pursuit.param.yaml
@@ -6,7 +6,6 @@
     long_ld_lateral_error_threshold: 0.5
     min_lookahead_distance: 4.35
     max_lookahead_distance: 15.0
-    converged_steer_rad: 0.1
     reverse_min_lookahead_distance: 7.0
     prediction_ds: 0.3
     prediction_distance_length: 21.0

diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp
@@ -82,7 +82,6 @@ struct Param
   double min_lookahead_distance;
   double max_lookahead_distance;
   double reverse_min_lookahead_distance;  // min_lookahead_distance in reverse gear
-  double converged_steer_rad_;
   double prediction_ds;
   double prediction_distance_length;  // Total distance of prediction trajectory
   double resampling_ds;
@@ -162,8 +161,6 @@ class PurePursuitLateralController : public LateralControllerBase
 
   AckermannLateralCommand generateOutputControlCmd();
 
-  bool calcIsSteerConverged(const AckermannLateralCommand & cmd);
-
   double calcLookaheadDistance(
     const double lateral_error, const double curvature, const double velocity, const double min_ld,
     const bool is_control_cmd);

diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp
@@ -79,7 +79,6 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node)
   param_.max_lookahead_distance = node.declare_parameter<double>("max_lookahead_distance");
   param_.reverse_min_lookahead_distance =
     node.declare_parameter<double>("reverse_min_lookahead_distance");
-  param_.converged_steer_rad_ = node.declare_parameter<double>("converged_steer_rad");
   param_.prediction_ds = node.declare_parameter<double>("prediction_ds");
   param_.prediction_distance_length = node.declare_parameter<double>("prediction_distance_length");
   param_.resampling_ds = node.declare_parameter<double>("resampling_ds");
@@ -348,11 +347,6 @@ LateralOutput PurePursuitLateralController::run(const InputData & input_data)
   if (param_.enable_path_smoothing) {
     averageFilterTrajectory(*trajectory_resampled_);
   }
-  const auto cmd_msg = generateOutputControlCmd();
-
-  LateralOutput output;
-  output.control_cmd = cmd_msg;
-  output.sync_data.is_steer_converged = calcIsSteerConverged(cmd_msg);
 
   // calculate predicted trajectory with iterative calculation
   const auto predicted_trajectory = generatePredictedTrajectory();
@@ -362,13 +356,14 @@ LateralOutput PurePursuitLateralController::run(const InputData & input_data)
     pub_predicted_trajectory_->publish(*predicted_trajectory);
   }
 
-  return output;
-}
+  const auto cmd_msg = generateOutputControlCmd();
 
-bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCommand & cmd)
-{
-  return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) <
-         static_cast<float>(param_.converged_steer_rad_);
+  LateralOutput output;
+  output.control_cmd = cmd_msg;
+  output.sync_data.is_controller_ready_to_move =
+    predicted_trajectory.has_value();  // If not, not ready.
+
+  return output;
 }
 
 AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd()