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