智能小车控制逻辑入口

目标跟踪

:::note 说明 运行场地尽量为夜间且照明充足的室内，避免由太阳光放射带来的曝光影响。 :::

1. 将小车放置在运动场地，并将手动控制的另一辆小车放置在智能小车前方位于摄像头画面的中下方位置。

2. 以root用户远程登录小车的开发者套件。

3. 进入获取代码中的样例代码目录，启动运行脚本。

   cd /home/HwHiAiUser/+Samples/src/E2E-Sample/Car/python
   python3 main.py -mode=cmd

   回显如下：

   (base) root@davinci-mini:/home/HwHiAiUser/E2ESamples/src/E2E-Sample/Car/python# python main.py -mode=cmd
   [EVENT] PROFILING(5821,python):2023-03-27-02:24:50.860.329 [msprof_callback_impl.cpp:236] >>> (tid:5821) Started to register profiling ctrl callback.
   2023-03-27 02:24:52 [MainProcess:5821][INFO]: start
   2023-03-27 02:24:52 [Process-2:5841][INFO]: update_controller_speed: True
   2023-03-27 02:24:52 [Process-2:5841][INFO]: self.speed: 0
   2023-03-27 02:24:52 [Process-2:5841][INFO]: [0, 0, 0, 0, 90, 65, -12345]
   2023-03-27 02:24:52 [Process-2:5841][INFO]: action SetServo execute SUCC
   2023-03-27 02:24:52 [Process-2:5841][INFO]: Manual loop start

   :::note 说明 运行python3 main.py报错请参见运行智能车主入口程序main.py时出现‘ttyUSB0’或'ttyUSB1'相关错误导致程序无法启动解决。 :::

4. 在命令行中输入Tracking，进入追踪模式。

5. 开始控制前方小车移动，智能小车会追踪前方小车的移动轨迹和速度进行目标跟踪移动。

父主题： 快速体验

主要代码文件介绍

表1 文件介绍

文件（夹）名称	说明
python/main.py	小车demo运行入口文件。
python/requirements.txt	智能小车样例代码运行所需依赖。
python/src/actions	小车基础与复杂运动代码。
python/src/utils	工具类python文件包含OpenCV、acl工具等。
python/src/scenes	小车预设场景相关代码。
python/src/model	推理模型相关代码。
python/src/weight	模型权重文件。

父主题： 代码实现

智能小车控制逻辑入口

“main.py”是小车控制代码的主入口，定义了小车控制模式及对应模式的后续调用实现。

首先导入一系列与参数有关的模块，以及启动摄像头的关键模块。

import os
from argparse import ArgumentParser
from multiprocessing import Process, Queue
from src.scenes import Manual, scene_initiator
from src.utils import getkey, log, CameraBroadcaster, CAMERA_INFO

定义出可选参数更改小车的控制模式，可选命令行cmd控制，手动控制，声音控制（正在规划中）以及测试等。

def parse_args():
    parser = ArgumentParser()
    parser.add_argument('--mode', type=str, required=False, default='manual',
                        choices=['cmd', 'voice', 'manual', 'test'])
    return parser.parse_args()

遵循简单性的原则，在程序的主入口需判断模式的设定后，进入到对应模式的实现中，包含摄像头端口的调用及行驶过程中各类消息队列的处理。

