fix(emergency_handler): fix mrm handling when mrm behavior is none (a…

…utowarefoundation#2476) Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>
HansRobo · Dec 8, 2022 · d3b86a3 · d3b86a3
1 parent 2dde073
commit d3b86a3
Showing 1 changed file with 8 additions and 0 deletions.
diff --git 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) {