From 04635407cb695346ec72a6c3114e89638bb98c53 Mon Sep 17 00:00:00 2001
From: Berkay Karaman <brkay54@gmail.com>
Date: Fri, 9 Feb 2024 14:31:22 +0300
Subject: [PATCH] fix(vehicle_cmd_gate): fix the problems while transition
 from/to stopped state (#5183)

---
 .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 19 +++++++++++++++++--
 .../vehicle_cmd_gate/src/vehicle_cmd_gate.hpp |  2 ++
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
index 1bed4ee67f412..aec2ede53d9e7 100644
--- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
+++ b/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;
diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
index c36aab240ae79..b79c58d120443 100644
--- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
+++ b/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_;