diff --git a/system/diagnostic_graph_utils/src/node/recovery.cpp b/system/diagnostic_graph_utils/src/node/recovery.cpp index 57b64c0e0b16b..da5cfb87cba8e 100644 --- a/system/diagnostic_graph_utils/src/node/recovery.cpp +++ b/system/diagnostic_graph_utils/src/node/recovery.cpp @@ -22,16 +22,11 @@ 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); @@ -63,11 +58,6 @@ void RecoveryNode::on_graph_update(DiagGraph::ConstSharedPtr graph) } } -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 @@ -82,8 +72,7 @@ void RecoveryNode::on_mrm_state(const MrmState::ConstSharedPtr msg) // 1. Not emergency // 2. Non-recoverable 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_) { clear_mrm(); } } diff --git a/system/diagnostic_graph_utils/src/node/recovery.hpp b/system/diagnostic_graph_utils/src/node/recovery.hpp index 0d01a07d3db77..e79a97b4b4234 100644 --- a/system/diagnostic_graph_utils/src/node/recovery.hpp +++ b/system/diagnostic_graph_utils/src/node/recovery.hpp @@ -25,7 +25,6 @@ #include #include -#include #include #include @@ -44,17 +43,14 @@ class RecoveryNode : public rclcpp::Node 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 @@ -64,7 +60,6 @@ class RecoveryNode : public rclcpp::Node 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();