机械臂主控通讯协议 v2
==========================================
此协议将于 1.5 MAX 启用，1.5 MAX 之前的版本请参考 v1 版本协议。

协议格式
+++++++++++++++++++++++++++++++++

“指令(数据)+分隔符+数据+分隔符+数据+分隔符+…+数据+分隔符+结束符”

发送和接收的数据格式如上所示，数据之间通过分隔符划分，最后以一个结束符来结尾。分隔符可以是英文逗号(',')或者空格(' ')，结束符可以使'#'或者'\\n'。分隔符和结束符根据机械臂的特性可以灵活调整。


error_code判断
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

.. warning:: 机械臂主控时，机械臂运行程序必须要对接收的返回数据进行 error_code 的判断。如果不处理 error_code，机械臂会发生碰撞或者造成其他更严重的后果，威胁使用者的人身安全！

指令
++++++++++++++++++++++++++

**心跳信号(xyzHeartBeat)**

+------------+----------------+-------------------------+
| 机械臂发送 | “500,#”        | 500: 机械臂ping工控机用 |
+------------+----------------+-------------------------+
| 机械臂接收 | “error_code,#” |                         |
+------------+----------------+-------------------------+

**切换任务(xyzSwitchTask)**

+------------+-----------------------+------------------------------------------------------+
| 机器人发送 | “502,task_codename,#” | 502: “切换任务”指令号                                |
|            |                       +------------------------------------------------------+
|            |                       | task_codename(string): 任务代号，以”.t“结尾，如“1.t” |
+------------+-----------------------+------------------------------------------------------+
| 机器人接收 | “error_code,#“        |                                                      |
+------------+-----------------------+------------------------------------------------------+

**切换工具(xyzSwitchTool)**

+------------+-----------------+------------------------+
| 机器人发送 | “503,tool_id,#” | 503: “切换工具”指令号  |
|            |                 +------------------------+
|            |                 | tool_id(int): 工具id   |
+------------+-----------------+------------------------+
| 机器人接收 | “error_code,#“  |                        |
+------------+-----------------+------------------------+

**呼叫视觉命令(xyzCallVisionCmd)**

+------------+-------------------------------+---------------------------------------+
| 机器人发送 | “504,vs_id,vision_codename,#” | 504: “呼叫视觉”指令号                 |
|            |                               +---------------------------------------+
|            |                               | vs_id(int): 视觉服务id                |
|            |                               +---------------------------------------+
|            |                               | vision_codename(string)：视觉命令代号 |
+------------+-------------------------------+---------------------------------------+
| 机器人接收 | “error_code,#“                |                                       |
+------------+-------------------------------+---------------------------------------+

**请求抓取目标点位(xyzReqGraspPose)**

+------------+----------------------+----------------------------------------------+
| 机器人发送 | “507,vs_id,#”        | 507: “请求抓取目标点位”指令号                |
|            |                      +----------------------------------------------+
|            |                      | vs_id(int): 视觉服务id                       |
+------------+----------------------+----------------------------------------------+
| 机器人接收 | “error_code,token,#” | token(int): 在执行”获取抓取目标点位“时使用到 |
+------------+----------------------+----------------------------------------------+

**获取抓取目标点位(xyzGetGraspPose)**

+------------+-----------------------------------------------+--------------------------------------------+
| 机器人发送 | “508,token,#”                                 | 508: ”获取抓取目标点位”指令号              |
|            |                                               +--------------------------------------------+
|            |                                               | token(int): “请求抓取目标点位”中返回的token|
+------------+-----------------------------------------------+--------------------------------------------+
| 机器人接收 | “error_code,x,y,z,a,b,c,d,num,pipeline_num,   | x,y,z,a,b,c,d(float): 抓取目标点的位姿数据 |
|            | register_num,int1,int2,int3,int4,int5,int6,#” +--------------------------------------------+
|            |                                               | num(int): 当前可供抓取的点位数量           |
|            |                                               +--------------------------------------------+
|            |                                               | pipeline_num(int): 运动流程编号            |
|            |                                               +--------------------------------------------+
|            |                                               | register_num(int): 抓取序号                |
|            |                                               +--------------------------------------------+
|            |                                               | int1~int6(int)：可在Task中自定义含义       |
|            |                                               +--------------------------------------------+
|            |                                               | 说明：眼在手上不能用异步方式               |
+------------+-----------------------------------------------+--------------------------------------------+

