ROS笔记[2]-获取OpenMV数据并发布到ROS消息

摘要

Orangepi(香橙派)通过USB-CDC获取OpenMV数据并使用Python发布到ROS的/openmv主题,实现打印"hello ros"字符串.

关键信息

python3.8
ROS1:Noetic
Ubuntu20.04
Orangepi 3B

原理简介

OpenMV的USB-CDC虚拟串口(VCP)通信

[https://blog.csdn.net/qq_34440409/article/details/125373760]
OpenMV IDE 通过usb cdc模式连接板端，也就是usb上虚拟了一个串口，基于串口实现了一套指令应答的交互协议。早期基于stm32 usb口实现，目前openmv适配扩展到多芯片上ide dbg调试通信口不在局限于usb，也可以是wifi socket、串口等形式。
协议为主从应答形式，主机端（PC上的IDE）发出指令帧，设备端（板端）应答指令执行相应动作。任何一次对话必然由主机主动发起,设备端视情况仅执行动作或执行动作并返回应答数据，设备端不可以主动发送给主机信息。

主机发送帧格式

帧头：固定为0x30。
命令：见下文枚举。
长度：4字节，小端字节序，无应答指令则表示数据段长度，有应答指令则表示应答数据长度。
数据：作为指令的补充参数，长度不定，由长度字段标识。

命令枚举:

enum usbdbg_cmd {
    USBDBG_NONE = 0x00,  // 空指令
    USBDBG_FW_VERSION = 0x80, // 获取版本号
    USBDBG_FRAME_SIZE = 0x81, // 获取帧大小
    USBDBG_FRAME_DUMP = 0x82, // 获取帧数据
    USBDBG_ARCH_STR = 0x83, // 获取板型号字符串
    USBDBG_SCRIPT_EXEC = 0x05, // 执行脚本
    USBDBG_SCRIPT_STOP = 0x06, // 停止执行脚本
    USBDBG_SCRIPT_SAVE = 0x07, // 保存脚本
    USBDBG_SCRIPT_RUNNING = 0x87, // 检测脚本是否运行
    USBDBG_TEMPLATE_SAVE = 0x08, // 模板保存,用途不明
    USBDBG_DESCRIPTOR_SAVE = 0x09,
    USBDBG_ATTR_READ = 0x8A, // sensor参数读取
    USBDBG_ATTR_WRITE = 0x0B, // sensor参数设置
    USBDBG_SYS_RESET = 0x0C, // 系统重启
    USBDBG_FB_ENABLE = 0x0D, // 开关 fb图像预览
    USBDBG_QUERY_STATUS = 0x8D, // maixpy-ide独有标识，值 0xFFEEBBAA
    USBDBG_TX_BUF_LEN = 0x8E, // 获取板端要发数据长度
    USBDBG_TX_BUF = 0x8F, // 获取板端数据
    USBDBG_SENSOR_ID = 0x90 // 读取 sensor id
};

示例:

获取版本号（以下数据皆为十六进制）：
发：30 80 0C 00 00 00
收：01 00 00 00 02 00 00 00 01 00 00 00
获取版本号那肯定是需要应答的，发送帧长度段（0C 00 00 00）表示应答数据长度应为12字节，这里收到的版本号用3个uint32_t整数表示，版本
为："1.2.1"。

通信过程:
OpenMV IDE建立连接时依次发送下列指令：

usbdbg_cmd:80 USBDBG_FW_VERSION
usbdbg_cmd:90 USBDBG_SENSOR_ID
usbdbg_cmd:83 USBDBG_ARCH_STR
usbdbg_cmd:87 USBDBG_SCRIPT_RUNNING
usbdbg_cmd:06 USBDBG_SCRIPT_STOP
usbdbg_cmd:0D USBDBG_FB_ENABLE
作用:获取板端的基本信息，版本号，设备描述，检测是否有脚本运行等，当然为了保证连接后用户正常使用
停止脚本运行的指令，最后使能或关闭fb图像预览，这个根据IDE的按钮状态来。

串口通信中的DTR流控简介

[https://blog.csdn.net/weixin_42118352/article/details/129425336]
代码中使用到了流控
在串口通信中，DTR（Data Terminal Ready）和DSR（Data Set Ready）是两个流控制信号。DTR/DSR流控制是一种硬件流控制方式，用于控制数据发送方和接收方之间的数据流量。

当数据发送方准备好发送数据时，它会将DTR信号置为高电平，表示“数据终端准备好”，并等待接收方的DSR信号确认接收方已经准备好接收数据。当接收方收到DTR信号后，如果它准备好接收数据，它会将DSR信号置为高电平，表示“数据集准备好”。接收方在准备好接收数据后，它可以将DSR信号保持高电平，表示可以继续接收数据。

如果接收方不准备好接收数据，它会将DSR信号置为低电平，表示“数据集未准备好”，此时发送方会停止发送数据，直到接收方再次将DSR信号置为高电平。

当发送方收到DSR信号为低电平时，它会停止发送数据，等待接收方再次将DSR信号置为高电平。这种流控制方式可以确保发送方和接收方之间的数据传输是同步的，可以防止数据丢失或出现错误。

ROS发布/订阅通信机制

[https://blog.csdn.net/qq_36914987/article/details/113359681]
发布/订阅者模型是基于话题(topic)和消息（message）来实现的。发布者负责发布某一话题topic，而话题的内容就是消息message，其有属于自己的消息类型。当某个话题被发布后，该话题的订阅者便会接收到该消息。

发布/订阅模型

实现

核心代码

配置环境

#扫描局域网设备
sudo nmap -sP -PI -PT 192.168.29.0/24

# 配置ssh连接Orangepi 3B,使用VSCODE的Remote Explorer插件连接到远程
sudo ssh-keygen -A
sudo service ssh restart
/etc/init.d/ssh status
ssh orangepi@192.168.29.88
# 密码:orangepi

# 安装需要的库
cd ~/catkin_ws/src
sudo apt install -y python3-pip
pip install pqi
/home/orangepi/.local/bin/pqi use ustc
pip install pyusb pyserial
roscore
python3 ~/catkin_ws/src/usb_with_openmv.py

# 订阅消息
rostopic echo /openmv

代码
usb_openmv_ros.py

#!/usr/bin/env python3
# -*- encoding:utf-8 -*-
'''
自动寻找OpenMV设备并通过USB-CDC协议连接OpenMV并获取字符串数据，然后发布到ROS话题
'''
import sys, struct, time, random
import serial
from usb.core import find
import usb.util
import rospy
from std_msgs.msg import String
openmv_pid = 0xabd1
openmv_vid = 0x1209
port = '/dev/ttyACM0'
# 初始化ROS节点
rospy.init_node('openmv_publisher', anonymous=True)
# 创建一个Publisher，发布到"/openmv"话题，消息类型为String
pub = rospy.Publisher('/openmv', String, queue_size=10)
while not rospy.is_shutdown():
    print("使用{}端口".format(port))
    # 打开串口
    try:
        sp = serial.Serial(port, baudrate=115200, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, xonxoff=False, rtscts=False, stopbits=serial.STOPBITS_ONE, timeout=None, dsrdtr=True)
        sp.setDTR(True)
        sp.write(b"snsn")
        sp.flush()
        size = struct.unpack('<L', sp.read(4))[0]
        message = sp.read(size).decode('utf-8')
        # 关闭串口
        sp.close()
        if message:
            print("message: {}".format(message))
            # 发布消息到ROS话题
            pub.publish(String(message))
    except serial.SerialException as e:
        print("串口错误: {}".format(e))
    # 等待一段时间后再次尝试
    time.sleep(5)
# 在节点结束时关闭串口
sp.close()