20220929

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

@@ -61,6 +61,7 @@ class GraspObject():
         self.pub1.publish(pos)
 
     def grasp_cp(self, msg):
+        global height
         if msg.data == '1':
             # 订阅摄像头话题,对图像信息进行处理
             self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
@@ -160,6 +161,7 @@ class GraspObject():
         print((pos.x,pos.y))
         # 开始吸取物体
         self.pub2.publish(1)
+        print('nmmsl')
         r2.sleep()
 
         # 提起物体
@@ -174,6 +176,9 @@ class GraspObject():
         # change to opencv
         try:
             cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
+            ht = len(cv_image1)
+            wit= len(cv_image1[0])
+            cv_image1[0:ht//3,0:wit]=[0,0,0]
         except CvBridgeError as e:
             print('error')
 
@@ -181,8 +186,8 @@ class GraspObject():
         cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
 
         # 蓝色物体颜色检测范围
-        LowerBlue = np.array([90, 95, 120])
-        UpperBlue = np.array([140, 255, 255])
+        LowerBlue = np.array([95, 90, 120])
+        UpperBlue = np.array([130, 255, 255])
         # LowerBlue = np.array([4, 103, 162])
         # UpperBlue = np.array([85, 136, 181])
         mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
@@ -213,11 +218,28 @@ class GraspObject():
                 rect = cv2.minAreaRect(c)
                 box = cv2.boxPoints(rect)
                 box = np.int0(box)
-                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
+                # if abs(y_mid) > 250 or abs(x_mid)>270:
+                #     continue
                 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)  
+                
+                if w>h:
+                    tmp = h
+                    h = w
+                    w = tmp
+                if  w !=0 and h/w >=1.3:
+                    Ph0MidX = (box[0][0]+box[3][0])/2
+                    Ph0MidY = (box[0][1]+box[1][1])/2
+                    Ph1MidX = (box[1][0]+box[2][0])/2
+                    Ph1MidY = (box[1][1]+box[2][1])/2
+                    x_mid = (Ph0MidX+Ph1MidX+box[0][0]+box[1][0])/4
+                    y_mid = (Ph0MidY+Ph1MidY+box[0][1]+box[1][1])/4
+                    size.append(w * h/2)
+                else:
+                    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
                     size.append(w * h)
+                # cv2.circle(pic, (int(x_mid),int(y_mid)), 3, (0, 0, 255), 3)
                 if size[i] > size_max:
                     size_max = size[i]
                     index = i
@@ -249,17 +271,21 @@ class GraspObject():
         pos.y = 0
         if height == 0:
             pos.z = -40
+            self.pub1.publish(pos)
         elif height ==1:
             pos.z = -40
-        elif height == 2:
-            pos.z = 55
-        elif height ==3:
-            pos.z = 150  #-80
             self.pub1.publish(pos)
-        r1.sleep()
+        elif height == 2:
+            pos.z = 80
+            self.pub1.publish(pos)
+        elif height ==3:
+            print('nmsl')
+            self.pub2.publish(0)
+        time.sleep(2)
         # stop pump
         self.pub2.publish(0)
-        r1.sleep(0.1)
+        time.sleep(1)
+        # r1.sleep()
         pos.x = 120
         pos.y = 0
         pos.z = 35

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

@@ -104,7 +104,7 @@ def keyboardLoop():
         elif ch == 'z':
             if can_release:
                 msg=String()
-                msg.data='2'
+                msg.data='4'
                 grasp_pub.publish(msg)
                 can_release=False
                 speed = 0