第六天：通过python代码控制越疆机械臂完成抓取投递整个过程

一、前言

有很多小伙伴问我怎么样控制机械臂完成整个过程，其实经过上一篇博文的探究，这个问题其实很容易就可以解决。把他们每个位姿连起来执行不过就行了吗，因为我们做这个的场景就是抓取——>放置——>投递，整个场景和动作很固定。 所以我们完全用不上所谓的路径规划，说实话，路径规划这玩意弄起来也不简单。所以这里就不去讨论这块问题。 后面有机会，等我去更加深入的自学一下，有机会可以交流！

二、机械臂的运动逻辑（直接上代码讲解，具体请看注释）

import rospy
import time
from dobot.srv import SetHOMECmd				 #调用回零话题
from dobot.srv import SetEndEffectorSuctionCup   #调用末端回零话题
from dobot.srv import SetPTPCmd					#调用机械臂点到点运动话题
from visual_grab.msg import ocr_posemsg			#视觉话题（后面单独讲）
from dobot.msg import navigational_state, dobot_state	#我自定义的消息

#相关标志量和全局变量初始化
grap_state = -1
deliver_state = -1
grap_flag = 0
box_index = -1
capture_pose_x = 0
capture_pose_y = 0
start_ocr_time = 0
spin_grap_x = -50
spincnt = 0


'''
-------------------------------------------------------------------------------------------------
下面就是各个坐标点的初始化
这里是观察点的坐标值，使用获取实时位姿的话题可以获取相应的坐标，
我的逻辑是当调用机械臂归零话题以后，然后就会来到这个点停止，然后进行观察识别
---------------------------------
'''
grap_up_x = 7
grap_up_y = -257
grap_up_z = 95
grap_up_r = -170



'''
-------------------------------------------------------------------------------------------------
抓取点，这里的坐标值，除了R值(末端执行器)和Z值固定，其它X,Y值是需要改变的，
因为它要同时识别裁剪计算出和观察点的相对误差，然后进行换算调整
注意，因为这里的观察点固定，所以我们很容易算出邮件距离画面中心点的差值，然后进行换算可以找到邮件的中心点抓取。
没有使用到计算机械臂的逆解，当然，我会在垃圾分类那个专栏进行简单讲解，这里图省事就可以直接计算
-------------------------------------------------------------------------------------------------
'''
grap_down_x = 0
grap_down_y = 0
grap_down_z = -11
grap_down_r = -170


'''
-------------------------------------------------------------------------------------------------
这里是移动到小车放置台的上方
-------------------------------------------------------------------------------------------------
'''
put_up_x = 17
put_up_y = 185
put_up_z = 67
put_up_r = 2


'''
-------------------------------------------------------------------------------------------------
这里是将邮件放下来的坐标
-------------------------------------------------------------------------------------------------
'''
put_down_x = 20
put_down_y = 191
put_down_z = -30
put_down_r = 2


'''
-------------------------------------------------------------------------------------------------
#这里是投递点的坐标
-------------------------------------------------------------------------------------------------
'''
deliver_up_x = 7
deliver_up_y = -319
deliver_up_z = 25
deliver_up_r = -170


'''
-------------------------------------------------------------------------------------------------
功能：机械臂初始化
参数：空
讲解：机械臂初始化，初始化两个话题，再发布机洗鼻归零话题，然后发布运动到坐标点的话题。注意程序运行到这里，
他是在基本相同的时间，将两条命令发出去，但是你要知道机械臂的运动是需要时间的，所以要注意控制，由于我写的
程序时那边运动完，会返回一个消息，再开启识别，这个机械臂模块会进行等待识别那边发过来的消息。
其实他的API函数有返回值，执行完那个话题，我们可以增加一个返回消息的却，再发下一条，会更好，可以自行测试！
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def init_arm():
    rospy.wait_for_service('DobotServer/SetHOMECmd')
    rospy.wait_for_service('DobotServer/SetPTPCmd')
    #t0 = rospy.Duration(5, 0)
    try:
        client = rospy.ServiceProxy('DobotServer/SetHOMECmd', SetHOMECmd)
        PTP_client = rospy.ServiceProxy('DobotServer/SetPTPCmd', SetPTPCmd)
        response = client()
        #rospy.sleep(t0)
        response = PTP_client(1, grap_up_x, grap_up_y, grap_up_z, grap_up_r)  # houmian,shangfang
    except rospy.ServiceException, e:
        print "Service call failed: %s" % e



'''
-------------------------------------------------------------------------------------------------
功能：机械臂抓取
参数：空
讲解：这个功能函数就是实现抓取功能，完成抓取整个过程。大概是收到了识别完成的消息，开始从
识别点——>抓取点——>启动夹爪气泵——>回到识别点——>运动到放置点上方——>放下——>关闭气泵——>回到放置点上方，
这里使用了MOVL模式点到点模式，使用JUMP（测试有点不可控，经常超范围，可能是我的方法不对）
和MOVJ关节运动模式（麻烦一点），所以一下全都采用MOVL模式
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def arm_grap():
    global capture_pose_x, capture_pose_y, grap_down_x, grap_down_y
    rospy.wait_for_service('DobotServer/SetEndEffectorSuctionCup')
    rospy.wait_for_service('DobotServer/SetPTPCmd')
    t1 = rospy.Duration(1,0)
    try:
        end_client = rospy.ServiceProxy('DobotServer/SetEndEffectorSuctionCup', SetEndEffectorSuctionCup)
        PTP_client = rospy.ServiceProxy('DobotServer/SetPTPCmd', SetPTPCmd)
        response = PTP_client(1, grap_up_x, grap_up_y, grap_up_z, grap_up_r)  # houmian,shangfang
        response = PTP_client(1, grap_down_x, grap_down_y, grap_down_z, grap_down_r)  # houmian,fangxia
        response = end_client(1, 1, True)
        rospy.sleep(t1)
        response = PTP_client(1, grap_up_x, grap_up_y, grap_up_z, grap_up_r)  # houmian,shangfang
        response = PTP_client(1, put_up_x, put_up_y, put_up_z, put_up_r)  # qianmian,shangfang
        response = PTP_client(1, put_down_x, put_down_y, put_down_z, put_down_r)  # qianmian,fangxia
        response = end_client(0, 0, True)
        rospy.sleep(t1)
        response = PTP_client(1, put_up_x, put_up_y, put_up_z, put_up_r)  # qianmian,shangfang
        #print("grap_down_x", grap_down_x, "grap_down_y", grap_down_y)


    except rospy.ServiceException, e:
        print"Service call failed: %s" % e


