From bf575b6ef7ee2b23ca90fe35f46dae3bfe974848 Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com>
Date: Fri, 24 May 2024 20:41:21 +0900
Subject: [PATCH] feat(default_ad_api): use diagnostic graph (#7043)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 .../launch/system.launch.xml                  | 74 ++++++++++---------
 system/default_ad_api/src/operation_mode.cpp  | 40 +++-------
 system/default_ad_api/src/operation_mode.hpp  |  8 +-
 .../system_diagnostic_monitor.launch.xml      |  2 +
 4 files changed, 53 insertions(+), 71 deletions(-)

diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml
index a1bbfc883de7c..e5f7aa4e18b33 100644
--- a/launch/tier4_system_launch/launch/system.launch.xml
+++ b/launch/tier4_system_launch/launch/system.launch.xml
@@ -25,11 +25,12 @@
   <arg name="run_mode" default="online" description="options: online, logging_simulation, planning_simulation"/>
   <arg name="sensor_model" description="sensor model name"/>
 
-  <arg name="use_diagnostic_graph" default="false" description="use diagnostic graph packages"/>
-  <arg name="mrm_handler_param_path" if="$(var use_diagnostic_graph)"/>
-  <arg name="diagnostic_graph_aggregator_param_path" if="$(var use_diagnostic_graph)"/>
-  <arg name="diagnostic_graph_aggregator_graph_path" if="$(var use_diagnostic_graph)"/>
-  <arg name="diagnostic_graph_aggregator_planning_simulator_graph_path" if="$(var use_diagnostic_graph)"/>
+  <!-- Emergency handler will be replaced by MRM handler. -->
+  <arg name="use_emergency_handler" default="true" description="use emergency handler packages"/>
+  <arg name="mrm_handler_param_path"/>
+  <arg name="diagnostic_graph_aggregator_param_path"/>
+  <arg name="diagnostic_graph_aggregator_graph_path_main"/>
+  <arg name="diagnostic_graph_aggregator_graph_path_psim"/>
 
   <let name="sensor_launch_pkg" value="$(find-pkg-share $(var sensor_model)_launch)"/>
 
@@ -57,6 +58,13 @@
       </include>
     </group>
 
+    <!-- Duplicated Node Checker -->
+    <group>
+      <include file="$(find-pkg-share duplicated_node_checker)/launch/duplicated_node_checker.launch.xml">
+        <arg name="config_file" value="$(var duplicated_node_checker_param_path)"/>
+      </include>
+    </group>
+
     <!-- Service Log Checker -->
     <group>
       <include file="$(find-pkg-share component_interface_tools)/launch/service_log_checker.launch.xml"/>
@@ -70,8 +78,20 @@
       </include>
     </group>
 
+    <!-- MRM Operator -->
+    <group>
+      <include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.py">
+        <arg name="config_file" value="$(var mrm_comfortable_stop_operator_param_path)"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share mrm_emergency_stop_operator)/launch/mrm_emergency_stop_operator.launch.py">
+        <arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
+      </include>
+    </group>
+
     <!-- Error Monitor -->
-    <group unless="$(var use_diagnostic_graph)">
+    <group if="$(var use_emergency_handler)">
       <let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
       <let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
       <let name="config_file" value="$(var system_error_monitor_planning_simulator_param_path)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
@@ -85,52 +105,34 @@
     </group>
 
     <!-- Emergency Handler -->
-    <group unless="$(var use_diagnostic_graph)">
+    <group if="$(var use_emergency_handler)">
       <include file="$(find-pkg-share emergency_handler)/launch/emergency_handler.launch.xml">
         <arg name="config_file" value="$(var emergency_handler_param_path)"/>
       </include>
     </group>
 
+    <!-- Diagnostic Graph Aggregator -->
     <group>
-      <include file="$(find-pkg-share duplicated_node_checker)/launch/duplicated_node_checker.launch.xml">
-        <arg name="config_file" value="$(var duplicated_node_checker_param_path)"/>
+      <let name="diagnostic_graph_aggregator_graph_path" value="$(var diagnostic_graph_aggregator_graph_path_main)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
+      <let name="diagnostic_graph_aggregator_graph_path" value="$(var diagnostic_graph_aggregator_graph_path_main)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
+      <let name="diagnostic_graph_aggregator_graph_path" value="$(var diagnostic_graph_aggregator_graph_path_psim)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
+      <include file="$(find-pkg-share system_diagnostic_monitor)/launch/system_diagnostic_monitor.launch.xml">
+        <arg name="param_file" value="$(var diagnostic_graph_aggregator_param_path)"/>
+        <arg name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)"/>
       </include>
     </group>
 
-    <!-- MRM Operator -->
-    <group>
-      <include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.py">
-        <arg name="config_file" value="$(var mrm_comfortable_stop_operator_param_path)"/>
-      </include>
-    </group>
-    <group>
-      <include file="$(find-pkg-share mrm_emergency_stop_operator)/launch/mrm_emergency_stop_operator.launch.py">
-        <arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
-      </include>
+    <!-- Hazard Status Converter -->
+    <group unless="$(var use_emergency_handler)">
+      <include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml"/>
     </group>
 
     <!-- MRM Handler -->
-    <group if="$(var use_diagnostic_graph)">
+    <group unless="$(var use_emergency_handler)">
       <include file="$(find-pkg-share mrm_handler)/launch/mrm_handler.launch.xml">
         <arg name="config_file" value="$(var mrm_handler_param_path)"/>
       </include>
     </group>
