ROS下dobot(magician)机械臂的python demo

和朋友一起试了试ROS里 roscpp代码改rospy

使用的是dobot magician ,这是客户端用python写的

 

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import sys

from std_msgs.msg import String
from dobot.srv import SetCmdTimeout
from dobot.srv import SetQueuedCmdClear
from dobot.srv import SetQueuedCmdStartExec
from dobot.srv import SetQueuedCmdForceStopExec
from dobot.srv import GetDeviceVersion
from dobot.srv import SetEndEffectorParams
from dobot.srv import SetPTPJointParams
from dobot.srv import SetPTPCoordinateParams
from dobot.srv import SetPTPJumpParams
from dobot.srv import SetPTPCommonParams
from dobot.srv import SetPTPCmd

class client_ptp():
    def __init__(self):
        rospy.init_node('DobotClient')
        self.setCmdTimeout()
        self.ClearTheCommandQueue()
        self.StartRunningTheCommandQueue()
        self.SetEndEffectorParameters()

    #SetCmdTimeout
    def setCmdTimeout(self):
        rospy.wait_for_service('/DobotServer/SetCmdTimeout')
        client = rospy.ServiceProxy('/DobotServer/SetCmdTimeout', SetCmdTimeout)
        resp1 = client(3000)
        if resp1 == False:
            rospy.logerror("Failed to call SetCmdTimeout. Maybe DobotServer isn't started yet!")
            exit(0)

    #Clear the command queue
    def ClearTheCommandQueue(self):
        rospy.wait_for_service('/DobotServer/SetQueuedCmdClear')
        self.callVal = rospy.ServiceProxy('/DobotServer/SetQueuedCmdClear', SetQueuedCmdClear)  
        self.callVal()

    #Start running the command queue
    def StartRunningTheCommandQueue(self):
        rospy.wait_for_service('/DobotServer/SetQueuedCmdStartExec')
        self.callVal = rospy.ServiceProxy('/DobotServer/SetQueuedCmdStartExec', SetQueuedCmdStartExec)
        self.callVal()

    #Set end effector parameters
    def SetEndEffectorParameters(self):
        rospy.wait_for_service('/DobotServer/SetEndEffectorParams')
        callVal = rospy.ServiceProxy('/DobotServer/SetEndEffectorParams', SetEndEffectorParams)
        callVal(70,0,0,1)

        flag1 = True
        flag2 = True
        flag3 = True
        flag4 = True

        #Set PTP joint parameters
        while flag1:
            rospy.wait_for_service('/DobotServer/SetPTPJointParams')
            self.callVal = rospy.ServiceProxy('/DobotServer/SetPTPJointParams', SetPTPJointParams)
            self.callVal([100,100,100,100],[100,100,100,100],1)
            flag1 = False

        #Set PTP coordinate parameters
        while flag2:
            rospy.wait_for_service('/DobotServer/SetPTPCoordinateParams')
            self.callVal = rospy.ServiceProxy('/DobotServer/SetPTPCoordinateParams', SetPTPCoordinateParams)
            self.callVal(100,100,100,100,1)
            flag2 = False
 
        #Set PTP jump parameters
        while flag3:
            rospy.wait_for_service('/DobotServer/SetPTPJumpParams')
            self.callVal = rospy.ServiceProxy('/DobotServer/SetPTPJumpParams', SetPTPJumpParams)
            self.callVal(20,200,1)
            flag3 = False

        #Set PTP common parameters
        while flag4:
            rospy.wait_for_service('/DobotServer/SetPTPCommonParams')
            self.callVal = rospy.ServiceProxy('/DobotServer/SetPTPCommonParams', SetPTPCommonParams)
            self.callVal(50,50,1)
            flag4 = False

        rospy.wait_for_service('/DobotServer/SetPTPCmd')
        self.callVal = rospy.ServiceProxy('/DobotServer/SetPTPCmd', SetPTPCmd)

        while not rospy.is_shutdown():
            #the first point
            while True:
                resp2 = self.callVal(1,200,0,0,0)

                if resp2.result != 0:
                    resp2 = self.callVal(1,200,0,0,0)

                if resp2.result == 0:
                    break

                if rospy.is_shutdown():
                    break

            while True:
                resp2 = self.callVal(1,250,0,0,0)

                if resp2.result != 0:
                    resp2.result = self.callVal(1,250,0,0,0)
                if resp2.result == 0:
                    break
                if rospy.is_shutdown():
                    break

if __name__ == '__main__':          
#    try:
    a = client_ptp()
    import time
    time.sleep(11111111111111)
#    except:
#        rospy.logerror('ERROR')