手动控制逻辑代码

“python/src/scenes/manual.py”为手动控制小车的核心部分。

1. 首先导入所有必须模块和基础运动模块。

   import datetime
   import os
   import cv2
   import numpy as np
   from src.actions import Advance, Stop, SetServo, TurnLeft, TurnRight, SpinClockwise, SpinAntiClockwise, BackUp, \
       ShiftLeft, ShiftRight, CustomAction
   from src.actions.complex_actions import ComplexAction, TurnAround
   from src.scenes.base_scene import BaseScene
   from src.utils import log

2. 基于场景的基类构建手动控制小车场景的手动类，初始化后，进入到loop循环的函数中，不断等待键盘输入的键值，再执行键值对应的指令

   def loop(self):
           ret = self.init_state() # 执行初始化
           if ret:
               log.error(f'{self.__class__.__name__} init failed.')
               return
           frame = np.ndarray((self.height, self.width, 3), dtype=np.uint8, buffer=self.broadcaster.buf) # 拉取共享内存中的图片
           log.info(f'{self.__class__.__name__} loop start')
           last_action = SetServo(servo=[90, 65]) # 设置舵机角度
    
           while True:
               try:
                   if not self.msg_queue.empty():
                       key = self.msg_queue.get()
                   else:
                       continue
               except KeyboardInterrupt:
                   self.ctrl.execute(Stop()) #捕获SIGINT之后停止小车
                   break
    
               degree = 0 
               if key == 'up':
                   self.speed = min(self.speed + 1, 60) #加速
               elif key == 'down':
                   self.speed = max(self.speed - 1, 25) #减速
               elif key == 'left':
                   last_action = ShiftLeft() #左平移
               elif key == 'right':
                   last_action = ShiftRight() #右平移
               elif key == 'w':
                   last_action = Advance() #前进
               elif key == 'a':
                   last_action = TurnLeft() #左转
                   degree = 1.1
               elif key == 's':
                   last_action = BackUp() #后退
               elif key == 'd':
                   last_action = TurnRight() #右转
                   degree = 1.1
               elif key == 'q':
                   last_action = SpinAntiClockwise() #逆时针旋转
               elif key == 'e':
                   last_action = SpinClockwise() #顺时针旋转
               elif key == 'space':
                   last_action = Stop() #停车
               elif key == 'esc':
                   self.ctrl.execute(Stop()) #退出循环前停下小车
                   break
               elif key == 'c':
                   save_img = frame.copy()
                   cv2.imwrite(os.path.join(self.save_dir, f'{datetime.datetime.now()}.jpg'), save_img) #保存当前摄像头中的画面
                   log.info(f'image saved.')
               elif key == 't':
                   last_action = CustomAction(motor_setting=[-62, 50, 50, -50])
               elif key == 'r':
                   last_action = CustomAction(motor_setting=[55, -50, -50, 50])
               elif key == 'z':
                   last_action = TurnAround() #掉头
               else:
                   continue
    
               if not isinstance(last_action, ComplexAction) and not isinstance(last_action, CustomAction):
                   last_action.update_speed = False
                   last_action.speed_setting = last_action.generate_speed_setting(speed=self.speed, degree=degree)
                   last_action.fix_speed()
               self.ctrl.execute(last_action)