if __name__ == '__main__':
    args = parse_args()
    log.info('start')
    msg_queue = Queue(maxsize=1)
    camera = CameraBroadcaster(CAMERA_INFO)
    shared_memory_name = camera.memory_name
    camera_process = Process(target=camera.run)
    camera_process.start()
    if args.mode == 'manual':
        task = Manual(shared_memory_name, CAMERA_INFO, msg_queue)
        process = Process(target=task.loop)
        process.start()
        try:
            while True:
                key = getkey()
                if key == 'esc':
                    process.join()
                    camera.stop_sign.value = True
                    camera_process.join()
                    break
                else:
                    msg_queue.put(key)
        except (KeyboardInterrupt, SystemExit):
            camera.stop_sign.value = True
            camera_process.join()
            os.system('stty sane')
            log.info('stopping.')
    elif args.mode == 'cmd':
        process_list = []
        record_map = {}
        try:
            log.info(f'start reading cmd')
            while True:
                command = input().strip()
                if command == 'stop':
                    for p in process_list:
                        p.kill()
                    log.info(f'start put stop sign')
                    camera.stop_sign.value = True
                    camera_process.join()
                    break
                elif command == 'clear':
                    for p in process_list:
                        p.kill()
                    process_list.clear()
                    log.info(f'clear succ')
                    continue
                elif command == 'Manual':
                    log.error(f'Does not support switching from cmd mode to manual mode')
                    continue
                log.info(f'building scene {command}')
                scene = scene_initiator(command)
                log.info(f'{scene}')
                if scene is not None:
                    scene_obj = scene(shared_memory_name, CAMERA_INFO, msg_queue)
                    process = Process(target=scene_obj.loop)
                    process.start()
                    process_list.append(process)
 
        except (KeyboardInterrupt, SystemExit):
            camera.stop_sign.value = True
            camera_process.join()
            for process in process_list:
                process.kill()
            log.info('stopping.')
 
    elif args.mode == 'voice':
        raise NotImplementedError('voice control is not currently supported.')
    elif args.mode == 'test':
        test_list = []
        test1 = Manual(shared_memory_name, CAMERA_INFO, msg_queue)
        test_list.append(Process(target=test1.loop))
        test2 = scene_initiator('Tracking')(shared_memory_name, CAMERA_INFO, msg_queue)
        test_list.append(Process(target=test2.loop))
        test3 = scene_initiator('Tracking')(shared_memory_name, CAMERA_INFO, msg_queue)
        test_list.append(Process(target=test3.loop))
 
        for process in test_list:
            process.start()
        try:
            while True:
                key = getkey()
                if key == 'esc':
                    for process in test_list:
                        process.kill()
                    camera.stop_sign.value = True
                    camera_process.join()
                    break
                else:
                    msg_queue.put(key)
        except (KeyboardInterrupt, SystemExit):
            camera.stop_sign.value = True
            camera_process.join()
            os.system('stty sane')
            log.info('stopping.')

父主题： 代码实现

智能小车运动控制代码

“python/src/actions”文件夹中包含的是与烧录到ESP32控制端对应的小车基础和复杂运动的python代码文件以及其中的函数说明

