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 a78b35be26daf..532a837a18422 100644
--- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
+++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
@@ -140,6 +140,7 @@ class EmergencyHandler : public rclcpp::Node
   void operateMrm();
   autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
   bool isStopped();
+  bool isDrivingBackwards();
   bool isEmergency();
 };
 
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 2770a14691d95..d711f8eb22170 100644
--- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
+++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
@@ -167,12 +167,24 @@ void EmergencyHandler::publishControlCommands()
   {
     GearCommand msg;
     msg.stamp = stamp;
-    if (param_.use_parking_after_stopped && isStopped()) {
-      msg.command = GearCommand::PARK;
-    } else {
-      msg.command = GearCommand::DRIVE;
+
+    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;
     pub_gear_cmd_->publish(msg);
+    return;
   }
 }
 
@@ -454,11 +466,13 @@ bool EmergencyHandler::isEmergency()
 bool EmergencyHandler::isStopped()
 {
   constexpr auto th_stopped_velocity = 0.001;
-  if (odom_->twist.twist.linear.x < th_stopped_velocity) {
-    return true;
-  }
+  return (std::abs(odom_->twist.twist.linear.x) < th_stopped_velocity);
+}
 
-  return false;
+bool EmergencyHandler::isDrivingBackwards()
+{
+  constexpr auto th_moving_backwards = -0.001;
+  return odom_->twist.twist.linear.x < th_moving_backwards;
 }
 
 #include <rclcpp_components/register_node_macro.hpp>
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 c7aaca96aae49..d51ba3030d3d2 100644
--- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
+++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
@@ -155,6 +155,7 @@ class MrmHandler : public rclcpp::Node
   void handleFailedRequest();
   autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
   bool isStopped();
+  bool isDrivingBackwards();
   bool isEmergency() const;
   bool isArrivedAtGoal();
 };
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 5e709946c2f46..3b4c450a52ae2 100644
--- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
+++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
@@ -191,13 +191,24 @@ void MrmHandler::publishGearCmd()
   GearCommand msg;
 
   msg.stamp = this->now();
-  if (param_.use_parking_after_stopped && isStopped()) {
-    msg.command = GearCommand::PARK;
-  } else {
-    msg.command = GearCommand::DRIVE;
+
+  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;
   pub_gear_cmd_->publish(msg);
+  return;
 }
 
 void MrmHandler::publishMrmState()
@@ -578,11 +589,13 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB
 bool MrmHandler::isStopped()
 {
   constexpr auto th_stopped_velocity = 0.001;
-  if (odom_->twist.twist.linear.x < th_stopped_velocity) {
-    return true;
-  }
+  return std::abs((odom_->twist.twist.linear.x < th_stopped_velocity) < th_stopped_velocity);
+}
 
-  return false;
+bool MrmHandler::isDrivingBackwards()
+{
+  constexpr auto th_moving_backwards = -0.001;
+  return odom_->twist.twist.linear.x < th_moving_backwards;
 }
 
 bool MrmHandler::isEmergency() const