fix(vehicle_cmd_gate): fix the problems while transition from/to stopped state

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

Author: brkay54

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -430,13 +430,17 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
 
   // Check engage
   if (!is_engaged_) {
-    filtered_commands.control = createStopControlCmd();
+    filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
   }
 
   // Check pause. Place this check after all other checks as it needs the final output.
   adapi_pause_->update(filtered_commands.control);
   if (adapi_pause_->is_paused()) {
-    filtered_commands.control = createStopControlCmd();
+    if (is_engaged_) {
+      filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
+    } else {
+      filtered_commands.control = createStopControlCmd();
+    }
   }
 
   // Check if command filtering option is enable
@@ -599,6 +603,17 @@ AckermannControlCommand VehicleCmdGate::createStopControlCmd() const
   return cmd;
 }
 
+LongitudinalCommand VehicleCmdGate::createLongitudinalStopControlCmd() const
+{
+  LongitudinalCommand cmd;
+  const auto t = this->now();
+  cmd.stamp = t;
+  cmd.speed = 0.0;
+  cmd.acceleration = stop_hold_acceleration_;
+
+  return cmd;
+}
+
 AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd() const
 {
   AckermannControlCommand cmd;

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -55,6 +55,7 @@ namespace vehicle_cmd_gate
 using autoware_adapi_v1_msgs::msg::MrmState;
 using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_auto_control_msgs::msg::AckermannControlCommand;
+using autoware_auto_control_msgs::msg::LongitudinalCommand;
 using autoware_auto_vehicle_msgs::msg::GearCommand;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::SteeringReport;
@@ -220,6 +221,7 @@ class VehicleCmdGate : public rclcpp::Node
   // Algorithm
   AckermannControlCommand prev_control_cmd_;
   AckermannControlCommand createStopControlCmd() const;
+  LongitudinalCommand createLongitudinalStopControlCmd() const;
   AckermannControlCommand createEmergencyStopControlCmd() const;
 
   std::shared_ptr<rclcpp::Time> prev_time_;