fix(recovery_node): recover from MRM even duaring manual driving (#1777)

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

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<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);
@@ -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();
   }
 }

system/diagnostic_graph_utils/src/node/recovery.hpp

@@ -25,7 +25,6 @@
 #include <component_interface_utils/rclcpp.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
-#include <autoware_system_msgs/msg/autoware_state.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <std_srvs/srv/trigger.hpp>
 
@@ -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<AutowareState>::SharedPtr sub_aw_state_;
   rclcpp::Subscription<MrmState>::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();