ROS机器人底盘(41)-使用USB代替串口以提升里程和IMU发
2020-06-27 本文已影响0人
PIBOT导航机器人
1.背景
当前PIBOT小车使用串口进行跟上位机通讯，在连接上位机可以看到输出的odom和imu的发布频率
对于odom，设机器人速度v，频率f，即两次发布odom间隔，机器人运动距离为s=v/f，当v=0.6m/s， f=30hz时s=20mm, 如果需要保障精确在速度固定情况下，我们只能提高频率。
同时robot_pose_ekf的输出频率默认值为30，我们知道robot_pose_ekf是使用odom和imu作为输入的，也就是目前采样频率还小于输出的频率
本文在STM32F4小车上介绍使用USB的HID通讯代替串口以提升发布odom和imu的频率
2. USB HID通讯
2.1 下位机实现
使用USB HID设备的一个好处就是操作系统自带了HID类的驱动程序，而用户无需去开发驱动程序，只要使用API系统调用即可完成通信。
HID具体实现略，具体可以可以自行搜索或者参考PIBOT Firmware代码，下面讲下如何替换串口通讯的部分逻辑。
前文介绍ROS机器人底盘(7)-PIBOT的Firmware的代码分析(1)
ROS机器人底盘(8)-PIBOT的Firmware的代码分析(2)
ROS机器人底盘(24)-嵌入式部分框架设计与实现介绍下位机固件的实现，通讯部分收发以及协议解析是解耦的
- 我们只需要实现一个
USB_transport替换USART_transport
#if MASTER_COMM_TYPE == COMM_USART
trans = new USART_transport(MASTER_USART, USART_MASTER_BAUD);
#else // MASTER_COMM_TYPE == COMM_USB
trans = new USB_transport();
#endif
- 同时之前串口中断中把数据添加到
RingBuffer改为USB HID的接收中添加到RingBuffer
static int8_t CUSTOM_HID_OutEvent_FS(uint8_t event_idx, uint8_t state)
{
/* USER CODE BEGIN 6 */
USBD_CUSTOM_HID_HandleTypeDef *hhid = hUsbDeviceFS.pClassData;
uint32_t len=USBD_CUSTOMHID_OUTREPORT_BUF_SIZE;
usb_recv_cb(hhid->Report_buf, &len);
return (USBD_OK);
/* USER CODE END 6 */
}
2.2 验证测试
使用USB线接入至主机测试
注意这里设备端的端口为核心板上的USB口，并非之前使用串口的USB口
- Windows中
1.设备管理器中可以看到新增一个人体输入设备，也就是HID
HID
2.使用HID测试工具测试
打开软件点击选择HID设备
找到vid_0483&pid_5751选择后点击连接即可
按照ROS机器人底盘(3)-通讯协议我们发送一个获取版本号的命令5A 00 00 5A,可以看到正常收发数据即可
- Linux中
使用lsusb可以查询到该设备
pibot@pibot:~$ lsusb
Bus 005 Device 001: ID 1d6b:0003 Linux Foundation 3.0 root hub
Bus 004 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
Bus 003 Device 003: ID 0483:5751 STMicroelectronics
Bus 003 Device 001: ID 1d6b:0001 Linux Foundation 1.1 root hub
Bus 002 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
查看详细信息
pibot@pibot:~$ lsusb -d 0483:5751 -v
Bus 003 Device 003: ID 0483:5751 STMicroelectronics
Device Descriptor:
bLength 18
bDescriptorType 1
bcdUSB 2.00
bDeviceClass 0 (Defined at Interface level)
bDeviceSubClass 0
bDeviceProtocol 0
bMaxPacketSize0 64
idVendor 0x0483 STMicroelectronics
idProduct 0x5751
bcdDevice 2.00
iManufacturer 1 STMicroelectronics
iProduct 2 PIBOT Driver
iSerial 3 387136763337
bNumConfigurations 1
Configuration Descriptor:
bLength 9
bDescriptorType 2
wTotalLength 41
bNumInterfaces 1
bConfigurationValue 1
iConfiguration 0
bmAttributes 0xc0
Self Powered
MaxPower 100mA
Interface Descriptor:
bLength 9
bDescriptorType 4
bInterfaceNumber 0
bAlternateSetting 0
bNumEndpoints 2
bInterfaceClass 3 Human Interface Device
bInterfaceSubClass 0 No Subclass
bInterfaceProtocol 0 None
iInterface 0
HID Device Descriptor:
bLength 9
bDescriptorType 33
bcdHID 1.11
bCountryCode 0 Not supported
bNumDescriptors 1
bDescriptorType 34 Report
wDescriptorLength 31
Report Descriptors:
** UNAVAILABLE **
Endpoint Descriptor:
bLength 7
bDescriptorType 5
bEndpointAddress 0x81 EP 1 IN
bmAttributes 3
Transfer Type Interrupt
Synch Type None
Usage Type Data
wMaxPacketSize 0x0040 1x 64 bytes
bInterval 5
Endpoint Descriptor:
bLength 7
bDescriptorType 5
bEndpointAddress 0x01 EP 1 OUT
bmAttributes 3
Transfer Type Interrupt
Synch Type None
Usage Type Data
wMaxPacketSize 0x0040 1x 64 bytes
bInterval 5
Device Status: 0x0001
Self Powered
3. ROS中测试
运行pibot_bringup做对比测试
[ INFO] [1600524754.407320144]: open 0483:5751
[ INFO] [1600524754.415206290]: Device Found
type: 0483 5751
path: 0001:0004:00
serial_number: (null)
[ INFO] [1600524754.415385561]: Manufacturer: (null)
[ INFO] [1600524754.415464311]: Product: (null)
[ INFO] [1600524754.415534988]: Release: 200
[ INFO] [1600524754.415598425]: Interface: 0
[ERROR] [1600524754.416348530]: unable to open device vid=0483 pid=5751
[ERROR] [1600524754.416440144]: oops!!! can't connect to main board
该日志输出为权限问题
使用lsusb找到设备，找到设备节点，如下如bus001 Deivce003
pibot@pibot-desktop:~$ lsusb
Bus 001 Device 004: ID 0483:5751 STMicroelectronics
修改设备权限,再次运行即可
chmod 777 /dev/bus/dev/001/004
使用rostopic hz /odom测试odom topic的发布频率为120HZ，已经超过stm32内部计算odom的频率了(100HZ)
pibot@pibot:~$ rostopic hz odom
subscribed to [/odom]
average rate: 109.121
min: 0.007s max: 0.016s std dev: 0.00233s window: 104
average rate: 116.692
min: 0.000s max: 0.040s std dev: 0.00497s window: 225
average rate: 119.534
min: 0.000s max: 0.040s std dev: 0.01022s window: 350
average rate: 120.917
min: 0.000s max: 0.040s std dev: 0.01193s window: 475
average rate: 121.762
min: 0.000s max: 0.040s std dev: 0.01283s window: 600
average rate: 122.326
min: 0.000s max: 0.040s std dev: 0.01341s window: 730
average rate: 122.705
min: 0.000s max: 0.040s std dev: 0.01379s window: 855
average rate: 122.863
min: 0.000s max: 0.040s std dev: 0.01405s window: 975
average rate: 122.889
min: 0.000s max: 0.041s std dev: 0.01428s window: 1103
average rate: 120.839
min: 0.000s max: 0.041s std dev: 0.01451s window: 1205
average rate: 121.222
min: 0.000s max: 0.041s std dev: 0.01464s window: 1330
average rate: 121.537
min: 0.000s max: 0.041s std dev: 0.01474s window: 1455
上述测试上位机和下位机代码git需要切换至usbcom-dev分支
