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

經過前面的兩篇文章，已經對ABB_Driver中的ROS Server的數據類型與Socket的相關功能牢記於心，總的來說之前分析的四類功能程序文件的同一目的就是確保通信通道的暢通，並能及時的接收與傳送消息，接下來這篇文章分析的程序文件則是對機器人的直接控制，所以會有許多與機器人示教編程中所接觸的相同的數據類型與程序代碼。

ROS_motionServer.mod

還是先來看一下代碼的整體，這一個文件的代碼稍微有點長，可以直接看代碼的分開理解的部分：

MODULE ROS_motionServer

LOCAL CONST num server_port := 11000;
LOCAL VAR socketdev server_socket;
LOCAL VAR socketdev client_socket;
LOCAL VAR ROS_joint_trajectory_pt trajectory{MAX_TRAJ_LENGTH};
LOCAL VAR num trajectory_size;

PROC main()
    VAR ROS_msg_joint_traj_pt message;

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

    WHILE ( true ) DO
        ! Recieve Joint Trajectory Pt Message
        ROS_receive_msg_joint_traj_pt client_socket, message;
        trajectory_pt_callback message;
    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 MotionServer disconnect", "Connection lost.  Resetting socket.";
        ExitCycle;  ! restart program
    ELSE
        TRYNEXT;
    ENDIF
UNDO
    IF (SocketGetStatus(client_socket) <> SOCKET_CLOSED) SocketClose client_socket;
    IF (SocketGetStatus(server_socket) <> SOCKET_CLOSED) SocketClose server_socket;
ENDPROC

LOCAL PROC trajectory_pt_callback(ROS_msg_joint_traj_pt message)
    VAR ROS_joint_trajectory_pt point;
    VAR jointtarget current_pos;
    VAR ROS_msg reply_msg;

    point := [message.joints, message.duration];

    ! use sequence_id to signal start/end of trajectory download
    TEST message.sequence_id
        CASE ROS_TRAJECTORY_START_DOWNLOAD:
            TPWrite "Traj START received";
            trajectory_size := 0;                 ! Reset trajectory size
            add_traj_pt point;                    ! Add this point to the trajectory
        CASE ROS_TRAJECTORY_END:
            TPWrite "Traj END received";
            add_traj_pt point;                    ! Add this point to the trajectory
            activate_trajectory;
        CASE ROS_TRAJECTORY_STOP:
            TPWrite "Traj STOP received";
            trajectory_size := 0;  ! empty trajectory
            activate_trajectory;
            StopMove; ClearPath; StartMove;  ! redundant, but re-issue stop command just to be safe
        DEFAULT:
            add_traj_pt point;                    ! Add this point to the trajectory
    ENDTEST

    ! send reply, if requested
    IF (message.header.comm_type = ROS_COM_TYPE_SRV_REQ) THEN
        reply_msg.header := [ROS_MSG_TYPE_JOINT_TRAJ_PT, ROS_COM_TYPE_SRV_REPLY, ROS_REPLY_TYPE_SUCCESS];
        ROS_send_msg client_socket, reply_msg;
    ENDIF

ERROR
    RAISE;  ! raise errors to calling code
ENDPROC

LOCAL PROC add_traj_pt(ROS_joint_trajectory_pt point)
    IF (trajectory_size = MAX_TRAJ_LENGTH) THEN
        ErrWrite \W, "Too Many Trajectory Points", "Trajectory has already reached its maximum size",
            \RL2:="max_size = " + ValToStr(MAX_TRAJ_LENGTH);
    ELSE
        Incr trajectory_size;
        trajectory{trajectory_size} := point; !Add this point to the trajectory
    ENDIF
ENDPROC

LOCAL PROC activate_trajectory()
    WaitTestAndSet ROS_trajectory_lock;  ! acquire data-lock
    TPWrite "Sending " + ValToStr(trajectory_size) + " points to MOTION task";
    ROS_trajectory := trajectory;
    ROS_trajectory_size := trajectory_size;
    ROS_new_trajectory := TRUE;
    ROS_trajectory_lock := FALSE;  ! release data-lock
ENDPROC

ENDMODULE

上面是文件的整體，但是直接把相關的理解與說明放到程序中並不方便，還是按照前兩篇文章的風格，將代碼分解開來，化整爲零，理解起來也容易一些。

