From c9e78d68c8373d66f7358ef4edc23ad1e4f44b32 Mon Sep 17 00:00:00 2001
From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com>
Date: Fri, 30 Aug 2024 19:26:50 +0900
Subject: [PATCH] feat(mrm_handler): input gear command (#8080) (#1498)

* feat(mrm_handler): input gear command



* style(pre-commit): autofix

* fix minor



---------

Signed-off-by: veqcc <ryuta.kambe@tier4.jp>
Co-authored-by: Ryuta Kambe <ryuta.kambe@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 .../include/mrm_handler/mrm_handler_core.hpp  |  3 +-
 .../mrm_handler/launch/mrm_handler.launch.xml |  2 ++
 .../src/mrm_handler/mrm_handler_core.cpp      | 31 +++++++------------
 3 files changed, 16 insertions(+), 20 deletions(-)

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 7d7deb8c4a504..f73d0df4153ce 100644
--- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
+++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
@@ -86,6 +86,8 @@ class MrmHandler : public rclcpp::Node
   autoware::universe_utils::InterProcessPollingSubscriber<
     autoware_adapi_v1_msgs::msg::OperationModeState>
     sub_operation_mode_state_{this, "~/input/api/operation_mode/state"};
+  autoware::universe_utils::InterProcessPollingSubscriber<autoware_vehicle_msgs::msg::GearCommand>
+    sub_gear_cmd_{this, "~/input/gear"};
 
   tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_;
 
@@ -146,7 +148,6 @@ class MrmHandler : public rclcpp::Node
   void handleFailedRequest();
   autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
   bool isStopped();
-  bool isDrivingBackwards();
   bool isEmergency() const;
   bool isControlModeAutonomous();
   bool isOperationModeAutonomous();
diff --git a/system/mrm_handler/launch/mrm_handler.launch.xml b/system/mrm_handler/launch/mrm_handler.launch.xml
index 7e761157956df..c99b22e10ad77 100644
--- a/system/mrm_handler/launch/mrm_handler.launch.xml
+++ b/system/mrm_handler/launch/mrm_handler.launch.xml
@@ -7,6 +7,7 @@
   <arg name="input_mrm_comfortable_stop_state" default="/system/mrm/comfortable_stop/status"/>
   <arg name="input_mrm_emergency_stop_state" default="/system/mrm/emergency_stop/status"/>
   <arg name="input_api_operation_mode_state" default="/api/operation_mode/state"/>
+  <arg name="input_gear" default="/control/command/gear_cmd"/>
 
   <arg name="output_gear" default="/system/emergency/gear_cmd"/>
   <arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/>
@@ -26,6 +27,7 @@
     <remap from="~/input/mrm/comfortable_stop/status" to="$(var input_mrm_comfortable_stop_state)"/>
     <remap from="~/input/mrm/emergency_stop/status" to="$(var input_mrm_emergency_stop_state)"/>
     <remap from="~/input/api/operation_mode/state" to="$(var input_api_operation_mode_state)"/>
+    <remap from="~/input/gear" to="$(var input_gear)"/>
 
     <remap from="~/output/gear" to="$(var output_gear)"/>
     <remap from="~/output/hazard" to="$(var output_hazard)"/>
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 47cbb0ae7d016..bac932a7d7e1e 100644
--- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
+++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
@@ -130,19 +130,20 @@ void MrmHandler::publishGearCmd()
 {
   using autoware_vehicle_msgs::msg::GearCommand;
   GearCommand msg;
-
   msg.stamp = this->now();
-  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;
+
+  if (isEmergency()) {
+    // gear command is created within mrm_handler
+    msg.command =
+      (param_.use_parking_after_stopped && isStopped()) ? GearCommand::PARK : last_gear_command_;
+  } else {
+    // use the same gear as the input gear
+    auto gear = sub_gear_cmd_.takeData();
+    msg.command = (gear == nullptr) ? last_gear_command_ : gear->command;
+    last_gear_command_ = msg.command;
+  }
+
   pub_gear_cmd_->publish(msg);
-  return;
 }
 
 void MrmHandler::publishMrmState()
@@ -524,14 +525,6 @@ bool MrmHandler::isStopped()
   return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity);
 }
 
-bool MrmHandler::isDrivingBackwards()
-{
-  auto odom = sub_odom_.takeData();
-  if (odom == nullptr) return false;
-  constexpr auto th_moving_backwards = -0.001;
-  return odom->twist.twist.linear.x < th_moving_backwards;
-}
-
 bool MrmHandler::isEmergency() const
 {
   return !operation_mode_availability_->autonomous || is_emergency_holding_ ||