refactor(control): Remove unused variables and functions

Signed-off-by: Berkay Karaman <[email protected]>

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
 
@@ -104,8 +105,6 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 | admissible_yaw_error_rad                     | double | stop vehicle when following yaw angle error is larger than this value [rad].                                                                         | 1.57          |
 | stop_state_entry_ego_speed <sup>\*1</sup>    | double | threshold value of the ego vehicle speed used to the stop state entry condition                                                                      | 0.0           |
 | stop_state_entry_target_speed <sup>\*1</sup> | double | threshold value of the target speed used to the stop state entry condition                                                                           | 0.0           |
-| converged_steer_rad                          | double | threshold value of the steer convergence                                                                                                             | 0.1           |
-| keep_steer_control_until_converged           | bool   | keep steer control until steer is converged                                                                                                          | true          |
 | new_traj_duration_time                       | double | threshold value of the time to be considered as new trajectory                                                                                       | 1.0           |
 | new_traj_end_dist                            | double | threshold value of the distance between trajectory ends to be considered as new trajectory                                                           | 0.3           |
 | mpc_converged_threshold_rps                  | double | threshold value to be sure output of the optimization is converged, it is used in stopped state                                                      | 0.3           |

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;
 
   /**

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]

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) {

control/pid_longitudinal_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

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

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
   {
@@ -495,8 +493,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist;
   const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist;
 
-  const bool keep_stopped_condition =
-    m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged;
+  const bool keep_stopped_condition = !lateral_sync_data_.is_controller_ready_to_move;
 
   const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
   if (

control/pure_pursuit/README.md

@@ -15,5 +15,6 @@ Return LateralOutput which contains the following to the controller node
 
 - `autoware_auto_control_msgs/AckermannLateralCommand`: target steering angle
 - LateralSyncData
-  - steer angle convergence
+  - can controller create predicted path for ego vehicle?
+    - if not, the vehicle should stop until the controller can create the predicted path
 - `autoware_auto_planning_msgs/Trajectory`: predicted path for ego vehicle

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

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);

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()

control/trajectory_follower_base/README.md

@@ -19,8 +19,8 @@ The interface class has the following base functions.
 - `isReady()`: Check if the control is ready to compute.
 - `run()`: Compute control commands and return to [Trajectory Follower Nodes](../trajectory_follower_node/README.md). This must be implemented by inherited algorithms.
 - `sync()`: Input the result of running the other controller.
-  - steer angle convergence
-    - allow keeping stopped until steer is converged.
+  - is lateral controller ready to move?
+    - allow keeping stopped until lateral controller output is stable.
   - velocity convergence(currently not used)
 
 See [the Design of Trajectory Follower Nodes](../trajectory_follower_node/README.md#Design) for how these functions work in the node.

control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp

@@ -19,7 +19,7 @@ namespace autoware::motion::control::trajectory_follower
 {
 struct LateralSyncData
 {
-  bool is_steer_converged{false};
+  bool is_controller_ready_to_move{false};
 };
 
 struct LongitudinalSyncData

control/trajectory_follower_base/src/longitudinal_controller_base.cpp

@@ -22,6 +22,6 @@ void LongitudinalControllerBase::sync(LateralSyncData const & lateral_sync_data)
 }
 void LongitudinalControllerBase::reset()
 {
-  lateral_sync_data_.is_steer_converged = false;
+  lateral_sync_data_.is_controller_ready_to_move = false;
 }
 }  // namespace autoware::motion::control::trajectory_follower

control/trajectory_follower_node/README.md

@@ -39,7 +39,7 @@ steering
 accel
 }
 struct LongitudinalSyncData {
-is_steer_converged
+is_controller_ready_to_move
 }
 struct LateralSyncData {
 }
@@ -118,13 +118,6 @@ lateral_controller_->sync(lon_out.sync_data);
 control_cmd_pub_->publish(out);
 ```
 
-Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true`
-
-- lateral controller
-  - `keep_steer_control_until_converged`
-- longitudinal controller
-  - `enable_keep_stopped_until_steer_convergence`
-
 ### Inputs / Outputs / API
 
 #### Inputs

control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp

@@ -78,7 +78,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
   rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
     control_cmd_pub_;
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
 
   autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_;
   nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_;
@@ -109,9 +108,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const;
   LongitudinalControllerMode getLongitudinalControllerMode(
     const std::string & algorithm_name) const;
-  void publishDebugMarker(
-    const trajectory_follower::InputData & input_data,
-    const trajectory_follower::LateralOutput & lat_out) const;
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 };

control/trajectory_follower_node/param/lateral/mpc.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]

control/trajectory_follower_node/param/lateral/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

control/trajectory_follower_node/src/controller_node.cpp

@@ -75,8 +75,6 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
     [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; });
   control_cmd_pub_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "~/output/control_cmd", rclcpp::QoS{1}.transient_local());
-  debug_marker_pub_ =
-    create_publisher<visualization_msgs::msg::MarkerArray>("~/output/debug_marker", rclcpp::QoS{1});
 
   // Timer
   {
@@ -221,39 +219,7 @@ void Controller::callbackTimerControl()
   out.lateral = lat_out.control_cmd;
   out.longitudinal = lon_out.control_cmd;
   control_cmd_pub_->publish(out);
-
-  // 6. publish debug marker
-  publishDebugMarker(*input_data, lat_out);
 }
-
-void Controller::publishDebugMarker(
-  const trajectory_follower::InputData & input_data,
-  const trajectory_follower::LateralOutput & lat_out) const
-{
-  visualization_msgs::msg::MarkerArray debug_marker_array{};
-
-  // steer converged marker
-  {
-    auto marker = tier4_autoware_utils::createDefaultMarker(
-      "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
-      tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0),
-      tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99));
-    marker.pose = input_data.current_odometry.pose.pose;
-
-    std::stringstream ss;
-    const double current = input_data.current_steering.steering_tire_angle;
-    const double cmd = lat_out.control_cmd.steering_tire_angle;
-    const double diff = current - cmd;
-    ss << "current:" << current << " cmd:" << cmd << " diff:" << diff
-       << (lat_out.sync_data.is_steer_converged ? " (converged)" : " (not converged)");
-    marker.text = ss.str();
-
-    debug_marker_array.markers.push_back(marker);
-  }
-
-  debug_marker_pub_->publish(debug_marker_array);
-}
-
 }  // namespace autoware::motion::control::trajectory_follower_node
 
 #include "rclcpp_components/register_node_macro.hpp"