MODULE ROS_motionServer
···
ENDMODULE

這兩行代碼的意思是在控制器的定義一個普通的模塊程序，模塊的名稱爲ROS_motionServer。

LOCAL CONST num server_port := 11000;
LOCAL VAR socketdev server_socket;
LOCAL VAR socketdev client_socket;
LOCAL VAR ROS_joint_trajectory_pt trajectory{MAX_TRAJ_LENGTH};
LOCAL VAR num trajectory_size;

上面這一段代碼定義了5個局部變量，第一行代碼定義了對機器人進行控制的通訊端口號，之前狀態服務器有一個端口號，和當前這個控制服務的端口號是不同的兩個端口。第二行與第三行分別定義了服務器與客戶端的局部套接字變量。第四行定義了ROS_joint_trajectory_pt類型的機器人軌跡變量，其實就是一個數組，最多可以存放MAX_TRAJ_LENGTH個機器人軌跡的關鍵運動點。第五行trajectory_size則是定義了一個計數器用來存放在trajectory{MAX_TRAJ_LENGTH}中實際上存放了多少個機器人的運動軌跡點。

PROC main()
    VAR ROS_msg_joint_traj_pt message;
    TPWrite "MotionServer: Waiting for connection.";
    ROS_init_socket server_socket, server_port;
    ROS_wait_for_client server_socket, client_socket;

    WHILE ( true ) DO
        ! Recieve Joint Trajectory Pt Message
        ROS_receive_msg_joint_traj_pt client_socket, message;
        trajectory_pt_callback message;
    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 MotionServer disconnect", "Connection lost.  Resetting socket.";
        ExitCycle;  ! restart program
    ELSE
        TRYNEXT;
    ENDIF
UNDO
    IF (SocketGetStatus(client_socket) <> SOCKET_CLOSED) SocketClose client_socket;
    IF (SocketGetStatus(server_socket) <> SOCKET_CLOSED) SocketClose server_socket;
ENDPROC

上面的一段代碼是ROS_motionServer的主程序，分兩部分來理解，一部分是程序主體，另一部分爲錯誤處理。程序主體如下段代碼所示：

    VAR ROS_msg_joint_traj_pt message;
    TPWrite "MotionServer: Waiting for connection.";
    ROS_init_socket server_socket, server_port;
    ROS_wait_for_client server_socket, client_socket;

    WHILE ( true ) DO
        ! Recieve Joint Trajectory Pt Message
        ROS_receive_msg_joint_traj_pt client_socket, message;
        trajectory_pt_callback message;
    ENDWHILE

在main()函數中，實現對整個程序的週期性控制。其中：

VAR ROS_msg_joint_traj_pt message;

在程序中定義一個ROS_msg_joint_traj_pt類型的變量，用於接收ROS端向服務器的數據輸入，ROS_msg_joint_traj_pt 數據類型寫在ROS_messages.sys文件中，在其中的定義如下：

RECORD ROS_msg_joint_traj_pt
    ROS_msg_header header;
    num sequence_id;
    robjoint joints;  ! in DEGREES
    num velocity;
    num duration;
ENDRECORD

從其定義中可以看出，其中包含了對機器人的運動進行控制所需要的信息，header用來說明消息類型、命令類型以及反饋代碼；sequence_id用來指明序列ID；joints指明六軸的控制角度；velocity指明關節空間中的角速度；duration指定運動時間。

TPWrite "MotionServer: Waiting for connection.";

向示教器中輸出提示信息，其實個人覺得這行代碼在與下面兩行代碼的順序應該放在下面兩行代碼的下方，在邏輯上的順序就通順了。

ROS_init_socket server_socket, server_port;

ROS_init_socket 爲上一篇文章中分析的ROS_socket.sys中函數，用來進行Socket的初始化，初始化的參數便是服務器的套接字與通訊端口號。

ROS_wait_for_client server_socket, client_socket;

令服務器進入等待外部接入狀態，在當前的應用背景下即是令機器人控制器進入等待ROS的連接狀態。

WHILE ( true ) DO
    ROS_receive_msg_joint_traj_pt client_socket, message;
    trajectory_pt_callback message;
ENDWHILE

