feat(mrm_handler): mrm recoverable option (#1316)

launch/tier4_system_launch/launch/system.launch.xml

@@ -22,6 +22,7 @@
 
   <arg name="launch_system_monitor" default="true" description="launch system monitor"/>
   <arg name="launch_dummy_diag_publisher" description="launch dummy diag publisher"/>
+  <arg name="launch_system_recover_operator" default="false" description="launch recover operator"/>
   <arg name="run_mode" default="online" description="options: online, logging_simulation, planning_simulation"/>
   <arg name="sensor_model" description="sensor model name"/>
 
@@ -142,4 +143,9 @@
       <arg name="launch_rqt_runtime_monitor_err" value="$(var launch_rqt_runtime_monitor_err)"/>
     </include>
   </group>
+
+  <!-- System Recover Operator -->
+  <group if="$(var launch_system_recover_operator)">
+    <node pkg="diagnostic_graph_utils" exec="recovery_node" name="recovery"/>
+  </group>
 </launch>

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)

system/diagnostic_graph_utils/package.xml

@@ -10,9 +10,13 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_system_msgs</depend>
+  <depend>component_interface_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>std_srvs</depend>
   <depend>tier4_system_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>

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 <algorithm>
+
+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);
+  callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  srv_clear_mrm_ = create_client<std_srvs::srv::Trigger>(
+    "/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 recovered (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-recoverable 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<std_srvs::srv::Trigger::Request>();
+
+  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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::RecoveryNode)

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 <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+
+// Autoware
+#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>
+
+#include <functional>
+#include <map>  // Use map for sorting keys.
+#include <memory>
+#include <string>
+#include <vector>
+
+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<AutowareState>::SharedPtr sub_aw_state_;
+  rclcpp::Subscription<MrmState>::SharedPtr sub_mrm_state_;
+
+  // service
+  rclcpp::Client<std_srvs::srv::Trigger>::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_

system/mrm_handler/config/mrm_handler.param.yaml

@@ -8,8 +8,10 @@
     timeout_cancel_mrm_behavior: 0.01
     use_emergency_holding: false
     timeout_emergency_recovery: 5.0
+    is_mrm_recoverable: true
     use_parking_after_stopped: false
     use_pull_over: false
+    use_pull_over_after_stopped: false
     use_comfortable_stop: false
 
     # setting whether to turn hazard lamp on for each situation

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

@@ -39,6 +39,7 @@
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 struct HazardLampPolicy
 {
@@ -53,8 +54,10 @@ struct Param
   double timeout_cancel_mrm_behavior;
   bool use_emergency_holding;
   double timeout_emergency_recovery;
+  bool is_mrm_recoverable;
   bool use_parking_after_stopped;
   bool use_pull_over;
+  bool use_pull_over_after_stopped;
   bool use_comfortable_stop;
   HazardLampPolicy turning_hazard_on{};
 };
@@ -93,6 +96,9 @@ class MrmHandler : public rclcpp::Node
 
   void onOperationModeAvailability(
     const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg);
+  void onRecoverMrm(
+    const std_srvs::srv::Trigger::Request::SharedPtr,
+    const std_srvs::srv::Trigger::Response::SharedPtr response);
 
   // Publisher
 
@@ -117,6 +123,9 @@ class MrmHandler : public rclcpp::Node
   rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_;
   rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_emergency_stop_;
 
+  // Services
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service_recover_mrm_;
+
   bool requestMrmBehavior(
     const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
     RequestType request_type) const;
@@ -142,6 +151,7 @@ class MrmHandler : public rclcpp::Node
   // Algorithm
   bool is_emergency_holding_ = false;
   uint8_t last_gear_command_{autoware_vehicle_msgs::msg::GearCommand::DRIVE};
+  bool is_mrm_holding_ = false;
   void transitionTo(const int new_state);
   void updateMrmState();
   void operateMrm();

system/mrm_handler/schema/mrm_handler.schema.json

@@ -36,6 +36,11 @@
           "description": "If the duration of the EMERGENCY state is longer than `timeout_emergency_recovery`, it does not recover to the NORMAL state.",
           "default": 5.0
         },
+        "is_mrm_recoverable": {
+          "type": "boolean",
+          "description": "If this parameter is true, mrm state never return to normal state",
+          "default": false
+        },
         "use_parking_after_stopped": {
           "type": "boolean",
           "description": "If this parameter is true, it will publish PARKING shift command.",
@@ -46,6 +51,11 @@
           "description": "If this parameter is true, operate pull over when latent faults occur.",
           "default": "false"
         },
+        "use_pull_over_after_stopped": {
+          "type": "boolean",
+          "description": "If this parameter is true, pull over can be operated after stopped.",
+          "default": "true"
+        },
         "use_comfortable_stop": {
           "type": "boolean",
           "description": "If this parameter is true, operate comfortable stop when latent faults occur.",
@@ -70,8 +80,10 @@
         "timeout_cancel_mrm_behavior",
         "use_emergency_holding",
         "timeout_emergency_recovery",
+        "is_mrm_recoverable",
         "use_parking_after_stopped",
         "use_pull_over",
+        "use_pull_over_after_stopped",
         "use_comfortable_stop",
         "turning_hazard_on"
       ],