feat(trajectory_follower): reset previous acceleration when the car s…

…tarts moving (tier4#303) Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
boyali · Jan 27, 2022 · 4c778ba · 4c778ba
1 parent 45446dd
commit 4c778ba
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp
@@ -532,12 +532,16 @@ LongitudinalController::ControlState LongitudinalController::updateControlState(
     if (departure_condition_from_stopping) {
       m_pid_vel.reset();
       m_lpf_vel_error->reset(0.0);
+      // prevent the car from taking a long time to start to move
+      m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc);
       return ControlState::DRIVE;
     }
   } else if (current_control_state == ControlState::STOPPED) {
     if (departure_condition_from_stopped) {
       m_pid_vel.reset();
       m_lpf_vel_error->reset(0.0);
+      // prevent the car from taking a long time to start to move
+      m_prev_ctrl_cmd.acc = std::max(0.0, m_prev_ctrl_cmd.acc);
       return ControlState::DRIVE;
     }
   } else if (m_control_state == ControlState::EMERGENCY) {