這一段代碼的意思就是讓motionServer不斷的接收與解析信息。ROS_receive_msg_joint_traj_pt 是在ROS_messages中定義的信息接收函數，而trajectory_pt_callback則是在當前文件中定義的信息處理函數。

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 MotionServer disconnect", "Connection lost.  Resetting socket.";
        ExitCycle;  ! restart program
    ELSE
        TRYNEXT;
    ENDIF
UNDO
    IF (SocketGetStatus(client_socket) <> SOCKET_CLOSED) SocketClose client_socket;
    IF (SocketGetStatus(server_socket) <> SOCKET_CLOSED) SocketClose server_socket;

好面ERROR錯誤處理的部分在上一篇文章中講解過，這裏就不再贅述了，而UNDO的內容，在此可以深入一點，UNDO在RAPID中用來進行錯誤恢復後的程序掃尾工作，比如在這個程序中，當出現錯誤時，UNDO的內容就是進行套接字的資源收回，將服務器與客戶端的套接字關閉，以便程序重新恢復啓動時重新進行二者的初始化。接下來我們分析其數據解析程序：

LOCAL PROC trajectory_pt_callback(ROS_msg_joint_traj_pt message)
    VAR ROS_joint_trajectory_pt point;
    VAR jointtarget current_pos;
    VAR ROS_msg reply_msg;

    point := [message.joints, message.duration];

    ! use sequence_id to signal start/end of trajectory download
    TEST message.sequence_id
        CASE ROS_TRAJECTORY_START_DOWNLOAD:
            TPWrite "Traj START received";
            trajectory_size := 0;                 ! Reset trajectory size
            add_traj_pt point;                    ! Add this point to the trajectory
        CASE ROS_TRAJECTORY_END:
            TPWrite "Traj END received";
            add_traj_pt point;                    ! Add this point to the trajectory
            activate_trajectory;
        CASE ROS_TRAJECTORY_STOP:
            TPWrite "Traj STOP received";
            trajectory_size := 0;  ! empty trajectory
            activate_trajectory;
            StopMove; ClearPath; StartMove;  ! redundant, but re-issue stop command just to be safe
        DEFAULT:
            add_traj_pt point;                    ! Add this point to the trajectory
    ENDTEST

    ! send reply, if requested
    IF (message.header.comm_type = ROS_COM_TYPE_SRV_REQ) THEN
        reply_msg.header := [ROS_MSG_TYPE_JOINT_TRAJ_PT, ROS_COM_TYPE_SRV_REPLY, ROS_REPLY_TYPE_SUCCESS];
        ROS_send_msg client_socket, reply_msg;
    ENDIF

ERROR
    RAISE;  ! raise errors to calling code
ENDPROC

其中：

LOCAL PROC trajectory_pt_callback(ROS_msg_joint_traj_pt message)
···
ERROR
    RAISE;  ! raise errors to calling code
ENDPROC

定義了一個程序調用過程，傳遞的參數就是ROS_receive_msg_joint_traj_pt調用過程所接收到的信息。中間兩行代碼爲錯誤處理。

VAR ROS_joint_trajectory_pt point;

ROS_joint_trajectory_pt類型定義寫在ROS_common文件中，其數據結構如下所示：

RECORD ROS_joint_trajectory_pt
    robjoint joint_pos;
    num duration;
ENDRECORD

只有兩個成員數據，機器人的關節角位置信息與運動時間。因此每一個ROS_joint_trajectory_pt對象都可以作爲一個完整的序列點插入到機器人的整個關節空間的運動軌跡序列中。

VAR jointtarget current_pos;

定義一個jointtarget變量，jointtarget比robjoint變量含有更多的信息。

VAR ROS_msg reply_msg;

定義一個回覆信息，而ROS_msg數據定義寫在ROS_messages文件中，只有header和二進制字節數據兩個成員變量。

point := [message.joints, message.duration];

將關節角度數據與運動時間數據賦值給point變量。

