Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(control): Remove unused variables and functions #5189

Closed
wants to merge 2 commits into from
Closed

refactor(control): Remove unused variables and functions #5189

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3a0d19f
fix(vehicle_cmd_gate): fix the problems while transition from/to stop…
brkay54 Sep 29, 2023
5979dfa
refactor(control): Remove unused variables and functions
brkay54 Nov 11, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion control/mpc_lateral_controller/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,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

Expand Down
13 changes: 0 additions & 13 deletions control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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;

Expand Down Expand Up @@ -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;

/**
Expand Down
2 changes: 0 additions & 2 deletions control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
31 changes: 3 additions & 28 deletions control/mpc_lateral_controller/src/mpc_lateral_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,8 +60,6 @@
/* 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]
Expand Down Expand Up @@ -271,13 +269,11 @@
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();

Check notice on line 276 in control/mpc_lateral_controller/src/mpc_lateral_controller.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

MpcLateralController::run decreases in cyclomatic complexity from 10 to 9, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

return output;
};
Expand All @@ -301,22 +297,6 @@
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);
Expand Down Expand Up @@ -407,11 +387,6 @@
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) {
Expand Down
33 changes: 16 additions & 17 deletions control/pid_longitudinal_controller/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -165,23 +165,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

Expand Down
1 change: 0 additions & 1 deletion ...ngitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,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
Expand Down
1 change: 0 additions & 1 deletion control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
15 changes: 1 addition & 14 deletions control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc. All rights reserved.

Check notice on line 1 in control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Overall Code Complexity 

The mean cyclomatic complexity decreases from 4.29 to 4.07, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -48,9 +48,7 @@
m_enable_overshoot_emergency = node.declare_parameter<bool>("enable_overshoot_emergency");
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");

Check notice on line 51 in control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Large Method 

diagnostic_updater_ decreases from 120 to 118 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
m_enable_keep_stopped_until_steer_convergence =
node.declare_parameter<bool>("enable_keep_stopped_until_steer_convergence");

// parameters for state transition
{
Expand Down Expand Up @@ -505,76 +503,71 @@

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

const double current_vel_cmd =
std::fabs(m_trajectory.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 info_throttle = [this](const auto & s) {
RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%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;
}
// 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) {
if (!is_under_control && stopped_condition) {

Check notice on line 570 in control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Complex Conditional 

PidLongitudinalController::updateControlState no longer has a complex conditional
// 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);
Expand Down Expand Up @@ -625,14 +618,8 @@
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.");
}
// ---------------

Check notice on line 622 in control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

PidLongitudinalController::updateControlState decreases in cyclomatic complexity from 41 to 35, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
if (keep_stopped_condition) {
return changeState(ControlState::STOPPED);
}
if (departure_condition_from_stopped) {
m_pid_vel.reset();
m_lpf_vel_error->reset(0.0);
Expand Down
2 changes: 1 addition & 1 deletion control/pure_pursuit/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
1 change: 0 additions & 1 deletion control/pure_pursuit/config/pure_pursuit.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
3 changes: 0 additions & 3 deletions control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down
19 changes: 7 additions & 12 deletions control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down Expand Up @@ -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();
Expand All @@ -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()
Expand Down
Loading
Loading