From 6e993d95b141ce97251563117862001e31684542 Mon Sep 17 00:00:00 2001
From: "Takagi, Isamu" <isamu.takagi@tier4.jp>
Date: Thu, 18 Jul 2024 19:40:40 +0900
Subject: [PATCH] feat: add control mode request adapter

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
---
 .../aichallenge_system_launch/CMakeLists.txt  |  1 +
 .../launch/aichallenge_system.launch.xml      |  3 ++
 .../script/control_mode_adapter.py            | 35 +++++++++++++++++++
 3 files changed, 39 insertions(+)
 create mode 100755 aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/script/control_mode_adapter.py

diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/CMakeLists.txt
index 4ac87b06..c418f7f4 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/CMakeLists.txt
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/CMakeLists.txt
@@ -5,6 +5,7 @@ find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
 install(PROGRAMS
+  script/control_mode_adapter.py
   script/object_marker.py
   DESTINATION lib/${PROJECT_NAME}
 )
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml
index 52ae91f7..eda86a3a 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml
@@ -4,6 +4,9 @@
     <log message="This is aichallenge_system_launch."/>
     <include file="$(find-pkg-share aichallenge_submit_launch)/launch/aichallenge_submit.launch.xml" />
 
+    <!-- Workaround because the simulator cannot use the service. -->
+    <node pkg="aichallenge_system_launch" exec="control_mode_adapter.py" output="screen"/>
+
     <!-- RViz -->
     <group>
         <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_config) -s $(find-pkg-share autoware_launch)/rviz/image/autoware.png"/>
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/script/control_mode_adapter.py b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/script/control_mode_adapter.py
new file mode 100755
index 00000000..97de5d17
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/script/control_mode_adapter.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+import rclpy
+import rclpy.node
+from std_msgs.msg import Bool
+from autoware_auto_vehicle_msgs.srv import ControlModeCommand
+
+# Workaround because the simulator cannot use the service.
+class ControlModeAdapterNode(rclpy.node.Node):
+    def __init__(self):
+        super().__init__("control_mode_adapter")
+        self.sub = self.create_service(ControlModeCommand, "/control/control_mode_request", self.callback)
+        self.pub = self.create_publisher(Bool, "/control/control_mode_request_topic", 1)
+
+    def callback(self, req, res):
+        msg = Bool()
+        if req.mode == ControlModeCommand.Request.AUTONOMOUS:
+            res.success = True
+            msg.data = True
+            self.pub.publish(msg)
+            return res
+        if req.mode == ControlModeCommand.Request.MANUAL:
+            res.success = True
+            msg.data = False
+            self.pub.publish(msg)
+            return res
+        res.success = False
+        return res
+
+def main(args=None):
+    rclpy.init(args=args)
+    rclpy.spin(ControlModeAdapterNode())
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()