TEST message.sequence_id
        CASE ROS_TRAJECTORY_START_DOWNLOAD:
            TPWrite "Traj START received";
            trajectory_size := 0;                 ! Reset trajectory size
            add_traj_pt point;                    ! Add this point to the trajectory
        CASE ROS_TRAJECTORY_END:
            TPWrite "Traj END received";
            add_traj_pt point;                    ! Add this point to the trajectory
            activate_trajectory;
        CASE ROS_TRAJECTORY_STOP:
            TPWrite "Traj STOP received";
            trajectory_size := 0;  ! empty trajectory
            activate_trajectory;
            StopMove; ClearPath; StartMove;  ! redundant, but re-issue stop command just to be safe
        DEFAULT:
            add_traj_pt point;                    ! Add this point to the trajectory
ENDTEST

TEST與CASE關鍵字類似於Switch與case，將測試數據與第一個CASE條件中的測試值進行比較。如果對比爲真，則執行相關指令。此後，通過ENDTEST後的指令，繼續程序執行。如果未滿足第一個CASE條件，則對其他CASE條件進行測試等。如果未滿足任何條件，則執行與DEFAULT相關的指令（如果存在）。其中sequence_id被用來作爲開始或結束軌跡數據下載的信號。有關三個CASE的判斷常量的定義寫在ROS_messages文件中。

CASE ROS_TRAJECTORY_START_DOWNLOAD:
    TPWrite "Traj START received";
    trajectory_size := 0;                 ! Reset trajectory size
    add_traj_pt point;                    ! Add this point to the trajectory

這段的意思就是如果滿足啓動下載的條件，則向示教器中輸出提示信息，並將軌跡數組中的實際長度置0，然後將當前的點加入到軌跡序列中。

CASE ROS_TRAJECTORY_END:
   TPWrite "Traj END received";
   add_traj_pt point;                    ! Add this point to the trajectory
   activate_trajectory;

如果接收到軌跡添加結束信號，則輸出提示信息，並將當前最後一個點加入到軌跡序列中，既而調用激活軌跡函數，控制機器人進行動作。

CASE ROS_TRAJECTORY_STOP:
   TPWrite "Traj STOP received";
   trajectory_size := 0;  ! empty trajectory
   activate_trajectory;
   StopMove; ClearPath; StartMove;  ! redundant, but re-issue stop command just to be safe

當收到停止信息時，輸出提示並將軌跡長度清0，重新調用一下軌跡運動函數，然後停止，消除緩存的軌跡，重新啓動機器人運動功能。

DEFAULT:
   add_traj_pt point;

接收到其它的信號量，則將點加入到軌跡序列中。

 IF (message.header.comm_type = ROS_COM_TYPE_SRV_REQ) THEN
        reply_msg.header := [ROS_MSG_TYPE_JOINT_TRAJ_PT, ROS_COM_TYPE_SRV_REPLY, ROS_REPLY_TYPE_SUCCESS];
        ROS_send_msg client_socket, reply_msg;
    ENDIF

如果ROS的指令中要求進行反饋，則對反饋信息進行打包併發送到客戶端。

LOCAL PROC add_traj_pt(ROS_joint_trajectory_pt point)
    IF (trajectory_size = MAX_TRAJ_LENGTH) THEN
        ErrWrite \W, "Too Many Trajectory Points", "Trajectory has already reached its maximum size",
                 \RL2:="max_size = " + ValToStr(MAX_TRAJ_LENGTH);
    ELSE
        Incr trajectory_size;
        trajectory{trajectory_size} := point; !Add this point to the trajectory
    ENDIF
ENDPROC

此段代碼向軌跡序列中添加點，IF語句進行序列長度的判斷，如果到達最大容量則向示教器中輸出提示信息，如果沒有到達果大值，則將實際序列值加1，然後將當前關節點的值加入到軌跡序列中。

LOCAL PROC activate_trajectory()
    WaitTestAndSet ROS_trajectory_lock;  ! acquire data-lock
    TPWrite "Sending " + ValToStr(trajectory_size) + " points to MOTION task";
    ROS_trajectory := trajectory;
    ROS_trajectory_size := trajectory_size;
    ROS_new_trajectory := TRUE;
    ROS_trajectory_lock := FALSE;  ! release data-lock
ENDPROC

這段代碼用於激活運行軌跡相關的數據。

WaitTestAndSet ROS_trajectory_lock;

