ROS连接ABB机械臂调试详细教程-ROS(indigo)和ABB RobotStudio 6.03.02-

在ROS industrial介绍中，给出了ROS和常用机械臂的连接方式。具体信息可以参考：http://wiki.ros.org/Industrial

ROS连接ABB机械臂调试详细教程-ROS(indigo)和ABB RobotStudio 6.03.02-

调试视频链接：http://v.youku.com/v_show/id_XMTc0MzUxNDU4OA

下面详细介绍，如何用ROS中MoveIt!规划并控制ABB RobotStudio中机械臂的运动。

MoveIt!：http://moveit.ros.org/

ABB RobotStudio：http://blog.csdn.net/ZhangRelay/article/details/51177098

----官方教程分为3个小节----

The following tutorials are provided to demonstrate installation and operation of an ABB robot using the ROS Industrial interfaces:

1. Installing the ABB ROS Server

   This tutorial walks through the steps of installing the ROS server code on the ABB robot controller and configuring the required controller settings.

2. Running the ROS Server

   This tutorial describes how to run the ABB ROS Server, so the robot will execute motion commands sent from the ROS client node.

The following tutorials show how to use the ABB Robot Studio with the driver:

1. Using Simulated Robot in Robot Studio
   This tutorial describes how to setup the ABB RobotStudio simulator for use with the ROS-Industrial driver.

分别为：安装ABB ROS服务器，运行ROS 服务器和在RobotStudio中使用仿真机器人，这里以仿真机器人为例，

当然配置完成后就可以控制真实机械臂运动了。

A 在RobotStudio中使用仿真机器人

通过使用仿真机械臂替代真实机械臂，可以在ROS和ABB机械臂进行通信仿真。这种情况下，通常有两台PC，

一台运行windows及ABB机械臂，winpc；另一台运行ubuntu和ROS，rospc。

1 需要熟悉ABB RobotStudio使用
1.1 新建一个空工作站解决方案：

1.2 在ABB模型库中选择一款机械臂，这里以IRB120_3_58__01为例：

1.3 选择机器人系统，点击从布局：

1.4 点击下一步，出现下面界面，点击选项：

1.5 通过过滤器快速添加616-1 PC interface 和 623-1 Multitasking后，点击完成：

2 安装RAPID文件

2.1 到创建的文档RobotStudioSystems下选择对应的工作站
2.2 打开，新建ROS文件夹，并复制abb_driver apid到其中（下载地址https://github.com/ros-industrial/abb）：

2.3 查看目前winpc的IP地址，并写入到ROS_socket.sys中对应处，如下：

3 选择控制器，配置示教器：

在配置中，为了使用方便先将语言设置为中文：

配置到手动模式，就可以进行下一步，安装ROS服务器。

B 安装ROS服务器

必须具备的组件清单：

Multitasking (623-1) -- for parallel socket communications
Socket Messaging (672-1) -- for communications with a ROS PC
for recent RobotWare versions, this option is included with "PC Interface (616-1)"
RobotWare OS >= 5.13 -- for required socket options
earlier versions may work, but will require modifications to the RAPID code

如果不全，仿真没有问题，但无法控制实际机器人。

