diff --git a/system/control_cmd_switcher/CMakeLists.txt b/system/control_cmd_switcher/CMakeLists.txt
new file mode 100644
index 0000000000000..2bd58dbeedaaa
--- /dev/null
+++ b/system/control_cmd_switcher/CMakeLists.txt
@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.14)
+project(control_cmd_switcher)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+ src/control_cmd_switcher/control_cmd_switcher.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+ PLUGIN "ControlCmdSwitcher"
+ EXECUTABLE ${PROJECT_NAME}_node
+)
+
+install(PROGRAMS
+ tool/relay_trajectory.py
+ DESTINATION lib/${PROJECT_NAME}
+ PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+ launch
+ config
+)
diff --git a/system/control_cmd_switcher/README.md b/system/control_cmd_switcher/README.md
new file mode 100644
index 0000000000000..7c688a50919a6
--- /dev/null
+++ b/system/control_cmd_switcher/README.md
@@ -0,0 +1 @@
+# control_cmd_switcher
diff --git a/system/control_cmd_switcher/config/control_cmd_switcher.yaml b/system/control_cmd_switcher/config/control_cmd_switcher.yaml
new file mode 100644
index 0000000000000..b908163301d69
--- /dev/null
+++ b/system/control_cmd_switcher/config/control_cmd_switcher.yaml
@@ -0,0 +1,4 @@
+# Default configuration for mrm handler
+---
+/**:
+ ros__parameters:
diff --git a/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml
new file mode 100644
index 0000000000000..c19abb1c69af4
--- /dev/null
+++ b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/system/control_cmd_switcher/package.xml b/system/control_cmd_switcher/package.xml
new file mode 100644
index 0000000000000..8ed70f43f51f1
--- /dev/null
+++ b/system/control_cmd_switcher/package.xml
@@ -0,0 +1,25 @@
+
+
+
+ control_cmd_switcher
+ 0.1.0
+ The control_cmd_switcher ROS 2 package
+
+ Tetsuhiro Kawaguchi
+ Apache License 2.0
+
+ ament_cmake_auto
+ autoware_cmake
+
+ autoware_auto_control_msgs
+ rclcpp
+ rclcpp_components
+ tier4_system_msgs
+
+ ament_lint_auto
+ autoware_lint_common
+
+
+ ament_cmake
+
+
diff --git a/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp
new file mode 100644
index 0000000000000..ddec94716c376
--- /dev/null
+++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp
@@ -0,0 +1,80 @@
+// 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.
+
+#include "control_cmd_switcher.hpp"
+
+#include
+#include
+#include
+#include
+
+ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options)
+: Node("control_cmd_switcher", node_options)
+{
+ // Subscriber
+ sub_main_control_cmd_ =
+ create_subscription(
+ "~/input/main/control_cmd", rclcpp::QoS{10},
+ std::bind(&ControlCmdSwitcher::onMainControlCmd, this, std::placeholders::_1));
+
+ sub_sub_control_cmd_ =
+ create_subscription(
+ "~/input/sub/control_cmd", rclcpp::QoS{10},
+ std::bind(&ControlCmdSwitcher::onSubControlCmd, this, std::placeholders::_1));
+
+ sub_election_status_main_ = create_subscription(
+ "~/input/election/status/main", rclcpp::QoS{10},
+ std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1));
+
+ sub_election_status_sub_ = create_subscription(
+ "~/input/election/status/sub", rclcpp::QoS{10},
+ std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1));
+
+ // Publisher
+ pub_control_cmd_ = create_publisher(
+ "~/output/control_cmd", rclcpp::QoS{1});
+
+ // Initialize
+ use_main_control_cmd_ = true;
+}
+
+void ControlCmdSwitcher::onMainControlCmd(
+ const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg)
+{
+ if (use_main_control_cmd_) {
+ pub_control_cmd_->publish(*msg);
+ }
+}
+
+void ControlCmdSwitcher::onSubControlCmd(
+ const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg)
+{
+ if (!use_main_control_cmd_) {
+ pub_control_cmd_->publish(*msg);
+ }
+}
+
+void ControlCmdSwitcher::onElectionStatus(
+ const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg)
+{
+ if (msg->election_start_count <= 0) return;
+ if (msg->in_election) return;
+ if (((msg->path_info >> 3) & 0x01) == 1) {
+ use_main_control_cmd_ = true;
+ } else if (((msg->path_info >> 2) & 0x01) == 1) {
+ use_main_control_cmd_ = false;
+ }
+}
+
+#include
+RCLCPP_COMPONENTS_REGISTER_NODE(ControlCmdSwitcher)
diff --git a/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp
new file mode 100644
index 0000000000000..446979ca1c2c4
--- /dev/null
+++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp
@@ -0,0 +1,56 @@
+// 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 CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_
+#define CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_
+
+// Core
+#include
+#include
+#include
+
+// Autoware
+#include
+#include
+
+// ROS 2 core
+#include
+
+class ControlCmdSwitcher : public rclcpp::Node
+{
+public:
+ explicit ControlCmdSwitcher(const rclcpp::NodeOptions & node_options);
+
+private:
+ // Subscribers
+ rclcpp::Subscription::SharedPtr
+ sub_main_control_cmd_;
+ rclcpp::Subscription::SharedPtr
+ sub_sub_control_cmd_;
+ rclcpp::Subscription::SharedPtr sub_election_status_main_;
+ rclcpp::Subscription::SharedPtr sub_election_status_sub_;
+ void onMainControlCmd(
+ const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
+ void onSubControlCmd(
+ const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
+ void onElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg);
+
+ // Publisher
+ rclcpp::Publisher::SharedPtr
+ pub_control_cmd_;
+
+ std::atomic use_main_control_cmd_;
+};
+
+#endif // CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_
diff --git a/system/control_cmd_switcher/tool/relay_trajectory.py b/system/control_cmd_switcher/tool/relay_trajectory.py
new file mode 100755
index 0000000000000..ef28badafdf70
--- /dev/null
+++ b/system/control_cmd_switcher/tool/relay_trajectory.py
@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+
+import threading
+
+from autoware_auto_planning_msgs.msg import Trajectory
+import rclpy
+from rclpy.node import Node
+
+
+class RelayTrajectoryNode(Node):
+ def __init__(self):
+ super().__init__("relay_trajectory")
+ self.subscription = self.create_subscription(
+ Trajectory, "/tmp/planning/scenario_planning/trajectory", self.listener_callback, 10
+ )
+ self.publisher = self.create_publisher(
+ Trajectory, "/planning/scenario_planning/trajectory", 10
+ )
+ self.running = True
+
+ def listener_callback(self, msg):
+ if self.running:
+ self.publisher.publish(msg)
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = RelayTrajectoryNode()
+
+ def input_thread():
+ nonlocal node
+ while True:
+ user_input = input("Enter 'y' to stop publishing: ")
+ if user_input.lower() == "y":
+ node.running = False
+ print("Publishing stopped.")
+ break
+
+ thread = threading.Thread(target=input_thread)
+ thread.start()
+
+ rclpy.spin(node)
+
+ thread.join()
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()