tier4 · tier4-autoware-public-bot · Nov 11, 2022 · Nov 10, 2022
diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml
@@ -26,6 +26,7 @@
     <remap from="input/emergency/control_cmd" to="/system/emergency/control_cmd"/>
     <remap from="input/emergency/hazard_lights_cmd" to="/system/emergency/hazard_lights_cmd"/>
     <remap from="input/emergency/gear_cmd" to="/system/emergency/gear_cmd"/>
+    <remap from="input/mrm_state" to="/system/fail_safe/mrm_state"/>
 
     <remap from="output/vehicle_cmd_emergency" to="/control/command/emergency_cmd"/>
     <remap from="output/control_cmd" to="/control/command/control_cmd"/>

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -70,8 +70,6 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     this->create_publisher<OperationModeState>("output/operation_mode", durable_qos);
 
   // Subscriber
-  emergency_state_sub_ = this->create_subscription<EmergencyState>(
-    "input/emergency_state", 1, std::bind(&VehicleCmdGate::onEmergencyState, this, _1));
   external_emergency_stop_heartbeat_sub_ = this->create_subscription<Heartbeat>(
     "input/external_emergency_stop_heartbeat", 1,
     std::bind(&VehicleCmdGate::onExternalEmergencyStopHeartbeat, this, _1));
@@ -84,6 +82,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   operation_mode_sub_ = this->create_subscription<OperationModeState>(
     "input/operation_mode", rclcpp::QoS(1).transient_local(),
     [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; });
