From 2aca1c99677624ae6f52984b0c95bd3094960031 Mon Sep 17 00:00:00 2001
From: Tomohito Ando <tomohito.ando@tier4.jp>
Date: Fri, 22 Nov 2024 19:52:25 +0900
Subject: [PATCH] feat: implement MRM automatic recovery feature

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
---
 .../launch/system.launch.xml                  |   6 +
 system/diagnostic_graph_utils/CMakeLists.txt  |   6 +
 system/diagnostic_graph_utils/package.xml     |   4 +
 .../src/node/recovery.cpp                     | 117 ++++++++++++++++++
 .../src/node/recovery.hpp                     |  75 +++++++++++
 5 files changed, 208 insertions(+)
 create mode 100644 system/diagnostic_graph_utils/src/node/recovery.cpp
 create mode 100644 system/diagnostic_graph_utils/src/node/recovery.hpp

diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml
index 6253dce864ddf..a6e62f9911270 100644
--- a/launch/tier4_system_launch/launch/system.launch.xml
+++ b/launch/tier4_system_launch/launch/system.launch.xml
@@ -22,6 +22,7 @@
 
   <arg name="launch_system_monitor" default="true" description="launch system monitor"/>
   <arg name="launch_dummy_diag_publisher" description="launch dummy diag publisher"/>
+  <arg name="launch_system_recover_operator" default="false" description="launch recover operator"/>
   <arg name="run_mode" default="online" description="options: online, logging_simulation, planning_simulation"/>
   <arg name="sensor_model" description="sensor model name"/>
 
@@ -142,4 +143,9 @@
       <arg name="launch_rqt_runtime_monitor_err" value="$(var launch_rqt_runtime_monitor_err)"/>
     </include>
   </group>
+
+  <!-- System Recover Operator -->
+  <group if="$(var launch_system_recover_operator)">
+    <node pkg="diagnostic_graph_utils" exec="recovery_node" name="recovery"/>
+  </group>
 </launch>
diff --git a/system/diagnostic_graph_utils/CMakeLists.txt b/system/diagnostic_graph_utils/CMakeLists.txt
index 0c36964f49237..fdc7281c48023 100644
--- a/system/diagnostic_graph_utils/CMakeLists.txt
+++ b/system/diagnostic_graph_utils/CMakeLists.txt
@@ -13,6 +13,7 @@ ament_auto_add_library(${PROJECT_NAME}_tools SHARED
   src/node/dump.cpp
   src/node/converter.cpp
   src/node/logging.cpp
+  src/node/recovery.cpp
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}_tools
@@ -30,4 +31,9 @@ rclcpp_components_register_node(${PROJECT_NAME}_tools
   EXECUTABLE logging_node
 )
 
+rclcpp_components_register_node(${PROJECT_NAME}_tools
+  PLUGIN "diagnostic_graph_utils::RecoveryNode"
+  EXECUTABLE recovery_node
+)
+
 ament_auto_package(INSTALL_TO_SHARE launch)
diff --git a/system/diagnostic_graph_utils/package.xml b/system/diagnostic_graph_utils/package.xml
index c5a70363bfecb..b455a631eccef 100644
--- a/system/diagnostic_graph_utils/package.xml
+++ b/system/diagnostic_graph_utils/package.xml
@@ -10,9 +10,13 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_system_msgs</depend>
+  <depend>component_interface_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>std_srvs</depend>
   <depend>tier4_system_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
