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語句的主體內容就會執行，通過SocketCreat 函數重新創建套接字，然後執行下一步；如果套接字還處於可用狀態，則直接執行下一步檢測套接字是否處於已創建狀態。
接下來分析第二步的代碼：

IF (SocketGetStatus(server_socket) = SOCKET_CREATED) SocketBind server_socket, GetSysInfo(\LanIp), port;

上述代碼承接上一步的結果，繼續對套接字讀取狀態，如果套接字已經創建則對其進行端口與IP地址的綁定，其中SocketBind 爲RAPID定義的綁定函數，GetSysInfo(\LanIp)這裏要替換成服務器所在的IP地址，port爲對應的端口號；如果不是處於SOCKET_CREATED狀態，比如已經可以進行信息通訊了，則直接執行下一步。檢測是否已經綁定：

IF (SocketGetStatus(server_socket) = SOCKET_BOUND) SocketListen server_socket;

承接第二步，如果已經綁定了，則進入監聽狀態；如果已經可以用於通訊（比如已經在監聽狀態了），則直接跳過，表示初始化過程已經完成。
如果中途出錯，比如套接字創建不成功或是創建過程中出現地址錯誤等則直接進入錯誤處理程序。
接下來分析第二大功能代碼：等待連接，先整體看一下代碼。

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

經過第一步的初始化過程之後，Socket過程已由其它的非等待連接狀態進入了監聽狀態，接下來只要讓其接入有效的外部連接就可以進行服務器與客戶端的正常通信。本段代碼的作用就是讓其進入等待外部連接的狀態，隨時響應外部的通信連接。

PROC ROS_wait_for_client(VAR socketdev server_socket, VAR socketdev client_socket, \num wait_time)
······
ENDPROC

通過對程序PROC的調用讓通信過程進入等待客戶端連接狀態，要提供的參數包括：服務器的套接字server_socket，用於存儲客戶端套接字的變量client_socket和可選參數等待時間wait_time。\num wait_time這種定義格式表示的爲可選參數定義形式，即在函數使用中此參數可用可不用。

VAR string client_ip;
VAR num time_val := WAIT_MAX;  ! default to wait-forever

上面的兩行代碼中，第一行代碼定義了一個string類型的客戶端IP地址變量，第二行則定義了等待時間，默認的爲一直等待。

IF Present(wait_time) time_val := wait_time;

上面的代碼中Present()函數之前有說過，用來檢測可選變量是否存在。所以此行代碼的意義便是：若可選參數wait_time存在，則用其初始化等待時間time_val ，若不存在，則用WAIT_MAX初始化等待時間。

IF (SocketGetStatus(client_socket) <> SOCKET_CLOSED) SocketClose client_socket;

此行代碼的意思是若還有未關閉的客戶端連接，則將其關閉，以便接入新的客戶端連接。

WaitUntil (SocketGetStatus(client_socket) = SOCKET_CLOSED);

此行代碼等待客戶端連接徹底關閉。WaitUntil ()爲等待函數，直接其條件表達式爲真則退出函數等待狀態，執行下一行代碼。

SocketAccept server_socket, client_socket, \ClientAddress:=client_ip, \Time:=time_val;

等待外部客戶端接入，其中server_socket爲正在等待輸入連接的服務器套接字。套接字必須已經創建、綁定並準備進行監聽。函數將按照接受的輸入連接請求，用接入的新客戶端套接字對client_socket進行更新。\ClientAddress:=client_ip的意思是將通過已接受輸入連接請求的客戶端IP地址\ClientAddress用來進行變量client_ip的更新。\Time:=time_val指程序執行等待輸入連接的最長時間量。如果在任意輸入連接之前耗盡時間，則將調用錯誤處理器，如果存在這樣的情況，則採用錯誤代碼ERR_SOCK_TIMEOUT。如果不存
在錯誤處理器，則將停止執行。如果未使用參數\Time，則等待時間爲60秒。爲了永久等待，則使用預定義常量WAIT_MAX。

