Merge pull request autowarefoundation#1215 from tier4/feature/add-mrm…

…-handler

feat(mrm_handler): add mrm handler

system/emergency_handler/config/emergency_handler.param.yaml

@@ -4,8 +4,6 @@
   ros__parameters:
     update_rate: 10
     timeout_hazard_status: 0.5
-    timeout_takeover_request: 10.0
-    use_takeover_request: false
     use_parking_after_stopped: false
     use_comfortable_stop: false
 

system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp

@@ -45,8 +45,6 @@ struct Param
 {
   int update_rate;
   double timeout_hazard_status;
-  double timeout_takeover_request;
-  bool use_takeover_request;
   bool use_parking_after_stopped;
   bool use_comfortable_stop;
   HazardLampPolicy turning_hazard_on{};
@@ -135,8 +133,6 @@ class EmergencyHandler : public rclcpp::Node
   rclcpp::Time stamp_hazard_status_;
 
   // Algorithm
-  rclcpp::Time takeover_requested_time_;
-  bool is_takeover_request_ = false;
   void transitionTo(const int new_state);
   void updateMrmState();
   void operateMrm();

system/emergency_handler/schema/emergency_handler.schema.json

@@ -16,16 +16,6 @@
           "description": "If the input `hazard_status` topic cannot be received for more than `timeout_hazard_status`, vehicle will make an emergency stop.",
           "default": 0.5
         },