-
-    <!-- Diagnostic Graph Aggregator -->
-    <group if="$(var use_diagnostic_graph)">
-      <let name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
-      <let name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
-      <let name="graph_file" value="$(var diagnostic_graph_aggregator_planning_simulator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
-      <include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/aggregator.launch.xml">
-        <arg name="param_file" value="$(var diagnostic_graph_aggregator_param_path)"/>
-        <arg name="graph_file" value="$(var graph_file)"/>
-      </include>
-    </group>
-
-    <!-- Hazard Status Converter -->
-    <group if="$(var use_diagnostic_graph)">
-      <include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml"/>
-    </group>
   </group>
 
   <!-- Dummy Diag Publisher -->
diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp
index 829585ed4b8b4..d4b430ecd0579 100644
--- a/system/default_ad_api/src/operation_mode.cpp
+++ b/system/default_ad_api/src/operation_mode.cpp
@@ -39,18 +39,15 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options)
   adaptor.init_cli(cli_mode_, group_cli_);
   adaptor.init_cli(cli_control_, group_cli_);
 
-  const std::vector<std::string> module_names = {
-    "sensing", "perception", "map", "localization", "planning", "control", "vehicle", "system",
+  const auto name = "/system/operation_mode/availability";
+  const auto qos = rclcpp::QoS(1);
+  const auto callback = [this](const OperationModeAvailability::ConstSharedPtr msg) {
+    mode_available_[OperationModeState::Message::STOP] = msg->stop;
+    mode_available_[OperationModeState::Message::AUTONOMOUS] = msg->autonomous;
+    mode_available_[OperationModeState::Message::LOCAL] = msg->local;
+    mode_available_[OperationModeState::Message::REMOTE] = msg->remote;
   };
-
-  for (size_t i = 0; i < module_names.size(); ++i) {
-    const auto name = "/system/component_state_monitor/component/autonomous/" + module_names[i];
-    const auto qos = rclcpp::QoS(1).transient_local();
-    const auto callback = [this, i, module_names](const ModeChangeAvailable::ConstSharedPtr msg) {
-      module_states_[module_names[i]] = msg->available;
-    };
-    sub_module_states_.push_back(create_subscription<ModeChangeAvailable>(name, qos, callback));
-  }
+  sub_availability_ = create_subscription<OperationModeAvailability>(name, qos, callback);
 
   timer_ = rclcpp::create_timer(
     this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this));
@@ -60,8 +57,8 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options)
   mode_available_[OperationModeState::Message::UNKNOWN] = false;
   mode_available_[OperationModeState::Message::STOP] = true;
   mode_available_[OperationModeState::Message::AUTONOMOUS] = false;
-  mode_available_[OperationModeState::Message::LOCAL] = true;
-  mode_available_[OperationModeState::Message::REMOTE] = true;
+  mode_available_[OperationModeState::Message::LOCAL] = false;
+  mode_available_[OperationModeState::Message::REMOTE] = false;
 }
 
 template <class ResponseT>
@@ -135,23 +132,6 @@ void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedP
 
 void OperationModeNode::on_timer()
 {
-  bool autonomous_available = true;
-  std::string unhealthy_components = "";
-  for (const auto & state : module_states_) {
-    if (!state.second) {
-      unhealthy_components += unhealthy_components.empty() ? state.first : ", " + state.first;
-    }
-    autonomous_available &= state.second;
-  }
-  mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available;
-
-  if (!unhealthy_components.empty()) {
-    RCLCPP_INFO_THROTTLE(
-      get_logger(), *get_clock(), 3000,
-      "%s component state is unhealthy. Autonomous is not available.",
-      unhealthy_components.c_str());
-  }
-
   update_state();
 }
 
diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp
index 1830b7041b566..7cd609b5eb852 100644
--- a/system/default_ad_api/src/operation_mode.hpp
+++ b/system/default_ad_api/src/operation_mode.hpp
@@ -25,7 +25,7 @@
 #include <vector>
 
 // TODO(Takagi, Isamu): define interface
-#include <tier4_system_msgs/msg/mode_change_available.hpp>
+#include <tier4_system_msgs/msg/operation_mode_availability.hpp>
 
 // This file should be included after messages.
 #include "utils/types.hpp"
@@ -47,7 +47,7 @@ class OperationModeNode : public rclcpp::Node
   using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote;
   using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request;
   using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request;
-  using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable;
+  using OperationModeAvailability = tier4_system_msgs::msg::OperationModeAvailability;
 
   OperationModeState::Message curr_state_;
   OperationModeState::Message prev_state_;
@@ -65,9 +65,7 @@ class OperationModeNode : public rclcpp::Node
   Sub<system_interface::OperationModeState> sub_state_;
   Cli<system_interface::ChangeOperationMode> cli_mode_;
   Cli<system_interface::ChangeAutowareControl> cli_control_;
-
-  std::unordered_map<std::string, bool> module_states_;
-  std::vector<rclcpp::Subscription<ModeChangeAvailable>::SharedPtr> sub_module_states_;
+  rclcpp::Subscription<OperationModeAvailability>::SharedPtr sub_availability_;
 
   void on_change_to_stop(
     const ChangeToStop::Service::Request::SharedPtr req,
diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml
index b00fcb8a26f68..8e2566a747abf 100644
--- a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml
+++ b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml
@@ -1,6 +1,8 @@
 <launch>
+  <arg name="param_file" default="$(find-pkg-share diagnostic_graph_aggregator)/config/default.param.yaml"/>
   <arg name="graph_file" default="$(find-pkg-share system_diagnostic_monitor)/config/autoware-main.yaml"/>
   <include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/aggregator.launch.xml">
+    <arg name="param_file" value="$(var param_file)"/>
     <arg name="graph_file" value="$(var graph_file)"/>
   </include>
   <node pkg="system_diagnostic_monitor" exec="component_state_diagnostics" name="component_state_diagnostics"/>