TPWrite "Client at "+client_ip+" connected.";

此行代碼僅爲了向示教器輸出客戶端連接信息。
爲了清楚明白我們再來理一下Socket的創建與使用流程：

對於服務器來講，主要的流程主要分爲：套接字創建、綁定、監聽、接收連接，關閉五個過程。所以在上面的服務器初始化過程中，在沒有錯誤的情況下，一個服務器套接字的狀態只可能處於這五種狀態之一，通過對上面的初始化部分代碼分析可以知道，如果套接字的狀態處於關閉、創建或綁定狀態，那上述初始化的過程都會執行，不管套接字處於上述三種狀態的哪種狀態都會依次（按照Socket的通信流程）將狀態轉換到監聽狀態，最終使得套接字的狀態處於監聽狀態。如果已經處於監聽狀態，則只需要等待下一步進行接收外部接入狀態即可，如果已經處於等待外部接入狀態，則表示Socket通信已經可以工作，不需要初始化過程。
接下來我們繼續分析服務器的信息收發代碼：

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;

    ! TBD: need to determine whether this handles split/merged messages correctly

    ! Read prefix INT (total message length)
    SocketReceive client_socket, \RawData:=buffer, \ReadNoOfBytes:=4, \Time:=time_val;
    UnpackRawBytes buffer, 1, msg_length, \IntX:=UDINT;

    ! Read remaining message bytes
    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

    ! Unpack message header/data
    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_receive_msg(VAR socketdev client_socket, VAR ROS_msg message, \num wait_time)
······
ENDPROC

上述代碼是一個信息接收程序的聲明，傳遞的參數爲socketdev client_socket接入的客戶端套接字，ROS_msg message用於信息傳出到函數外的變量，還有一個可選參數，設定處理時間。

VAR rawbytes buffer;

此行代碼定義一個用於數據接收的臨時變量，類型爲rawbytes類型，此類型在前面提到過，用於接收字節數據，最大值爲1024個字節。

VAR num time_val := WAIT_MAX;  ! default to wait-forever

此行代碼定義數據處理的等待時間，默認爲一直等待。

VAR num bytes_rcvd;

此行代碼用於定義一個存儲接收到的字節數據長度的變量，用於記錄從Socket中接收到的字節數據長度。

VAR num msg_length;

此行代碼定義了一個信息長度數據，用於記錄要接收到的信息的長度，以進行數據的校驗。

ClearRawBytes buffer;

ClearRawBytes是RAPID提供的接口函數，用於清除原始數據字節數據的內容，這裏是清空將要用來進行數據接收的臨時變量的內容。

IF Present(wait_time) time_val := wait_time;

此行代碼在前面有過類似的應用，用於判斷可選參數是否定義，若定義，剛用可選參數更新默認的時間等待參數。

SocketReceive client_socket, \RawData:=buffer, \ReadNoOfBytes:=4, \Time:=time_val;

其中，SocketReceive用於從遠程計算機接收數據。以上代碼的意義爲從client_socket中接收類型爲\RawData，即字節類型數據，的數據，並將其存儲到buffer中，而接收的數據長度爲\ReadNoOfBytes參數指字的，設置爲4，則意味着只接收4個字節，程序的執行時間由\Time指定。

UnpackRawBytes buffer, 1, msg_length, \IntX:=UDINT;

通過此代碼我們可以知道，其先接收了4個字節的數據到buffer中，然後從其第一個字節開始解析，將解析後的數據放到到msg_length中，而用於解析的數據類型指定爲數值類型\IntX，並且具體的類型爲UDINT，即爲無符號整數。從而我們就可以分析出，ROS向機器人控制器傳遞的消息中，前4個字節爲接下來要傳輸的字符數據的長度，從下一行要解析的代碼中就可以得出這個結論。

SocketReceive client_socket, \RawData:=buffer, \ReadNoOfBytes:=msg_length, \NoRecBytes:=bytes_rcvd, \Time:=time_val;

