Merge pull request #18 from BoZhiStudying233/master

feat:增加判断微调超时

src/rmus_solution/scripts/img_processor.py

@@ -81,7 +81,7 @@ def __init__(self, initial_mode=0, verbose=True) -> None:
         self.pub_all_ID = rospy.Publisher("/all_detect_ID", UInt8MultiArray, queue_size=1)
         self.detected_gameinfo = [-1, -1, -1]
         self.uint32data = [None] * 9
-
+        
     def imageCallback(self, image):
         self.image = self.bridge.imgmsg_to_cv2(image, "bgr8")
         self.this_image_time_ms = int(image.header.stamp.nsecs / 1e6) + int(

src/rmus_solution/scripts/manipulater.py

@@ -5,9 +5,12 @@
 import numpy as np
 from scipy.spatial.transform import Rotation as sciR
 
+
+
 import rospy
 from geometry_msgs.msg import Twist, Pose, Point
 from rmus_solution.srv import graspsignal, graspsignalResponse
+from std_msgs.msg import Bool
 
 import tf2_ros
 import tf2_geometry_msgs
@@ -17,9 +20,15 @@ class manipulater:
     def __init__(self) -> None:
         self.arm_gripper_pub = rospy.Publisher("arm_gripper", Point, queue_size=2)
         self.arm_position_pub = rospy.Publisher("arm_position", Pose, queue_size=2)
+
+        self.timeout_pub = rospy.Publisher("/timeout", Bool, queue_size=2)#发布微调是否超时
+
+
         self.cmd_vel_puber = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
         rospy.Subscriber("/get_blockinfo", Pose, self.markerPoseCb, queue_size=1)
 
+
+
         self.current_marker_poses = None
         self.image_time_now = 0
         self.observation = np.array([0.0, 0.0])
@@ -104,6 +113,7 @@ def trimerworkCallback(self, req):
         rate = rospy.Rate(self.ros_rate)
 
         resp = graspsignalResponse()
+        current_time1 = rospy.Time.now()
 
         if req.mode == 1:
             rospy.loginfo("First trim then grasp")
@@ -157,14 +167,21 @@ def trimerworkCallback(self, req):
                     cmd_vel[1] = -0.11
 
                 flag = 1
-
+                
                 cmd_vel[2] = 0
                 if np.abs(target_pos[0] - x_dis_tar) <= 0.02 and (
                     (target_pos[1] - 0.0) <= y_threshold_p
                     and (0.0 - target_pos[1]) <= y_threshold_n
                 ):
                     cmd_vel = [0.0, 0.0, 0.0]
                 self.sendBaseVel(cmd_vel)
+                current_time2 = rospy.Time.now()
+                if current_time2.secs - current_time1.secs > 20:#如果微调时间超过20秒
+                    rospy.logerr("微调时间过长")
+                    self.timeout_pub.publish(True)
+                    rospy.sleep(2)
+                    return resp
+                self.timeout_pub.publish(False)
                 if np.abs(target_pos[0] - x_dis_tar) <= 0.02 and (
                     (target_pos[1] - 0.0) <= y_threshold_p
                     and (0.0 - target_pos[1]) <= y_threshold_n
@@ -212,7 +229,7 @@ def trimerworkCallback(self, req):
                 target_marker_pose = self.current_marker_poses
                 if target_marker_pose is None:
                     continue
-
+                
                 target_pos, target_angle = self.getTargetPosAndAngleInBaseLinkFrame(
                     target_marker_pose
                 )
@@ -260,8 +277,11 @@ def trimerworkCallback(self, req):
                     break
                 rate.sleep()
 
+
             rospy.loginfo("Trim well in the horizon dimention")
 
+
+
             target_pos, target_angle = self.getTargetPosAndAngleInBaseLinkFrame(
                 self.current_marker_poses
             )
@@ -278,7 +298,7 @@ def trimerworkCallback(self, req):
 
             resp.res = True
             resp.response = "Successfully Place"
-
+            
         return resp
 
     def open_gripper(self):

src/rmus_solution/scripts/play_game.py

@@ -6,6 +6,11 @@
 from rmus_solution.srv import switch, setgoal, graspsignal
 from geometry_msgs.msg import Pose
 
+from std_msgs.msg import Bool
+
+
+timeout = False
+
 def get_boxid_blockid_inorder(gameinfo):
     return [0,1,2], [gameinfo[0], gameinfo[1], gameinfo[2]]
 
@@ -25,37 +30,49 @@ def compare_poses(pose1, pose2):
         return False
 
     return True
-
+def set_timeout(data):
+    global timeout 
+    timeout = data
+
 def check_topic_messages(topic_name):
     try:
         rospy.wait_for_message(topic_name, Pose, timeout=1)
     except rospy.exceptions.ROSException:
         return False
-    return True;
+    return True
 
+def catch():
+    trimer_response = trimer(1,"")
+    global timeout
+    if timeout.data == True:
+        rospy.logerr("微调超时,放弃此次抓取")
+        return True   #True代表超时了
+    return False
 
 def grip(gameinfo , is_here, response):
     for i, target in enumerate(gameinfo.data):
         response = img_switch_mode(target)#切换为识别对应目标
         if check_topic_messages("/get_blockinfo") and is_here == 0:#判断是否识别到了目标方块
             print("这里有目标方块"+ str(target))
-            trimer_response = trimer(1,"")
-            rospy.sleep(1)#给抓取好方块留出时间
+            if catch():
+                is_here = 2#表示微调超时了 
+                return is_here,response
+            rospy.sleep(2)#给抓取好方块留出时间
             try:
                 blockinfo = rospy.wait_for_message("/all_detect_ID", UInt8MultiArray, timeout=1)
                 if target in blockinfo.data :
                     times = 0#尝试重新抓取的次数
-                    while target in blockinfo.data and times<1:#若抓不上会尝试抓20次
+                    while target in blockinfo.data and times<20:#若抓不上会尝试抓20次
                         times = times + 1
-                        rospy.logerr("抓取失败,将进行下一次尝试")
-
-                        trimer_response = trimer(1,"")
-                        rospy.sleep(2)
+                        rospy.logerr("抓取失败,将进行第"+str(times+1)+"次抓取")
+                        if catch():
+                            is_here = 2
+                            return is_here,response
                         try:
                             blockinfo = rospy.wait_for_message("/all_detect_ID", UInt8MultiArray, timeout=1)
                         except:
                             blockinfo = [-1]#若收不到话题消息，则另其为-1
-
+                        
                     if target in blockinfo.data == False:
                         rospy.logeinfo("另一次抓取成功了！！")
                         is_here = 1#抓到目标方块
@@ -66,16 +83,19 @@ def grip(gameinfo , is_here, response):
                     rospy.loginfo("抓取成功")
                     is_here = 1#抓到目标方块
             except:
-                rospy.loginfo("抓取成功")
+                rospy.loginfo("抓取成功~~")
                 is_here = 1#抓到目标方块
             if is_here == 1:
                 break;
     return is_here,response
 
+# def go_to_another_side(location):
+#     rospy.loginfo("准备去此矿区")
+
 
 if __name__ == '__main__':
     rospy.init_node("gamecore_node")
-
+    
     while not rospy.is_shutdown():
         try:
             rospy.wait_for_service("/set_navigation_goal", 1.0)
@@ -106,8 +126,13 @@ def grip(gameinfo , is_here, response):
     img_switch_mode = rospy.ServiceProxy("/image_processor_switch_mode", switch)
     rospy.sleep(2)
 
+    rospy.Subscriber("/timeout", Bool, set_timeout)
+
     trim_res = trimer(0, "")
     response = img_switch_mode(10)
+
+
+
     navigation_result = navigation(9, "")
 
     gameinfo = rospy.wait_for_message("/get_gameinfo", UInt8MultiArray, timeout=7)
@@ -116,28 +141,38 @@ def grip(gameinfo , is_here, response):
        rospy.logwarn("Waiting for gameinfo detection.")
        rospy.sleep(0.5)
 
+
+
     response = img_switch_mode(0)
     for i in range(0,3):
         is_here = 0#判断这里有没有目标方块
 
         navigation_result = navigation(2, "")
+        location = 2#在矿区2
         is_here, response = grip(gameinfo , is_here, response)
         if is_here == 0:
             navigation_result = navigation(4, "")
+            location = 4#在矿区4
             is_here, response = grip(gameinfo , is_here, response)
         if is_here == 0:
             navigation_result = navigation(1, "")
+            location = 1#在矿区1
             is_here, response = grip(gameinfo , is_here, response)
         navigation_result = navigation(6+i, "")#暂时没写放到哪个上面
         print("         到达目的地"+str(6+i))#输出到达了兑换站 
 
+        # if is_here == 2:
+        #     #去此矿区的另一侧
+        #     go_to_another_side(location)
+
 
         response = img_switch_mode(7+i)
         rospy.sleep(0.5)
         trimer_response = trimer(2,"")
         response = img_switch_mode(0)
 
 
+
     navigation_result = navigation(11, "")
 
     response = img_switch_mode(0)