feat(vehicle_cmd_gate): improve transition filter for longitudinal (a…

…utowarefoundation#4334)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -521,15 +521,15 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
     (mode.mode == OperationModeState::AUTONOMOUS && mode.is_autoware_control_enabled);
   auto prev_values = in_autonomous ? out : current_status_cmd;
 
+  if (ego_is_stopped) {
+    prev_values.longitudinal = out.longitudinal;
+  }
+
   // TODO(Horibe): To prevent sudden acceleration/deceleration when switching from manual to
   // autonomous, the filter should be applied for actual speed and acceleration during manual
   // driving. However, this means that the output command from Gate will always be close to the
-  // driving state during manual driving. Since the Gate's output is checked by various modules as
-  // the intended value of Autoware, it should be closed to planned values. Conversely, it is
-  // undesirable for the target vehicle speed to be non-zero in a situation where the vehicle is
-  // supposed to stop. Until the appropriate handling will be done, previous value is used for the
-  // filter in manual mode.
-  prev_values.longitudinal = out.longitudinal;  // TODO(Horibe): to be removed
+  // driving state during manual driving. Here, let autoware publish the stop command when the ego
+  // is stopped to intend the autoware is trying to keep stopping.
 
   filter_.setPrevCmd(prev_values);
   filter_on_transition_.setPrevCmd(prev_values);