feat(mrm_handler): add mrm_handler

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
+    use_emergency_holding: false
+    timeout_emergency_recovery: 5.0
+    timeout_takeover_request: 10.0
+    use_takeover_request: false
+    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,160 @@
+// 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;
+  bool use_emergency_holding;
+  double timeout_emergency_recovery;
+  double timeout_takeover_request;
+  bool use_takeover_request;
+  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:
+  // 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_;
+
+  autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg();
+  autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg();
+  void publishControlCommands();
+
+  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_;
+
+  void callMrmBehavior(
+    const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const;
+  void cancelMrmBehavior(
+    const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) 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;
+
+  // Algorithm
+  rclcpp::Time takeover_requested_time_;
+  bool is_takeover_request_ = false;
+  bool is_emergency_holding_ = false;
+  void transitionTo(const int new_state);
+  void updateMrmState();
+  void operateMrm();
+  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>

system/mrm_handler/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mrm_handler</name>
+  <version>0.1.0</version>
+  <description>The mrm_handler ROS 2 package</description>
+  <maintainer email="makoto.kurihara@tier4.jp">Makoto Kurihara</maintainer>
+  <maintainer email="ryuta.kambe@tier4.jp">Ryuta Kambe</maintainer>
+  <maintainer email="tetsuhiro.kawaguchi@tier4.jp">Tetsuhiro Kawaguchi</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_system_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tier4_system_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>