diff --git a/system/diagnostic_graph_utils/src/node/recovery.cpp b/system/diagnostic_graph_utils/src/node/recovery.cpp
new file mode 100644
index 0000000000000..95e190ce2cf5b
--- /dev/null
+++ b/system/diagnostic_graph_utils/src/node/recovery.cpp
@@ -0,0 +1,117 @@
+// Copyright 2023 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "recovery.hpp"
+
+#include <algorithm>
+
+namespace diagnostic_graph_utils
+{
+
+RecoveryNode::RecoveryNode(const rclcpp::NodeOptions & options) : Node("dump", options)
+{
+  using std::placeholders::_1;
+  const auto qos_aw_state = rclcpp::QoS(1);
+  const auto qos_mrm_state = rclcpp::QoS(1);
+
+  sub_graph_.register_update_callback(std::bind(&RecoveryNode::on_graph_update, this, _1));
+  sub_graph_.subscribe(*this, 1);
+
+  const auto callback_aw_state = std::bind(&RecoveryNode::on_aw_state, this, _1);
+  sub_aw_state_ =
+    create_subscription<AutowareState>("/autoware/state", qos_aw_state, callback_aw_state);
+
+  const auto callback_mrm_state = std::bind(&RecoveryNode::on_mrm_state, this, _1);
+  sub_mrm_state_ =
+    create_subscription<MrmState>("/system/fail_safe/mrm_state", qos_mrm_state, callback_mrm_state);
+  callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  srv_clear_mrm_ = create_client<std_srvs::srv::Trigger>(
+    "/system/clear_mrm", rmw_qos_profile_services_default, callback_group_);
+
+  fatal_error_ = false;
+  mrm_occur_ = false;
+  autonomous_available_ = false;
+  mrm_by_fatal_error_ = false;
+}
+
+void RecoveryNode::on_graph_update(DiagGraph::ConstSharedPtr graph)
+{
+  for (const auto & node : graph->nodes()) {
+    if (node->path() == "/autoware/modes/autonomous") {
+      autonomous_available_ = node->level() == DiagnosticStatus::OK;
+    }
+
+    // aggregate non-recoverable error
+    if (node->path() == "/autoware/fatal_error/autonomous_available") {
+      if (node->level() != DiagnosticStatus::OK) {
+        fatal_error_ = true;
+      } else {
+        fatal_error_ = false;
+      }
+    }
+  }
+}
+
+void RecoveryNode::on_aw_state(const AutowareState::ConstSharedPtr msg)
+{
+  auto_driving_ = msg->state == AutowareState::DRIVING;
+}
+
+void RecoveryNode::on_mrm_state(const MrmState::ConstSharedPtr msg)
+{
+  // set flag if mrm happened by fatal error
+  if (msg->state != MrmState::NORMAL && fatal_error_) {
+    mrm_by_fatal_error_ = true;
+  }
+  // reset flag if recoverd (transition from mrm to normal)
+  if (mrm_occur_ && msg->state == MrmState::NORMAL) {
+    mrm_by_fatal_error_ = false;
+  }
+  mrm_occur_ = msg->state != MrmState::NORMAL;
+  // 1. Not emergency
+  // 2. Non-recovoerable MRM have not happened
+  // 3. on MRM
+  // 4. on autonomous driving
+  if (autonomous_available_ && !mrm_by_fatal_error_ && mrm_occur_ && auto_driving_) {
+    clear_mrm();
+  }
+}
+
+void RecoveryNode::clear_mrm()
+{
+  const auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
+
+  auto logger = get_logger();
+  if (!srv_clear_mrm_->service_is_ready()) {
+    RCLCPP_ERROR(logger, "MRM clear server is not ready.");
+    return;
+  }
+  RCLCPP_INFO(logger, "Recover MRM automatically.");
+  auto res = srv_clear_mrm_->async_send_request(req);
+  std::future_status status = res.wait_for(std::chrono::milliseconds(50));
+  if (status == std::future_status::timeout) {
+    RCLCPP_INFO(logger, "Service timeout");
+    return;
+  }
+  if (!res.get()->success) {
+    RCLCPP_INFO(logger, "Recovering MRM failed.");
+    return;
+  }
+  RCLCPP_INFO(logger, "Recovering MRM succeed.");
+}
+
+}  // namespace diagnostic_graph_utils
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::RecoveryNode)
diff --git a/system/diagnostic_graph_utils/src/node/recovery.hpp b/system/diagnostic_graph_utils/src/node/recovery.hpp
new file mode 100644
index 0000000000000..0d01a07d3db77
--- /dev/null
+++ b/system/diagnostic_graph_utils/src/node/recovery.hpp
@@ -0,0 +1,75 @@
+// Copyright 2023 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NODE__RECOVERY_HPP_
+#define NODE__RECOVERY_HPP_
+
+#include "diagnostic_graph_utils/subscription.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+
+// Autoware
+#include <component_interface_utils/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
+#include <autoware_system_msgs/msg/autoware_state.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+#include <functional>
+#include <map>  // Use map for sorting keys.
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace diagnostic_graph_utils
+{
+
+class RecoveryNode : public rclcpp::Node
+{
+public:
+  explicit RecoveryNode(const rclcpp::NodeOptions & options);
+
+private:
+  using AutowareState = autoware_system_msgs::msg::AutowareState;
+  using MrmState = autoware_adapi_v1_msgs::msg::MrmState;
+  using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus;
+
+  bool fatal_error_;
+  bool autonomous_available_;
+  bool mrm_occur_;
+  bool auto_driving_;
+  bool mrm_by_fatal_error_;
+  DiagGraphSubscription sub_graph_;
+  rclcpp::Subscription<AutowareState>::SharedPtr sub_aw_state_;
+  rclcpp::Subscription<MrmState>::SharedPtr sub_mrm_state_;
+
+  // service
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv_clear_mrm_;
+
+  // callback group for service
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+
+  void on_graph_update(DiagGraph::ConstSharedPtr graph);
+  void on_aw_state(const AutowareState::ConstSharedPtr msg);
+  void on_mrm_state(const MrmState::ConstSharedPtr msg);
+
+  void clear_mrm();
+};
+
+}  // namespace diagnostic_graph_utils
+
+#endif  // NODE__RECOVERY_HPP_