From c6c3b1c6e6e2dc3a1f8a84a9d72dce8a171fece7 Mon Sep 17 00:00:00 2001
From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com>
Date: Thu, 14 Nov 2024 19:22:29 +0900
Subject: [PATCH] feat(mrm_handler): mrm handler publish emergecy holding
 (#9285)

* feat: add publisher for emrgency holding

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* modify: fix msg element

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

* style(pre-commit): autofix

---------

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 .../include/mrm_handler/mrm_handler_core.hpp          |  5 +++++
 system/mrm_handler/launch/mrm_handler.launch.xml      |  2 ++
 .../mrm_handler/src/mrm_handler/mrm_handler_core.cpp  | 11 +++++++++++
 3 files changed, 18 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..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,6 +29,7 @@
 #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/srv/operate_mrm.hpp>
@@ -109,6 +110,10 @@ 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..4022bdaadebef 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.is_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)