**请求物体位姿(xyzReqObjPose)**

+------------+----------------------+------------------------------------------+
| 机器人发送 | “509,vs_id,#”        | 509: “请求物体位姿”指令号                |
|            |                      +------------------------------------------+
|            |                      | vs_id(int): 视觉服务id                   |
+------------+----------------------+------------------------------------------+
| 机器人接收 | “error_code,token,#“ | token(int): 在执行”获取物体位姿“时使用到 |
+------------+----------------------+------------------------------------------+

**获取物体位姿(xyzGetObjPose)**

+------------+--------------------------------------------+-----------------------------------------+
| 机器人发送 | “510,token,#”                              | 510: “获取物体位姿”指令号               |
|            |                                            +-----------------------------------------+
|            |                                            | token(int) :“请求物体位姿”中返回的token |
+------------+--------------------------------------------+-----------------------------------------+
| 机器人接收 | “error_code,x,y,z,a,b,c,d,num,object_name, | x,y,z,a,b,c,d(float): 物体的位姿数据    |
|            | int1,int2,int3,int4,int5,int6,#“           +-----------------------------------------+
|            |                                            | num(int): 当前物体位姿个数              |
|            |                                            +-----------------------------------------+
|            |                                            | object_name(string) :物体名称           |
|            |                                            +-----------------------------------------+
|            |                                            | int1~int6(int)：可在Task中自定义其含义  |
+------------+--------------------------------------------+-----------------------------------------+

**重置任务(xyzResetTask)**

+------------+----------------+--------------------------------+
| 机器人发送 | “511,#”        | 511: “重置任务”指令号          |
|            |                +--------------------------------+
|            |                | 下位机在初始化时需要调用该指令 |
+------------+----------------+--------------------------------+
| 机器人接收 | “error_code,#“ |                                |
+------------+----------------+--------------------------------+

**发送机器人当前关节坐标(xyzSendCurrentJoints)**

+------------+---------------------------+-----------------------------------------------------------+
| 机器人发送 | “512,j1,j2,j3,j4,j5,j6,#” | 512: “发送机器人当前关节坐标”指令号                       |
|            |                           +-----------------------------------------------------------+
|            |                           | j1~j6(float): 机器人当前的角度信息。                      |
|            |                           |                                                           |
|            |                           | 如果机器人轴数小于6轴，则需要对应位置补零，依旧发送六个数 |
+------------+---------------------------+-----------------------------------------------------------+
| 机器人接收 | “error_code,#“            |                                                           |
+------------+---------------------------+-----------------------------------------------------------+

**发送机器人当前笛卡尔坐标(xyzSendCurrentCartPose)**

+------------+-----------------------+--------------------------------------------------------+
| 机器人发送 | “513,x,y,z,a,b,c,d,#” | 513: “发送机器人当前笛卡尔坐标”指令号                  |
|            |                       +--------------------------------------------------------+
|            |                       | x,y,z(float): 机器人当前位置数据                       |
|            |                       +--------------------------------------------------------+
|            |                       | a,b,c,d(float): 机器人当前姿态数据                     |
|            |                       |                                                        |
|            |                       | 如果机器人是欧拉角形式，则只需 给a,b,c赋值，d赋值0即可 |
+------------+-----------------------+--------------------------------------------------------+
| 机器人接收 | “error_code,#“        |                                                        |
+------------+-----------------------+--------------------------------------------------------+

