机械狗控制逻辑入口

机械狗锁定目标追踪

1. 将机械狗放置在距离追踪目标人2-3m距离。

2. 执行以下命令，进入到开发板上机械狗的代码目录下：

   cd  /home/HwHiAiUser/E2ESample/ascend-devkit-master/src/E2E-Sample/dogee/demo

3. 执行以下命令启动脚本，等待机械狗初始化。

   python3 main.py --mode=tracking

   初始化完成回显如下所示：

   图1 命令回显

   

4. 在机械狗面前摆出“OK”的手势，等待开发者套件远程登录界面回显中出现locked，即表示已经锁定了追踪的目标，接下来就可以缓慢移动，机械狗就会跟随目标了。

   

   解除追踪锁定，可参见以下两种方式：

   • 面对机械狗，手掌对向摄像头，五指张开，持续3s左右，开发者套件远程登录界面回显中出现unlock字样，即表示解除了锁定，机械狗会保持静止状态。
   • 快速脱离机械狗的视线超过10s左右，机械狗会认为丢失目标，开发者套件远程登录界面回显中出现unlock字样，即表示解除了锁定，机械狗会保持静止状态。

   

父主题： 快速体验

主要代码文件介绍

表1 文件介绍

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

父主题： 代码实现

机械狗控制逻辑入口

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

首先导入一系列与参数有关的模块。

import os
from argparse import ArgumentParser
from multiprocessing import Process, Queue

from src.actions import Stop
from src.scenes import Manual, scene_initiator
from src.utils import getkey, log, CameraBroadcaster, SystemInfo, Controller, get_port, STM32_NAME, ESP32_NAME

定义出可选参数更改小车的控制模式，可选命令行cmd控制（预留接口），手动控制，简易模式（自动巡线行走），声音控制（预留接口）、手动控制行走等。

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

遵循简单性的原则，在程序的主入口需判断模式的设定后，进入到对应模式的实现中。

if __name__ == '__main__':

    # 获取STM32和ESP32设备的端口
    stm32_port = get_port(STM32_NAME)
    esp32_port = get_port(ESP32_NAME)

    # 使用获得的端口创建SystemInfo实例
    system_info = SystemInfo(stm32_port=stm32_port, esp32_port=esp32_port)

    # 初始化Controller
    ctrl = Controller()

    # 解析命令行参数
    args = parse_args()
    log.info('start')

    # 创建一个最大大小为1的消息队列
    msg_queue = Queue(maxsize=1)
    camera = CameraBroadcaster(system_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, system_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':
                    # 如果命令是'stop'，终止所有进程，包括摄像头，并退出循环
                    for p in process_list:
                        p.kill()
                    log.info(f'start put stop sign')

                    ctrl.execute(Stop())
                    camera.stop_sign.value = True
                    camera_process.join()
                    break
                elif command == 'clear':
                    # 如果命令是'clear'，清空进程列表，重置控制器，并继续
                    for p in process_list:
                        p.kill()
                    process_list.clear()
                    ctrl = Controller()
                    ctrl.execute(Stop())
                    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, system_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 == 'easy':
        process_list = []
        task2 = scene_initiator('LF')(shared_memory_name, system_info, msg_queue)
        process_list.append(Process(target=task2.loop))

        for process in process_list:
            process.start()
        try:
            while True:
                key = getkey()
                if key == 'esc':
                    for process in process_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.')
    elif args.mode == 'tracking':
        # 对于跟踪模式，启动对应场景（Tracking）的单独进程
        process_list = []
        task1 = scene_initiator('Tracking')(shared_memory_name, system_info, msg_queue)
        process_list.append(Process(target=task1.loop))

        for process in process_list:
            process.start()
        try:
            while True:
                key = getkey()
                if key == 'esc':
                    for process in process_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.')

父主题： 代码实现

手动控制机械狗行走

导入一系列与参数有关的模块。

import datetime
import os

import cv2
import numpy as np

from src.actions import MoveForward, SetServo, Stop, MoveBack, BaseAction
from src.scenes.base_scene import BaseScene
from src.utils import log

手动控制行走模式功能定义。

class Manual(BaseScene):
    def __init__(self, memory_name, camera_info, msg_queue):
        super().__init__(memory_name, camera_info, msg_queue)
        # 初始速度设置为100
        self.speed = 100
        self.save_dir = os.path.join(os.getcwd(), 'capture')
        if not os.path.exists(self.save_dir):
            os.makedirs(self.save_dir, exist_ok=True)

    def init_state(self):
        # 初始化状态，执行设置Servo的动作
        self.ctrl.execute(SetServo(servo=[91, 10]))

    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')
        # 设置摄像头的角度为91，10
        last_action = SetServo(servo=[91, 10])

        while True:
            try:
                if not self.msg_queue.empty():
                    key = self.msg_queue.get()
                else:
                    continue
            except KeyboardInterrupt:
                self.ctrl.execute(Stop())
                break

            # degree为z轴的角度数，正数为左转，负数为右转
            degree = 0
            if key == 'up':
                self.speed = min(self.speed + 1, 100)
            elif key == 'down':
                self.speed = max(self.speed - 1, 100)
            elif key == 'right':
                last_action = SetServo(servo=[91, 10])
            elif key == 'left':
                last_action = SetServo(servo=[91, 90])
            elif key == 'w':
                last_action = MoveForward(x=self.speed)
            elif key == 'a':
                last_action = MoveForward()
                degree = 40
            elif key == 's':
                last_action = MoveBack(x=self.speed)
            elif key == 'd':
                last_action = MoveForward()
                degree = -40
            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.')
            else:
                continue

            if isinstance(last_action, BaseAction):
                last_action.update_z_speed = False
                last_action.update_x_speed = False
                # 设置动作中z轴的角度变化
                last_action.z_speed = degree
                last_action.speed_setting = last_action.generate_speed_setting(self.speed, degree)
            self.ctrl.execute(last_action)

父主题： 代码实现

机械狗巡引导线行走

导入一系列与参数有关的模块。

import os
import time

import numpy as np

from src.actions import SetServo, Stop
from src.models import LFNet
from src.scenes.base_scene import BaseScene
from src.utils import log

自动巡线行走功能定义。

class LF(BaseScene):
    def __init__(self, memory_name, camera_info, msg_queue):
        super().__init__(memory_name, camera_info, msg_queue)
        self.net = None
        self.forward_spd = 22

    def init_state(self):
        log.info(f'start init {self.__class__.__name__}')
        lfnet_path = os.path.join(os.getcwd(), 'weights', 'lfnet.om')
        if not os.path.exists(lfnet_path):
            log.error(f'Cannot find the offline inference model(.om) file needed for {self.__class__.__name__}  scene.')
            return True
        self.net = LFNet(lfnet_path)
        log.info(f'{self.__class__.__name__} model init succ.')
        # 设置舵机水平角度90度，垂直角度10度
        self.ctrl.execute(SetServo(servo=[90, 10]))
        return False

    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')
        try:
            while True:
                if self.stop_sign.value:
                    break
                if self.pause_sign.value:
                    continue
                start = time.time()
                img_bgr = frame.copy()
                curr_steering_val = float(self.net.infer(img_bgr)[0])
                log.info(f'lfnet: {curr_steering_val}')

                log.info(f'infer cost {time.time() - start}')
        except KeyboardInterrupt:
            self.ctrl.execute(Stop())

父主题： 代码实现

在线提单