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 @@ + @@ -142,4 +143,9 @@ + + + + + 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 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs + autoware_system_msgs + component_interface_utils diagnostic_msgs rclcpp rclcpp_components + std_srvs tier4_system_msgs ament_lint_auto 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 + +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("/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; + 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(); + + 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(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 + +#include + +// Autoware +#include + +#include +#include +#include +#include + +#include +#include // Use map for sorting keys. +#include +#include +#include + +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::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_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_