重构代码，删除不再使用的LCM类型定义文件，更新导入路径，添加图像处理功能到主程序和任务1中，并在适当位置调用图像处理器的销毁方法。

main.py

@@ -9,14 +9,11 @@ import sys
 import os
 import time
 from threading import Thread, Lock
-
-# For keyboard input
-import tty
-import termios
 import sys
 
-from robot_control_cmd_lcmt import robot_control_cmd_lcmt
-from robot_control_response_lcmt import robot_control_response_lcmt
+from utils.robot_control_cmd_lcmt import robot_control_cmd_lcmt
+from utils.robot_control_response_lcmt import robot_control_response_lcmt
+from utils.image_raw import ImageProcessor
 
 from task_1.task_1 import run_task_1
 
@@ -24,6 +21,8 @@ def main():
     Ctrl = Robot_Ctrl()
     Ctrl.run()
     msg = robot_control_cmd_lcmt()
+    image_processor = ImageProcessor()
+    image_processor.run()
 
     try:
         print("Recovery stand")
@@ -41,6 +40,7 @@ def main():
         pass
 
     Ctrl.quit()
+    image_processor.destroy()
     sys.exit()
 
 
@@ -107,7 +107,6 @@ class Robot_Ctrl(object):
         self.rec_thread.join()
         self.send_thread.join()
 
-
 # Main function
 if __name__ == '__main__':
     main()

task_1/task_1.py

@@ -1,7 +1,7 @@
 import time
+import cv2
 
-
-def run_task_1(ctrl, msg):
+def run_task_1(ctrl, msg, image_processor):
     print('Running task 1...')
     
     # 右前方
@@ -23,8 +23,8 @@ def run_task_1(ctrl, msg):
     time.sleep(0.2)
 
     # TAG take photo
-    # while True:
-    #     pass
+    image = image_processor.get_current_image()
+    cv2.imwrite('photo.jpg', image)
 
     # msg.mode = 11
     # msg.gait_id = 26

utils/image_raw.py

@@ -46,11 +46,12 @@ class ImageProcessor:
         except KeyboardInterrupt:
             pass
 
-        self.image_subscriber.destroy_node()
-        rclpy.shutdown()
-
     def get_current_image(self):
         return self.image_subscriber.cv_image
+    
+    def destroy(self):
+        self.image_subscriber.destroy_node()
+        rclpy.shutdown()
 
 """ DEBUG """