-        "timeout_takeover_request": {
-          "type": "number",
-          "description": "Transition to MRR_OPERATING if the time from the last takeover request exceeds `timeout_takeover_request`.",
-          "default": 10.0
-        },
-        "use_takeover_request": {
-          "type": "boolean",
-          "description": "If this parameter is true, the handler will record the time and make take over request to the driver when emergency state occurs.",
-          "default": "false"
-        },
         "use_parking_after_stopped": {
           "type": "boolean",
           "description": "If this parameter is true, it will publish PARKING shift command.",
@@ -51,8 +41,6 @@
       "required": [
         "update_rate",
         "timeout_hazard_status",
-        "timeout_takeover_request",
-        "use_takeover_request",
         "use_parking_after_stopped",
         "use_comfortable_stop",
         "turning_hazard_on"

system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

@@ -23,8 +23,6 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
   // Parameter
   param_.update_rate = declare_parameter<int>("update_rate");
   param_.timeout_hazard_status = declare_parameter<double>("timeout_hazard_status");
-  param_.timeout_takeover_request = declare_parameter<double>("timeout_takeover_request");
-  param_.use_takeover_request = declare_parameter<bool>("use_takeover_request");
   param_.use_parking_after_stopped = declare_parameter<bool>("use_parking_after_stopped");
   param_.use_comfortable_stop = declare_parameter<bool>("use_comfortable_stop");
   param_.turning_hazard_on.emergency = declare_parameter<bool>("turning_hazard_on.emergency");
@@ -375,41 +373,23 @@ void EmergencyHandler::updateMrmState()
 
   // Get mode
   const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS;
-  const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL;
 
   // State Machine
   if (mrm_state_.state == MrmState::NORMAL) {
     // NORMAL
     if (is_auto_mode && is_emergency) {
-      if (param_.use_takeover_request) {
-        takeover_requested_time_ = this->get_clock()->now();
-        is_takeover_request_ = true;
-        return;
-      } else {
-        transitionTo(MrmState::MRM_OPERATING);
-        return;
-      }
+      transitionTo(MrmState::MRM_OPERATING);
+      return;
     }
   } else {
     // Emergency
-    // Send recovery events if "not emergency" or "takeover done"
+    // Send recovery events if "not emergency"
     if (!is_emergency) {
       transitionTo(MrmState::NORMAL);
       return;
     }
-    // TODO(Kenji Miyake): Check if human can safely override, for example using DSM
-    if (is_takeover_done) {
-      transitionTo(MrmState::NORMAL);
-      return;
-    }
 
-    if (is_takeover_request_) {
-      const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_;
-      if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) {
-        transitionTo(MrmState::MRM_OPERATING);
-        return;
-      }
-    } else if (mrm_state_.state == MrmState::MRM_OPERATING) {
+    if (mrm_state_.state == MrmState::MRM_OPERATING) {
       // TODO(Kenji Miyake): Check MRC is accomplished
       if (isStopped()) {
         transitionTo(MrmState::MRM_SUCCEEDED);

system/mrm_handler/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.14)
+project(mrm_handler)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(mrm_handler
+  src/mrm_handler/mrm_handler_node.cpp
+  src/mrm_handler/mrm_handler_core.cpp
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)

system/mrm_handler/README.md

@@ -0,0 +1,44 @@
+# mrm_handler
+
+## Purpose
+
+MRM Handler is a node to select a proper MRM from a system failure state contained in OperationModeAvailability.
+
+## Inner-workings / Algorithms
+
+### State Transitions
+
+![mrm-state](image/mrm-state.svg)
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                   | Type                                                 | Description                                                                                         |
+| -------------------------------------- | ---------------------------------------------------- | --------------------------------------------------------------------------------------------------- |
+| `/localization/kinematic_state`        | `nav_msgs::msg::Odometry`                            | Used to decide whether vehicle is stopped or not                                                    |
+| `/system/operation_mode/availability`  | `tier4_system_msgs::msg::OperationModeAvailability`  | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability |
+| `/vehicle/status/control_mode`         | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual                                                    |
+| `/system/mrm/emergency_stop/status`    | `tier4_system_msgs::msg::MrmBehaviorStatus`          | Used to check if MRM emergency stop operation is available                                          |
+| `/system/mrm/comfortable_stop/status`  | `tier4_system_msgs::msg::MrmBehaviorStatus`          | Used to check if MRM comfortable stop operation is available                                        |
+| `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus`          | Used to check if MRM pull over operation is available                                               |
+| `/api/operation_mode/state`            | `autoware_adapi_v1_msgs::msg::OperationModeState`    | Used to check whether the current operation mode is AUTO or STOP.                                   |
+
+### Output
+
+| Name                                    | Type                                                   | Description                                           |
+| --------------------------------------- | ------------------------------------------------------ | ----------------------------------------------------- |
+| `/system/emergency/gear_cmd`            | `autoware_auto_vehicle_msgs::msg::GearCommand`         | Required to execute proper MRM (send gear cmd)        |
+| `/system/emergency/hazard_lights_cmd`   | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) |
+| `/system/fail_safe/mrm_state`           | `autoware_adapi_v1_msgs::msg::MrmState`                | Inform MRM execution state and selected MRM behavior  |
+| `/system/mrm/emergency_stop/operate`    | `tier4_system_msgs::srv::OperateMrm`                   | Execution order for MRM emergency stop                |
+| `/system/mrm/comfortable_stop/operate`  | `tier4_system_msgs::srv::OperateMrm`                   | Execution order for MRM comfortable stop              |
+| `/system/mrm/pull_over_manager/operate` | `tier4_system_msgs::srv::OperateMrm`                   | Execution order for MRM pull over                     |
+
+## Parameters
+
+{{ json_to_markdown("system/mrm_handler/schema/mrm_handler.schema.json") }}
+
+## Assumptions / Known limits
+
+TBD.

system/mrm_handler/config/mrm_handler.param.yaml

@@ -0,0 +1,17 @@
+# Default configuration for mrm handler
+---
+/**:
+  ros__parameters:
+    update_rate: 10
+    timeout_operation_mode_availability: 0.5
+    timeout_call_mrm_behavior: 0.01
+    timeout_cancel_mrm_behavior: 0.01
+    use_emergency_holding: false
+    timeout_emergency_recovery: 5.0
+    use_parking_after_stopped: false
+    use_pull_over: false
+    use_comfortable_stop: false
+
+    # setting whether to turn hazard lamp on for each situation
+    turning_hazard_on:
+      emergency: true

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

@@ -0,0 +1,162 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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 MRM_HANDLER__MRM_HANDLER_CORE_HPP_
+#define MRM_HANDLER__MRM_HANDLER_CORE_HPP_
+
+// Core
+#include <memory>
+#include <optional>
+#include <string>
+#include <variant>
+
+// Autoware
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
+#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
+#include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
+#include <tier4_system_msgs/msg/operation_mode_availability.hpp>
+#include <tier4_system_msgs/srv/operate_mrm.hpp>
+
+// ROS 2 core
+#include <rclcpp/create_timer.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+struct HazardLampPolicy
+{
+  bool emergency;
+};
+
+struct Param
+{
+  int update_rate;
+  double timeout_operation_mode_availability;
+  double timeout_call_mrm_behavior;
+  double timeout_cancel_mrm_behavior;
+  bool use_emergency_holding;
+  double timeout_emergency_recovery;
+  bool use_parking_after_stopped;
+  bool use_pull_over;
+  bool use_comfortable_stop;
+  HazardLampPolicy turning_hazard_on{};
+};
+
+class MrmHandler : public rclcpp::Node
+{
+public:
+  MrmHandler();
+
+private:
+  // type
+  enum RequestType { CALL, CANCEL };
+
+  // Subscribers
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
+  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
+    sub_control_mode_;
+  rclcpp::Subscription<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr
+    sub_operation_mode_availability_;
+  rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
+    sub_mrm_pull_over_status_;
+  rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
+    sub_mrm_comfortable_stop_status_;
+  rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
+    sub_mrm_emergency_stop_status_;
+  rclcpp::Subscription<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr
+    sub_operation_mode_state_;
+
+  nav_msgs::msg::Odometry::ConstSharedPtr odom_;
+  autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_;
+  tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_;
+  tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_pull_over_status_;
+  tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_;
+  tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_;
+  autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_;
+
+  void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
+  void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
+  void onOperationModeAvailability(
+    const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg);
+  void onMrmPullOverStatus(const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
+  void onMrmComfortableStopStatus(
+    const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
+  void onMrmEmergencyStopStatus(
+    const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
+  void onOperationModeState(
+    const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg);
+
+  // Publisher
+
+  // rclcpp::Publisher<tier4_vehicle_msgs::msg::ShiftStamped>::SharedPtr pub_shift_;
+  // rclcpp::Publisher<tier4_vehicle_msgs::msg::TurnSignal>::SharedPtr pub_turn_signal_;
+  rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::HazardLightsCommand>::SharedPtr
+    pub_hazard_cmd_;
+  rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::GearCommand>::SharedPtr pub_gear_cmd_;
+
+  void publishHazardCmd();
+  void publishGearCmd();
+
+  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
+
+  autoware_adapi_v1_msgs::msg::MrmState mrm_state_;
+  void publishMrmState();
+
+  // Clients
+  rclcpp::CallbackGroup::SharedPtr client_mrm_pull_over_group_;
+  rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_pull_over_;
+  rclcpp::CallbackGroup::SharedPtr client_mrm_comfortable_stop_group_;
+  rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_comfortable_stop_;
+  rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_;
+  rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_emergency_stop_;
+
+  bool requestMrmBehavior(
+    const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
+    RequestType request_type) const;
+  void logMrmCallingResult(
+    const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior,
+    bool is_call) const;
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  // Parameters
+  Param param_;
+
+  bool isDataReady();
+  void onTimer();
+
+  // Heartbeat
+  rclcpp::Time stamp_operation_mode_availability_;
+  std::optional<rclcpp::Time> stamp_autonomous_become_unavailable_ = std::nullopt;
+  bool is_operation_mode_availability_timeout;
+  void checkOperationModeAvailabilityTimeout();
+
+  // Algorithm
+  bool is_emergency_holding_ = false;
+  void transitionTo(const int new_state);
+  void updateMrmState();
+  void operateMrm();
+  void handleFailedRequest();
+  autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
+  bool isStopped();
+  bool isEmergency() const;
+  bool isArrivedAtGoal();
+};
+
+#endif  // MRM_HANDLER__MRM_HANDLER_CORE_HPP_

system/mrm_handler/launch/mrm_handler.launch.xml

@@ -0,0 +1,39 @@
+<launch>
+  <arg name="input_operation_mode_availability" default="/system/operation_mode/availability"/>
+  <!-- To be replaced by ControlCommand -->
+  <arg name="input_odometry" default="/localization/kinematic_state"/>
+  <arg name="input_control_mode" default="/vehicle/status/control_mode"/>
+  <arg name="input_mrm_pull_over_state" default="/system/mrm/pull_over_manager/status"/>
+  <arg name="input_mrm_comfortable_stop_state" default="/system/mrm/comfortable_stop/status"/>
+  <arg name="input_mrm_emergency_stop_state" default="/system/mrm/emergency_stop/status"/>
+  <arg name="input_api_operation_mode_state" default="/api/operation_mode/state"/>
+
+  <arg name="output_gear" default="/system/emergency/gear_cmd"/>
+  <arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/>
+  <arg name="output_mrm_state" default="/system/fail_safe/mrm_state"/>
+  <arg name="output_mrm_pull_over_operate" default="/system/mrm/pull_over_manager/operate"/>
+  <arg name="output_mrm_comfortable_stop_operate" default="/system/mrm/comfortable_stop/operate"/>
+  <arg name="output_mrm_emergency_stop_operate" default="/system/mrm/emergency_stop/operate"/>
+
+  <arg name="config_file" default="$(find-pkg-share mrm_handler)/config/mrm_handler.param.yaml"/>
+
+  <!-- mrm_handler -->
+  <node pkg="mrm_handler" exec="mrm_handler" name="mrm_handler" output="screen">
+    <remap from="~/input/operation_mode_availability" to="$(var input_operation_mode_availability)"/>
+    <remap from="~/input/odometry" to="$(var input_odometry)"/>
+    <remap from="~/input/control_mode" to="$(var input_control_mode)"/>
+    <remap from="~/input/mrm/pull_over/status" to="$(var input_mrm_pull_over_state)"/>
+    <remap from="~/input/mrm/comfortable_stop/status" to="$(var input_mrm_comfortable_stop_state)"/>
+    <remap from="~/input/mrm/emergency_stop/status" to="$(var input_mrm_emergency_stop_state)"/>
+    <remap from="~/input/api/operation_mode/state" to="$(var input_api_operation_mode_state)"/>
+
+    <remap from="~/output/gear" to="$(var output_gear)"/>
+    <remap from="~/output/hazard" to="$(var output_hazard)"/>
+    <remap from="~/output/mrm/state" to="$(var output_mrm_state)"/>
+    <remap from="~/output/mrm/pull_over/operate" to="$(var output_mrm_pull_over_operate)"/>
+    <remap from="~/output/mrm/comfortable_stop/operate" to="$(var output_mrm_comfortable_stop_operate)"/>
+    <remap from="~/output/mrm/emergency_stop/operate" to="$(var output_mrm_emergency_stop_operate)"/>
+
+    <param from="$(var config_file)"/>
+  </node>
+</launch>