feat: add emergency_handler package (#13)

* release v0.4.0 * remove ROS1 packages temporarily Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Revert "remove ROS1 packages temporarily" This reverts commit bb6fdb15dd23df7eab978525dd7a1541cf61df4a. Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Rename launch files to launch.xml (autowarefoundation#28) * ROS2 Porting: emergency_handler (autowarefoundation#40) * Pull emergency handler package from master for porting * Fix CMakeList - Change to format similar to the simple_simulation_planner - Doesn't compile due to src files * Rename of core implementation to be compliant with ROS2 naming guidelines * Use the node/core naming convention - Add _core file to hold the implementation - _node file now holds the executable - Modify CMakeLists to point to the new executable * Conversion of msg types - Compiles now - Add back functions * Convert node intrinsics to ROS2 - Add subscriptions - Add publisher - Add timer subscription - Add publishing and logging * Rearrange files and folders * Fix cmake and package xml * Interface with parameters and fix launch file - Fix configuration files - Correct main implementation - Clean up - Fix headers * Use correct timer implementation * Use throttle logs * Use class method to get time instead of static method * Make parameter file agnostic to node name Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> * fix duration unit for RCLCPP_*_THROTTLE (autowarefoundation#75) Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143) * Use quotes for includes where appropriate (autowarefoundation#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Add linters (autowarefoundation#206) * Ros2 v0.8.0 emergency handler (autowarefoundation#280) * Ros2 v0.8.0 fix packages2 (autowarefoundation#354) * fix topic name * fix duration rate * fix sensing.yaml * fix topic name * add latch-option to autoware_state_monitor * fix timer callback * fix autoware state monitor config * fix sensing.yaml * fix emergency handler (autowarefoundation#361) * Sync with Ros2 v0.8.0 beta (autowarefoundation#393) * add nullptr check when publish concatenate data (autowarefoundation#369) * Add warning msg when concat pointcloud is not published (autowarefoundation#388) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * add timeout notification stamp msgs (autowarefoundation#363) * add timeout notification stamp msgs * fix uncursify * delete timeout notification stamped * Revert "delete timeout notification stamped" This reverts commit 365d29209f6a7f5ec75eb80c5d8c2ef38daeae79. * fix message Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * fix state_timeout (autowarefoundation#399) Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * fix param name (autowarefoundation#412) * add timeout_ignore_state to emergency_handler (autowarefoundation#415) * add timeout_ignore_state in emergency_handler * do not ignore is_state_timeout_->is_timeout * sort condition * Ros2 fix topic name part1 (autowarefoundation#408) * Fix topic name of lane_departure_checker debug Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of mpc_follower debug Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of velocity_controller debug Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of motion_velocity_optimizer debug Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of lane_change_planner debug Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of behavior_velocity_planner debug Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of obstacle_avoidance_planner debug Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of motion_velocity_optimizer Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of lane_departure_checker Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of mpc_follower Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of velocity_controller Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of lane_change_planner Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of obstacle_avoidance_planner Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of obstacle_stop_planner Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of freespace_planner Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of surround_obstacle_checker Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of emergency_handler Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint errors Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix typo Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix launch arg (autowarefoundation#426) Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * add use_sim-time option (autowarefoundation#454) * Fix for rolling (autowarefoundation#1226) * Replace doc by description Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Replace ns by push-ros-namespace Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove state_timeout_checker (autowarefoundation#1247) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Prevent emergency hold during manual driving (autowarefoundation#1390) * Prevent emergency hold during manual driving Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Update README.md * Fix typo * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * Add autoware api (autowarefoundation#1979) * enable autonomous recovery (autowarefoundation#1904) * Use EmergencyState instead of deprecated EmergencyMode (autowarefoundation#2030) * Use EmergencyState instead of deprecated EmergencyMode Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Use stamped type Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Add takeover-request feature in emergency_handler (autowarefoundation#2032) * Add takeover-request feature in emergency_handler Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Rename tor to takeover_request Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Use transitionTo Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Replace image URL Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add error handling Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix state transition Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix state transition Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix cpplint Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Return after state transition Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix state transition Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add initialization for control mode (autowarefoundation#2118) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * [emergency_handler]support autoware.auto msg (autowarefoundation#513) * support auto msg * add readme * remove VehicleStateReport/VehicleStateCommand/VehicleControlCommand (autowarefoundation#549) * fix autoware_error_monitor * fix state monitor * fix emergency handler(vehicle_state_report) * fix emergency Handler(vehicle_state_command) * fix shift_decider * fix emergency_handler(vehicle_control_command) * fix topic name * fix readme * Update system/autoware_state_monitor/Readme.md Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * fix format * Update system/autoware_state_monitor/launch/autoware_state_monitor.launch.xml Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * fix typo Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * [autowere_web_controller/autoware_state_monitor/emergency_handler]fix some packages (autowarefoundation#603) * fix vehicle_engage.js * fix autoware_state_monitor param * fix emergency_handler * oh * update autoware_state.js * fix topic name (autowarefoundation#679) * Fix/psim topics emergency handler awapi (autowarefoundation#702) * fix emergency handler * fix awapi * remove unused topic * remove duplecated vehicle cmd Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> Co-authored-by: Nikolai Morin <nnmmgit@gmail.com> Co-authored-by: Jilada Eccleston <jilada.eccleston@gmail.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
badai-nguyen · Dec 5, 2021 · 6639b57 · 6639b57
1 parent 3c03b5d
commit 6639b57
Show file tree

Hide file tree

Showing 8 changed files with 625 additions and 0 deletions.
diff --git a/system/emergency_handler/CMakeLists.txt b/system/emergency_handler/CMakeLists.txt
@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5)
+project(emergency_handler)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+### Dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+### Target executable
+set(EMERGENCY_HANDLER_SRC
+  src/emergency_handler/emergency_handler_core.cpp)
+
+set(EMERGENCY_HANDLER_HEADERS
+  include/emergency_handler/emergency_handler_core.hpp)
+
+ament_auto_add_executable(emergency_handler
+  src/emergency_handler/emergency_handler_node.cpp
+  ${EMERGENCY_HANDLER_SRC}
+  ${EMERGENCY_HANDLER_HEADERS}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)
diff --git a/system/emergency_handler/README.md b/system/emergency_handler/README.md
@@ -0,0 +1,51 @@
+# emergency_handler
+
+## Purpose
+
+Emergency Handler is a node to select proper MRM from from system failure state contained in HazardStatus.
+
+## Inner-workings / Algorithms
+
+### State Transitions
+
+![fail-safe-state](https://tier4.github.io/autoware.proj/tree/main/design/apis/image/fail-safe-state.drawio.svg)
+
+## Inputs / Outputs
+
+### 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.                             |
+
+### 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 |
+
+## Parameters
+
+### Node Parameters
+
+| Name        | Type | Default Value | Explanation            |
+| ----------- | ---- | ------------- | ---------------------- |
+| update_rate | int  | `10`          | Timer callback period. |
+
+### Core Parameters
+
+| Name                        | Type   | Default Value | Explanation                                                                                                                       |
+| --------------------------- | ------ | ------------- | --------------------------------------------------------------------------------------------------------------------------------- |
+| 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.                                                 |
+
+## Assumptions / Known limits
+
+TBD.
diff --git a/system/emergency_handler/config/emergency_handler.param.yaml b/system/emergency_handler/config/emergency_handler.param.yaml
@@ -0,0 +1,13 @@
+# Default configuration for emergency handler
+---
+/**:
+  ros__parameters:
+    update_rate: 10
+    timeout_hazard_status: 0.5
+    timeout_takeover_request: 10.0
+    use_takeover_request: false
+    use_parking_after_stopped: false
+
+    # setting whether to turn hazard lamp on for each situation
+    turning_hazard_on:
+      emergency: true
diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
@@ -0,0 +1,120 @@
+// Copyright 2020 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 EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_
+#define EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_
+
+// Core
+#include <memory>
+#include <string>
+
+// Autoware
+#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>
+
+// ROS2 core
+#include <autoware_utils/system/heartbeat_checker.hpp>
+#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_hazard_status;
+  double timeout_takeover_request;
+  bool use_takeover_request;
+  bool use_parking_after_stopped;
+  HazardLampPolicy turning_hazard_on{};
+};
+
+class EmergencyHandler : public rclcpp::Node
+{
+public:
+  EmergencyHandler();
+
+private:
+  // Subscribers
+  rclcpp::Subscription<autoware_auto_system_msgs::msg::HazardStatusStamped>::SharedPtr
+    sub_hazard_status_stamped_;
+  rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
+    sub_prev_control_command_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
+  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
+    sub_control_mode_;
+
+  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_;
+
+  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);
+
+  // Publisher
+  rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
+    pub_control_command_;
+
+  // rclcpp::Publisher<autoware_vehicle_msgs::msg::ShiftStamped>::SharedPtr pub_shift_;
+  // rclcpp::Publisher<autoware_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_;
+  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();
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  // Parameters
+  Param param_;
+
+  bool isDataReady();
+  void onTimer();
+
+  // Heartbeat
+  std::shared_ptr<HeaderlessHeartbeatChecker<autoware_auto_system_msgs::msg::HazardStatusStamped>>
+    heartbeat_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_;
+
+  void transitionTo(const int new_state);
+  void updateEmergencyState();
+  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
@@ -0,0 +1,29 @@
+<launch>
+  <arg name="input_hazard_status" default="/system/emergency/hazard_status" />
+  <!-- To be replaced by ControlCommand -->
+  <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="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="config_file" default="$(find-pkg-share emergency_handler)/config/emergency_handler.param.yaml" />
+
+  <!-- emergency_handler -->
+  <node pkg="emergency_handler" exec="emergency_handler" name="emergency_handler" output="screen">
+    <remap from="~/input/hazard_status" to="$(var input_hazard_status)"/>
+    <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="~/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)"/>
+
+    <param from="$(var config_file)" />
+  </node>
+</launch>
diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml
@@ -0,0 +1,26 @@
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>emergency_handler</name>
+  <version>0.1.0</version>
+  <description>The emergency_handler ROS2 package</description>
+  <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_system_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>