removed unnecessary stop state params

Signed-off-by: Berkay Karaman <[email protected]>
autowarefoundation · Dec 11, 2022 · a5e6378 · a5e6378
1 parent e2c1e1a
commit a5e6378
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 7 deletions.
diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp
@@ -98,8 +98,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   double m_traj_resample_dist;
 
   /* parameters for stop state */
-  double m_stop_state_entry_ego_speed;
-  double m_stop_state_entry_target_speed;
+  double m_stop_state_steering_change_threshold;  // [rad/s]
   double m_converged_steer_rad;
   double m_new_traj_duration_time;  // check trajectory shape change
   double m_new_traj_end_dist;       // check trajectory shape change

diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp
@@ -67,9 +67,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
   m_mpc.m_param.steer_tau = node_->declare_parameter<double>("vehicle_model_steer_tau");
 
   /* stop state parameters */
-  m_stop_state_entry_ego_speed = node_->declare_parameter<double>("stop_state_entry_ego_speed");
-  m_stop_state_entry_target_speed =
-    node_->declare_parameter<double>("stop_state_entry_target_speed");
+  m_stop_state_steering_change_threshold =
+    node_->declare_parameter<double>("stop_state_steering_change_threshold");
   m_converged_steer_rad = node_->declare_parameter<double>("converged_steer_rad");
   m_keep_steer_control_until_converged =
     node_->declare_parameter<bool>("keep_steer_control_until_converged");
@@ -246,11 +245,10 @@ bool MpcLateralController::isSteerConverged(
   }
 
   const auto isCmdChanging = [this](const auto & cmd) {
-    constexpr double min_steer_rate_treshold = 0.001;  // [rad/s]
     const double steer_cmd_diff =
       std::abs(cmd.steering_tire_angle - m_ctrl_cmd_prev.steering_tire_angle);
     const double steer_cmd_rate = steer_cmd_diff / m_mpc.m_ctrl_period;
-    return steer_cmd_rate > min_steer_rate_treshold;  // need to consider which thresh is reasonable
+    return steer_cmd_rate > m_stop_state_steering_change_threshold;
   };
 
   const bool is_converged =