运动控制协议（UDP）

软件需求

• V2.2.90或更高版本的jy_exe；
• V4.0.0或更高版本的整机软件包。

如需查看jy_exe版本，可进入运动主机（默认地址为192.168.1.103）运行命令：

dpkg -l jy_exe

注意： 运动主机、感知主机、手柄之间的通信采用UDP协议，以小端字节序存放原始数据。

UDP 协议格式

根据是否携带复杂数据，UDP指令分为两类：简单指令和复杂指令。

简单指令

简单指令格式为 [xxxx yyyy zzzz]，一个字母表示一个字节。

简单指令采用结构体 CommandHead 存储原始数据内容：

struct CommandHead{
    uint32_t code;
    uint32_t paramters_size;
    uint32_t type;
};

• xxxx 是 CommandHead.code 的值，表示指令码。
• yyyy 是 CommandHead.paramters_size 的值，表示指令码的指令值，当指令码没有有效指令值时，yyyy = 0。
• zzzz 是 CommandHead.type 的值，用于区分指令类型，简单指令的类型值为0，即 zzzz = 0。

使用案例如下：

struct CommandHead command_head = {0};
command_head.code = 1;           //指令码
command_head.paramters_size = 0; // 指令值
command_head.type = 0;           // 指令类型
sendto(sfd,&command_head,sizeof(command_head),目标地址，地址长度)

复杂指令

复杂指令携带复杂的数据内容，格式为 [xxxx yyyy zzzz bbbbb……]，一个字母表示一个字节。

复杂指令采用结构体 Command 存储原始数据内容：

struct CommandHead{
    uint32_t code;
    uint32_t paramters_size;
    uint32_t type;
};

const uint32_t kDataSize = 256;

struct Command{
    CommandHead head;
    uint32_t data[kDataSize];
};

• xxxx 是 Command.head.code 的值，表示指令码。
• yyyy 是 Command.head.paramters_size 的值，表示数据内容 bbbbb…… 的长度。
• zzzz 是 Command.head.type 的值，复杂指令的类型值为1，即 zzzz=1。
• bbbbb…… 是 Command.data 的值，表示该指令携带的数据内容。

使用案例如下：

int32_t to_be_send_data[64];
struct Command command = {0};
command.head.code = 52;        // 指令码
command.head.paramters_size = sizeof(to_be_send_data);// 数据长度
command.head.type = 1;         // 指令类型
memcpy(&command.data,&to_be_send_data,sizeof(to_be_send_data));
sendto(sfd,&command,sizeof(command.head)+command.head.paramters_size,目标地址，地址长度);

复杂指令接收

以关节速度为例，使用案例如下：

struct RobotJointVel{
    double joint_vel[12];
}; // 以关节速度为例

struct CommandMessage{
    CommandHead command;
    uint8_t data_buffer[1024];
};

CommandMessage cm;
size_t recv_size = 0;
recv_size = recvfrom(server_fd,&cm,sizeof(cm),0,(sockaddr*)&client_addr,&sockaddr_in_size);
if(cm.command.type == 1){ // 判断为复杂指令
    if(recv_size == cm.command.paramters_size + sizeof(cm.command)){
        uint8_t* temp_values = new uint8_t[cm.command.paramters_size];
        memcpy(temp_values,cm.data_buffer,cm.command.paramters_size);
        if(cm.command.code == 0x0903){ // 判断指令码，解析数据
            RobotJointVel *joint_vel = (RobotJointVel *)temp_values;
        }
    }
}

指令码来源标识

注意： 本节中介绍的十六进制形式的所有指令码，前2位用于区分指令码发送源：当使用手柄等遥控终端发送指令码时，指令码前两位请使用 0x21，例如 0x21040001；当通过导航模块等自主移动算法模块下发指令码时，指令码前两位请使用 0x31，例如 0x31040001。后文中的指令码默认以 0x21 开头进行介绍。