RoboRTS源码解读（一） roborts_base

[imuncle]

源码地址：https://github.com/RoboMaster/RoboRTS/tree/ros/roborts_base

---

1.文件目录

roborts_base
├── CMakeLists.txt
├── cmake_module
│   └── FindGlog.cmake
├── chassis                          #底盘模块ROS接口封装类
│   ├── chassis.cpp
│   └── chassis.h
├── gimbal                           #云台模块ROS接口封装类
│   ├── gimbal.cpp
│   └── gimbal.h
├── referee_system                   #裁判系统ROS接口封装类
│   ├── referee_system.cpp
│   └── referee_system.h
├── config
│   └── roborts_base_parameter.yaml  #参数配置文件
├── roborts_base_config.h            #参数读取类
├── roborts_base_node.cpp            #核心节点Main函数
├── ros_dep.h                        #包括所有协议对应ROS消息的头文件
├── roborts_sdk
│   ├── hardware                 #硬件层完成对协议数据的收发
│   │   ├── hardware_interface.h #硬件层基类
│   │   ├── serial_device.cpp    #串口设备实现类
│   │   └── serial_device.h
│   ├── protocol                 #协议层完成对协议的解包与打包
│   │   ├── protocol.cpp         #协议层类
│   │   ├── protocol_define.h    #协议具体定义消息类型的头文件
│   │   └── protocol.h
│   ├── dispatch                 #分发层完成对消息的分发
│   │   ├── dispatch.h           #分发层类
│   │   ├── execution.cpp        #分发层执行类
│   │   ├── execution.h
│   │   ├── handle.cpp           #roborts_sdk三层的对外接口类
│   │   └── handle.h
│   ├── sdk.h                    #roborts_sdk对外头文件
│   ├── test
│   │   └── sdk_test.cpp         #协议测试文件
│   └── utilities
│       ├── circular_buffer.h    #环形缓冲池
│       ├── crc.h                #crc校验文件
│       ├── log.h                #日志记录文件
│       └── memory_pool.h        #内存池
└── package.xml

注：sdk_test.h里面的测试代码版本太老了，DJI自己在代码迭代的过程中忘记将测试代码跟着迭代，需修改后使用。

2.代码结构

整个roborts_base节点的main函数在roborts_base_node.cpp中。

在该主函数中，创建了一个信息收发解析处理（Handle）对象，一个底盘（Chassis）对象，一个云台（Gimbal）对象，一个裁判系统（RefereeSystem）对象。

(1)底盘对象

底盘对象的定义在chassis.cpp里，这里开启了一个专门发送心跳包（heartbeat）的线程，设置了里程计（odom），定位信息（uwb），弹丸供应（projectile_info），调试信息（chassis_gimbal_mode）四个消息发布器，订阅了底盘速度（cmd_vel），底盘加速度（cmd_vel_acc）两个消息，提供底盘控制接口。创建了一个设置底盘工作模式（set_chassis_mode）的服务器。

对应的消息类型如下：

消息名称	消息类型
里程计	nav_msgs::Odometry
定位信息	geometry_msgs::PoseStamped
弹丸供应	roborts_msgs::ProjectileInfo
调试信息	roborts_msgs::Debug
底盘速度	geometry_msgs::Twist
底盘加速度	roborts_msgs::TwistAccel
底盘工作模式	roborts_msgs::ChassisMode

(2)云台对象

云台对象的定义在gimbal.cpp里，和底盘对象差别不大，也有心跳包的发送（为什么底盘有了云台还要？），订阅了云台角度（cmd_gimbale_angle）、弹丸供应（projectile_info）、弹丸速度（set_fric_speed）三个消息，用于提供云台控制接口。创建了设置云台工作模式（set_gimbal_mode），控制摩擦轮（cmd_fric_wheel），控制发射（cmd_shoot）三个服务器。

对应的消息类型如下：

消息名称	消息类型
云台角度	roborts_msgs::GimbalAngle
弹丸供应	roborts_msgs::ProjectileInfo
弹丸速度	roborts_msgs::FricSpeed
云台工作模式	roborts_msgs::GimbalMode
控制摩擦轮	roborts_msgs::FricWhl
控制发射	roborts_msgs::ShootCmd

