From b8e99f045a05b31d1cfbf47115867257d6482afb Mon Sep 17 00:00:00 2001
From: "litian.zhuang" <litian.zhuang@nxrobo.com>
Date: Mon, 10 Oct 2022 20:37:26 +0800
Subject: [PATCH] 20221010

---
 src/3rd_app/move2grasp/scripts/grasp_d435.py |  2 +-
 src/3rd_app/move2grasp/scripts/move.py       | 22 +++++++++++++++++---
 2 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/3rd_app/move2grasp/scripts/grasp_d435.py b/src/3rd_app/move2grasp/scripts/grasp_d435.py
index 1c7292e..c249cc4 100755
--- a/src/3rd_app/move2grasp/scripts/grasp_d435.py
+++ b/src/3rd_app/move2grasp/scripts/grasp_d435.py
@@ -186,7 +186,7 @@ class GraspObject():
         cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
 
         # 蓝色物体颜色检测范围
-        LowerBlue = np.array([95, 90, 120])
+        LowerBlue = np.array([95, 90, 130])
         UpperBlue = np.array([130, 255, 255])
         # LowerBlue = np.array([4, 103, 162])
         # UpperBlue = np.array([85, 136, 181])
diff --git a/src/3rd_app/move2grasp/scripts/move.py b/src/3rd_app/move2grasp/scripts/move.py
index 7303617..76bde89 100755
--- a/src/3rd_app/move2grasp/scripts/move.py
+++ b/src/3rd_app/move2grasp/scripts/move.py
@@ -1,6 +1,9 @@
 #!/usr/bin/env python3
 # -*- coding: UTF-8 -*-
  
+from glob import glob
+from itertools import cycle
+from sre_constants import SUCCESS
 import rospy  
 import actionlib  
 from actionlib_msgs.msg import *
@@ -10,8 +13,8 @@ from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 from tf.transformations import quaternion_from_euler  
 from visualization_msgs.msg import Marker  
 from math import radians, pi  
- 
 class Move2Grasp():  
+    successCount = 0
     def __init__(self):  
         rospy.init_node('move2grasp', anonymous=False)  
  
@@ -38,6 +41,7 @@ class Move2Grasp():
  
         rospy.loginfo("Connected to move base server")  
         rospy.loginfo("Starting navigation test")  
+        
   
  
     def cp_callback(self, msg):
@@ -75,16 +79,28 @@ class Move2Grasp():
             # 物体抓取成功,让机器人回起始点
             if msg.data=='1': 
                 goal = MoveBaseGoal()
+                self.successCount+=1
                 goal.target_pose.header.frame_id = 'map' 
                 goal.target_pose.header.stamp = rospy.Time.now() 
-                pose = Pose(Point(0.3,0.0,0.0), Quaternion(0.0, 0.0, 1.0, 0.0))
+                if self.successCount<=3:
+                    pose = Pose(Point(0.3,0.0,0.0), Quaternion(0.0, 0.0, 1.0, 0.0))
+                elif self.successCount <=6:
+                    pose = Pose(Point(0.4,0.0,0.0), Quaternion(0.0, 0.0, 1.0, 0.0))
+                elif self.successCount <=9:
+                    pose = Pose(Point(0.5,0.0,0.0), Quaternion(0.0, 0.0, 1.0, 0.0))
                 goal.target_pose.pose=pose  
                 status=self.move(goal)
                 #status=self.move(goal)
                 # 到达起始点,放下物体
                 if status==True:
                     msg=String()
-                    msg.data='0'
+                    selfCycle = self.successCount%3
+                    if selfCycle == 1:
+                        msg.data='0'
+                    elif selfCycle == 2:
+                        msg.data='2'
+                    elif selfCycle == 0:
+                        msg.data='3'
                     self.grasp_pub.publish(msg)