此行代碼與之前的數據接收代碼一樣，從客戶端client_socket中接收字節數據長度爲msg_length的字節數據到buffer中，並且實際收到的字節數據長度存儲到bytes_rcvd中，處理時間設定爲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

這一段代碼的作用就是用於簡單的校驗，如果實際接收到的數據與之前指定的數據長度msg_length不一致，則向示教器輸出錯誤並返回，若數據長度一致則繼續執行。

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;

繼續執行的代碼進行數據解析，解析分爲兩步，第一步處理數據的頭部幀，用於確定數據用類型與用途，而第二步則是數據的對應用途的實際解析與使用。此段代碼中第一行將接收到的數據的前4個字節解碼到ROS_msg 信息類型的頭信息的msg_type中，數值數據的格式爲有符號整數。而第二行中則是將5-8四個字節的數據解碼到comm_type中，接下來的4個字節解碼到reply_code中，數值數據類型皆爲有符號整數。最後將後面跟隨的用於實際控制的數據信息整體轉移到message.data中，其中CopyRawBytes用於將所有或部分內容從一個rawbytes變量複製到另一個。其接口格式如下所示：

CopyRawBytes FromRawData，FromIndex，ToRawData，ToIndex;

將FromRawData的數據從索引FromIndex開始複製到從索引ToIndex開始的變量ToRawData中。所以上段代碼最後一行的意思就是將buffer的數據從第13個字節開始都複製到message.data中，message.data的第一個字節就已經開始進行數據的存儲。
數據接收功能的最後的ERROR代碼用於進行錯誤處理，不再贅述。接下來看此文件的最後一個功能代碼段，數據發送：

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

首先分析調用程序聲明：

PROC ROS_send_msg(VAR socketdev client_socket, VAR ROS_msg message)
···
ERROR
    RAISE;  ! raise errors to calling code
ENDPROC

我們將錯誤處理代碼一起放在調用程序聲明中，調用 參數有兩個，一個是客戶端的套接字，一個要發送的數據。

VAR rawbytes buffer;

定義一個字節類型變量，用於按Socket的傳送規則存放要傳送的數據，即對數據進行按字節的從ROS_msg message中轉換到用於實際傳輸的rawbytes buffer中。

PackRawBytes 12 + RawBytesLen(message.data), buffer,  1, \IntX := UDINT; ! Packet length (excluding this prefix)

此行代碼用於向傳輸的數據中寫入有效數據的長度。從之前的數據接收函數中我們可以分析出其傳輸的數據格式爲：

有效數據長度	信息頭數據	後續控制參數或數據

其中信息頭數據格式又包括msg_type、comm_type和reply_code三個有效的有符號整數。所以在向用於向ROS傳輸的數據變量buffer中打包數據的時候，首要寫入有效數據的長度，而有效數據長度是從信息頭數據開始計算，是信息頭數據與後續控制參數或數據的長度之和。所以有效數據的長度便是頭數據三個變量每個佔用4個字節，其12個字節，加上message.data的長度。

    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

這4行代碼就是分別將有效數據打包放入buffer中。相關函數的使用方法不再贅述。

SocketSend client_socket \RawData:=buffer;

這一代碼是將數據發送到客戶端。
結合以上代碼的分析與理解，可知在ROS_socket.sys中進行了服務器的初始化與對外部客戶的接入任務，對於數據的接收和發送只進行數據的初步處理，並不對後續的具體數據進行解析與處理，相當於只起到一箇中專站的作用。

ROS_stateServer.sys

先來看一下整體的代碼，有個大體的概念：

MODULE ROS_stateServer

LOCAL CONST num server_port := 11002;
LOCAL CONST num update_rate := 0.10;  ! broadcast rate (sec)

LOCAL VAR socketdev server_socket;
LOCAL VAR socketdev client_socket;

PROC main()

    TPWrite "StateServer: Waiting for connection.";
    ROS_init_socket server_socket, server_port;
    ROS_wait_for_client server_socket, client_socket;

    WHILE (TRUE) DO
        send_joints;
        WaitTime update_rate;
    ENDWHILE

