20221010

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])

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)  
  
@@ -40,6 +43,7 @@ class Move2Grasp():
         rospy.loginfo("Starting navigation test")  
         
   
+ 
     def cp_callback(self, msg):
             rospy.loginfo("POINT:%f,%f,%f", msg.point.x, msg.point.y, msg.point.z)
 
@@ -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)