From 11f2a6a56fe10192188afff1ea135b583a8c424c Mon Sep 17 00:00:00 2001
From: "litian.zhuang" <litian.zhuang@nxrobo.com>
Date: Thu, 29 Sep 2022 21:29:05 +0800
Subject: [PATCH] 20220929

---
 src/3rd_app/move2grasp/scripts/grasp_d435.py | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/src/3rd_app/move2grasp/scripts/grasp_d435.py b/src/3rd_app/move2grasp/scripts/grasp_d435.py
index 17d7e9e..1c7292e 100755
--- a/src/3rd_app/move2grasp/scripts/grasp_d435.py
+++ b/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