From 14fa9902bb6a49438c2202c26fc0b2b276eec71a Mon Sep 17 00:00:00 2001
From: TetsuKawa <kawaguchitnon@icloud.com>
Date: Thu, 7 Nov 2024 17:39:29 +0900
Subject: [PATCH 1/3] feat: add publisher for emrgency holding

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
---
 .../include/mrm_handler/mrm_handler_core.hpp          |  4 ++++
 system/mrm_handler/launch/mrm_handler.launch.xml      |  2 ++
 .../mrm_handler/src/mrm_handler/mrm_handler_core.cpp  | 11 +++++++++++
 3 files changed, 17 insertions(+)

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 f73d0df4153ce..c062c39fdd382 100644
--- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
+++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
@@ -31,6 +31,7 @@
 #include <autoware_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
 #include <tier4_system_msgs/msg/operation_mode_availability.hpp>
+#include <tier4_system_msgs/msg/emergency_holding_state.hpp>
 #include <tier4_system_msgs/srv/operate_mrm.hpp>
 
 // ROS 2 core
@@ -109,6 +110,9 @@ class MrmHandler : public rclcpp::Node
   autoware_adapi_v1_msgs::msg::MrmState mrm_state_;
   void publishMrmState();
 
+  rclcpp::Publisher<tier4_system_msgs::msg::EmergencyHoldingState>::SharedPtr pub_emergency_holding_;
+  void publishEmergencyHolding();
+
   // Clients
   rclcpp::CallbackGroup::SharedPtr client_mrm_pull_over_group_;
   rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_pull_over_;
diff --git a/system/mrm_handler/launch/mrm_handler.launch.xml b/system/mrm_handler/launch/mrm_handler.launch.xml
index c99b22e10ad77..51a22cf92bebc 100644
--- a/system/mrm_handler/launch/mrm_handler.launch.xml
+++ b/system/mrm_handler/launch/mrm_handler.launch.xml
@@ -12,6 +12,7 @@
   <arg name="output_gear" default="/system/emergency/gear_cmd"/>
   <arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/>
   <arg name="output_mrm_state" default="/system/fail_safe/mrm_state"/>
+  <arg name="output_emergency_holding" default="/system/emergency_holding"/>
   <arg name="output_mrm_pull_over_operate" default="/system/mrm/pull_over_manager/operate"/>
   <arg name="output_mrm_comfortable_stop_operate" default="/system/mrm/comfortable_stop/operate"/>
   <arg name="output_mrm_emergency_stop_operate" default="/system/mrm/emergency_stop/operate"/>
@@ -32,6 +33,7 @@
     <remap from="~/output/gear" to="$(var output_gear)"/>
     <remap from="~/output/hazard" to="$(var output_hazard)"/>
     <remap from="~/output/mrm/state" to="$(var output_mrm_state)"/>
+    <remap from="~/output/emergency_holding" to="$(var output_emergency_holding)"/>
     <remap from="~/output/mrm/pull_over/operate" to="$(var output_mrm_pull_over_operate)"/>
     <remap from="~/output/mrm/comfortable_stop/operate" to="$(var output_mrm_comfortable_stop_operate)"/>
     <remap from="~/output/mrm/emergency_stop/operate" to="$(var output_mrm_emergency_stop_operate)"/>
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 0fc0cb29ecf21..908d04004b163 100644
--- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
+++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
@@ -49,6 +49,8 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler"
     create_publisher<autoware_vehicle_msgs::msg::GearCommand>("~/output/gear", rclcpp::QoS{1});
   pub_mrm_state_ =
     create_publisher<autoware_adapi_v1_msgs::msg::MrmState>("~/output/mrm/state", rclcpp::QoS{1});
+  pub_emergency_holding_ = create_publisher<tier4_system_msgs::msg::EmergencyHoldingState>(
+    "~/output/emergency_holding", rclcpp::QoS{1});
 
   // Clients
   client_mrm_pull_over_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -153,6 +155,14 @@ void MrmHandler::publishMrmState()
   pub_mrm_state_->publish(mrm_state_);
 }
 
+void MrmHandler::publishEmergencyHolding()
+{
+  tier4_system_msgs::msg::EmergencyHoldingState msg;
+  msg.stamp = this->now();
+  msg.holding = is_emergency_holding_;
+  pub_emergency_holding_->publish(msg);
+}
+
 void MrmHandler::operateMrm()
 {
   using autoware_adapi_v1_msgs::msg::MrmState;
@@ -352,6 +362,7 @@ void MrmHandler::onTimer()
   publishMrmState();
   publishHazardCmd();
   publishGearCmd();
+  publishEmergencyHolding();
 }
 
 void MrmHandler::transitionTo(const int new_state)

From c74345190e55c73238ddb66decced065c1a589b6 Mon Sep 17 00:00:00 2001
From: TetsuKawa <kawaguchitnon@icloud.com>
Date: Mon, 11 Nov 2024 12:49:43 +0900
Subject: [PATCH 2/3] modify: fix msg element

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
---
 system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 908d04004b163..4022bdaadebef 100644
--- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
+++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
@@ -159,7 +159,7 @@ void MrmHandler::publishEmergencyHolding()
 {
   tier4_system_msgs::msg::EmergencyHoldingState msg;
   msg.stamp = this->now();
-  msg.holding = is_emergency_holding_;
+  msg.is_holding = is_emergency_holding_;
   pub_emergency_holding_->publish(msg);
 }
 

From af5325750f096e8767076455ffecf97ac336142b Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Mon, 11 Nov 2024 06:04:09 +0000
Subject: [PATCH 3/3] style(pre-commit): autofix

---
 system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp | 5 +++--
 1 file changed, 3 insertions(+), 2 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 c062c39fdd382..b292ab1d874d3 100644
--- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
+++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
@@ -29,9 +29,9 @@
 #include <autoware_vehicle_msgs/msg/control_mode_report.hpp>
 #include <autoware_vehicle_msgs/msg/gear_command.hpp>
 #include <autoware_vehicle_msgs/msg/hazard_lights_command.hpp>
+#include <tier4_system_msgs/msg/emergency_holding_state.hpp>
 #include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
 #include <tier4_system_msgs/msg/operation_mode_availability.hpp>
-#include <tier4_system_msgs/msg/emergency_holding_state.hpp>
 #include <tier4_system_msgs/srv/operate_mrm.hpp>
 
 // ROS 2 core
@@ -110,7 +110,8 @@ class MrmHandler : public rclcpp::Node
   autoware_adapi_v1_msgs::msg::MrmState mrm_state_;
   void publishMrmState();
 
-  rclcpp::Publisher<tier4_system_msgs::msg::EmergencyHoldingState>::SharedPtr pub_emergency_holding_;
+  rclcpp::Publisher<tier4_system_msgs::msg::EmergencyHoldingState>::SharedPtr
+    pub_emergency_holding_;
   void publishEmergencyHolding();
 
   // Clients