diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml
index 70633869c6fc5..1e43464ee954d 100644
--- a/launch/tier4_system_launch/launch/system.launch.xml
+++ b/launch/tier4_system_launch/launch/system.launch.xml
@@ -92,6 +92,12 @@
+
+
+
+
+
+
diff --git a/system/emergency_goal_manager/CMakeLists.txt b/system/emergency_goal_manager/CMakeLists.txt
new file mode 100644
index 0000000000000..834ed25931e14
--- /dev/null
+++ b/system/emergency_goal_manager/CMakeLists.txt
@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.14)
+project(emergency_goal_manager)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(emergency_goal_manager
+ src/emergency_goal_manager_node.cpp
+ src/emergency_goal_manager_core.cpp
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+ launch
+ config
+)
diff --git a/system/emergency_goal_manager/README.md b/system/emergency_goal_manager/README.md
new file mode 100644
index 0000000000000..c27ed818a50a4
--- /dev/null
+++ b/system/emergency_goal_manager/README.md
@@ -0,0 +1,39 @@
+# emergency_goal_manager
+
+## Purpose
+
+The Emergency goal manager is responsible for coordinating the goal poses for emergency rerouting and communicating it to the mission planner.
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+| Name | Type | Description |
+| ---- | ---- | ----------- |
+| | | |
+
+### Output
+
+| Name | Type | Description |
+| ---- | ---- | ----------- |
+| | | |
+
+## Parameters
+
+### Node Parameters
+
+| Name | Type | Default value | Explanation |
+| ---- | ---- | ------------- | ----------- |
+| | | | |
+
+### Core Parameters
+
+| Name | Type | Default value | Explanation |
+| ---- | ---- | ------------- | ----------- |
+| | | | |
+
+## Assumptions / Known limits
+
+TBD.
diff --git a/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml b/system/emergency_goal_manager/config/emergency_goal_manager.param.yaml
new file mode 100644
index 0000000000000..e69de29bb2d1d
diff --git a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp
new file mode 100644
index 0000000000000..6d6db5ea9ca59
--- /dev/null
+++ b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp
@@ -0,0 +1,70 @@
+// Copyright 2022 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_GOAL_MANAGER_CORE_HPP_
+#define EMERGENCY_GOAL_MANAGER_CORE_HPP_
+
+// Autoware
+#include
+#include
+#include
+#include
+
+// ROS 2 core
+#include
+
+#include
+#include
+
+#include
+#include
+#include
+
+namespace emergency_goal_manager
+{
+class EmergencyGoalManager : public rclcpp::Node
+{
+public:
+ EmergencyGoalManager();
+
+private:
+ using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints;
+ using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute;
+
+ // Subscriber
+ rclcpp::Subscription::SharedPtr
+ sub_emergency_goals_;
+ rclcpp::Subscription::SharedPtr
+ sub_emergency_goals_clear_command_;
+
+ void onEmergencyGoals(const tier4_system_msgs::msg::EmergencyGoalsStamped::SharedPtr msg);
+ void onEmergencyGoalsClearCommand(
+ const tier4_system_msgs::msg::EmergencyGoalsClearCommand::SharedPtr msg);
+
+ // Client
+ rclcpp::CallbackGroup::SharedPtr client_set_mrm_route_points_callback_group_;
+ rclcpp::Client::SharedPtr client_set_mrm_route_points_;
+ rclcpp::CallbackGroup::SharedPtr client_clear_mrm_route_callback_group_;
+ rclcpp::Client::SharedPtr client_clear_mrm_route_;
+
+ // Variables
+ std::unordered_map> emergency_goals_map_;
+
+ // Algorithm
+ void callSetMrmRoutePoints();
+ void callClearMrmRoute();
+};
+} // namespace emergency_goal_manager
+
+#endif // EMERGENCY_GOAL_MANAGER_CORE_HPP_
diff --git a/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml b/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml
new file mode 100644
index 0000000000000..1d85c1172d06b
--- /dev/null
+++ b/system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
diff --git a/system/emergency_goal_manager/package.xml b/system/emergency_goal_manager/package.xml
new file mode 100644
index 0000000000000..5b7d6e35f0bc7
--- /dev/null
+++ b/system/emergency_goal_manager/package.xml
@@ -0,0 +1,26 @@
+
+
+
+ emergency_goal_manager
+ 0.1.0
+ The emergency goal manager package
+ Makoto Kurihara
+ Tomohito Ando
+ Ryuta Kambe
+ Apache License 2.0
+
+ ament_cmake_auto
+ autoware_cmake
+
+ autoware_adapi_v1_msgs
+ rclcpp_components
+ std_srvs
+ tier4_system_msgs
+
+ ament_lint_auto
+ autoware_lint_common
+
+
+ ament_cmake
+
+
diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp
new file mode 100644
index 0000000000000..07cf8d895c454
--- /dev/null
+++ b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp
@@ -0,0 +1,152 @@
+// Copyright 2022 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.
+
+#include
+
+namespace emergency_goal_manager
+{
+EmergencyGoalManager::EmergencyGoalManager() : Node("emergency_goal_manager")
+{
+ // Subscriber
+ sub_emergency_goals_ = create_subscription(
+ "~/input/emergency_goals", rclcpp::QoS{1},
+ std::bind(&EmergencyGoalManager::onEmergencyGoals, this, std::placeholders::_1));
+ sub_emergency_goals_clear_command_ =
+ create_subscription(
+ "~/input/emergency_goals_clear_command", rclcpp::QoS{1},
+ std::bind(&EmergencyGoalManager::onEmergencyGoalsClearCommand, this, std::placeholders::_1));
+
+ // Client
+ client_set_mrm_route_points_callback_group_ =
+ create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+ client_set_mrm_route_points_ = create_client(
+ "/planning/mission_planning/mission_planner/srv/set_mrm_route",
+ rmw_qos_profile_services_default, client_set_mrm_route_points_callback_group_);
+ client_clear_mrm_route_callback_group_ =
+ create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+ client_clear_mrm_route_ = create_client(
+ "/planning/mission_planning/mission_planner/srv/clear_mrm_route",
+ rmw_qos_profile_services_default, client_clear_mrm_route_callback_group_);
+
+ // Initialize
+ while (!client_set_mrm_route_points_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
+ }
+ while (!client_clear_mrm_route_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
+ }
+}
+
+void EmergencyGoalManager::onEmergencyGoals(
+ const tier4_system_msgs::msg::EmergencyGoalsStamped::SharedPtr msg)
+{
+ if (!emergency_goals_map_.empty()) {
+ emergency_goals_map_.clear();
+ }
+
+ std::queue emergency_goals_queue;
+ for (const auto & goal : msg->goals) {
+ emergency_goals_queue.push(goal);
+ }
+ emergency_goals_map_.emplace(msg->sender, emergency_goals_queue);
+
+ callSetMrmRoutePoints();
+}
+
+void EmergencyGoalManager::onEmergencyGoalsClearCommand(
+ const tier4_system_msgs::msg::EmergencyGoalsClearCommand::SharedPtr msg)
+{
+ if (emergency_goals_map_.count(msg->sender) == 0) {
+ RCLCPP_WARN(get_logger(), "Emergency goals from %s is empty.", msg->sender.c_str());
+ }
+
+ if (msg->command) {
+ emergency_goals_map_.erase(msg->sender);
+
+ if (emergency_goals_map_.empty()) {
+ callClearMrmRoute();
+ } else {
+ callSetMrmRoutePoints();
+ }
+ }
+}
+
+void EmergencyGoalManager::callSetMrmRoutePoints()
+{
+ auto request = std::make_shared();
+ request->header.frame_id = "map";
+ request->header.stamp = this->now();
+ request->option.allow_goal_modification = true;
+
+ while (!emergency_goals_map_.empty()) {
+ // TODO: set goals with the highest priority
+ auto goals = emergency_goals_map_.begin();
+
+ auto sender = goals->first;
+ auto & goal_queue = goals->second;
+ if (goal_queue.empty()) {
+ emergency_goals_map_.erase(sender);
+ continue;
+ }
+
+ request->goal = goal_queue.front();
+ goal_queue.pop();
+
+ auto future = client_set_mrm_route_points_->async_send_request(request);
+ const auto duration = std::chrono::duration>(10);
+ if (future.wait_for(duration) != std::future_status::ready) {
+ RCLCPP_WARN(get_logger(), "MRM Route service timeout.");
+ continue;
+ } else {
+ if (future.get()->status.success) {
+ RCLCPP_INFO(get_logger(), "MRM Route has been successfully sent.");
+ return;
+ } else {
+ RCLCPP_WARN(get_logger(), "MRM Route service has failed.");
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+ continue;
+ }
+ }
+ }
+
+ callClearMrmRoute();
+}
+
+void EmergencyGoalManager::callClearMrmRoute()
+{
+ auto request = std::make_shared();
+ const auto duration = std::chrono::duration>(10);
+ const auto start_time = std::chrono::steady_clock::now();
+
+ while (rclcpp::ok()) {
+ if (std::chrono::steady_clock::now() - start_time > duration) {
+ RCLCPP_WARN(get_logger(), "Clear MRM Route operation timeout.");
+ return;
+ }
+
+ auto future = client_clear_mrm_route_->async_send_request(request);
+ if (future.wait_for(duration) != std::future_status::ready) {
+ RCLCPP_WARN(get_logger(), "Clear MRM Route service timeout.");
+ return;
+ } else {
+ if (future.get()->status.success) {
+ RCLCPP_INFO(get_logger(), "Clear MRM Route has been successfully sent.");
+ return;
+ } else {
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+ RCLCPP_WARN(get_logger(), "Clear MRM Route has failed.");
+ continue;
+ }
+ }
+ }
+}
+} // namespace emergency_goal_manager
diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp
new file mode 100644
index 0000000000000..c307bbf70ef57
--- /dev/null
+++ b/system/emergency_goal_manager/src/emergency_goal_manager_node.cpp
@@ -0,0 +1,28 @@
+// Copyright 2023 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.
+
+#include "emergency_goal_manager_core.hpp"
+
+int main(int argc, char ** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::executors::MultiThreadedExecutor executor;
+ auto node = std::make_shared();
+ executor.add_node(node);
+ executor.spin();
+ executor.remove_node(node);
+ rclcpp::shutdown();
+
+ return 0;
+}
diff --git a/system/mrm_pull_over_manager/CMakeLists.txt b/system/mrm_pull_over_manager/CMakeLists.txt
new file mode 100644
index 0000000000000..10e36f9746f71
--- /dev/null
+++ b/system/mrm_pull_over_manager/CMakeLists.txt
@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.14)
+project(mrm_pull_over_manager)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(mrm_pull_over_manager
+ src/mrm_pull_over_manager/mrm_pull_over_manager_node.cpp
+ src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+ launch
+ config
+)
diff --git a/system/mrm_pull_over_manager/README.md b/system/mrm_pull_over_manager/README.md
new file mode 100644
index 0000000000000..23a6ed29442dc
--- /dev/null
+++ b/system/mrm_pull_over_manager/README.md
@@ -0,0 +1,23 @@
+# mrm_pull_over_manager
+
+## Purpose
+
+## Inner-workings / Algorithms
+
+### State Transitions
+
+## Inputs / Outputs
+
+### Input
+
+### Output
+
+## Parameters
+
+### Node Parameters
+
+### Core Parameters
+
+## Assumptions / Known limits
+
+TBD.
diff --git a/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml
new file mode 100644
index 0000000000000..1d988cc09ece8
--- /dev/null
+++ b/system/mrm_pull_over_manager/config/mrm_pull_over_manager.param.yaml
@@ -0,0 +1,6 @@
+/**:
+ ros__parameters:
+ update_rate: 10.0
+ max_goal_pose_num: 3
+ yaw_deviation_threshold: 0.5 # [rad]
+ margin_time_to_goal: 10.0 # [sec]
diff --git a/system/mrm_pull_over_manager/config/pull_over_point.csv b/system/mrm_pull_over_manager/config/pull_over_point.csv
new file mode 100644
index 0000000000000..e8061f25967fa
--- /dev/null
+++ b/system/mrm_pull_over_manager/config/pull_over_point.csv
@@ -0,0 +1,5 @@
+x,y,z,yaw,lane_id
+65401.7, 684.4, 715.8, 0, 19464
+65626.6, 681.4, 714.0, 0, 19779
+65808.1, 720.5, 712.6, -1.3, 20976
+65570.5, 673.9, 714.5, 3.14, 20478
diff --git a/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp
new file mode 100644
index 0000000000000..dc844d2dc8383
--- /dev/null
+++ b/system/mrm_pull_over_manager/include/mrm_pull_over_manager/csv_parser.hpp
@@ -0,0 +1,103 @@
+// Copyright 2023 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_PULL_OVER_MANAGER__CSV_PARSER_HPP_
+#define MRM_PULL_OVER_MANAGER__CSV_PARSER_HPP_
+
+#include
+
+#include
+
+#include
+#include
+#include