From 9dac0da3f99a33c99c4153716de3e3144dd40aab Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Wed, 4 Sep 2024 16:21:23 +0900 Subject: [PATCH] implement MRM automatic recovery feature --- .../CMakeLists.txt | 5 + .../diagnostic_graph_aggregator/package.xml | 4 + .../src/node/recovery.cpp | 125 ++++++++++++++++++ .../src/node/recovery.hpp | 69 ++++++++++ 4 files changed, 203 insertions(+) create mode 100644 system/diagnostic_graph_aggregator/src/node/recovery.cpp create mode 100644 system/diagnostic_graph_aggregator/src/node/recovery.hpp diff --git a/system/diagnostic_graph_aggregator/CMakeLists.txt b/system/diagnostic_graph_aggregator/CMakeLists.txt index 574978ec5adce..cfde99b3dcf75 100644 --- a/system/diagnostic_graph_aggregator/CMakeLists.txt +++ b/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 diff --git a/system/diagnostic_graph_aggregator/package.xml b/system/diagnostic_graph_aggregator/package.xml index 2a9efad2c0d6e..ec4d02a8a92ee 100644 --- a/system/diagnostic_graph_aggregator/package.xml +++ b/system/diagnostic_graph_aggregator/package.xml @@ -12,7 +12,11 @@ diagnostic_msgs rclcpp + std_srvs tier4_system_msgs + autoware_auto_system_msgs + autoware_adapi_v1_msgs + component_interface_utils yaml_cpp_vendor ament_cmake_gtest diff --git a/system/diagnostic_graph_aggregator/src/node/recovery.cpp b/system/diagnostic_graph_aggregator/src/node/recovery.cpp new file mode 100644 index 0000000000000..bb6311aeca429 --- /dev/null +++ b/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 + +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("/diagnostics_graph", qos_graph, callback_graph); + const auto callback_aw_state = std::bind(&RecoveryNode::on_aw_state, this, _1); + sub_aw_state_ = create_subscription("/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("/system/fail_safe/mrm_state", + qos_mrm_state, callback_mrm_state); + callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + srv_clear_mrm_ = create_client("/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(); + + 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(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/system/diagnostic_graph_aggregator/src/node/recovery.hpp b/system/diagnostic_graph_aggregator/src/node/recovery.hpp new file mode 100644 index 0000000000000..bd3d544e89f5a --- /dev/null +++ b/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 + +// Autoware +#include +#include +#include +#include +#include + +#include +#include // Use map for sorting keys. +#include +#include +#include + +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::SharedPtr sub_graph_; + rclcpp::Subscription::SharedPtr sub_aw_state_; + rclcpp::Subscription::SharedPtr sub_mrm_state_; + + // service + rclcpp::Client::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_