feat(mrm_handler): mrm handler publish emergecy holding (autowarefoun…

…dation#9285)

* feat: add publisher for emrgency holding

Signed-off-by: TetsuKawa <[email protected]>

* modify: fix msg element

Signed-off-by: TetsuKawa <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: TetsuKawa <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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_;

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)"/>

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)