implement MRM automatic recovery feature

Author: saka1-s

system/diagnostic_graph_aggregator/CMakeLists.txt

@@ -22,6 +22,11 @@ ament_auto_add_executable(converter
 )
 target_include_directories(converter PRIVATE src/common)
 
+ament_auto_add_executable(recovery
+  src/node/recovery.cpp
+)
+target_include_directories(recovery PRIVATE src/common)
+
 ament_auto_add_executable(tree
   src/tool/tree.cpp
   src/tool/utils/loader.cpp

system/diagnostic_graph_aggregator/package.xml

@@ -12,7 +12,11 @@
 
   <depend>diagnostic_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>std_srvs</depend>
   <depend>tier4_system_msgs</depend>
+  <depend>autoware_auto_system_msgs</depend>
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>component_interface_utils</depend>
   <depend>yaml_cpp_vendor</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>

system/diagnostic_graph_aggregator/src/node/recovery.cpp

@@ -0,0 +1,125 @@
+// 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_aggregator
+{
+
+std::string level_to_string(DiagnosticLevel level)
+{
+  switch (level) {
+    case DiagnosticStatus::OK:
+      return "OK";
+    case DiagnosticStatus::WARN:
+      return "WARN";
+    case DiagnosticStatus::ERROR:
+      return "ERROR";
+    case DiagnosticStatus::STALE:
+      return "STALE";
+  }
+  return "UNKNOWN";
+}
+
+
+RecoveryNode::RecoveryNode() : Node("recovery")
+{
+  using std::placeholders::_1;
+  const auto qos_graph = rclcpp::QoS(1);
+  const auto qos_aw_state = rclcpp::QoS(1);
+  const auto qos_mrm_state = rclcpp::QoS(1);
+
+  const auto callback_graph = std::bind(&RecoveryNode::on_graph, this, _1);
+  sub_graph_ = create_subscription<DiagnosticGraph>("/diagnostics_graph", qos_graph, callback_graph);
+  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;
+}
+
+void RecoveryNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg)
+{
+  bool autonomous_available = false;
+  for (const auto & node : msg->nodes) {
+    if (node.status.name == "/autoware/modes/autonomous") {
+      autonomous_available = node.status.level == DiagnosticStatus::OK;
+    }
+    // aggregate non-recoverable error
+    if (node.status.name == "/autoware/fatal_error/autonomous_available") {
+      if (node.status.level != DiagnosticStatus::OK){
+        fatal_error_ = true;
+      }
+    }
+  }
+
+  // 1. Not emergency
+  // 2. Non-recovoerable errors have not happened
+  // 3. on MRM
+  if (autonomous_available && !fatal_error_ && mrm_occur_){
+    clear_mrm();
+  }
+}
+
+void RecoveryNode::on_aw_state(const AutowareState::ConstSharedPtr msg){
+  if (msg->state != AutowareState::DRIVING)
+    fatal_error_ = false;
+}
+
+void RecoveryNode::on_mrm_state(const MrmState::ConstSharedPtr msg){
+  mrm_occur_ = msg->state != MrmState::NORMAL;
+}
+
+void RecoveryNode::clear_mrm(){
+  const auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
+
+  if (!srv_clear_mrm_->service_is_ready()) {
+    throw component_interface_utils::ServiceUnready("MRM clear server is not ready.");
+  }
+  auto logger = get_logger();
+  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)
+  {
+    return;
+  }
+  if (!res.get()->success) {
+    RCLCPP_INFO(logger, "Recovering MRM failed.");
+    throw component_interface_utils::NoEffectWarning("MRM clear server is not available.");
+  }
+  RCLCPP_INFO(logger, "Recovering MRM succeed.");
+}
+
+}  // namespace diagnostic_graph_aggregator
+
+int main(int argc, char ** argv)
+{
+  using diagnostic_graph_aggregator::RecoveryNode;
+  rclcpp::init(argc, argv);
+  rclcpp::executors::SingleThreadedExecutor executor;
+  auto node = std::make_shared<RecoveryNode>();
+  executor.add_node(node);
+  executor.spin();
+  executor.remove_node(node);
+  rclcpp::shutdown();
+}

system/diagnostic_graph_aggregator/src/node/recovery.hpp

@@ -0,0 +1,69 @@
+// 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 "graph/types.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+// Autoware
+#include <std_msgs/msg/string.hpp>
+#include <std_srvs/srv/trigger.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <autoware_auto_system_msgs/msg/autoware_state.hpp>
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
+
+#include <functional>
+#include <map>  // Use map for sorting keys.
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace diagnostic_graph_aggregator
+{
+
+class RecoveryNode : public rclcpp::Node
+{
+public:
+  RecoveryNode();
+
+private:
+  using AutowareState = autoware_auto_system_msgs::msg::AutowareState;
+  using MrmState = autoware_adapi_v1_msgs::msg::MrmState;
+
+  bool fatal_error_;
+  bool mrm_occur_;
+  rclcpp::Subscription<DiagnosticGraph>::SharedPtr 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(const DiagnosticGraph::ConstSharedPtr msg);
+  void on_aw_state(const AutowareState::ConstSharedPtr msg);
+  void on_mrm_state(const MrmState::ConstSharedPtr msg);
+
+  void clear_mrm();
+
+};
+
+}  // namespace diagnostic_graph_aggregator
+
+#endif  // NODE__RECOVERY_HPP_