其中WaitTestAndSet指令等待指定bool永久變量值成爲FALSE。當變量值成爲FALSE時，本指令將設置值爲TRUE，並繼續執行。永久變量可用作有關同步和互斥的二元信號。這裏的ROS_trajectory_lock是對軌跡進行操作的一個信號量，用於鎖定或解鎖對軌跡序列的操作。

TPWrite "Sending " + ValToStr(trajectory_size) + " points to MOTION task";

向示教器的輸出提示信息，輸出接收到的點的總量。

ROS_trajectory := trajectory;

將局部的軌跡數據賦值給全局的軌跡變量，而ROS_trajectory是在ROS_common文件中定義的一個全局的軌跡變量。

ROS_trajectory_size := trajectory_size;

將實際的軌跡長度賦值給全局的軌跡長度。

ROS_new_trajectory := TRUE;

新軌跡的指示邏輯變量。

ROS_trajectory_lock := FALSE;

將數據讀寫操作的鎖定解鎖。
到這裏ROS_motionServer的功能就已經很清楚了，就是將ROS中的軌跡加載到控制器端的全局變量中，併爲機器人的實際運行做好數據準備。

ROS_motion.mod

慣例，先來看一個整個文件的代碼：

MODULE ROS_motion

LOCAL CONST zonedata DEFAULT_CORNER_DIST := z10;
LOCAL VAR ROS_joint_trajectory_pt trajectory{MAX_TRAJ_LENGTH};
LOCAL VAR num trajectory_size := 0;
LOCAL VAR intnum intr_new_trajectory;

PROC main()
    VAR num current_index;
    VAR jointtarget target;
    VAR speeddata move_speed := v10;  ! default speed
    VAR zonedata stop_mode;
    VAR bool skip_move;

    ! Set up interrupt to watch for new trajectory
    IDelete intr_new_trajectory;    ! clear interrupt handler, in case restarted with ExitCycle
    CONNECT intr_new_trajectory WITH new_trajectory_handler;
    IPers ROS_new_trajectory, intr_new_trajectory;

    WHILE true DO
        ! Check for new Trajectory
        IF (ROS_new_trajectory)
            init_trajectory;

        ! execute all points in this trajectory
        IF (trajectory_size > 0) THEN
            FOR current_index FROM 1 TO trajectory_size DO
                target.robax := trajectory{current_index}.joint_pos;

                skip_move := (current_index = 1) AND is_near(target.robax, 0.1);

                stop_mode := DEFAULT_CORNER_DIST;  ! assume we're smoothing between points
                IF (current_index = trajectory_size) stop_mode := fine;  ! stop at path end

                ! Execute move command
                IF (NOT skip_move)
                    MoveAbsJ target, move_speed, \T:=trajectory{current_index}.duration, stop_mode, tool0;
            ENDFOR

            trajectory_size := 0;  ! trajectory done
        ENDIF

        WaitTime 0.05;  ! Throttle loop while waiting for new command
    ENDWHILE
ERROR
    ErrWrite \W, "Motion Error", "Error executing motion.  Aborting trajectory.";
    abort_trajectory;
ENDPROC

LOCAL PROC init_trajectory()
    clear_path;                    ! cancel any active motions

    WaitTestAndSet ROS_trajectory_lock;  ! acquire data-lock
      trajectory := ROS_trajectory;            ! copy to local var
      trajectory_size := ROS_trajectory_size;  ! copy to local var
      ROS_new_trajectory := FALSE;
    ROS_trajectory_lock := FALSE;         ! release data-lock
ENDPROC

LOCAL FUNC bool is_near(robjoint target, num tol)
    VAR jointtarget curr_jnt;

    curr_jnt := CJointT();

    RETURN ( ABS(curr_jnt.robax.rax_1 - target.rax_1) < tol )
       AND ( ABS(curr_jnt.robax.rax_2 - target.rax_2) < tol )
       AND ( ABS(curr_jnt.robax.rax_3 - target.rax_3) < tol )
       AND ( ABS(curr_jnt.robax.rax_4 - target.rax_4) < tol )
       AND ( ABS(curr_jnt.robax.rax_5 - target.rax_5) < tol )
       AND ( ABS(curr_jnt.robax.rax_6 - target.rax_6) < tol );
ENDFUNC

LOCAL PROC abort_trajectory()
    trajectory_size := 0;  ! "clear" local trajectory
    clear_path;
    ExitCycle;  ! restart program
