From ec173abfd803399d797a2280ffbc9b5308b64f0a Mon Sep 17 00:00:00 2001 From: Azumi Suzuki <38061530+s-azumi@users.noreply.github.com> Date: Wed, 19 Oct 2022 09:33:40 +0900 Subject: [PATCH] fix(trajectory_follower): fixed stop distance calculation in transitioning to emergency_stop (#1915) (#157) Signed-off-by: Mamoru Sobue Signed-off-by: Mamoru Sobue Co-authored-by: Mamoru Sobue --- .../src/pid_longitudinal_controller.cpp | 9 +- ...udinalControllerStateTransition.drawio.svg | 352 +----------------- 2 files changed, 11 insertions(+), 350 deletions(-) diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index 1a6f3601090b6..c95e15bc800db 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -498,8 +498,13 @@ PidLongitudinalController::ControlState PidLongitudinalController::updateControl ? (node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time : false; - const bool8_t emergency_condition = - m_enable_overshoot_emergency && stop_dist < -p.emergency_state_overshoot_stop_dist; + static constexpr double vel_epsilon = + 1e-3; // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity + const float64_t current_vel_cmd = + std::fabs(m_trajectory_ptr->points.at(control_data.nearest_idx).longitudinal_velocity_mps); + const bool8_t emergency_condition = m_enable_overshoot_emergency && + stop_dist < -p.emergency_state_overshoot_stop_dist && + current_vel_cmd < vel_epsilon; // transit state if (current_control_state == ControlState::DRIVE) { diff --git a/control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg b/control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg index 94706f229733e..e391161723048 100644 --- a/control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg +++ b/control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg @@ -1,348 +1,4 @@ - - - - - - - - - - - - -
-
-
- - - 0. DRIVE - - -
-
-
- - v = interpolateTargetVel() - -
-
- - a = applyVelocityFeedback() - -
-
-
-
- - - 0. DRIVE... - - - - - - - - - - - -
-
-
- - - 1. STOPPING - -
- -
-
- - v = stopped_vel_ - -
-
- - a = smooth_stop_.calculate() - -
-
-
-
- - - 1. STOPPING... - - - - - - - - - -
-
-
- - 2. STOPPED -
- -
-
- - v = stopped_vel_ - -
-
- - a = stopped_acc_ - -
-
-
-
- - - 2. STOPPED... - - - - - - - - - -
-
-
- - 3. EMERGENCY -
- -
-
- - v = emergency_vel_ - -
-
- - a = emergency_acc_ - -
-
-
-
- - - 3. EMERGENCY... - - - - - - - -
-
-
- stop condition -
-
-
- - - stop condition - - - - - - - -
-
-
-

- - strict departure condition - - : Distance to stop is larger than A1. -
- - loose departure condition - - : Distance to stop is larger than A1 + A2. -
- - stopping condition - - : Distance to stop is smaller than B1. -
- - stop condition - - : For 500 [ms], self velocity is smaller than C1, and self acceleration is smaller than C2. -
- - emergency condition - - - : Distance to stop is smaller than D1, or self pose is not within D2, D3 from trajectory. - -

-
-
-
- - - strict departure condition : Distance to stop is larger than... - - - - - - - -
-
-
- strict departure -
- condition -
-
-
- - - strict departure... - - - - - - - -
-
-
- stopping -
- condition -
-
-
- - - stopping... - - - - - - - -
-
-
- loose departure -
- condition -
-
-
- - - loose departure... - - - - - - - -
-
-
- emergency condition -
-
-
- - - emergency con... - - - - - - - -
-
-
- - stop condition - -
- and -
- not - - emergency condition - -
-
-
- - - stop condition... - - - - - - - -
-
-
-

- A1 : drive_state_stop_dist -
- A2 : drive_state_offset_stop_dist -
- B1 : stopping_state_stop_dist -
- C1 : stopped_state_entry_vel -
- C2 : stopped_state_entry_acc -
- D1 : emergency_state_overshoot_stop_dist -
- D2 : emergency_state_traj_trans_dev -
- D3 : emergency_state_traj_rot_dev -

-
-
-
- - - A1 : drive_state_stop_dist... - - - - - - - - - Viewer does not support full SVG 1.1 - - - - + + + +
0. DRIVE

v = interpolateTargetVel()
a = applyVelocityFeedback() 
0. DRIVE...
1. STOPPING

v = stopped_vel_
a = smooth_stop_.calculate()
1. STOPPING...
2. STOPPED

v = stopped_vel_
a = stopped_acc_
2. STOPPED...
3. EMERGENCY

v = emergency_vel_
a = emergency_acc_
3. EMERGENCY...
stop condition
stop condition

strict departure condition : Distance to stop is larger than A1.
loose departure condition : Distance to stop is larger than A1 + A2.
stopping condition : Distance to stop is smaller than B1.
stop condition : For 500 [ms], self velocity is smaller than C1, and self acceleration is smaller than C2.
emergency condition : Speed command is zero velocity, and distance to stop is smaller than D1, or self pose is not within D2, D3 from trajectory.

strict departure condition : Distance to stop is larger than...
strict departure
condition
strict departure...
stopping
condition
stopping...
loose departure
condition
loose departure...
emergency condition
emergency con...
stop condition
and
not emergency condition
stop condition...

A1 : drive_state_stop_dist
A2 : drive_state_offset_stop_dist
B1 : stopping_state_stop_dist
C1 : stopped_state_entry_vel
C2 : stopped_state_entry_acc
D1 : emergency_state_overshoot_stop_dist
D2 : emergency_state_traj_trans_dev
D3 : emergency_state_traj_rot_dev

A1 : drive_state_stop_dist...Text is not SVG - cannot display \ No newline at end of file