Skip to content

Commit

Permalink
fix(pid_longitudinal_control): not check steering convergence when mo…
Browse files Browse the repository at this point in the history
…ving

Signed-off-by: Takamasa Horibe <[email protected]>
  • Loading branch information
TakaHoribe committed Nov 1, 2023
1 parent 2dea1c3 commit 827959e
Showing 1 changed file with 7 additions and 4 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -489,8 +489,13 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist;
const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist;

const bool keep_stopped_condition =
m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged;
// 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;
if (
Expand All @@ -503,8 +508,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
? (node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time
: false;

static constexpr double vel_epsilon =
1e-3; // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity
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 &&
Expand Down

0 comments on commit 827959e

Please sign in to comment.