add gear history

Signed-off-by: Daniel Sanchez <[email protected]>
autowarefoundation · Jun 5, 2024 · d48b060 · d48b060
1 parent 96b6321
commit d48b060
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 32 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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;
 }