ROS-Industrial 之 ABB_Driver ——ROS Server(1/3)
为了研究ABB机器人与ROS的交互控制，最近和ABB的机器人还有ROS-Industrial杠上了，之前把ROSLAUNCH的.launch/XML文件的语法格式弄清楚了，现在搞abb_driver的东西！
1. 概述与前置条件
ABB_Driver是ROS-Industrial的一部分，而其中的ABB ROS Server是其中最重要的组成部分之一，其是用ABB自己开发的机器人编程语言RAPID编写的，通过Socket接口和多任务处理完成与ROS的交互。ROS Server的代码在IRC5控制器上测试可以使用，在其它类型的ABB控制器可能要经过调整，要在ABB机器人控制器上安装ROS Server首先要满足以下两个条件：
- 623-1： Multitasking
- 616-1： PC Interface
就是说你的控制器里要启动这两个功能组件。
上述条件满足后先从机器人端开始研究ROS Server到底怎么用。
2. ROS Server
ROS Server的文件组成
ROS Server按照功能主要由六类文件组成，对于单臂机器人来讲每一类文件包含一个程序文件，但对于双臂机器人来说，每类文件要包含两个程序文件，因为在程序执行的时候会在每一个单臂下生成各自的执行程序文件。从功能上讲，这六类文件为：
| 文件名 | 功能 |
|---|---|
| ROS_common.sys | 对共用的数据进行定义与赋值 |
| ROS_messages.sys | 对轨迹与关节点数据进行定义，并定义数据的接收与发送函数 |
| ROS_socket.sys | 定义Socket通讯功能相关的所有函数 |
| ROS_stateServer.mod | 状态反馈服务器，向ROS发送机器人相关状态 |
| ROS_motionServer.mod | 运动控制服务器 |
| ROS_motion.mod | 机器人实际运动控制程序 |
以上文件是于2017-12-22从GitHub里直接下载下来的，还没有进行代码的修改，接下来先分析每一个文件的功能。
哎，等一下：关于权利声明和软件License什么的都放在这里了，适用于后面所有ROS Server的代码，后面就不写在代码里了。
Software License Agreement (BSD License)
Copyright (c) 2012, Edward Venator, Case Western Reserve University
Copyright (c) 2012, Jeremy Zoss, Southwest Research Institute下面开始真正的程序内容。
ROS_common.sys
MODULE ROS_common(SYSMODULE)
RECORD ROS_joint_trajectory_pt
robjoint joint_pos;
num duration;
ENDRECORD
CONST num MAX_TRAJ_LENGTH := 100;
PERS bool ROS_trajectory_lock := false;
PERS ROS_joint_trajectory_pt ROS_trajectory{MAX_TRAJ_LENGTH};
PERS num ROS_trajectory_size := 0;
PERS bool ROS_new_trajectory := false;
ENDMODULE此程序文件作为控制器中的一个系统模块对以下各大程序共用的变量和结构体进行了定义，下面一行一行分析每一行代码的意思。
第一行代码与最后一行代码合起来看：
MODULE ROS_common(SYSMODULE)
······
ENDMODULE这两行代码在RAPID中的意思是，在控制器中定义一个名为ROS_common的系统模块。MODULE和ENDMODULE是其中的关键字。(SYSMODULE)是程序属性声明。ROS_common就是定义的模块名字了。
RECORD ROS_joint_trajectory_pt
robjoint joint_pos;
num duration;
ENDRECORD定义一个关节空间内机器运行路径的结构体，RECORD 和ENDRECORD为定义结构体的关键字，ROS_joint_trajectory_pt为定义的结构体的名称，其中robjoint为定义机器目标点的关键字，robjoint类数据用于储存机械臂轴1到6的轴位置，以度计。将轴位置定义为各轴（臂）从轴校准位置沿正方向或负方向旋转的度数。其结构为：
RECORD robjoint
num rax_1;
num rax_2;
num rax_3;
num rax_4;
num rax_5;
num rax_6;
ENDRECORD也就是说robjoint是用于六轴机器人进行关节位置定义的数据结构，但对于像Yumi这类七轴的机器人，则要用另一种数据结构进行定义，等我们对Yumi进行控制的时候会说明如何对程序进行修改，现在的任务是弄懂每类程序的意思。
num duration;这一行代码用来进行关节空间的周期控制，通过对周期进行控制进而控制关节空间点到点的关节速度。
CONST num MAX_TRAJ_LENGTH := 100;此行代码是定义关节点序列最大序列点的个数为100，是一个常量，也就是说控制器最多从ROS端接收100个关节位置序列。CONST代表常量，num代表数值类型，MAX_TRAJ_LENGTH := 100是将名为MAX_TRAJ_LENGTH 的数值类型变量赋值为100，而 :=为RAPID里的赋值运算符。
PERS bool ROS_trajectory_lock := false;此行代码对是对关节轨迹数据定义一个读写信号量，读的时候可以不调用信号量，但是在写的时候要对路径数据进行锁定，隔断其它的读操作。其中PERS是定义永久变量的关键字，可以类似的理解为C++中的全局变量。bool是逻辑变量的关键字，在RAPID中其与C++中定义逻辑变量的关键字是一样的。
PERS ROS_joint_trajectory_pt ROS_trajectory{MAX_TRAJ_LENGTH};此行代码定义全局的机器人运行路径数据，是一个结构体数组，基础元素为ROS_joint_trajectory_pt，其中包括关节轴的度数数据与对应的关节空间点到点的周期，数组长度为100。
PERS num ROS_trajectory_size := 0;此行代码定义机器人路径数据中实际关节空间点的个数。初始值为0。
PERS bool ROS_new_trajectory := false;此行代码定义机器人控制器是否有新的路径轨迹。
总体上来讲ROS_common.sys程序文件中定义了机器人运动的路径信息及对路径信息进行读写等控制的相关对应数据。
ROS_messages.sys
整个ROS_messages.sys的代码如下所示：
MODULE ROS_messages(SYSMODULE)
RECORD ROS_msg_header
num msg_type;
num comm_type;
num reply_code;
ENDRECORD
RECORD ROS_msg
ROS_msg_header header;
rawbytes data;
ENDRECORD
RECORD ROS_msg_joint_traj_pt
ROS_msg_header header;
num sequence_id;
robjoint joints; ! in DEGREES
num velocity;
num duration;
ENDRECORD
RECORD ROS_msg_joint_data
ROS_msg_header header;
num sequence_id;
robjoint joints; ! in DEGREES
ENDRECORD
! Message Type Codes (from simple_message/simple_message.h)
CONST num ROS_MSG_TYPE_INVALID := 0;
CONST num ROS_MSG_TYPE_JOINT := 10; ! joint-position feedback
CONST num ROS_MSG_TYPE_JOINT_TRAJ_PT := 11; ! joint-trajectory-point (for path downloading)
CONST num ROS_COM_TYPE_TOPIC := 1;
CONST num ROS_COM_TYPE_SRV_REQ := 2;
CONST num ROS_COM_TYPE_SRV_REPLY := 3;
CONST num ROS_REPLY_TYPE_INVALID := 0;
CONST num ROS_REPLY_TYPE_SUCCESS := 1;
CONST num ROS_REPLY_TYPE_FAILURE := 2;
! "Special" Sequence Codes (from simple_message/joint_traj_pt.h)
CONST num ROS_TRAJECTORY_START_DOWNLOAD := -1;
CONST num ROS_TRAJECTORY_END := -3;
CONST num ROS_TRAJECTORY_STOP := -4;
! Other message constants
CONST num ROS_MSG_MAX_JOINTS := 10; ! from joint_data.h
PROC ROS_receive_msg_joint_traj_pt(VAR socketdev client_socket, VAR ROS_msg_joint_traj_pt message, \num wait_time)
VAR ROS_msg raw_message;
! Read raw message data
IF Present(wait_time) THEN
ROS_receive_msg client_socket, raw_message, \wait_time:=wait_time;
ELSE
ROS_receive_msg client_socket, raw_message;
ENDIF
! Integrity Check: Message Type
IF (raw_message.header.msg_type <> ROS_MSG_TYPE_JOINT_TRAJ_PT) THEN
ErrWrite \W, "ROS Socket Type Mismatch", "Unexpected message type",
\RL2:="expected: " + ValToStr(ROS_MSG_TYPE_JOINT_TRAJ_PT),
\RL3:="received: " + ValToStr(raw_message.header.msg_type);
RAISE ERR_ARGVALERR; ! TBD: define specific error code
ENDIF
! Integrity Check: Data Size
IF (RawBytesLen(raw_message.data) < 52) THEN
ErrWrite \W, "ROS Socket Missing Data", "Insufficient data for joint_trajectory_pt",
\RL2:="expected: 52",
\RL3:="received: " + ValToStr(RawBytesLen(raw_message.data));
RAISE ERR_OUTOFBND; ! TBD: define specific error code
ENDIF
! Copy Header data
message.header := raw_message.header;
! Unpack data fields
UnpackRawBytes raw_message.data, 1, message.sequence_id, \IntX:=DINT;
UnpackRawBytes raw_message.data, 5, message.joints.rax_1, \Float4;
UnpackRawBytes raw_message.data, 9, message.joints.rax_2, \Float4;
UnpackRawBytes raw_message.data, 13, message.joints.rax_3, \Float4;
UnpackRawBytes raw_message.data, 17, message.joints.rax_4, \Float4;
UnpackRawBytes raw_message.data, 21, message.joints.rax_5, \Float4;
UnpackRawBytes raw_message.data, 25, message.joints.rax_6, \Float4;
! Skip bytes 29-44. UNUSED. Reserved for Joints 7-10. TBD: copy to extAx?
UnpackRawBytes raw_message.data, 29+(ROS_MSG_MAX_JOINTS-6)*4, message.velocity, \Float4;
UnpackRawBytes raw_message.data, 33+(ROS_MSG_MAX_JOINTS-6)*4, message.duration, \Float4;
! Convert data from ROS units to ABB units
message.joints := rad2deg_robjoint(message.joints);
! TBD: convert velocity
ERROR
RAISE; ! raise errors to calling code
ENDPROC
PROC ROS_send_msg_joint_data(VAR socketdev client_socket, ROS_msg_joint_data message)
VAR ROS_msg raw_message;
VAR robjoint ROS_joints;
VAR num i;
! Force message header to the correct values
raw_message.header.msg_type := ROS_MSG_TYPE_JOINT;
raw_message.header.comm_type := ROS_COM_TYPE_TOPIC;
raw_message.header.reply_code := ROS_REPLY_TYPE_INVALID;
! Convert data from ABB units to ROS units
ROS_joints := deg2rad_robjoint(message.joints);
! Pack data into message
PackRawBytes message.sequence_id, raw_message.data, 1, \IntX:=DINT;
PackRawBytes ROS_joints.rax_1, raw_message.data, 5, \Float4;
PackRawBytes ROS_joints.rax_2, raw_message.data, 9, \Float4;
PackRawBytes ROS_joints.rax_3, raw_message.data, 13, \Float4;
PackRawBytes ROS_joints.rax_4, raw_message.data, 17, \Float4;
PackRawBytes ROS_joints.rax_5, raw_message.data, 21, \Float4;
PackRawBytes ROS_joints.rax_6, raw_message.data, 25, \Float4;
FOR i FROM 1 TO ROS_MSG_MAX_JOINTS-6 DO
! Insert placeholders for joints 7-10 (message expects 10 joints)
PackRawBytes 0, raw_message.data, 25+i*4, \Float4;
ENDFOR
ROS_send_msg client_socket, raw_message;
ERROR
RAISE; ! raise errors to calling code
ENDPROC
LOCAL FUNC num deg2rad(num deg)
RETURN deg * pi / 180;
ENDFUNC
LOCAL FUNC robjoint deg2rad_robjoint(robjoint deg)
VAR robjoint rad;
rad.rax_1 := deg2rad(deg.rax_1);
rad.rax_2 := deg2rad(deg.rax_2);
rad.rax_3 := deg2rad(deg.rax_3);
rad.rax_4 := deg2rad(deg.rax_4);
rad.rax_5 := deg2rad(deg.rax_5);
rad.rax_6 := deg2rad(deg.rax_6);
RETURN rad;
ENDFUNC
LOCAL FUNC num rad2deg(num rad)
RETURN rad * 180 / pi;
ENDFUNC
LOCAL FUNC robjoint rad2deg_robjoint(robjoint rad)
VAR robjoint deg;
deg.rax_1 := rad2deg(rad.rax_1);
deg.rax_2 := rad2deg(rad.rax_2);
deg.rax_3 := rad2deg(rad.rax_3);
deg.rax_4 := rad2deg(rad.rax_4);
deg.rax_5 := rad2deg(rad.rax_5);
deg.rax_6 := rad2deg(rad.rax_6);
RETURN deg;
ENDFUNC
ENDMODULE接下来分段的理解：
RECORD ROS_msg_header
num msg_type;
num comm_type;
num reply_code;
ENDRECORD这部分结构体对机器人与控制器交换的信息的开头端ROS_msg_header进行定义，在其中分别定义了信息的类型msg_type，向机器人传递的命令的类型comm_type，以及向ROS进行回复的代码reply_code，所有的类型表示与代码表示都是num数值类型的。
RECORD ROS_msg
ROS_msg_header header;
rawbytes data;
ENDRECORD这部分结构体对机器人控制器与ROS交互的信息整体进行了定义，ROS_msg的信息整体包含信息帧的开头ROS_msg_header header和其后所接收到的对应指令的数据rawbytes data。rawbytes 通过支持指令/函数，可用任意类型数据 num、byte、string来填充rawbytes数据。在rawbytes的任意变量中，系统同时储存当前有效字节的长度。
RECORD ROS_msg_joint_traj_pt
ROS_msg_header header;
num sequence_id;
robjoint joints; ! in DEGREES
num velocity;
num duration;
ENDRECORD定义关节空间运行机器人运行路径的数据结构，包括信息头，序列ID，关节位置，速度和周期。这个数据结构主要是机器人控制器的内部使用，用于对机器人的运动进行控制，所以需要的信息全面一些。
RECORD ROS_msg_joint_data
ROS_msg_header header;
num sequence_id;
robjoint joints; ! in DEGREES
ENDRECORD定义关节位置数据的结构体，包括信息头，序列ID和关节位置数据，比上一种数据类型少了速度与周期。这种数据格式主要用于向ROS发送机器人的状态，也就是机器人的当前关节角度，所以需要的数据类型数量相对上一种数据格式要少。
! Message Type Codes (from simple_message/simple_message.h)
CONST num ROS_MSG_TYPE_INVALID := 0;
CONST num ROS_MSG_TYPE_JOINT := 10; ! joint-position feedback
CONST num ROS_MSG_TYPE_JOINT_TRAJ_PT := 11; ! joint-trajectory-point (for path downloading)
CONST num ROS_COM_TYPE_TOPIC := 1;
CONST num ROS_COM_TYPE_SRV_REQ := 2;
CONST num ROS_COM_TYPE_SRV_REPLY := 3;
CONST num ROS_REPLY_TYPE_INVALID := 0;
CONST num ROS_REPLY_TYPE_SUCCESS := 1;
CONST num ROS_REPLY_TYPE_FAILURE := 2;这一段定义信息的类型对应于ROS_msg_header中的msg_type，是非常重要的一段代码，对不同的信息类型进行数值划分，用于数据的封装与解析。无效数据都定义为0，不管是接收的还是回复的数据；关节位置反馈为10，用于向ROS反馈当前关节位置；关节轨迹序列数据用11，代表后面还要进行序列的下载与解析；与ROS话题相关的数据用1表示，与ROS服务相关的数据用2表示，对服务的回复数据用3表示；回复无效为0，成功为1，失败为2。
CONST num ROS_TRAJECTORY_START_DOWNLOAD := -1;
CONST num ROS_TRAJECTORY_END := -3;
CONST num ROS_TRAJECTORY_STOP := -4;特殊的序列代码，轨迹开始下载到控制器为-1，轨迹结束为-3，轨迹停止为-4。
CONST num ROS_MSG_MAX_JOINTS := 10;此行代码为定义最大的关节轴数为10。接下来这段函数很重要，关系到ROS的数据是否能够正确的接收到解析，长的代码直接把对每一行的理解写在代码段里了。
PROC ROS_receive_msg_joint_traj_pt(VAR socketdev client_socket, VAR ROS_msg_joint_traj_pt message, \num wait_time)
---定义关节序列接收函数，参数分别为客户端(ROS)设备的套接字，关节空间路径数据（解析的数据要写入的变量），可选参数处理时间，最终函数返回的为ROS_msg_joint_traj_pt类型信息--->
VAR ROS_msg raw_message;
IF Present(wait_time) THEN
ROS_receive_msg client_socket, raw_message, \wait_time:=wait_time;
ELSE
ROS_receive_msg client_socket, raw_message;
ENDIF
IF (raw_message.header.msg_type <> ROS_MSG_TYPE_JOINT_TRAJ_PT) THEN
ErrWrite \W, "ROS Socket Type Mismatch", "Unexpected message type",
\RL2:="expected: " + ValToStr(ROS_MSG_TYPE_JOINT_TRAJ_PT),
\RL3:="received: " + ValToStr(raw_message.header.msg_type);
RAISE ERR_ARGVALERR; ! TBD: define specific error code
ENDIF
IF (RawBytesLen(raw_message.data) < 52) THEN
ErrWrite \W, "ROS Socket Missing Data", "Insufficient data for joint_trajectory_pt",
\RL2:="expected: 52",
\RL3:="received: " + ValToStr(RawBytesLen(raw_message.data));
RAISE ERR_OUTOFBND; ! TBD: define specific error code
ENDIF
---检查数据大小是否为满足最小的数据大小，否则输出错误，并进行错误处理，为什么是52呢，其中包括sequence_id,还有10个轴和速度与周期，是13个num的数据量，而每个num为4个字节--->
message.header := raw_message.header;
UnpackRawBytes raw_message.data, 1, message.sequence_id, \IntX:=DINT;
UnpackRawBytes raw_message.data, 5, message.joints.rax_1, \Float4;
UnpackRawBytes raw_message.data, 9, message.joints.rax_2, \Float4;
UnpackRawBytes raw_message.data, 13, message.joints.rax_3, \Float4;
UnpackRawBytes raw_message.data, 17, message.joints.rax_4, \Float4;
UnpackRawBytes raw_message.data, 21, message.joints.rax_5, \Float4;
UnpackRawBytes raw_message.data, 25, message.joints.rax_6, \Float4;
UnpackRawBytes raw_message.data, 29+(ROS_MSG_MAX_JOINTS-6)*4, message.velocity, \Float4;
UnpackRawBytes raw_message.data, 33+(ROS_MSG_MAX_JOINTS-6)*4, message.duration, \Float4;
message.joints := rad2deg_robjoint(message.joints);
! TBD: convert velocity
ERROR
RAISE;
ENDPROC以上是对socket接收到的数据进行解析的功能函数，下面分析数据发送函数。
PROC ROS_send_msg_joint_data(VAR socketdev client_socket, ROS_msg_joint_data message)
VAR ROS_msg raw_message;
VAR robjoint ROS_joints;
VAR num i;
! Force message header to the correct values
raw_message.header.msg_type := ROS_MSG_TYPE_JOINT;
raw_message.header.comm_type := ROS_COM_TYPE_TOPIC;
raw_message.header.reply_code := ROS_REPLY_TYPE_INVALID;
ROS_joints := deg2rad_robjoint(message.joints);
! Pack data into message
PackRawBytes message.sequence_id,raw_message.data, 1, \IntX:=DINT;
PackRawBytes ROS_joints.rax_1, raw_message.data, 5, \Float4;
PackRawBytes ROS_joints.rax_2, raw_message.data, 9, \Float4;
PackRawBytes ROS_joints.rax_3, raw_message.data, 13, \Float4;
PackRawBytes ROS_joints.rax_4, raw_message.data, 17, \Float4;
PackRawBytes ROS_joints.rax_5, raw_message.data, 21, \Float4;
PackRawBytes ROS_joints.rax_6, raw_message.data, 25, \Float4;
FOR i FROM 1 TO ROS_MSG_MAX_JOINTS-6 DO
PackRawBytes 0, raw_message.data, 25+i*4, \Float4;
ENDFOR
ROS_send_msg client_socket, raw_message;
ERROR
RAISE;
ENDPROC以上代码段用于控制器向ROS进行关节位置的反馈上传。好了，上面两个大的函数分析完了，下面来几个简单的函数压压惊：
LOCAL FUNC num deg2rad(num deg)
RETURN deg * pi / 180;
ENDFUNC上面定义了一个局部函数，这个说明一上PROC定义一个可调用程序（也可以理解为函数，但是无返回值），FUNC定义一个函数，有返回值。上面这个函数的作用就是将角度值向弧度值进行转换。
LOCAL FUNC robjoint deg2rad_robjoint(robjoint deg)
VAR robjoint rad;
rad.rax_1 := deg2rad(deg.rax_1);
rad.rax_2 := deg2rad(deg.rax_2);
rad.rax_3 := deg2rad(deg.rax_3);
rad.rax_4 := deg2rad(deg.rax_4);
rad.rax_5 := deg2rad(deg.rax_5);
rad.rax_6 := deg2rad(deg.rax_6);
RETURN rad;
ENDFUNC上面这一段代码就是将结构体robjoint 中的关节数据对应的转换为弧度值。
LOCAL FUNC num rad2deg(num rad)
RETURN rad * 180 / pi;
ENDFUNC这段代码与上面的转换函数类似不过是进行弧度到角度的转换。
LOCAL FUNC robjoint rad2deg_robjoint(robjoint rad)
VAR robjoint deg;
deg.rax_1 := rad2deg(rad.rax_1);
deg.rax_2 := rad2deg(rad.rax_2);
deg.rax_3 := rad2deg(rad.rax_3);
deg.rax_4 := rad2deg(rad.rax_4);
deg.rax_5 := rad2deg(rad.rax_5);
deg.rax_6 := rad2deg(rad.rax_6);
RETURN deg;
ENDFUNC通过上面三个代码段的理解，这个应该就比较好理解了，就是将对应数据结构中的弧度值转换为角度值。
本来想一次性把六类文件都写完，但是写到这里发现已经好长了，如果太长估计我自己以后就不会看了，所以这次先分析两个程序文件，把后面的四类程序文件分成两组放在后面的两篇博客中。