diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 41aabe8fa51cf..3c7d15149061f 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -157,7 +157,7 @@ - - - + + + diff --git a/system/diagnostic_graph_aggregator/package.xml b/system/diagnostic_graph_aggregator/package.xml index ec4d02a8a92ee..591248e115ecb 100644 --- a/system/diagnostic_graph_aggregator/package.xml +++ b/system/diagnostic_graph_aggregator/package.xml @@ -10,13 +10,13 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs + autoware_auto_system_msgs + component_interface_utils 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 index a57ea9ba18bd8..c8980bb582980 100644 --- a/system/diagnostic_graph_aggregator/src/node/recovery.cpp +++ b/system/diagnostic_graph_aggregator/src/node/recovery.cpp @@ -27,15 +27,17 @@ RecoveryNode::RecoveryNode() : Node("recovery") 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); + 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); + 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); + 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_); + srv_clear_mrm_ = create_client( + "/system/clear_mrm", rmw_qos_profile_services_default, callback_group_); fatal_error_ = false; mrm_occur_ = false; @@ -51,27 +53,28 @@ void RecoveryNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg) } // aggregate non-recoverable error if (node.status.name == "/autoware/fatal_error/autonomous_available") { - if (node.status.level != DiagnosticStatus::OK){ + if (node.status.level != DiagnosticStatus::OK) { fatal_error_ = true; } else { fatal_error_ = false; } } } - } -void RecoveryNode::on_aw_state(const AutowareState::ConstSharedPtr msg){ +void RecoveryNode::on_aw_state(const AutowareState::ConstSharedPtr msg) +{ auto_driving_ = msg->state == AutowareState::DRIVING; } -void RecoveryNode::on_mrm_state(const MrmState::ConstSharedPtr msg){ +void RecoveryNode::on_mrm_state(const MrmState::ConstSharedPtr msg) +{ // set flag if mrm happened by fatal error - if (msg->state != MrmState::NORMAL && 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){ + if (mrm_occur_ && msg->state == MrmState::NORMAL) { mrm_by_fatal_error_ = false; } mrm_occur_ = msg->state != MrmState::NORMAL; @@ -79,12 +82,13 @@ void RecoveryNode::on_mrm_state(const MrmState::ConstSharedPtr msg){ // 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_){ + if (autonomous_available_ && !mrm_by_fatal_error_ && mrm_occur_ && auto_driving_) { clear_mrm(); } } -void RecoveryNode::clear_mrm(){ +void RecoveryNode::clear_mrm() +{ const auto req = std::make_shared(); auto logger = get_logger(); @@ -95,8 +99,7 @@ void RecoveryNode::clear_mrm(){ 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) - { + if (status == std::future_status::timeout) { RCLCPP_INFO(logger, "Service timeout"); return; } diff --git a/system/diagnostic_graph_aggregator/src/node/recovery.hpp b/system/diagnostic_graph_aggregator/src/node/recovery.hpp index ad30a42ab4e69..28861043b477a 100644 --- a/system/diagnostic_graph_aggregator/src/node/recovery.hpp +++ b/system/diagnostic_graph_aggregator/src/node/recovery.hpp @@ -20,11 +20,12 @@ #include // Autoware -#include -#include #include -#include + #include +#include +#include +#include #include #include // Use map for sorting keys. @@ -64,7 +65,6 @@ class RecoveryNode : public rclcpp::Node void on_mrm_state(const MrmState::ConstSharedPtr msg); void clear_mrm(); - }; } // namespace diagnostic_graph_aggregator