From d3b86a37ae7c3a0d59832caf56afa13b148d562c Mon Sep 17 00:00:00 2001
From: Makoto Kurihara <mkuri8m@gmail.com>
Date: Thu, 8 Dec 2022 22:59:32 +0900
Subject: [PATCH] fix(emergency_handler): fix mrm handling when mrm behavior is
 none (#2476)

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>
---
 .../src/emergency_handler/emergency_handler_core.cpp      | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
index de8ca1cca88a2..405359e2020ff 100644
--- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
+++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
@@ -224,6 +224,10 @@ void EmergencyHandler::callMrmBehavior(
   auto request = std::make_shared<tier4_system_msgs::srv::OperateMrm::Request>();
   request->operate = true;
 
+  if (mrm_behavior == MrmState::NONE) {
+    RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing.");
+    return;
+  }
   if (mrm_behavior == MrmState::COMFORTABLE_STOP) {
     auto result = client_mrm_comfortable_stop_->async_send_request(request).get();
     if (result->response.success == true) {
@@ -253,6 +257,10 @@ void EmergencyHandler::cancelMrmBehavior(
   auto request = std::make_shared<tier4_system_msgs::srv::OperateMrm::Request>();
   request->operate = false;
 
+  if (mrm_behavior == MrmState::NONE) {
+    // Do nothing
+    return;
+  }
   if (mrm_behavior == MrmState::COMFORTABLE_STOP) {
     auto result = client_mrm_comfortable_stop_->async_send_request(request).get();
     if (result->response.success == true) {