**发送机器人当前扩展轴坐标(xyzSendCurrentExtJoints)**

+------------+---------------------------+--------------------------------------------+
| 机器人发送 | “514,j1,j2,j3,j4,j5,j6,#” | 514: ”发送机器人当前扩展轴坐标“指令号      |
|            |                           +--------------------------------------------+
|            |                           | j1~j6(float): 机器人当前扩展轴的角度信息， |
|            |                           | 如果扩展轴数不足6的,需要补零后发送六个数   |
+------------+---------------------------+--------------------------------------------+
| 机器人接收 | “error_code,#“            |                                            |
+------------+---------------------------+--------------------------------------------+

**请求抓放规划(xyzReqPickPlace)**

+------------+----------------+---------------------------+
| 机器人发送 | “517,vs_id,#”  | 517: “请求抓放规划“指令号 |
|            |                +---------------------------+
|            |                | vs_id(int): 视觉服务id    |
+------------+----------------+---------------------------+
| 机器人接收 | “error_code,#“ |                           |
+------------+----------------+---------------------------+

**获取取料入框轨迹(xyzGetPickin)**

+------------+-----------------------------------------------+-------------------------------------------------------------+
| 机器人发送 | “518,vs_id,#”                                 | 518: ”获取取料入框轨迹“指令号                               |
|            |                                               +-------------------------------------------------------------+
|            |                                               | vs_id(int): 视觉服务id                                      |
+------------+-----------------------------------------------+-------------------------------------------------------------+
| 机器人接收 | “error_code,pipeline_num,register_num,traj,#“ | pipeline_num(int): 运动流程编号                             |
|            |                                               +-------------------------------------------------------------+
|            |                                               | register_num(int): 抓取序号                                 |
|            |                                               +-------------------------------------------------------------+
|            |                                               | traj(轨迹)：num+ wp_type + wp + wp_type + wp +….            |
|            |                                               +-------------------------------------------------------------+
|            |                                               | num(int)：轨迹中的点位数量                                  |
|            |                                               +-------------------------------------------------------------+
|            |                                               | wp_type(int)：点位运动类型                                  |
|            |                                               |                                                             |
|            |                                               |      - 11: moveJ(joints)                                    |
|            |                                               |                                                             |
|            |                                               |      - 12: moveL(joints)                                    |
|            |                                               |                                                             |
|            |                                               |      - 21: moveJ(cart_pose)                                 |
|            |                                               |                                                             |
|            |                                               |      - 22: moveL(cart_pose)                                 |
|            |                                               +-------------------------------------------------------------+
|            |                                               | wp(6个float)：点位                                          |
|            |                                               |                                                             |                                        
|            |                                               | - 当 wp_type 是11或12时,joints: j1~j6                       |
|            |                                               | - 当 wp_type 是21或22时,cart_pose: xyzabc                   |
+------------+-----------------------------------------------+-------------------------------------------------------------+


**获取取料出框轨迹(xyzGetPickout)**

+------------+-----------------------------------------------+----------------------------------+
| 机器人发送 | “519,vs_id,#”                                 | 519: ”获取取料出框轨迹“指令号    |
|            |                                               +----------------------------------+
|            |                                               | vs_id(int): 视觉服务id           |
+------------+-----------------------------------------------+----------------------------------+
| 机器人接收 | “error_code,pipeline_num,register_num,traj,#“ | pipeline_num(int): 运动流程编号  |
|            |                                               +----------------------------------+
|            |                                               | register_num(int): 抓取序号      |
|            |                                               +----------------------------------+
|            |                                               | traj(轨迹),同 xyzGetPickin traj  |
+------------+-----------------------------------------------+----------------------------------+

**获取放料入框轨迹(xyzGetPlacein)**