ERROR (ERR_SOCK_TIMEOUT, ERR_SOCK_CLOSED)
    IF (ERRNO=ERR_SOCK_TIMEOUT) OR (ERRNO=ERR_SOCK_CLOSED) THEN
        SkipWarn;  ! TBD: include this error data in the message logged below?
        ErrWrite \W, "ROS StateServer disconnect", "Connection lost.  Waiting for new connection.";
        ExitCycle;  ! restart program
    ELSE
        TRYNEXT;
    ENDIF
UNDO
ENDPROC

LOCAL PROC send_joints()
    VAR ROS_msg_joint_data message;
    VAR jointtarget joints;

    ! get current joint position (degrees)
    joints := CJointT();

    ! create message
    message.header := [ROS_MSG_TYPE_JOINT, ROS_COM_TYPE_TOPIC, ROS_REPLY_TYPE_INVALID];
    message.sequence_id := 0;
    message.joints := joints.robax;

    ! send message to client
    ROS_send_msg_joint_data client_socket, message;

ERROR
    RAISE;  ! raise errors to calling code
ENDPROC

ENDMODULE

這個文件的代碼不長，也容易理解一些，在代碼結構上，這是一個完整的程序文件，而不僅僅是提供程序接口，下面先看一下其定義的變量，模塊聲明就不說了，之前涉及過。

LOCAL CONST num server_port := 11002;
LOCAL CONST num update_rate := 0.10;  ! broadcast rate (sec)

LOCAL VAR socketdev server_socket;
LOCAL VAR socketdev client_socket;

代碼中第一行定義了一個局部數值類型的常量，即定義服務器的端口號爲11002；第二行中定義了一個局部數值常更新頻率爲10Hz，即是0.1s更新一次。第四行與第五行分別定義了服務器與客戶端的套接字。下面來看主執行函數：

PROC main()

    TPWrite "StateServer: Waiting for connection.";
    ROS_init_socket server_socket, server_port;
    ROS_wait_for_client server_socket, client_socket;

    WHILE (TRUE) DO
        send_joints;
        WaitTime update_rate;
    ENDWHILE

ERROR (ERR_SOCK_TIMEOUT, ERR_SOCK_CLOSED)
    IF (ERRNO=ERR_SOCK_TIMEOUT) OR (ERRNO=ERR_SOCK_CLOSED) THEN
        SkipWarn;  ! TBD: include this error data in the message logged below?
        ErrWrite \W, "ROS StateServer disconnect", "Connection lost.  Waiting for new connection.";
        ExitCycle;  ! restart program
    ELSE
        TRYNEXT;
    ENDIF
UNDO
ENDPROC

主程序可以分成兩部分來看，以關鍵字ERROR爲界，以上爲程序的主體部分，其後爲程序的錯誤處理部分，先看主體部分：

    TPWrite "StateServer: Waiting for connection.";
    ROS_init_socket server_socket, server_port;
    ROS_wait_for_client server_socket, client_socket;

    WHILE (TRUE) DO
        send_joints;
        WaitTime update_rate;
    ENDWHILE

主體部分第一代碼，是向示教器中輸出信息提示：表示狀態服務器正在等待外部客戶端的接入。第二行中ROS_init_socket是在ROS_socket.sys文件中定義的函數，用來進行套接字的初始化任務，參數就是服務器的套接字和對應的端口號。完成初始化之後，第三行則是讓狀態服務器進入等待外部連接狀態。下面的while循環則是狀態服務器的核心代碼，功能就是每0.1s向ROS發送機器的狀態信息，是一個死循環，會一直執行。send_joints是定義的一個調用，之後我們將分析其代碼與功用，WaitTime是一個等待命令，讓控制器等待後面參數給定的時間，同時暫時釋放資源。
再來看一下錯誤處理部分：