ENDPROC

LOCAL PROC clear_path()
    IF ( NOT (IsStopMoveAct(\FromMoveTask) OR IsStopMoveAct(\FromNonMoveTask)) )
        StopMove;          ! stop any active motions
    ClearPath;             ! clear queued motion commands
    StartMove;             ! re-enable motions
ENDPROC

LOCAL TRAP new_trajectory_handler
    IF (NOT ROS_new_trajectory) RETURN;

    abort_trajectory;
ENDTRAP

ENDMODULE

這個文件的功能就是告訴機器人控制器如何去進行運動，爲此我們先來看一下如果要控制機器人運動，一個典型的指令是什麼樣子的（ABB的機器人哈，不同廠家的指令是不同的），就以MoveAbsJ（Move Absolute Joint）指令爲例，MoveAbsJ用於將機械臂和外軸移動至軸位置中指定的絕對位置。給一個典型的例子，其它的可選參數什麼的這裏不深入去討論了，感興趣的可以去查ABB的手冊。

MoveAbsJ p50, v1000, z50, tool2;

一個典型的運動指令如此行代碼所示，通過速度數據v1000和區域數據z50，機械臂以及工具tool2 得以沿非線性路勁運動至絕對軸位置p50。
接下來進入正題：

LOCAL CONST zonedata DEFAULT_CORNER_DIST := z10;
LOCAL VAR ROS_joint_trajectory_pt trajectory{MAX_TRAJ_LENGTH};
LOCAL VAR num trajectory_size := 0;
LOCAL VAR intnum intr_new_trajectory;

這裏定義程序執行過程中要用到的諸多變量，第一行定義了局部的區域常量，用來指示機器人運動過程中，最後到達目標點的方式。第二行中定義局部軌跡變量，這樣就可以釋放全局的軌跡變量，不會影響新的軌跡寫入全局的軌跡中。第三行定義局部的軌跡序列長度數據，最後一行則是引入一箇中斷識別號，用來檢測是否有新的軌跡輸入。

PROC main()
    VAR num current_index;
    VAR jointtarget target;
    VAR speeddata move_speed := v10;  ! default speed
    VAR zonedata stop_mode;
    VAR bool skip_move;

    ! Set up interrupt to watch for new trajectory
    IDelete intr_new_trajectory;    ! clear interrupt handler, in case restarted with ExitCycle
    CONNECT intr_new_trajectory WITH new_trajectory_handler;
    IPers ROS_new_trajectory, intr_new_trajectory;

    WHILE true DO
        ! Check for new Trajectory
        IF (ROS_new_trajectory)
            init_trajectory;

        ! execute all points in this trajectory
        IF (trajectory_size > 0) THEN
            FOR current_index FROM 1 TO trajectory_size DO
                target.robax := trajectory{current_index}.joint_pos;

                skip_move := (current_index = 1) AND is_near(target.robax, 0.1);

                stop_mode := DEFAULT_CORNER_DIST;  ! assume we're smoothing between points
                IF (current_index = trajectory_size) stop_mode := fine;  ! stop at path end

                ! Execute move command
                IF (NOT skip_move)
                    MoveAbsJ target, move_speed, \T:=trajectory{current_index}.duration, stop_mode, tool0;
            ENDFOR

            trajectory_size := 0;  ! trajectory done
        ENDIF

        WaitTime 0.05;  ! Throttle loop while waiting for new command
    ENDWHILE
ERROR
    ErrWrite \W, "Motion Error", "Error executing motion.  Aborting trajectory.";
    abort_trajectory;
ENDPROC

main()函數中執行機器人的實際運行，其中:

    VAR num current_index;
    VAR jointtarget target;
    VAR speeddata move_speed := v10;  ! default speed
    VAR zonedata stop_mode;
    VAR bool skip_move;

第一行定義一個軌跡序列計數器，用來記錄當前機器人運行到軌跡序列的哪個序列點，第二行中定義機器人運動過程中的目標點，也就是典型運動指令中的p50，第三行定義機器人運動過程中的速度信息，也就是典型運動指令中的v1000，第四行中定義機器人運動的區域數據，也就是典型例子中的z50，最後一行定義一個邏輯變量，用來指示機器人是否要跳過當前目標點的運動。比如你設置的第一個點正好是機器人的當前點，那麼這個點就會跳過執行，即不執行。

    ! Set up interrupt to watch for new trajectory
    IDelete intr_new_trajectory;    ! clear interrupt handler, in case restarted with ExitCycle
    CONNECT intr_new_trajectory WITH new_trajectory_handler;
    IPers ROS_new_trajectory, intr_new_trajectory;