之前，已经将代码文件复制到相应文件夹下了，如(e.g. /<system>/HOME/ROS/*)。

进行下一步操作。

1 配置控制器

这里文件说明如下：

Shared by all tasks
ROS_common.sys -- Global variables and data types shared by all files
ROS_socket.sys -- Socket handling and simple_message implementation
ROS_messages.sys -- Implementation of specific message types
Specific task modules
ROS_stateServer.mod -- Broadcast joint position and state data
ROS_motionServer.mod -- Receive robot motion commands
ROS_motion.mod -- Issues motion commands to the robot

1.1 创建任务

在ABB--控制器--配置编辑器--Controller--Task：

Create 3 tasks as follows:

Name	Type	Trust Level	Entry	Motion Task
ROS_StateServer	SEMISTATIC	NoSafety	main	NO
ROS_MotionServer	SEMISTATIC	SysStop	main	NO
T_ROB1	NORMAL		main	YES

It is easiest to wait until all configuration tasks are completed before rebooting the controller.

NOTES:

1. The T_ROB1 motion task probably already exists on your controller.

2. If T_ROB1 has existing motion-control modules, you may need to rename the main() routine in ROS_Motion.mod to ROS_main(). In this case, set the Entry point for T_ROB1 task to ROS_main().

3. For multi-robot controllers, specify the desired robot (e.g. rob1) for each task

4. SEMISTATIC tasks will auto-start when controller is booted. They are visible, but cannot be easily seen for troubleshooting. For debug or development purposes, it may be desired to set both ROS_*Server tasks to Type=NORMAL.

1.2 加载模块到任务

在ABB--控制器--配置编辑器--Controller--Automatic Loading of Modules：

File	Task	Installed	All Tasks	Hidden
HOME:/ROS/ROS_common.sys		NO	YES	NO
HOME:/ROS/ROS_socket.sys		NO	YES	NO
HOME:/ROS/ROS_messages.sys		NO	YES	NO
HOME:/ROS/ROS_stateServer.mod	ROS_StateServer	NO	NO	NO
HOME:/ROS/ROS_motionServer.mod	ROS_MotionServer	NO	NO	NO
HOME:/ROS/ROS_motion.mod	T_ROB1	NO	NO	NO

添加完成后如下所示：

然后，重启控制器，应用更改。

1.3 更新

在控制器--重启中选择合适模式进行重启，完成后可以看到如下：

如果IP错误：

设置正确IP后，显示：

补充RAPID：

ROS_motionServer：

MODULE ROS_motionServer

! Software License Agreement (BSD License)
!
! Copyright (c) 2012, Edward Venator, Case Western Reserve University
! Copyright (c) 2012, Jeremy Zoss, Southwest Research Institute
! All rights reserved.
!
! Redistribution and use in source and binary forms, with or without modification,
! are permitted provided that the following conditions are met:
!
!   Redistributions of source code must retain the above copyright notice, this
!       list of conditions and the following disclaimer.
!   Redistributions in binary form must reproduce the above copyright notice, this
!       list of conditions and the following disclaimer in the documentation
!       and/or other materials provided with the distribution.
!   Neither the name of the Case Western Reserve University nor the names of its contributors
!       may be used to endorse or promote products derived from this software without
!       specific prior written permission.
!
! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

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

ROS_stateServer

MODULE ROS_stateServer

! Software License Agreement (BSD License)
!
! Copyright (c) 2012, Edward Venator, Case Western Reserve University
! Copyright (c) 2012, Jeremy Zoss, Southwest Research Institute
! All rights reserved.
!
! Redistribution and use in source and binary forms, with or without modification,
! are permitted provided that the following conditions are met:
!
!   Redistributions of source code must retain the above copyright notice, this
!       list of conditions and the following disclaimer.
!   Redistributions in binary form must reproduce the above copyright notice, this
!       list of conditions and the following disclaimer in the documentation
!       and/or other materials provided with the distribution.
!   Neither the name of the Case Western Reserve University nor the names of its contributors
!       may be used to endorse or promote products derived from this software without
!       specific prior written permission.
!
! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

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

ROS_motion

MODULE ROS_motion

! Software License Agreement (BSD License)
!
! Copyright (c) 2012, Edward Venator, Case Western Reserve University
! Copyright (c) 2012, Jeremy Zoss, Southwest Research Institute
! All rights reserved.
!
! Redistribution and use in source and binary forms, with or without modification,
! are permitted provided that the following conditions are met:
!
!   Redistributions of source code must retain the above copyright notice, this
!       list of conditions and the following disclaimer.
!   Redistributions in binary form must reproduce the above copyright notice, this
!       list of conditions and the following disclaimer in the documentation
!       and/or other materials provided with the distribution.
!   Neither the name of the Case Western Reserve University nor the names of its contributors
!       may be used to endorse or promote products derived from this software without
!       specific prior written permission.
!
! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
! EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
! OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
! SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
! INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
! TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
! BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
! CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
! WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

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

C 运行ROS服务器

注意，RAPID运行模式为连续。

在ROS端编译完成abb和abb_experimental包，可从github下载。

支持IRB2400、IRB5400、IRB6600、IRB6640、IRB120、IRB120T和IRB4400等。

在终端启动：

exbot@relay-Aspire-4741:~$ roslaunch abb_irb120_moveit_config moveit_planning_execution.launch sim:=false robot_ip:=192.168.1.100

winpc显示如下：

rospc显示如下：

1 手动模式

程序指针 "PP to Main"，Enable使得 "Motors On"，点击运行按钮。即可在rospc端控制机械臂运动。

winpc端：

rospc端：

手动模式，规划执行过程有可能中断，请查阅相关文档。

2 自动模式

 "Motors On"并点击运行模式。

winpc：

如果是实际机器人，请注意为全速运行。

rospc：

--