(3)裁判系统对象

裁判系统对象的定义在referee_system.cpp中。每一种裁判系统数据（比赛状态，比赛结果，存活机器人，buff信息，机器人状态，热量，伤害，发射，机器人间通信等）都设置了对应的消息发布器，供其他节点订阅。订阅补给站补弹（projectile_supply）、机器人间通信（send_to_teammate）两个消息。

消息类型过多不作整理，具体参考ICRA2019裁判系统手册。

(4)信息收发解析处理对象

Handle对象是整个节点的灵魂所在，主要负责PC与STM32的通信工作，实现了消息的打包发送和解包接收功能。具体的实现在roborts_sdk中，roborts_sdk中有五个文件夹，各个文件夹的简介见前面的文件目录一节。

该模块不依赖其他模块实现了一个类似ROS的消息发布订阅机制，本质是回调函数，该机制的实现在dispatch文件夹中，核心是cmd_set和cmd_id两个uint8_t数字，相关的定义见protocol_defeine.h（这里和github上底层STM32代码的协议文档里有差别，以实际机器人上跑的代码定义为准）。

订阅消息的实现

订阅消息的函数原型如下：

/**
 * @brief Create the subscriber for the protocol command without need of ack (Receive command)
 * @tparam Cmd Command DataType
 * @param cmd_set Command set for different module, i.e. gimbal, chassis
 * @param cmd_id Command id for different commands in the module
 * @param sender Sender address
 * @param receiver Receiver address
 * @param function Subscriber Callback function
 * @return Pointer of subscription handle
 */
template<typename Cmd>
std::shared_ptr<Subscription<Cmd>> CreateSubscriber(uint8_t cmd_set, uint8_t cmd_id,
                                                    uint8_t sender, uint8_t receiver,
                                                    typename Subscription<Cmd>::CallbackType &&function) {
    auto subscriber = std::make_shared<Subscription<Cmd>>(shared_from_this(),
                                                          cmd_set, cmd_id,
                                                          sender, receiver,
                                                          std::forward<typename Subscription<Cmd>::CallbackType>(
                                                                  function));
    subscription_factory_.push_back(
            std::dynamic_pointer_cast<SubscriptionBase>(subscriber));
    return subscriber;
}

创建订阅器时提供了cmd_set``cmd_id``sender``receiver和回调函数，每个订阅器的信息都存储在subscription_factory_中。

在protocol.cpp中开启了一个线程专门用于接收STM32的信息，并对信息进行解包处理，将相关信息提取出来，存储在subscription_factory_中，在Handle::Spin()中对解析后的信息进行处理，根据信息的帧头执行对应的订阅回调函数。该函数在roborts_base_node.cpp中调用：

    while (ros::ok()) {
        handle->Spin();
        ros::spinOnce();
        usleep(1000);
    }

发布消息的实现

创建Pulisher的函数原型如下：

        /**
         * @brief Create the publisher for the protocol command without need of ack (Send command)
         * @tparam Cmd Command DataType
         * @param cmd_set Command set for different module, i.e. gimbal, chassis
         * @param cmd_id Command id for different commands in the module
         * @param sender Sender address
         * @param receiver Receiver address
         * @return Pointer of publisher handle
         */
        template<typename Cmd>
        std::shared_ptr<Publisher<Cmd>>
        CreatePublisher(uint8_t cmd_set, uint8_t cmd_id, uint8_t sender, uint8_t receiver) {
            auto publisher = std::make_shared<Publisher<Cmd>>(shared_from_this(),
                                                              cmd_set, cmd_id,
                                                              sender, receiver);
            publisher_factory_.push_back(
                    std::dynamic_pointer_cast<PublisherBase>(publisher));
            return publisher;
        }

同样的，创建时提供cmd_set``cmd_id``sender``receiver，用于数据打包时使用。

发布消息使用Pulish函数，该函数最终会调用SendCMD函数，该函数在protocol.cpp中定义。

发布消息主要就是在真正的数据前面加上帧头信息和CRC校验位，简化函数如下：

