ROS-Industrial 之 ABB_Driver ——ROS Server(2/3)

前面的ROS-Industrial 之 ABB_Driver——ROS Server (1/3)对ABB_Driver中的ROS Server部分中的ROS_common和ROS_messages两个功能文件进行了分析与学习，现在继续分析接下来的内容，最终的目的就是彻底搞清楚ROS Server里到底是如何实现ROS与机器人控制器的交互控制的，借鉴ABB的经验实现最终目的。

ROS_socket.sys

先把整个代码文件贴上来欣赏一下，不想看太长的话直接到后面按功能分段分析：

MODULE ROS_socket(SYSMODULE)

PROC ROS_init_socket(VAR socketdev server_socket, num port)
    IF (SocketGetStatus(server_socket) = SOCKET_CLOSED) SocketCreate server_socket;
    IF (SocketGetStatus(server_socket) = SOCKET_CREATED) SocketBind server_socket, GetSysInfo(\LanIp), port;
    IF (SocketGetStatus(server_socket) = SOCKET_BOUND) SocketListen server_socket;
ERROR
    RAISE;  ! raise errors to calling code
ENDPROC

PROC ROS_wait_for_client(VAR socketdev server_socket, VAR socketdev client_socket, \num wait_time)
    VAR string client_ip;
    VAR num time_val := WAIT_MAX;  ! default to wait-forever
    IF Present(wait_time) time_val := wait_time;
    IF (SocketGetStatus(client_socket) <> SOCKET_CLOSED) SocketClose client_socket;
    WaitUntil (SocketGetStatus(client_socket) = SOCKET_CLOSED);
    SocketAccept server_socket, client_socket, \ClientAddress:=client_ip, \Time:=time_val;
    TPWrite "Client at "+client_ip+" connected.";
ERROR
    RAISE;  ! raise errors to calling code
ENDPROC

PROC ROS_receive_msg(VAR socketdev client_socket, VAR ROS_msg message, \num wait_time)
    VAR rawbytes buffer;
    VAR num time_val := WAIT_MAX;  ! default to wait-forever
    VAR num bytes_rcvd;
    VAR num msg_length;
    ClearRawBytes buffer;
    IF Present(wait_time) time_val := wait_time;
    SocketReceive client_socket, \RawData:=buffer, \ReadNoOfBytes:=4, \Time:=time_val;
    UnpackRawBytes buffer, 1, msg_length, \IntX:=UDINT;
    SocketReceive client_socket, \RawData:=buffer, \ReadNoOfBytes:=msg_length, \NoRecBytes:=bytes_rcvd, \Time:=time_val;
    IF (bytes_rcvd <> msg_length) THEN
        ErrWrite \W, "ROS Socket Recv Failed", "Did not receive expected # of bytes.",
                 \RL2:="Expected: " + ValToStr(msg_length),
                 \RL3:="Received: " + ValToStr(bytes_rcvd);
        RETURN;
    ENDIF
    UnpackRawBytes buffer, 1, message.header.msg_type, \IntX:=DINT;
    UnpackRawBytes buffer, 5, message.header.comm_type, \IntX:=DINT;
    UnpackRawBytes buffer, 9, message.header.reply_code, \IntX:=DINT;
    CopyRawBytes   buffer, 13, message.data, 1;
ERROR
    RAISE;  ! raise errors to calling code
ENDPROC

PROC ROS_send_msg(VAR socketdev client_socket, VAR ROS_msg message)
    VAR rawbytes buffer;
    PackRawBytes 12 + RawBytesLen(message.data), buffer,  1, \IntX := UDINT; ! Packet length (excluding this prefix)
    PackRawBytes message.header.msg_type,        buffer,  5, \IntX := DINT;  ! Message type
    PackRawBytes message.header.comm_type,       buffer,  9, \IntX := DINT;  ! Comm type
    PackRawBytes message.header.reply_code,      buffer, 13, \IntX := DINT;  ! Reply code
    CopyRawBytes message.data, 1,                buffer, 17;                 ! Message data
    SocketSend client_socket \RawData:=buffer;
ERROR
    RAISE;  ! raise errors to calling code
ENDPROC

ENDMODULE

以上Socket通信的代码主要分为四大功能，初始化、等待连接、接收信息与发送信息，接下来我们按功能分段按行来分析其内容。
先来研究Socket的初始化代码：

PROC ROS_init_socket(VAR socketdev server_socket, num port)
    IF (SocketGetStatus(server_socket) = SOCKET_CLOSED) SocketCreate server_socket;
    IF (SocketGetStatus(server_socket) = SOCKET_CREATED) SocketBind server_socket, GetSysInfo(\LanIp), port;
    IF (SocketGetStatus(server_socket) = SOCKET_BOUND) SocketListen server_socket;
ERROR
    RAISE;  ! raise errors to calling code
ENDPROC

初始化代码完成对ROS Server服务器的初始化工作，在RAPID中直接定义了一个程序调用过程PROC进行初始化工作。

PROC ROS_init_socket(VAR socketdev server_socket, num port)
······
ENDPROC

其参数一个为服务器的套接字，一个为对应的通信端口。socketdev是在RAPID语言中对套接字数据类型的定义，端口对应数值数据类型num。从上一段代码中可以分析出，其Socket初始化的过程由三步完成，首先：

 IF (SocketGetStatus(server_socket) = SOCKET_CLOSED) SocketCreate server_socket;

直观上理解，如果server_socket还处于连接可用的状态过就执行下一步，如果已经关闭不可用了就重新创建有效的套接字后执行下一步。对给定的服务器套接字server_socket使用SocketGetStatus()函数读取状态，判断套接字是否已经关闭，如果已经关闭，则上面IF语句的主体内容就会执行，通过Soc