feat(operation_mode_transition_manager): transition to auto quickly w…

…hen vehicle stops (autowarefoundation#2427)

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

control/operation_mode_transition_manager/src/state.cpp

@@ -83,6 +83,14 @@ bool AutonomousMode::isModeChangeCompleted()
   constexpr auto dist_max = 5.0;
   constexpr auto yaw_max = M_PI_4;
 
+  const auto current_speed = kinematics_.twist.twist.linear.x;
+  const auto & param = engage_acceptable_param_;
+
+  // Engagement completes quickly if the vehicle is stopped.
+  if (param.allow_autonomous_in_stopped && std::abs(current_speed) < 0.01) {
+    return true;
+  }
+
   const auto unstable = [this]() {
     stable_start_time_.reset();
     return false;