void data_packet_pack(uint8_t cmd_id, uint8_t cmd_set, uint8_t *p_data, uint16_t len, uint8_t *tx_buf)
{
    uint8_t cmd_set_prefix[2] = {cmd_set, cmd_id};
    uint32_t crc_data;
    uint16_t frame_length = HEADER_LEN + CMD_SET_PREFIX_LEN + len + CRC_DATA_LEN;
    Header *header = (Header*)tx_buf;
    header->sof = SOF;
    header->length = frame_length;
    header->version = VERSION;
    header->session_id = 0;
    header->is_ack = 0;
    header->reserved0 = 0;
    header->sender = sender;
    header->receiver = receiver;
    header->reserved1 = 0;
    header->seq_num = 0;
    header->crc = CRC16Calc(tx_buf, HEADER_LEN - CRC_HEAD_LEN);
    memcpy(&tx_buf[HEADER_LEN], cmd_set_prefix, CMD_SET_PREFIX_LEN);
    memcpy(&tx_buf[HEADER_LEN + CMD_SET_PREFIX_LEN], p_data, len);
    crc_data = CRC32Calc(tx_buf, frame_length - CRC_DATA_LEN);
    memcpy(&tx_buf[frame_length - CRC_DATA_LEN], &crc_data, CRC_DATA_LEN);
}

打包完数据后通过串口发送出去即可。

3.通信数据结构

一帧协议数据分为 4 个部分，分别是帧头数据、命令码ID、数据、帧尾校验数据。

(1)帧头数据

typedef struct
{
  uint8_t sof; /*!< Identify Of A Package */
  union {
    struct
    {
      uint16_t data_len : 10; /*!< Data Length, Include Header And Crc */
      uint16_t version : 6;   /*!< Protocol Version */
    };
    uint16_t ver_data_len;
  };
  union {
    struct
    {
      uint8_t session : 5;   /*!< Need(0~1) Or Not Need(2~63) Ack */
      uint8_t pack_type : 1; /*!< Ack Package Or Normal Package */
      uint8_t res : 2;       /*!< Reserve */
    };
    uint8_t S_A_R_c;
  };
  uint8_t sender;   /*!< Sender Module Information */
  uint8_t reciver;  /*!< Receiver Module Information */
  uint16_t res1;    /*!< Reserve 1 */
  uint16_t seq_num; /*!< Sequence Number */
  uint16_t crc_16;  /*!< CRC16 */
  uint8_t pdata[];
} protocol_pack_desc_t;

在ROS的代码中帧头结构体定义如下：

/**
 * @brief Package header used to resolve package
 */
    typedef struct Header {
        uint32_t sof : 8;
        uint32_t length : 10;
        uint32_t version : 6;
        uint32_t session_id : 5;
        uint32_t is_ack : 1;
        uint32_t reserved0 : 2; // Always 0
        uint32_t sender: 8;
        uint32_t receiver: 8;
        uint32_t reserved1 : 16;
        uint32_t seq_num : 16;
        uint32_t crc : 16;
    } Header;

帧头数据	占用字节	详细描述
sof	1	数据的域ID
ver_data_len	2	每帧内数据的长度和协议版本号
session	1	包序号，在0xA0域中保留
sender	1	发送者地址
reciver	1	发送者地址
res	2	保留位
seq	2	包序号
crc16	2	帧头的crc校验结果

(2)命令码

命令码	占用字节
cmdid	2

一个命令码对应一帧包含具体信息的数据，具体见protocol_define.h中的定义。

4.调试

运行该节点之前请确认端口号，端口号默认为/dev/serial_sdk，它是通过udev规则映射的。

连接STM32设备的虚拟串口，lsusb可以查看Vendor和Product的ID，然后创建并配置/etc/udev/rules.d/roborts.rules文件：

KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0777", SYMLINK+="serial_sdk"

也可能不是ttyACM*，也可能是ttyUSB*，具体取决与设备的驱动类型，需注意分辨。

编译

catkin_make -j4 roborts_base_node

运行

rosrun roborts_base roborts_base_node

控制

可以运行RoboRTS的其他模块控制机器人，也可以使用以下两种方式（以底盘为例）：

(1)发布Twist消息

新建一个节点来控制，实例代码如下：

#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>
#include <std_msgs/UInt8.h>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int kfd = 0;
struct termios cooked, raw;

