20221021 grasp best

src/3rd_app/move2grasp/scripts/grasp_best.py

@@ -0,0 +1,434 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+from copy import deepcopy
+import os
+import rospy
+import math
+import cmath
+import cv2
+import numpy as np
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+from sensor_msgs.msg import Image,LaserScan
+from cv_bridge import CvBridge, CvBridgeError
+from spark_carry_object.msg import *
+from matplotlib import pyplot as plt
+
+
+class GraspObject():
+    '''
+    监听主控，用于物品抓取功能
+    '''
+    image_last_time = None
+
+    def __init__(self):
+        '''
+        初始化
+        '''
+        self.xc = 0
+        self.yc = 0
+        self.xc_prev = 0
+        self.yc_prev = 0
+        self.found_count = 0
+        self.is_have_object = False
+        self.is_found_object = False
+        self.can_grasp = True
+        self.def_object_union = {
+            'p': [],
+            'theta': [],
+            'w': [],
+            'h': [],
+            'size': [],
+            'x': [],
+            'y': [],
+        }
+        self.object_union = deepcopy(self.def_object_union)
+
+        filename = os.environ['HOME'] + "/thefile.txt"
+        with open(filename, 'r') as f:
+            s = f.read()
+        arr = s.split()
+        self.x_kb = [float(arr[0]), float(arr[1])]
+        self.y_kb = [float(arr[2]), float(arr[3])]
+        rospy.logwarn('X axia k and b value: ' + str(self.x_kb))
+        rospy.logwarn('X axia k and b value: ' + str(self.y_kb))
+
+        self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
+        # 订阅机械臂抓取指令
+        self.sub2 = rospy.Subscriber(
+            '/grasp', String, self.grasp_cp, queue_size=1)
+        # 发布机械臂位姿
+        self.pub1 = rospy.Publisher(
+            'position_write_topic', position, queue_size=10)
+        # 发布机械臂吸盘
+        self.pub2 = rospy.Publisher('pump_topic', status, queue_size=1)
+        # 发布机械臂状态
+        self.grasp_status_pub = rospy.Publisher(
+            'grasp_status', String, queue_size=1)
+        # 发布TWist消息控制机器人底盘
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+        self.arm_to_home()
+
+    # CV检测部分，负责返回识别出的方块位置     
+    def image_cb(self, data):
+        xc = -1
+        yc = -1
+        # change to opencv
+        try:
+            cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
+        except CvBridgeError as e:
+            print('error')
+
+        # 转化成HSV格式
+        cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
+        # 蓝色物体颜色检测范围
+        LowerBlue = np.array([95, 90, 80])
+        UpperBlue = np.array([130, 255, 255])
+        # 阈值处理，蓝色部分变为白色，其他部分变为黑色
+        mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
+        # 与运算，将白色部分与原来图片的蓝色部分结合
+        cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
+        # 取三维数组的第一维所有的数据
+        cv_image4 = cv_image3[:, :, 0]
+        # 低通滤波器，9*9滤波
+        blurred = cv2.blur(cv_image4, (9, 9))
+        # 简单阈值函数，处理图像灰度值，大于90的都归为255，其他归0
+        (_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY)
+        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
+        cv_image5 = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
+        cv_image5 = cv2.erode(cv_image5, None, iterations=4)
+        cv_image5 = cv2.dilate(cv_image5, None, iterations=4)
+
+        # 找出轮廓
+        contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+        # 将识别出的轮廓画出来，显示在原图中
+        # cv2.drawContours(cv_image1, contours, 0, (0, 255, 0), 2)
+        # cv2.imshow("test",cv_image1)
+        self.object_union = deepcopy(self.def_object_union)
+        # if find contours, pick the biggest box
+        if len(contours) == 0:
+            self.is_have_object = False
+            self.is_found_object = False
+            self.found_count = 0
+        else:
+            self.is_have_object = True
+            index = -1
+            p_min = 10000
+            for i, c in enumerate(contours):
+                rect = cv2.minAreaRect(c)
+                box = cv2.boxPoints(rect)
+                box = np.int0(box)
+                cv2.drawContours(cv_image1, [box], 0, (0, 255, 0), 2)
+
+                x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
+                y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
+
+                w = math.sqrt((box[0][0] - box[1][0]) ** 2 + (box[0][1] - box[1][1]) ** 2)
+                h = math.sqrt((box[0][0] - box[3][0]) ** 2 + (box[0][1] - box[3][1]) ** 2)
+
+                size = w * h
+
+                p, theta = cmath.polar(complex(x_mid - 320, 480 - y_mid))
+                cn = cmath.rect(p, theta)
+                
+                if p > 350:
+                    continue
+
+                self.object_union['p'].append(p)
+                self.object_union['theta'].append(theta)
+                self.object_union['size'].append(size)
+                self.object_union['w'].append(w)
+                self.object_union['h'].append(h)
+                self.object_union['x'].append(x_mid)
+                self.object_union['y'].append(y_mid)
+                
+                if p < p_min:
+                    index = index + 1
+                    p_min = p
+                    xc = x_mid
+                    yc = y_mid
+                    
+            # if box is not moving for 20 times
+            # print found_count
+            if self.found_count >= 30:
+                self.found_count = 0
+                self.is_found_object = True
+                self.xc = xc
+                self.yc = yc
+            else:
+                # if box is not moving
+                if index == -1:
+                    # rospy.logwarn("No object eligible for grasp")
+                    self.found_count = 0
+                    self.is_found_object = False
+                elif abs(xc - self.xc_prev) <= 4 and abs(yc - self.yc_prev) <= 4:
+                    # print(xc, " ", yc)
+                    cn = cmath.rect(self.object_union['p'][index], self.object_union['theta'][index])
+                    # cv2.line(cv_image1, (320, 480), (int(320 + cn.real), int(480 - cn.imag)), (255, 0, 0), 2)
+                    self.found_count = self.found_count + 1
+                else:
+                    self.found_count = 0
+        self.xc_prev = xc
+        self.yc_prev = yc
+        
+        cv2.imshow("source", cv_image1)
+        # cv2.imshow("gray", cv_image5)
+        cv2.waitKey(1)
+
+        self.image_last_time = rospy.get_rostime()
+
+    def grasp_cp(self, msg):
+        # 判断收到的抓取或放的信息
+        if msg.data == '0':
+            print("收到抓取信号")
+            times=0
+            steps=0
+            if self.is_found_object:
+                if self.can_grasp:
+                    print("现在允许抓取")
+                    if self.grasp():
+                        print("已成功抓取")
+                        return
+                    else:
+                        # 抓取不成功，将允许重新抓取
+                        self.can_grasp = True
+                        return
+                else:
+                    print("抓取执行中，不允许重复抓取")
+                    return
+
+            # 转一圈没有发现可抓取物体,退出抓取
+            else:
+                vel = Twist()
+                vel.angular.z = 0.2
+                rate = rospy.Rate(10)
+                
+                # 判断视野范围内有无方块
+                while not self.is_have_object:
+                    rate.sleep()
+                    times = times + 1
+                    if (times > math.pi * 2 / vel.angular.z * 10):
+                        rospy.logerr("can not found object!!! Waiting next clicked_point...")
+                        # self.grasp_status_pub.publish(String('0'))
+                        return
+                    self.cmd_vel_pub.publish(vel)
+
+                times = 0
+                while not self.is_found_object:
+                    rate.sleep()
+                    times = times + 1
+                    if (times > 30):
+                        rospy.logerr("object founded but can not location it!!! Waiting next clicked_point...")
+                        # self.grasp_status_pub.publish(String('0'))
+                        return
+
+            # print("unregisting sub\n")
+            # self.sub.unregister()
+
+            # 抓取检测到的物体    
+            if self.grasp():
+                print("已成功抓取")
+            else:
+                # 抓取不成功，将允许重新抓取
+                self.can_grasp = True
+ 
+        elif msg.data=='1':
+            # 放下物体
+            self.is_found_object = False
+            self.release_object(1)
+        elif msg.data=='2':
+            # 放下物体
+            self.is_found_object = False
+            self.release_object(2)
+        elif msg.data=='3':
+            # 放下物体
+            self.is_found_object = False
+            self.release_object(3)
+
+    # 执行抓取
+    def grasp(self):
+        # 设置是否允许抓取值为False，避免抓取过程中重复收到抓取的信号
+        self.can_grasp = False
+        print("现在开始抓取\n")
+        # 设置睡眠时间
+        r1 = rospy.Rate(0.1)
+        r2 = rospy.Rate(10)
+
+        pos = position()
+
+        # 物体所在坐标+标定误差
+        # 先到目标位置上方
+        pos.x = self.x_kb[0] * self.yc + self.x_kb[1]
+        pos.y = self.y_kb[0] * self.xc + self.y_kb[1]
+        pos.z = 20
+        self.pub1.publish(pos)
+        r2.sleep()
+
+        # 开始往下放
+        pos.z = -50
+        self.pub1.publish(pos)
+        r2.sleep()
+
+        # 开始吸取物体，pub2 1为吸取，0为释放
+        self.pub2.publish(1)
+        r2.sleep()
+
+        # 提起物体到检查位置
+        pos.x = 250  
+        pos.y = 0
+        pos.z = 70
+        self.pub1.publish(pos)
+        r2.sleep()
+
+        # 利用雷达检查是否有方块
+        if self.check_grasp_state():
+            print("检测到抓取成功")
+            pos.x = 220  #160
+            pos.y = 0
+            pos.z = 160
+            self.pub1.publish(pos)
+            r2.sleep()
+            return True
+        else:
+            print("检测到抓取失败") 
+            self.arm_to_home()
+            return False
+
+
+    def arm_to_home(self):
+        pos = position()
+
+        pos.x = 140
+        pos.y = 0
+        pos.z = 60
+        self.pub1.publish(pos)  
+        rospy.sleep(1)
+
+        pos.x = 120
+        pos.y = 0
+        pos.z = 35
+        self.pub1.publish(pos)
+        rospy.sleep(2)
+      
+    
+    # 检查是否成功抓取，成功返回True，反之返回False
+    def check_grasp_state(self):
+        
+        self.success_grasp_num = 0
+        scan_num = 0
+        # 读十次雷达数据，10hz
+        while scan_num < 60:
+
+            scan_msg = rospy.wait_for_message("/scan",LaserScan,timeout=None)
+
+            # 计算激光束数量,num值为1817
+            num = int((scan_msg.angle_max-scan_msg.angle_min)/scan_msg.angle_increment)
+
+            # 输入想要检测的角度与范围，机器人正后方为起始点
+            expect_angle = 180
+            ranges = 20
+            test_direction = int(expect_angle/360*num)
+
+            detect_num = 0 # 已经检测的次数
+            sum = 0 # 数据总和
+
+            # 计算检测区域的距离值 
+            for i in range(test_direction-ranges,test_direction+ranges):
+                # 排除检测距离值有出现0值的情况
+                if scan_msg.ranges[i] != 0:
+                    sum = scan_msg.ranges[i]+sum
+                    detect_num +=1
+                    # print("scan_msg.ranges[",i,"]=",scan_msg.ranges[i])
+
+            # 避免检测距离值出现均为0导致计算报错
+            if detect_num == 0:
+                continue
+            
+            # 取距离值,抓取成功值约为0.26——0.32,单位为米
+            averlaser = sum/detect_num
+            print("averlaser=",averlaser)
+
+            # 判断是否成功抓取
+            if averlaser < 0.35:
+                self.success_grasp_num += 1
+                # 判断成功抓取次数超过3次，返回true
+                if self.success_grasp_num >= 3:
+                    self.success_grasp_num = 0
+                    return True
+            print("scan_num=",scan_num)
+            scan_num += 1
+        return False
+        
+                
+
+    # 释放物体
+    def release_object(self,height):
+        r1 = rospy.Rate(1)     # 1s
+        pos = position()
+        # go forward
+        if height == 1:
+            pos.x = 220
+            pos.y = 0
+            pos.z = -40  #-80
+            self.pub1.publish(pos)
+            r1.sleep()
+            
+        if height == 2:
+            pos.x = 220
+            pos.y = 0
+            pos.z = 80  #-80
+            self.pub1.publish(pos)
+            r1.sleep()
+        
+        if height == 3:
+            pos.x = 220
+            pos.y = 0
+            pos.z = 160  #-80
+            self.pub1.publish(pos)
+            r1.sleep()
+
+        # stop pump
+        self.pub2.publish(0)
+        rospy.sleep(1)
+
+        if height == 3:
+            pos.x = 220
+            pos.y = 80
+            pos.z = 160
+            print("避开第三层方块")
+            self.pub1.publish(pos)  
+            rospy.sleep(2)
+
+            pos.x = 120
+            pos.y = 150
+            pos.z = 35
+            self.pub1.publish(pos)  
+            rospy.sleep(1)
+
+        self.arm_to_home()
+        # 释放后，修改成能够成功抓取
+        self.can_grasp = True
+
+        return 'succeeded'
+
+    # 转动机器人到一定角度       
+    def turn_body(self):
+        cmd_vel = Twist()
+        cmd_vel.angular.z = 0.25
+        rate = rospy.Rate(10)
+        for i in range(40):            
+            self.cmd_vel_pub.publish(cmd_vel)            
+            rate.sleep()
+       
+
+        
+if __name__ == '__main__':
+    try:
+        rospy.init_node('GraspObject', anonymous=False)
+        rospy.loginfo("Init GraspObject main")   
+        GraspObject()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        rospy.loginfo("End spark GraspObject main")
+