這三行代碼用來在控制器的程序中設置一個軟中斷，以檢測是否有新的軌跡輸入。其中IDelete（中斷刪除）用於取消（刪除）中斷預定，即刪除intr_new_trajectory變量本身的中斷連接。CONNECT用於發現中斷識別號，並將其與軟中斷程序相連。通過下達中斷事件指令並規定其識別號，確定中斷。因此，當出現該事件時，自動執行軟中斷程序。IPers（Interrupt Persistent）用於在永久變量的數值發生改變時，下達中斷指令和啓用中斷，即當永久變量ROS_new_trajectory數值發生改變時，啓動與中斷識別號intr_new_trajectory相關聯的中斷程序。

WHILE true DO

    IF (ROS_new_trajectory)
        init_trajectory;

    ! execute all points in this trajectory
    IF (trajectory_size > 0) THEN
        FOR current_index FROM 1 TO trajectory_size DO
            target.robax := trajectory{current_index}.joint_pos;

            skip_move := (current_index = 1) AND is_near(target.robax, 0.1);

            stop_mode := DEFAULT_CORNER_DIST;  ! assume we're smoothing between points
            IF (current_index = trajectory_size) stop_mode := fine;  ! stop at path end

            ! Execute move command
            IF (NOT skip_move)
                MoveAbsJ target, move_speed, \T:=trajectory{current_index}.duration, stop_mode, tool0;
        ENDFOR

        trajectory_size := 0;  ! trajectory done
    ENDIF

    WaitTime 0.05;  ! Throttle loop while waiting for new command
ENDWHILE

是控制機器人運動的關鍵代碼，其中：

IF (ROS_new_trajectory)
        init_trajectory;

如果當前的軌跡是作爲一條新的軌跡加入到執行序列中，則要爲新的軌跡重新初始化軌跡運行的環境。init_trajectory用於初始化軌跡運行的環境條件，包括刪除舊有軌跡等。

IF (trajectory_size > 0) THEN
      FOR current_index FROM 1 TO trajectory_size DO
      ···
      ENDFOR
      trajectory_size := 0;  ! trajectory done
ENDIF

判斷新的軌跡中是否有有效的軌跡序列點，如果有，那就執行IF語句內的程序，沒有則結束。而IF語句內的程序爲一個FOR循環：

FOR current_index FROM 1 TO trajectory_size DO
    target.robax := trajectory{current_index}.joint_pos;
    skip_move := (current_index = 1) AND is_near(target.robax, 0.1);
    stop_mode := DEFAULT_CORNER_DIST;  ! assume we're smoothing between points
    IF (current_index = trajectory_size) stop_mode := fine;  ! stop at path end
    ! Execute move command
    IF (NOT skip_move)
        MoveAbsJ target, move_speed, \T:=trajectory{current_index}.duration, stop_mode, tool0;
ENDFOR

FOR循環中current_index 作爲一個循環變量，從第一個有效的軌跡序列點開始運行，一直到所有的即trajectory_size 個軌跡序列點全部運行爲止。

target.robax := trajectory{current_index}.joint_pos;

用於將軌跡序列點的關節數據賦值給機器人要運動到的目標點變量。

skip_move := (current_index = 1) AND is_near(target.robax, 0.1);

判斷是否要跳過當前的執行點，跳過的條件其實就是如果第一個點就是當前點就不運行，如果不是，就要繼續運動。

stop_mode := DEFAULT_CORNER_DIST;

指定機器人運動到目標點時的行爲，即相當於典型運動指令中的z50。

IF (current_index = trajectory_size) stop_mode := fine;

如果運動到最後一個點，則更改機器人運動到目標點時的行爲爲fine模式。

IF (NOT skip_move)
        MoveAbsJ target, move_speed, \T:=trajectory{current_index}.duration, stop_mode, tool0;