void CloseKeyboard(int sig)
{
    tcsetattr(kfd, TCSANOW, &cooked);
    ROS_INFO_STREAM("Shut Down!");
    ros::shutdown();
}

int main(int argc, char** argv)
{
    uint8_t c;
    ros::init(argc,argv,"keyboard_node");
    ros::NodeHandle n;
    ros::Publisher keyboard_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    tcgetattr(kfd, &cooked);
    memcpy(&raw, &cooked, sizeof(struct termios));
    raw.c_lflag &=~ (ICANON | ECHO);
    raw.c_cc[VEOL] = 1;
    raw.c_cc[VEOF] = 2;
    tcsetattr(kfd, TCSANOW, &raw);
    
    puts("Reading from keyboard");
    
    struct pollfd ufd;
    ufd.fd = kfd;
    ufd.events = POLLIN;

    signal(SIGINT, CloseKeyboard);    //重定向shutdown函数，ROS默认调用ros::shutdown()

    while(ros::ok())
    {
        int num;
        geometry_msgs::Twist vel;
        if ((num = poll(&ufd, 1, 250)) < 0)
        {
            perror("poll():");
            return -1;
        }
        else if(num > 0)
        {
            if(read(kfd, &c, 1) < 0)
            {
                perror("read():");
                return -1;
            }
        }
        else
        {
            c = 0;
            vel.linear.x = 0;
            vel.linear.y = 0;
            vel.angular.z = 0;
        }

        if(c != 0)
        {
            ROS_INFO_STREAM("Push Down: " << c);
        }

        if(c == 'a')
        {
            vel.linear.x = -1;
        }
        else if(c == 'd')
        {
            vel.linear.x = 1;
        }
        else if(c == 'w')
        {
            vel.linear.y = 1;
        }
        else if(c == 's')
        {
            vel.linear.y = 1;
        }
        else if(c == 'q')
        {
            vel.angular.z = -1;
        }
        else if(c == 'e')
        {
            vel.angular.z = 1;
        }
        else if(c != 0)
        {
            ROS_INFO_STREAM("Invalid Command!");
            vel.linear.x = 0;
            vel.linear.y = 0;
            vel.angular.z = 0;
        }
        keyboard_pub.publish(vel);
    }
}

(2)直接发送串口命令

实例代码如下：

#include <termios.h>
#include <fcntl.h>
#include <unistd.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt8.h>
#include "chassis.h"
#include "crc.h"

using namespace std;

int serial_fd_;

cmd_chassis_speed chassis_speed;
cmd_heartbeat heartbeat;

static const size_t HEADER_LEN = sizeof(Header);
static const size_t CMD_SET_PREFIX_LEN = 2 * sizeof(uint8_t);
static const size_t CRC_DATA_LEN = sizeof(uint32_t);
static const size_t CRC_HEAD_LEN = sizeof(uint16_t);

int Write(const uint8_t *buf, int len) {
  return write(serial_fd_, buf, len);
}

uint16_t CRC16Update(uint16_t crc, uint8_t ch) {
  uint16_t tmp;
  uint16_t msg;

  msg = 0x00ff & static_cast<uint16_t>(ch);
  tmp = crc ^ msg;
  crc = (crc >> 8) ^ crc_tab16[tmp & 0xff];

  return crc;
}

uint32_t CRC32Update(uint32_t crc, uint8_t ch) {
  uint32_t tmp;
  uint32_t msg;

  msg = 0x000000ffL & static_cast<uint32_t>(ch);
  tmp = crc ^ msg;
  crc = (crc >> 8) ^ crc_tab32[tmp & 0xff];
  return crc;
}

uint16_t CRC16Calc(const uint8_t *data_ptr, size_t length) {
  size_t i;
  uint16_t crc = CRC_INIT;

  for (i = 0; i < length; i++) {
    crc = CRC16Update(crc, data_ptr[i]);
  }

  return crc;
}

uint32_t CRC32Calc(const uint8_t *data_ptr, size_t length) {
  size_t i;
  uint32_t crc = CRC_INIT;

  for (i = 0; i < length; i++) {
    crc = CRC32Update(crc, data_ptr[i]);
  }

  return crc;
}