+  mrm_state_sub_ = this->create_subscription<MrmState>(
+    "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1));
 
   // Subscriber for auto
   auto_control_cmd_sub_ = this->create_subscription<AckermannControlCommand>(
@@ -546,14 +546,6 @@ AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd() const
   return cmd;
 }
 
-void VehicleCmdGate::onEmergencyState(EmergencyState::ConstSharedPtr msg)
-{
-  is_system_emergency_ = (msg->state == EmergencyState::MRM_OPERATING) ||
-                         (msg->state == EmergencyState::MRM_SUCCEEDED) ||
-                         (msg->state == EmergencyState::MRM_FAILED);
-  emergency_state_heartbeat_received_time_ = std::make_shared<rclcpp::Time>(this->now());
-}
-
 void VehicleCmdGate::onExternalEmergencyStopHeartbeat(
   [[maybe_unused]] Heartbeat::ConstSharedPtr msg)
 {
@@ -587,6 +579,15 @@ void VehicleCmdGate::onSteering(SteeringReport::ConstSharedPtr msg)
   current_steer_ = msg->steering_tire_angle;
 }
 
+void VehicleCmdGate::onMrmState(MrmState::ConstSharedPtr msg)
+{
+  is_system_emergency_ =
+    (msg->state == MrmState::MRM_OPERATING || msg->state == MrmState::MRM_SUCCEEDED ||
+     msg->state == MrmState::MRM_FAILED) &&
+    (msg->behavior == MrmState::EMERGENCY_STOP);
+  emergency_state_heartbeat_received_time_ = std::make_shared<rclcpp::Time>(this->now());
+}
+
 double VehicleCmdGate::getDt()
 {
   if (!prev_time_) {

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -23,6 +23,7 @@
 #include <std_srvs/srv/trigger.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 #include <autoware_auto_system_msgs/msg/emergency_state.hpp>
@@ -38,16 +39,17 @@
 #include <tier4_external_api_msgs/msg/heartbeat.hpp>
 #include <tier4_external_api_msgs/srv/engage.hpp>
 #include <tier4_external_api_msgs/srv/set_emergency.hpp>
+#include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
 #include <tier4_vehicle_msgs/msg/vehicle_emergency_stamped.hpp>
 
 #include <memory>
 
 namespace vehicle_cmd_gate
 {
 
+using autoware_adapi_v1_msgs::msg::MrmState;
 using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_auto_control_msgs::msg::AckermannControlCommand;
-using autoware_auto_system_msgs::msg::EmergencyState;
 using autoware_auto_vehicle_msgs::msg::GearCommand;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::SteeringReport;
@@ -56,6 +58,7 @@ using tier4_control_msgs::msg::GateMode;
 using tier4_external_api_msgs::msg::Emergency;
 using tier4_external_api_msgs::msg::Heartbeat;
 using tier4_external_api_msgs::srv::SetEmergency;
+using tier4_system_msgs::msg::MrmBehaviorStatus;
 using tier4_vehicle_msgs::msg::VehicleEmergencyStamped;
 
 using diagnostic_msgs::msg::DiagnosticStatus;
@@ -93,22 +96,23 @@ class VehicleCmdGate : public rclcpp::Node
   rclcpp::Publisher<OperationModeState>::SharedPtr operation_mode_pub_;
 
   // Subscription
-  rclcpp::Subscription<EmergencyState>::SharedPtr emergency_state_sub_;
   rclcpp::Subscription<Heartbeat>::SharedPtr external_emergency_stop_heartbeat_sub_;
   rclcpp::Subscription<GateMode>::SharedPtr gate_mode_sub_;
   rclcpp::Subscription<SteeringReport>::SharedPtr steer_sub_;
   rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_sub_;
+  rclcpp::Subscription<MrmState>::SharedPtr mrm_state_sub_;
 
   void onGateMode(GateMode::ConstSharedPtr msg);
-  void onEmergencyState(EmergencyState::ConstSharedPtr msg);
   void onExternalEmergencyStopHeartbeat(Heartbeat::ConstSharedPtr msg);
   void onSteering(SteeringReport::ConstSharedPtr msg);
+  void onMrmState(MrmState::ConstSharedPtr msg);
 
   bool is_engaged_;
   bool is_system_emergency_ = false;
   bool is_external_emergency_stop_ = false;
   double current_steer_ = 0;
   GateMode current_gate_mode_;
+  MrmState current_mrm_state_;
 
   // Heartbeat
   std::shared_ptr<rclcpp::Time> emergency_state_heartbeat_received_time_;

diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py
@@ -187,6 +187,7 @@ def launch_setup(context, *args, **kwargs):
             ("input/emergency/control_cmd", "/system/emergency/control_cmd"),
             ("input/emergency/hazard_lights_cmd", "/system/emergency/hazard_lights_cmd"),
             ("input/emergency/gear_cmd", "/system/emergency/gear_cmd"),
+            ("input/mrm_state", "/system/fail_safe/mrm_state"),
             ("output/vehicle_cmd_emergency", "/control/command/emergency_cmd"),
             ("output/control_cmd", "/control/command/control_cmd"),
             ("output/gear_cmd", "/control/command/gear_cmd"),

diff --git a/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml b/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml
@@ -7,6 +7,7 @@
     timeout_takeover_request: 10.0
     use_takeover_request: false
     use_parking_after_stopped: false
+    use_comfortable_stop: false
 
     # setting whether to turn hazard lamp on for each situation
     turning_hazard_on:

diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml
@@ -66,5 +66,13 @@
         <arg name="config_file" value="$(var tier4_system_launch_param_path)/emergency_handler/emergency_handler.param.yaml"/>
       </include>
     </group>
+
+    <!-- MRM Operator -->
+    <include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.py">
+      <arg name="config_file" value="$(find-pkg-share mrm_comfortable_stop_operator)/config/mrm_comfortable_stop_operator.param.yaml"/>
+    </include>
+    <include file="$(find-pkg-share mrm_emergency_stop_operator)/launch/mrm_emergency_stop_operator.launch.py">
+      <arg name="config_file" value="$(find-pkg-share mrm_emergency_stop_operator)/config/mrm_emergency_stop_operator.param.yaml"/>
+    </include>
   </group>
 </launch>
diff --git a/system/emergency_handler/README.md b/system/emergency_handler/README.md
@@ -14,21 +14,24 @@ Emergency Handler is a node to select proper MRM from from system failure state
 
 ### Input
 
-| Name                              | Type                                                       | Description                                                                   |
-| --------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------------------------------- |
-| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped`      | Used to select proper MRM from system failure state contained in HazardStatus |
-| `/control/vehicle_cmd`            | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Used as reference when generate Emergency Control Command                     |
-| `/localization/kinematic_state`   | `nav_msgs::msg::Odometry`                                  | Used to decide whether vehicle is stopped or not                              |
-| `/vehicle/status/control_mode`    | `autoware_auto_vehicle_msgs::msg::ControlModeReport`       | Used to check vehicle mode: autonomous or manual.                             |
+| Name                                      | Type                                                       | Description                                                                   |
+| ----------------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------------------------------- |
+| `/system/emergency/hazard_status`         | `autoware_auto_system_msgs::msg::HazardStatusStamped`      | Used to select proper MRM from system failure state contained in HazardStatus |
+| `/control/vehicle_cmd`                    | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Used as reference when generate Emergency Control Command                     |
+| `/localization/kinematic_state`           | `nav_msgs::msg::Odometry`                                  | Used to decide whether vehicle is stopped or not                              |
+| `/vehicle/status/control_mode`            | `autoware_auto_vehicle_msgs::msg::ControlModeReport`       | Used to check vehicle mode: autonomous or manual                              |
+| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus`                | Used to check if MRM comfortable stop operation is available                  |
+| `/system/api/mrm/emergency_stop/status`   | `tier4_system_msgs::msg::MrmBehaviorStatus`                | Used to check if MRM emergency stop operation is available                    |
 
 ### Output
 
-| Name                                | Type                                                       | Description                                           |
-| ----------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------- |
-| `/system/emergency/control_cmd`     | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Required to execute proper MRM                        |
-| `/system/emergency/shift_cmd`       | `autoware_auto_vehicle_msgs::msg::GearCommand`             | Required to execute proper MRM (send gear cmd)        |
-| `/system/emergency/hazard_cmd`      | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand`     | Required to execute proper MRM (send turn signal cmd) |
-| `/system/emergency/emergency_state` | `autoware_auto_system_msgs::msg::EmergencyStateStamped`    | Used to inform the emergency situation of the vehicle |
+| Name                                       | Type                                                   | Description                                           |
+| ------------------------------------------ | ------------------------------------------------------ | ----------------------------------------------------- |
+| `/system/emergency/shift_cmd`              | `autoware_auto_vehicle_msgs::msg::GearCommand`         | Required to execute proper MRM (send gear cmd)        |
+| `/system/emergency/hazard_cmd`             | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) |
+| `/api/fail_safe/mrm_state`                 | `autoware_adapi_v1_msgs::msg::MrmState`                | Inform MRM execution state and selected MRM behavior  |
+| `/system/api/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm`                   | Execution order for MRM comfortable stop              |
+| `/system/api/mrm/emergency_stop/operate`   | `tier4_system_msgs::srv::OperateMrm`                   | Execution order for MRM emergency stop                |
 
 ## Parameters
 
@@ -45,6 +48,7 @@ Emergency Handler is a node to select proper MRM from from system failure state
 | timeout_hazard_status       | double | `0.5`         | If the input `hazard_status` topic cannot be received for more than `timeout_hazard_status`, vehicle will make an emergency stop. |
 | use_parking_after_stopped   | bool   | `false`       | If this parameter is true, it will publish PARKING shift command.                                                                 |
 | turning_hazard_on.emergency | bool   | `true`        | If this parameter is true, hazard lamps will be turned on during emergency state.                                                 |
+| use_comfortable_stop        | bool   | `false`       | If this parameter is true, operate comfortable stop when latent faults occur.                                                     |
 
 ## Assumptions / Known limits
 

diff --git a/system/emergency_handler/config/emergency_handler.param.yaml b/system/emergency_handler/config/emergency_handler.param.yaml
@@ -7,6 +7,7 @@
     timeout_takeover_request: 10.0
     use_takeover_request: false
     use_parking_after_stopped: false
+    use_comfortable_stop: false
 
     # setting whether to turn hazard lamp on for each situation
     turning_hazard_on:

diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
@@ -20,12 +20,14 @@
 #include <string>
 
 // Autoware
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
-#include <autoware_auto_system_msgs/msg/emergency_state.hpp>
 #include <autoware_auto_system_msgs/msg/hazard_status_stamped.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/srv/operate_mrm.hpp>
 
 // ROS2 core
 #include <rclcpp/create_timer.hpp>
@@ -46,6 +48,7 @@ struct Param
   double timeout_takeover_request;
   bool use_takeover_request;
   bool use_parking_after_stopped;
+  bool use_comfortable_stop;
   HazardLampPolicy turning_hazard_on{};
 };
 
@@ -63,18 +66,28 @@ class EmergencyHandler : public rclcpp::Node
   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::MrmBehaviorStatus>::SharedPtr
+    sub_mrm_comfortable_stop_status_;
+  rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
+    sub_mrm_emergency_stop_status_;
 
   autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_;
   autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr prev_control_command_;
   nav_msgs::msg::Odometry::ConstSharedPtr odom_;
   autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_;
+  tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_;
+  tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_;
 
   void onHazardStatusStamped(
     const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg);
   void onPrevControlCommand(
     const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
   void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
   void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
+  void onMrmComfortableStopStatus(
+    const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
+  void onMrmEmergencyStopStatus(
+    const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
 
   // Publisher
   rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
@@ -85,12 +98,30 @@ class EmergencyHandler : public rclcpp::Node
   rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::HazardLightsCommand>::SharedPtr
     pub_hazard_cmd_;
   rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::GearCommand>::SharedPtr pub_gear_cmd_;
-  rclcpp::Publisher<autoware_auto_system_msgs::msg::EmergencyState>::SharedPtr pub_emergency_state_;
 
   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_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_;
 
@@ -104,15 +135,14 @@ class EmergencyHandler : public rclcpp::Node
   rclcpp::Time stamp_hazard_status_;
 
   // Algorithm
-  autoware_auto_system_msgs::msg::EmergencyState::_state_type emergency_state_{
-    autoware_auto_system_msgs::msg::EmergencyState::NORMAL};
   rclcpp::Time takeover_requested_time_;
-
+  bool is_takeover_request_ = false;
   void transitionTo(const int new_state);
-  void updateEmergencyState();
+  void updateMrmState();
+  void operateMrm();
+  autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
   bool isStopped();
   bool isEmergency(const autoware_auto_system_msgs::msg::HazardStatus & hazard_status);
-  autoware_auto_control_msgs::msg::AckermannControlCommand selectAlternativeControlCommand();
 };
 
 #endif  // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_
diff --git a/system/emergency_handler/launch/emergency_handler.launch.xml b/system/emergency_handler/launch/emergency_handler.launch.xml
@@ -4,11 +4,14 @@
   <arg name="input_prev_control_command" default="/control/command/control_cmd"/>
   <arg name="input_odometry" default="/localization/kinematic_state"/>
   <arg name="input_control_mode" default="/vehicle/status/control_mode"/>
+  <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="output_control_command" default="/system/emergency/control_cmd"/>
   <arg name="output_gear" default="/system/emergency/gear_cmd"/>
   <arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/>
-  <arg name="output_emergency_state" default="/system/emergency/emergency_state"/>
+  <arg name="output_mrm_state" default="/system/fail_safe/mrm_state"/>
+  <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 emergency_handler)/config/emergency_handler.param.yaml"/>
 
@@ -18,11 +21,14 @@
     <remap from="~/input/prev_control_command" to="$(var input_prev_control_command)"/>
     <remap from="~/input/odometry" to="$(var input_odometry)"/>
     <remap from="~/input/control_mode" to="$(var input_control_mode)"/>
+    <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="~/output/control_command" to="$(var output_control_command)"/>
     <remap from="~/output/gear" to="$(var output_gear)"/>
     <remap from="~/output/hazard" to="$(var output_hazard)"/>
-    <remap from="~/output/emergency_state" to="$(var output_emergency_state)"/>
+    <remap from="~/output/mrm/state" to="$(var output_mrm_state)"/>
+    <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>