Skip to content

Commit

Permalink
feat(longitudinal_controller): add disable emergency option (autoware…
Browse files Browse the repository at this point in the history 
…foundation#1201)

* feat(longitudinal_controller): add disable emergency option

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update readme

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add param

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
  • Loading branch information
@TakaHoribe
TakaHoribe authored and yukke42 committed Oct 14, 2022
1 parent 4b12b32 commit 611b6d2
Show file tree
Hide file tree
Showing 5 changed files with 25 additions and 17 deletions.
33 changes: 17 additions & 16 deletions control/trajectory_follower_nodes/design/longitudinal_controller-design.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,22 +146,23 @@ In this controller, the predicted ego-velocity and the target velocity after the
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 |
| :----------------------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
| longitudinal_ctrl_period | double | longitudinal control period [s] | 0.03 |
| 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_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 |
| Name | Type | Description | Default value |
| :------------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
| longitudinal_ctrl_period | double | longitudinal control period [s] | 0.03 |
| 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: 1 addition & 0 deletions ...jectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node
bool8_t m_enable_smooth_stop;
bool8_t m_enable_overshoot_emergency;
bool8_t m_enable_slope_compensation;
bool8_t m_enable_large_tracking_error_emergency;

// smooth stop transition
struct StateTransitionParams
Expand Down
1 change: 1 addition & 0 deletions control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

enable_smooth_stop: true
enable_overshoot_emergency: true
enable_large_tracking_error_emergency: true
enable_slope_compensation: false

# state transition
Expand Down
6 changes: 5 additions & 1 deletion control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@ LongitudinalController::LongitudinalController(const rclcpp::NodeOptions & node_
// parameters to enable functions
m_enable_smooth_stop = declare_parameter<bool8_t>("enable_smooth_stop");
m_enable_overshoot_emergency = declare_parameter<bool8_t>("enable_overshoot_emergency");
m_enable_large_tracking_error_emergency =
declare_parameter<bool8_t>("enable_large_tracking_error_emergency");
m_enable_slope_compensation = declare_parameter<bool8_t>("enable_slope_compensation");

// parameters for state transition
Expand Down Expand Up @@ -371,7 +373,9 @@ void LongitudinalController::callbackTimerControl()

// self pose is far from trajectory
if (control_data.is_far_from_trajectory) {
m_control_state = ControlState::EMERGENCY; // update control state
if (m_enable_large_tracking_error_emergency) {
m_control_state = ControlState::EMERGENCY; // update control state
}
const Motion raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); // calculate control command
m_prev_raw_ctrl_cmd = raw_ctrl_cmd;
publishCtrlCmd(raw_ctrl_cmd, control_data.current_motion.vel); // publish control command
Expand Down
1 change: 1 addition & 0 deletions launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

enable_smooth_stop: true
enable_overshoot_emergency: true
enable_large_tracking_error_emergency: true
enable_slope_compensation: false

# state transition
Expand Down

0 comments on commit 611b6d2

Please sign in to comment.