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

最新推荐文章于 2024-01-19 01:03:46 发布

太初有泪

最新推荐文章于 2024-01-19 01:03:46 发布

阅读量1k

点赞数 2

分类专栏： ROS-I 文章标签：机器人 ROS-I ROS-Server ABB-Driver socket

本文链接：https://blog.csdn.net/u012424737/article/details/78895797

版权

经过前面的两篇文章，已经对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()函数中，实现对整个程序的周期性控制。其中：