diff --git a/system/control_cmd_switcher/CMakeLists.txt b/system/control_cmd_switcher/CMakeLists.txt
new file mode 100644
index 0000000000000..947d79a9909f6
--- /dev/null
+++ b/system/control_cmd_switcher/CMakeLists.txt
@@ -0,0 +1,23 @@
+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
+ 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
+)
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/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..f26e9ae63a065
--- /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_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..921988ff33455
--- /dev/null
+++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp
@@ -0,0 +1,78 @@
+// 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_control_msgs::msg::Control::ConstSharedPtr msg)
+{
+ if (use_main_control_cmd_) {
+ pub_control_cmd_->publish(*msg);
+ }
+}
+
+void ControlCmdSwitcher::onSubControlCmd(
+ const autoware_control_msgs::msg::Control::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..a730c6cc630ac
--- /dev/null
+++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp
@@ -0,0 +1,51 @@
+// 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_control_msgs::msg::Control::ConstSharedPtr msg);
+ void onSubControlCmd(const autoware_control_msgs::msg::Control::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_