'''
-------------------------------------------------------------------------------------------------
功能：机械臂投递
参数：空
讲解：这个功能函数就是实现投递功能，逻辑类似
放置点上——>下降抓取邮件——>启动气泵——>回到放置点上方——>移动到放置点——>关闭气泵——>回到观察点
这样就与抓取函数实现了闭环
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def arm_deliver():
    rospy.wait_for_service('DobotServer/SetEndEffectorSuctionCup')
    rospy.wait_for_service('DobotServer/SetPTPCmd')
    t2 = rospy.Duration(1, 0)
    try:
        end_client = rospy.ServiceProxy('DobotServer/SetEndEffectorSuctionCup', SetEndEffectorSuctionCup)
        PTP_client = rospy.ServiceProxy('DobotServer/SetPTPCmd', SetPTPCmd)
        response = PTP_client(1, put_down_x, put_down_y, put_down_z, put_down_r)  # qianmian,fangxia
        response = end_client(1, 1, True)
        rospy.sleep(t2)
        response = PTP_client(1, put_up_x, put_up_y, put_up_z, put_up_r)  # qianmian,shangfang
        response = PTP_client(1, deliver_up_x, deliver_up_y, deliver_up_z, deliver_up_r)
        response = end_client(0, 0, True)
        response = PTP_client(1, 7, grap_up_y, grap_up_z, grap_up_r)  # houmian,shangfang

        rospy.sleep(t2)
    except rospy.ServiceException, e:
        print"Service call failed: %s" % e

'''
-------------------------------------------------------------------------------------------------
功能：导航状态回调函数
参数：导航消息
讲解：导航成功后，到抓取点，启动是被、抓取，到投递点后，启动投递
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def navigational_statecallback(data):
    global grap_state, deliver_state, start_ocr_time
    try:
        grap_state = data.nav_grap_flag
        deliver_state = data.nav_deliver_flag
        if deliver_state == 0:
            start_ocr_time = rospy.get_time()
            return grap_state, deliver_state, start_ocr_time
        else:
            return grap_state, deliver_state
    except:
        print("receive dobot_state error")


'''
-------------------------------------------------------------------------------------------------
功能：文字识别回调函数
参数：文字识别消息
讲解：
回调的消息包括（识别到的省份信息和物体的相对偏差值），身份会已知省份对比，确认投递点；
相对误差值，在观察点的坐标值上做修改，同时判断范围，避免超出机械臂运动范围
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def ocr_posCallback(data):
    global text, grap_flag, box_index, capture_pose_x, capture_pose_y, \
        grap_down_x, grap_down_y
    deliver_point = ['shandongsheng', 'hunansheng', 'fujiansheng', 'sichuansheng', 'henansheng',
                     'yunnansheng','zhejiangsheng','guangxi', 'hubeisheng','guangdongsheng']
    try:
        capture_pose_x = data.x * -1000
        capture_pose_y = data.y * -1000
        text = data.text
        box_index = deliver_point.index(text)
        grap_flag = 1
        grap_down_x = grap_up_x + capture_pose_x
        grap_down_y = grap_up_y + capture_pose_y
        rospy.loginfo("text: %s", text)
        rospy.loginfo("grap_flag:%d", grap_flag)
        print("capture_pose_x:", capture_pose_x, "capture_pose_y:", capture_pose_y)
        if grap_down_y > -190:
            grap_down_y = -192
            rospy.loginfo("grap_down_y value too large!")
        if grap_down_y < -310:
            grap_down_y = -308
            rospy.loginfo("grap_down_y value too small")
        #print("grap_down_x", grap_down_x, "grap_down_y", grap_down_y)
        return box_index, grap_flag, grap_down_x, grap_down_y
    except:
        print("receive ocr_pose error")



'''
-------------------------------------------------------------------------------------------------
功能：main函数
参数：空
讲解：
这里就不列代码了；
逻辑大概就行，机械臂最先归零移动到观察点；向文字识别话题发送消息，然后等待回信，那边识别完成向这边发送抓取消息；
抓取完成后向导航话题发送消息，等到那边导航完成，这边就运行投递程序，投递完成，再向导航发送消息，回到分拣台；
这样就是一项工作的闭环。
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''

三、总结

其实这里的机械臂控制逻辑上是通俗易懂的，也不用去考虑机械臂的路径规划和机械臂本身的控制。 因为它都做好了函数封装，调用API就可以直接用。有兴趣的可以参考它的机械臂话题的源文件。 可以学习机械臂的控制！
后面的建图和文字识别，图像处理等工作才是更加繁琐有难度。我也会尽快继续整理与大家分享！