ERROR (ERR_SOCK_TIMEOUT, ERR_SOCK_CLOSED)
    IF (ERRNO=ERR_SOCK_TIMEOUT) OR (ERRNO=ERR_SOCK_CLOSED) THEN
        SkipWarn;  ! TBD: include this error data in the message logged below?
        ErrWrite \W, "ROS StateServer disconnect", "Connection lost.  Waiting for new connection.";
        ExitCycle;  ! restart program
    ELSE
        TRYNEXT;
    ENDIF
UNDO

錯誤處理程序中，錯誤類型有兩種，一種是ERR_SOCK_TIMEOUT指連接超時，另一種是ERR_SOCK_CLOSED連接關閉，當兩種滿足其中之一時會進入錯誤處理程序，進入之後首先進行錯誤判斷，然後執行後續的錯誤處理步驟，SkipWarn跳過錯誤警告，ErrWrite 向示教器輸出提示信息，ExitCycle用於中斷當前循環，將程序指針（PP）移回至主程序中第一個指令處。如果以連續模式執行程序，則其將開始執行下一循環。如果以循環模式執行，則將在主程序中的第一個指令處停止執行。TRYNEXT指令用於在錯誤後恢復執行，以本指令開始，隨後進行引起錯誤的指令。UNDO用於對程序的後續結尾問題進行處理，也暗示着程序的終止。簡而言之就是，錯誤之後進入錯誤處理，然後重新啓動本程序。

LOCAL PROC send_joints()
    VAR ROS_msg_joint_data message;
    VAR jointtarget joints;

    ! get current joint position (degrees)
    joints := CJointT();

    ! create message
    message.header := [ROS_MSG_TYPE_JOINT, ROS_COM_TYPE_TOPIC, ROS_REPLY_TYPE_INVALID];
    message.sequence_id := 0;
    message.joints := joints.robax;

    ! send message to client
    ROS_send_msg_joint_data client_socket, message;

ERROR
    RAISE;  ! raise errors to calling code
ENDPROC

這段程序的功能負責週期性的讀取當前機器人的各關節位置數據，並通過狀態服務器向ROS發送反饋數據。

VAR ROS_msg_joint_data message;
VAR jointtarget joints;

第一行定義一個ROS_msg_joint_data類型的變量，ROS_msg_joint_data類型包含3個子元素，一個ROS_msg_header，一個序列sequence_id和一個robjoint（六個軸的關節數據）變量。而jointtarget變量則包含比robjoint更多的信息，除了六個軸之後還有外部軸的信息。

joints := CJointT();

CJointT( )函數是Current Joint Target的簡稱，用於讀取機械臂軸和外軸的當前角度。

message.header := [ROS_MSG_TYPE_JOINT, ROS_COM_TYPE_TOPIC, ROS_REPLY_TYPE_INVALID];

對header的三個變量進行賦值。其實這裏賦的值在後面的發送函數之中還要重新賦值，並且是直接賦值的，所以這裏給什麼值是無所謂的。

message.sequence_id := 0;

對序列進行賦值。

message.joints := joints.robax;

對關節角度值進行賦值。只是六個關節角度，所以如果要用七軸，這裏的代碼都是要改的，後面我們接七軸機器人的時候再詳細說明如何修改整個代碼。

ROS_send_msg_joint_data client_socket, message;

調用在ROS_messages.sys中定義的發送函數進行信息發送，實際上之後還要調用其它的函數進行數據的發送處理，最後纔會用ROS_socket.sys中的發送函數將數據傳送到ROS。

ERROR
    RAISE;

最後的錯誤處理的RAISE語句用於明確發出或傳遞錯誤。
呼~~~，鬆口氣，這麼長，估計之後又不想看了，終於把ROS_socket和ROS_stateServer兩個文件分析完了，這兩個文件中前者提供了最基礎的Socket通信功能與對應的數據發送與接收接口，而後者則是週期性的讀取機器人位置數據併發送給ROS，但數據的發送要經過ROS_send_msg_joint_data和ROS_send_msg兩個函數的中轉，才能最終發送成功。
好，完成2/3了，對於ROS Server的理解成功在即了！