+------------+-----------------------------------------------+---------------------------------+
| 机器人发送 | “520,vs_id,#”                                 | 520: ”获取放料入框轨迹“指令号   |
|            |                                               +---------------------------------+
|            |                                               | vs_id(int): 视觉服务id          |
+------------+-----------------------------------------------+---------------------------------+
| 机器人接收 | “error_code,pipeline_num,register_num,traj,#“ | pipeline_num(int): 运动流程编号 |
|            |                                               +---------------------------------+
|            |                                               | register_num(int): 抓取序号     |
|            |                                               +---------------------------------+
|            |                                               | traj(轨迹),同 xyzGetPickin traj |
+------------+-----------------------------------------------+---------------------------------+

**获取放料出框轨迹(xyzGetPlaceout)**

+------------+-----------------------------------------------+---------------------------------+
| 机器人发送 | “521,vs_id,#”                                 | 521: ”获取放料出框轨迹“指令号   |
|            |                                               +---------------------------------+
|            |                                               | vs_id(int): 视觉服务id          |
+------------+-----------------------------------------------+---------------------------------+
| 机器人接收 | “error_code,pipeline_num,register_num,traj,#“ | pipeline_num(int): 运动流程编号 |
|            |                                               +---------------------------------+
|            |                                               | register_num(int): 抓取序号     |
|            |                                               +---------------------------------+
|            |                                               | traj(轨迹),同 xyzGetPickin traj |
+------------+-----------------------------------------------+---------------------------------+

**料箱重定位(xyzUpdateTotePose)**

+------------+----------------+-------------------------+
| 机器人发送 | “523,vs_id,#”  | 523: ”料箱重定位“指令号 |
|            |                +-------------------------+
|            |                | vs_id(int): 视觉服务id  |
+------------+----------------+-------------------------+
| 机器人接收 | “error_code,#“ |                         |
+------------+----------------+-------------------------+

**工件在手上的二次定位(xyzUpdateObjPoseInHand)**

+------------+------------------------------+-------------------------------------+
| 机器人发送 | “524,#”                      | 524: ”工件在手上的二次定位“指令号   |
+------------+------------------------------+-------------------------------------+
| 机器人接收 | “error_code,x,y,z,a,b,c,d,#“ |  x,y,z,a,b,c,d(float): 放置位姿数据 |
+------------+------------------------------+-------------------------------------+

**切换工件(xyzSwitchItem)**

+------------+-----------------------------+---------------------------------+
| 机器人发送 | “528,vs_id,item_codename,#” | 528: “切换工件”指令号           |
|            |                             +---------------------------------+
|            |                             | vs_id(int): 视觉服务id          |
|            |                             +---------------------------------+
|            |                             | item_codename(string): 工件代号 |
+------------+-----------------------------+---------------------------------+
| 机器人接收 | “error_code,#“              |                                 |
+------------+-----------------------------+---------------------------------+

**计算抓取目标点位(xyzCalculateGraspPose)**

+------------+-----------------------------------------------+----------------------------------------------+
| 机器人发送 | “529,vs_id,#”                                 | 529: “计算抓取目标点位”指令号                |
|            |                                               +----------------------------------------------+
|            |                                               | 该指令等价于xyzReqGraspPose +xyzGetGraspPose |
|            |                                               +----------------------------------------------+
|            |                                               | vs_id(int): 视觉服务id                       |
+------------+-----------------------------------------------+----------------------------------------------+
| 机器人接收 | “error_code,x,y,z,a,b,c,d,num,pipeline_num,   | x,y,z,a,b,c,d(float): 抓取目标点的位姿数据   |
|            | register_num,int1,int2,int3,int4,int5,int6,#” +----------------------------------------------+
|            |                                               | num(int): 当前可供抓取的点位数量             |
|            |                                               +----------------------------------------------+
|            |                                               | pipeline_num(int): 运动流程编号              |
|            |                                               +----------------------------------------------+
|            |                                               | register_num(int): 抓取序号                  |
|            |                                               +----------------------------------------------+
|            |                                               | int1~int6(int)：可在Task中自定义含义         |
+------------+-----------------------------------------------+----------------------------------------------+