void data_packet_pack(uint8_t cmd_id, uint8_t cmd_set, uint8_t *p_data, uint16_t len, uint8_t *tx_buf)
{
    uint8_t cmd_set_prefix[2] = {cmd_set, cmd_id};
    uint32_t crc_data;
    uint16_t frame_length = HEADER_LEN + CMD_SET_PREFIX_LEN + len + CRC_DATA_LEN;
    Header *header = (Header*)tx_buf;
    header->sof = 0xAA;
    header->length = frame_length;
    header->version = 0;
    header->session_id = 0;
    header->is_ack = 0;
    header->reserved0 = 0;
    header->sender = 0x00;
    header->receiver = 0x01;
    header->reserved1 = 0;
    header->seq_num = 0;
    header->crc = CRC16Calc(tx_buf, HEADER_LEN - CRC_HEAD_LEN);
    memcpy(&tx_buf[HEADER_LEN], cmd_set_prefix, CMD_SET_PREFIX_LEN);
    memcpy(&tx_buf[HEADER_LEN + CMD_SET_PREFIX_LEN], p_data, len);
    crc_data = CRC32Calc(tx_buf, frame_length - CRC_DATA_LEN);
    memcpy(&tx_buf[frame_length - CRC_DATA_LEN], &crc_data, CRC_DATA_LEN);
}

void keyboard_callback(const std_msgs::UInt8::ConstPtr& msg)
{
    if(msg->data == 'a')
    {
        chassis_speed.vx = -1000;
    }
    else if(msg->data == 'd')
    {
        chassis_speed.vx = 1000;
    }
    else if(msg->data == 'w')
    {
        chassis_speed.vy = 1000;
    }
    else if(msg->data == 's')
    {
        chassis_speed.vy = -1000;
    }
    else if(msg->data == 'q')
    {
        chassis_speed.vw = -1000;
    }
    else if(msg->data == 'w')
    {
        chassis_speed.vw = 1000;
    }
    else if(msg->data == 0)
    {
        chassis_speed.vx = 0;
        chassis_speed.vy = 0;
        chassis_speed.vw = 0;
    }
    else
    {
        ROS_INFO_STREAM("Invalid Command!\r\n");
        return;
    }
    chassis_speed.rotate_x_offset = 0;
    chassis_speed.rotate_y_offset = 0;
    uint8_t tx_buf[100];
    uint16_t frame_length = HEADER_LEN + CMD_SET_PREFIX_LEN + sizeof(cmd_chassis_speed) + CRC_DATA_LEN;
    data_packet_pack(0x02, 0x03, (uint8_t *)&chassis_speed, sizeof(cmd_chassis_speed), tx_buf);
    Write(tx_buf, frame_length);
    if(msg->data != 0)
        ROS_INFO_STREAM("Send chassis speed command.");
}

int main(int argc, char **argv)
{
    string port_name_ = "/dev/serial_sdk";
    #ifdef __arm__
    serial_fd_ = open(port_name_.c_str(), O_RDWR | O_NONBLOCK);
    #elif __x86_64__
    serial_fd_ = open(port_name_.c_str(), O_RDWR | O_NOCTTY);
    #else
    serial_fd_ = open(port_name_.c_str(), O_RDWR | O_NOCTTY);
    #endif

    if (serial_fd_ < 0) {
        ROS_INFO_STREAM("cannot open device " << serial_fd_ << " " << port_name_);
        return false;
    }
    ROS_INFO_STREAM("Serial Port initialized");
    ros::init(argc, argv, "test");
    ros::NodeHandle n;
    ros::Subscriber write_sub = n.subscribe("keyboard", 1000, keyboard_callback);
    ros::Rate loop_rate(50);
    while(ros::ok())
    {
        heartbeat.heartbeat = 0;
        uint8_t tx_buf[100];
        uint16_t frame_length = HEADER_LEN + CMD_SET_PREFIX_LEN + sizeof(cmd_heartbeat) + CRC_DATA_LEN;
        data_packet_pack(0x00, 0x01, (uint8_t *)&heartbeat, sizeof(cmd_heartbeat), tx_buf);
        Write(tx_buf, frame_length);
        ros::spinOnce();
        loop_rate.sleep();
    }
}