diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
index 532a837a18422..72eb549ebf012 100644
--- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
+++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
@@ -135,6 +135,7 @@ class EmergencyHandler : public rclcpp::Node
   void checkHazardStatusTimeout();
 
   // Algorithm
+  uint8_t last_gear_command_{autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE};
   void transitionTo(const int new_state);
   void updateMrmState();
   void operateMrm();
diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
index d711f8eb22170..da9d18deb3079 100644
--- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
+++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
@@ -167,22 +167,15 @@ void EmergencyHandler::publishControlCommands()
   {
     GearCommand msg;
     msg.stamp = stamp;
-
-    if (isStopped()) {
-      if (param_.use_parking_after_stopped) {
-        msg.command = GearCommand::PARK;
-        pub_gear_cmd_->publish(msg);
-      }
-      return;
-    }
-
-    if (isDrivingBackwards()) {
-      msg.command = GearCommand::REVERSE;
-      pub_gear_cmd_->publish(msg);
-      return;
-    }
-
-    msg.command = GearCommand::DRIVE;
+    const auto command = [&]() {
+      // If stopped and use_parking is not true, send the last gear command
+      if (isStopped())
+        return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_;
+      return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE;
+    }();
+
+    msg.command = command;
+    last_gear_command_ = msg.command;
     pub_gear_cmd_->publish(msg);
     return;
   }
diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
index d51ba3030d3d2..50788ce9c2034 100644
--- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
+++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
@@ -149,6 +149,7 @@ class MrmHandler : public rclcpp::Node
 
   // Algorithm
   bool is_emergency_holding_ = false;
+  uint8_t last_gear_command_{autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE};
   void transitionTo(const int new_state);
   void updateMrmState();
   void operateMrm();
diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
index 3b4c450a52ae2..0d4e3c4542821 100644
--- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
+++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
@@ -191,22 +191,15 @@ void MrmHandler::publishGearCmd()
   GearCommand msg;
 
   msg.stamp = this->now();
-
-  if (isStopped()) {
-    if (param_.use_parking_after_stopped) {
-      msg.command = GearCommand::PARK;
-      pub_gear_cmd_->publish(msg);
-    }
-    return;
-  }
-
-  if (isDrivingBackwards()) {
-    msg.command = GearCommand::REVERSE;
-    pub_gear_cmd_->publish(msg);
-    return;
-  }
-
-  msg.command = GearCommand::DRIVE;
+  const auto command = [&]() {
+    // If stopped and use_parking is not true, send the last gear command
+    if (isStopped())
+      return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_;
+    return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE;
+  }();
+
+  msg.command = command;
+  last_gear_command_ = msg.command;
   pub_gear_cmd_->publish(msg);
   return;
 }