**计算物体位姿(xyzCalculateObjectPose)**

+------------+--------------------------------------------+-----------------------------------------+
| 机器人发送 | “530,vs_id,#”                              | 530: “计算物体位姿”指令号               |
|            |                                            +-----------------------------------------+
|            |                                            | 该指令等价于xyzReqObjPose+xyzGetObjPose |
|            |                                            +-----------------------------------------+
|            |                                            | vs_id(int): 视觉服务id                  |
+------------+--------------------------------------------+-----------------------------------------+
| 机器人接收 | “error_code,x,y,z,a,b,c,d,num,object_name, | x,y,z,a,b,c,d(float): 物体的位姿数据    |
|            | int1,int2,int3,int4,int5,int6,#“           +-----------------------------------------+
|            |                                            | num(int): 当前物体位姿个数              |
|            |                                            +-----------------------------------------+
|            |                                            | object_name(string) :物体名称           |
|            |                                            +-----------------------------------------+
|            |                                            | int1~int6(int)：可在Task中自定义含义    |
+------------+--------------------------------------------+-----------------------------------------+

**用户指令(xyzUsrCmd)**

+------------+---------------------------------------------------------------+------------------------------------------------------------+
| 机器人发送 | “600,str_1,str_2,str_3,str_4,str_5,                           | 600: ”用户指令”的指令号                                    |
|            | int_1,int_2,int_3,int_4,int_5,int_6,int_7,int_8,int_9,int_10, +------------------------------------------------------------+
|            | float_1,float_2,float_3,float_4,float_5,                      | 用于基础指令不支持的情况下，配合任务流图完成用户特定功能。 |
|            | float_6,float_7,float_8,float_9,float_10,                     +------------------------------------------------------------+
|            | x,y,z,a,b,c,d,                                                | 各个参数的含义取决于任务流图中设定的输入输出。             |
|            | j1,j2,j3,j4,j5,j6,#”                                          +------------------------------------------------------------+
|            |                                                               | str_1~str_5：提供5个字符串供用户输入                       |
|            |                                                               +------------------------------------------------------------+
|            |                                                               | int_1~int_10：提供10个整数类型供用户输入                   |
|            |                                                               +------------------------------------------------------------+
|            |                                                               | float_1~float_10：提供10个浮点数供用户输入                 |
|            |                                                               +------------------------------------------------------------+
|            |                                                               | xyzabcd(float)：笛卡尔坐标值，如果机器人                   |
|            |                                                               | 是欧拉角形式，则只需给a,b,c赋值，d赋0即可                  |
|            |                                                               +------------------------------------------------------------+
|            |                                                               | j1~j6：关节坐标值                                          |
+------------+---------------------------------------------------------------+------------------------------------------------------------+
| 机器人接收 | “error_code,str_1,str_2,str_3,str_4,str_5,                    | str_1~str_5：返回5个字符串                                 |
|            | int_1,int_2,int_3,int_4,int_5,int_6,int_7,int_8,int_9,int_10, +------------------------------------------------------------+
|            | float_1,float_2,float_3,float_4,float_5,                      | int_1~int_10：返回10个整数值                               |
|            | float_6,float_7,float_8,float_9,float_10,                     +------------------------------------------------------------+
|            | x,y,z,a,b,c,d,                                                | float_1~float_10：返回10个浮点数                           |
|            | j1,j2,j3,j4,j5,j6,#”                                          +------------------------------------------------------------+
|            |                                                               | xyzabcd：返回一个笛卡尔位姿数据                            |
|            |                                                               +------------------------------------------------------------+
|            |                                                               | j1~j6：返回一个关节坐标                                    |
+------------+---------------------------------------------------------------+------------------------------------------------------------+