AGV协议

实时上报当前坐标
和控制通信不用一个端口，此上报坐标端口为6002，控制通信使用6001
{
"positionx":3.0746, //x坐标
"positiony":-0.3723,    // y坐标
"positionz":0,         // z坐标
"orientationx":0,         //  四元数x
"orientationy":0,         //  四元数y
"orientationz":1,         //  四元数z
"orientationw":0.002     //   四元数w

"position":"AP1-AP2"  //"position":"AP1-AP2"为AP1到AP2路上，  "position":"AP1"为小车目前在AP1点位
}

平面图的四元数xy为0，四元数zw可以根据公式算出目前小车朝向


假定q=[0,0,0.5,0.866]
q.w=cos((a/2)*pi/180) =  0.866
q.x=RAix.x*sin((a/2)*pi/180) = 0
q.y=RAix.y*sin((a/2)*pi/180) = 0
q.z=RAix.z*sin((a/2)*pi/180) = 0.5


由cos((a/2)*pi/180) =  0.866，得到
a = 60或120
由RAix.z*sin((a/2)*pi/180) = 0.5，得到
a = 60或150
            因此a = 60度(四元数的旋转角度一般是在0-360之间，之后就是多一圈的问题。
            于是可得RAix.z = 1,因此其他q=[0.866,0,0,0.5]意味着旋转轴是RAxis =[0 0 1]，旋转角度是60度，其他的类似可以计算

手动控制小车移动

{
"vel":0,   //线速度值
"ang":0    //角速度值
}

移动到下一坐标点
发送
{
"request":1,  //标识符号
"position":"AP1" //AP1是第一个点位，之后点位依次为AP2， AP3 ，AP4
}

成功 回复  返回当前位姿
{
    "response":1  //正确标识符
}

失败回复  返回当前位姿 

{
    "response":501,       //错误标识符号
}

以自身为中心进行移动或旋转

发送
{
"request":2,   //标识符号
"positionx":0,       //前进的距离
"positiony":0,        //左右移动的距离
"angle":90             //旋转的角度
}

回复  返回当前位姿
{
"response":2        //正确标识符
}

失败回复  返回当前位姿
{
"response":502    //错误标识符号

}

这一条可以先不加，之后扩展功能再加

读取小车设备信息，需要读信息不用全部发送，发送想要读的数据并设置值为1即可，
或者将想要读的数据值设置为1，不想读的数据设置为0
{
    "request":3,   //标识符号
    "ip":1,       //小车IP地址
    "port":0,    //小车端口号
}