ROS机器人底盘(41)-使用USB代替串口以提升里程和IMU发布频率

1.背景

当前PIBOT小车使用串口进行跟上位机通讯,在连接上位机可以看到输出的odomimu的发布频率

对于odom,设机器人速度v,频率f,即两次发布odom间隔,机器人运动距离为s=v/f,当v=0.6m/s, f=30hz时s=20mm, 如果需要保障精确在速度固定情况下,我们只能提高频率。

同时robot_pose_ekf的输出频率默认值为30,我们知道robot_pose_ekf是使用odomimu作为输入的,也就是目前采样频率还小于输出的频率

本文在STM32F4小车上介绍使用USBHID通讯代替串口以提升发布odomimu的频率

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分支