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 #include #include +#include #include #include #include @@ -109,6 +110,10 @@ class MrmHandler : public rclcpp::Node autoware_adapi_v1_msgs::msg::MrmState mrm_state_; void publishMrmState(); + rclcpp::Publisher::SharedPtr + pub_emergency_holding_; + void publishEmergencyHolding(); + // Clients rclcpp::CallbackGroup::SharedPtr client_mrm_pull_over_group_; rclcpp::Client::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 @@ + @@ -32,6 +33,7 @@ + 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("~/output/gear", rclcpp::QoS{1}); pub_mrm_state_ = create_publisher("~/output/mrm/state", rclcpp::QoS{1}); + pub_emergency_holding_ = create_publisher( + "~/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)