feat: add control mode request adapter (#28)

Signed-off-by: Takagi, Isamu <[email protected]>

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}
 )

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"/>

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()