20220929

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

@@ -176,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')
 
@@ -183,8 +186,8 @@ class GraspObject():
         cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
 
         # 蓝色物体颜色检测范围
-        LowerBlue = np.array([100, 100, 150])
-        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)
@@ -278,9 +281,10 @@ class GraspObject():
         elif height ==3:
             print('nmsl')
             self.pub2.publish(0)
-        r1.sleep()
+        time.sleep(2)
         # stop pump
         self.pub2.publish(0)
+        time.sleep(1)
         # r1.sleep()
         pos.x = 120
         pos.y = 0