From e480966518e704d6bbcee4b6f1db6d5969e39a4a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 18 Oct 2023 21:04:44 +0900 Subject: [PATCH] feat(pid_longitudinal_controller): transit to STOPPED even if the sign of the ego velocity goes opposite (#5255) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(pid_longitudinal_controller): transit to STOPPED even if the sign of the ego velocity goes opposite Signed-off-by: Takayuki Murooka * pid_longitudinal_controller.cpp を更新 Co-authored-by: Takamasa Horibe * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka Co-authored-by: Takamasa Horibe --- .../src/pid_longitudinal_controller.cpp | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 83c02d5c913e1..f088a3a5cee56 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -493,9 +493,26 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged; const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; - if ( - std::fabs(current_vel) > p.stopped_state_entry_vel || - std::fabs(current_acc) > p.stopped_state_entry_acc) { + + 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(node_->now()); } const bool stopped_condition =