如果不跳過運行點，則讓機器人運動到目標點。運行結束之後，將軌跡序列的實際長度歸零，然後將控制循環的時間設置等待0.05s，以降低CPU的消耗。

ERROR
    ErrWrite \W, "Motion Error", "Error executing motion.  Aborting trajectory.";
    abort_trajectory;

錯誤處理，如果發生錯誤則輸出錯誤信息，然後放棄當前的軌跡。

LOCAL PROC init_trajectory()
    clear_path;                    ! cancel any active motions
    WaitTestAndSet ROS_trajectory_lock;  ! acquire data-lock
    trajectory := ROS_trajectory;            ! copy to local var
    trajectory_size := ROS_trajectory_size;  ! copy to local var
    ROS_new_trajectory := FALSE;
    ROS_trajectory_lock := FALSE;         ! release data-lock
ENDPROC

軌跡的初始化操作由本段代碼決定，程序調用內的第一行代碼clear_path爲取消所有的運動軌跡信息。

WaitTestAndSet ROS_trajectory_lock;

等待軌跡數據讀寫信號的釋放。

trajectory := ROS_trajectory;            ! copy to local var
trajectory_size := ROS_trajectory_size;  ! copy to local var

將軌跡數據從全局變量中複製到局部變量。

ROS_new_trajectory := FALSE;
ROS_trajectory_lock := FALSE;

將是否爲新的軌跡的標識量從true變爲false代表軌跡數據已從全局變量接收到了局部變量。並釋放對全局變量的佔有使用信號。

LOCAL FUNC bool is_near(robjoint target, num tol)
    VAR jointtarget curr_jnt;
    curr_jnt := CJointT();

    RETURN ( ABS(curr_jnt.robax.rax_1 - target.rax_1) < tol )
       AND ( ABS(curr_jnt.robax.rax_2 - target.rax_2) < tol )
       AND ( ABS(curr_jnt.robax.rax_3 - target.rax_3) < tol )
       AND ( ABS(curr_jnt.robax.rax_4 - target.rax_4) < tol )
       AND ( ABS(curr_jnt.robax.rax_5 - target.rax_5) < tol )
       AND ( ABS(curr_jnt.robax.rax_6 - target.rax_6) < tol );
ENDFUNC

此函數用於判斷機器人當前的位置是否與給定的目標點之間的距離小於給定的值，如果是則返回TRUE，否則返回FALSE。

LOCAL PROC abort_trajectory()
    trajectory_size := 0;  ! "clear" local trajectory
    clear_path;
    ExitCycle;  ! restart program
ENDPROC

用於廢棄當前的軌跡，將軌跡實際的序列計算值清零，然後取消所有運行軌跡，最後重新運行程序。

LOCAL PROC clear_path()
    IF ( NOT (IsStopMoveAct(\FromMoveTask) OR IsStopMoveAct(\FromNonMoveTask)) )
        StopMove;          ! stop any active motions
    ClearPath;             ! clear queued motion commands
    StartMove;             ! re-enable motions
ENDPROC

IsStopMoveAct用於獲取有關當前或相關運動任務的停止移動標誌的狀態，IsStopMoveAct(\FromMoveTask)如果在當前或相關運動任務中設置非運動任務的停止移動標誌，則返回值將爲TRUE，否則，其將爲FALSE。簡而言之就是如果機器人在運動，則停止運動，並且消除軌跡序列信息，然後重置移動標誌。

LOCAL TRAP new_trajectory_handler
    IF (NOT ROS_new_trajectory) RETURN;

    abort_trajectory;
ENDTRAP

這是一箇中斷程序，如果是新軌跡，則廢棄已有軌跡，如果不是則直接返回。
呼~~ 呼~~ 呼~~ 這個必須要大喘氣了，終於弄完了，完成了這三篇的內容，對ROS Server的實現已經掌握了，如果要根據自己的需求去改變代碼以適應我們的需要，也不會感覺無從下口了，啊，呸，無從下手了！後面就開始研究ROS端的，離最終的目標又近了一步。

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

ROS_motionServer.mod

ROS_motion.mod

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

ROS-Industrial 之 MoveIt —— 碰撞物體約束添加

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

ROS-Industrial 之 MoveIt —— 讀取實際機器人的狀態

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

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結