1. 小车动作函数定义。

   import time
   from abc import ABC, abstractmethod
    
    
   class BaseAction(ABC):
       """
       基础动作的基类，所有基本动作均继承于该类
       """
    
       def __init__(self, *args, **kwds) -> None:
           """
           基础动作类的初始化方法，通过args与kwds控制输入参数
           :param args:
           :param kwds:
           """
           # 抽象的速度信息
           self.speed = kwds.get('speed', -1)
           # 电机角度
           self.servo_angle = kwds.get('servo', [-1, -1])
    
           # 根据电机的实际情况修改下发到电机的速度
           self.motor_rating = [1.45, 1, 1, 1]
    
           # 确定是否需要在运行时根据前动作更新电机角度及电机速度
           self.update_speed = False
           self.update_servo = False
    
           if self.speed == -1:
               self.update_speed = True
    
           if self.servo_angle[0] == -1 and self.servo_angle[1] == -1:
               self.update_servo = True
    
           # 由速度生成方法将抽象的总体速度计算为4个电机的速度并输出为list
           self.speed_setting = self.generate_speed_setting(self.speed)
           self.fix_speed()
    
       def fix_speed(self):
           self.speed_setting = [int(speed * ratio) for speed, ratio in zip(self.speed_setting, self.motor_rating)]
    
       @staticmethod
       @abstractmethod
       def generate_speed_setting(speed, degree=0):
           """
           生成4个电机的速度，并输出为列表
           抽象类，需要根据具体情况进行设置
           :param speed: 抽象的速度。 当前动作初始化时设置 或 控制器根据前一动作速度进行设置
           :param degree: 如需转弯，速度计算需要的角度信息
           :return:
           """
           pass
    
       def __call__(self, speed, servo_angle):
           """
           call魔法函数，两个输入参数由控制器输入
           当init方法设置了相关信息，则忽略控制器输入的参数
           当init方法没有设置相关信息，相关信息的将由控制器输入的参数进行更新
    
           :param speed: 抽象速度
           :param servo_angle: 舵机的角度
           :return: 长度为6的列表，前4位为4个电机的速度，后2位为舵机的两个角度
           """
           if self.update_servo:
               self.servo_angle = servo_angle
           if self.update_speed:
               degree = 0
               if hasattr(self, 'degree'):
                   degree = self.degree
               self.speed_setting = self.generate_speed_setting(speed, degree)
               self.fix_speed()
    
           return self.speed_setting + self.servo_angle
    
    
   class Advance(BaseAction):
       """
       小车前进
       """
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [-speed, -speed, speed, speed]
    
    
   class BackUp(BaseAction):
       """
       小车后退
       """
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [speed, speed, -speed, -speed]
    
    
   class CustomAction(BaseAction):
       """
       自定义动作
       """
    
       def __init__(self, *args, **kwds):
           super().__init__(*args, **kwds)
           self.speed_setting = kwds.get('motor_setting', [0, 0, 0, 0])
           self.update_controller_speed = False
           self.update_speed = False
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [0, 0, 0, 0]
    
    
   class Stop(BaseAction):
       """
       小车停止
       """
    
       def __init__(self, *args, **kwds):
           super().__init__(*args, **kwds)
           self.speed = 0
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [0, 0, 0, 0]
    
    
   class TurnLeft(BaseAction):
       """
       小车左转
       """
    
       def __init__(self, *args, **kwds):
           super().__init__(*args, **kwds)
           self.degree = kwds.get('degree', 0)
           self.speed_setting = self.generate_speed_setting(speed=self.speed, degree=self.degree)
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [-speed, -speed, int(speed * (1 + degree)), int(speed * (1 + degree))]
    
    
   class TurnRight(BaseAction):
       """
       小车右转
       """
    
       def __init__(self, *args, **kwds):
           super().__init__(*args, **kwds)
           self.degree = kwds.get('degree', 0)
           self.speed_setting = self.generate_speed_setting(speed=self.speed, degree=self.degree)
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [-int(speed * (1 + degree)), -int(speed * (1 + degree)), speed, speed]
    
    
   class ShiftLeft(BaseAction):
       """
       向左平移
       """
    
       def __init__(self, *args, **kwds):
           super().__init__(*args, **kwds)
           self.motor_rating = [1.5, 1.3, 1.25, 1.25]
           self.fix_speed()
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [speed, -speed, -speed, speed]
    
    
   class ShiftRight(BaseAction):
       """
       向右平移
       """
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [-speed, speed, speed, -speed]
    
    
   class LeftOblique(BaseAction):
       """
       斜向左前方
       """
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [0, -speed, 0, speed]
    
    
   class RightOblique(BaseAction):
       """
       斜向右前方
       """
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [-speed, 0, speed, 0]
    
    
   class SpinClockwise(BaseAction):
       """
       顺时针旋转
       """
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [-speed] * 4
    
    
   class SpinAntiClockwise(BaseAction):
       """
       逆时针旋转
       """
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [speed] * 4
    
    
   class SetServo(BaseAction):
       """
       舵机转动
       """
    
       def __init__(self, *args, **kwds):
           super().__init__(*args, **kwds)
           self.speed = 0
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [0, 0, 0, 0]
    
    
   class Sleep(BaseAction):
       """
       Sleep(1)等同于time.sleep(1)
       可加入至动作序列进行使用
       """
    
       def __init__(self, *args, **kwds):
           super().__init__(*args, **kwds)
           self.sleep_time = args[0]
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return []
    
       def __call__(self, speed, servo_angle):
           time.sleep(self.sleep_time)
           return None

2. 以逆时针旋转的操作为例，方法首先继承于运动的基类，实现运动动作的方式是修改不同位置电机的旋转方向和转速。

   class SpinAntiClockwise(BaseAction):
       """
       逆时针旋转
       """
    
       @staticmethod
       def generate_speed_setting(speed, degree=0):
           return [speed] * 4

3. 复杂动作则以掉头行驶为例，是多种不同的简单动作的组合，首先要导入所有的简单动作。

   from abc import ABC
    
   from src.actions.base_action import Advance, Sleep, SpinAntiClockwise, Stop, SpinClockwise, CustomAction

4. 再利用简单动作形成复杂行驶动作。

   class TurnAround(ComplexAction):
       def __init__(self):
           super().__init__()
           self.action_seq = [
               Stop(),
               Sleep(0.5),
               Advance(speed=30),
               Sleep(0.35),
               Stop(),
               Sleep(0.3),
               SpinAntiClockwise(speed=50),
               Sleep(0.55),
               Advance(speed=30),
               Sleep(1.2),
               SpinAntiClockwise(speed=50),
               Sleep(0.55),
               Stop(),
               # Advance(speed=35),
           ]

父主题： 代码实现

手动控制逻辑代码

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

父主题： 代码实现

在线提单