20220905晚

修正大部分识别问题，少量修改机械臂放下功能（z轴修改）

src/3rd_app/move2grasp/rviz/spark_gmapping_d435.rviz

@@ -7,7 +7,7 @@ Panels:
         - /TF1/Frames1
         - /PointStamped1
       Splitter Ratio: 0.594406008720398
-    Tree Height: 210
+    Tree Height: 341
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -23,7 +23,6 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
     SyncSource: LaserScan
@@ -159,14 +158,14 @@ Visualization Manager:
                 camera_bottom_screw_frame:
                   camera_link:
                     camera_aligned_depth_to_color_frame:
-                      camera_color_optical_frame:
-                        {}
+                      {}
                     camera_aligned_depth_to_infra1_frame:
                       camera_infra1_optical_frame:
                         {}
-                    camera_color_frame:
-                      {}
                     camera_depth_frame:
+                      camera_color_frame:
+                        camera_color_optical_frame:
+                          {}
                       camera_depth_optical_frame:
                         {}
                       camera_left_ir_frame:
@@ -521,7 +520,7 @@ Window Geometry:
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000017afc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d0000017a000000e60100001cfa000000000100000002fb0000000a0049006d0061006700650100000000ffffffff0000005e00fffffffb000000100044006900730070006c00610079007301000000000000016a0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003b80000003efc0100000002fb0000000800540069006d00650100000000000003b8000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002480000017a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000017afc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d0000017a000000e60100001cfa000000000100000002fb0000000a0049006d0061006700650100000000ffffffff0000005e00fffffffb000000100044006900730070006c00610079007301000000000000016a0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004180000003efc0100000002fb0000000800540069006d0065010000000000000418000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000002a80000017a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -530,6 +529,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 952
-  X: 72
-  Y: 27
+  Width: 1048
+  X: -164
+  Y: 56

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

@@ -32,12 +32,13 @@ class GraspObject():
         初始化
         '''
 
-        global xc, yc, xc_prev, yc_prev, found_count
+        global xc, yc, xc_prev, yc_prev, found_count, height
         xc = 0
         yc = 0
         xc_prev = xc
         yc_prev = yc
         found_count = 0
+        height = 0
         self.is_found_object = False
         # self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
         # 订阅机械臂抓取指令
@@ -94,6 +95,23 @@ class GraspObject():
             self.grasp_status_pub.publish(status) 
         if msg.data=='0':
             # 放下物体
+            height = 1
+            self.is_found_object = False
+            self.release_object()
+            status=String()
+            status.data='0'
+            self.grasp_status_pub.publish(status) 
+        if msg.data=='2':
+            # 放下物体
+            height = 2
+            self.is_found_object = False
+            self.release_object()
+            status=String()
+            status.data='0'
+            self.grasp_status_pub.publish(status) 
+        if msg.data=='3':
+            # 放下物体
+            height = 3
             self.is_found_object = False
             self.release_object()
             status=String()
@@ -127,19 +145,19 @@ class GraspObject():
         r2 = rospy.Rate(10)
         pos = position()
         # 物体所在坐标+标定误差
-        pos.x = a[0] * yc + a[1]
-        pos.y = b[0] * xc + b[1]
+        pos.x = round(a[0] * yc + a[1],2)
+        pos.y = round(b[0] * xc + b[1],2)
         pos.z = 20
-	    # pos.z = 20
-        print("z = 20\n")
+	    # # pos.z = 20
+        # print("z = 20\n")
         self.pub1.publish(pos)
         r2.sleep()
         # go down -100
-        pos.z = -70
+        pos.z = -60
         self.pub1.publish(pos)
         print("z = -83\n")
         r2.sleep()
-
+        print((pos.x,pos.y))
         # 开始吸取物体
         self.pub2.publish(1)
         r2.sleep()
@@ -163,7 +181,7 @@ class GraspObject():
         cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
 
         # 蓝色物体颜色检测范围
-        LowerBlue = np.array([100, 100, 140])
+        LowerBlue = np.array([90, 95, 120])
         UpperBlue = np.array([140, 255, 255])
         # LowerBlue = np.array([4, 103, 162])
         # UpperBlue = np.array([85, 136, 181])
@@ -185,7 +203,7 @@ class GraspObject():
         # cv2.imshow("win1", cv_image1)
         # cv2.imshow("win2", cv_image5)
         # cv2.waitKey(1)
-        contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+        contours, hier = cv2.findContours(cv_image4, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
 
         # if find contours, pick the biggest box
         if len(contours) > 0:
@@ -229,14 +247,19 @@ class GraspObject():
         # go forward
         pos.x = 200
         pos.y = 0
-        pos.z = -40  #-80
+        if height == 0:
+            pos.z = -40
+        elif height ==1:
+            pos.z = -40
+        elif height == 2:
+            pos.z = 55
+        elif height ==3:
+            pos.z = 150  #-80
         self.pub1.publish(pos)
         r1.sleep()
-
         # stop pump
         self.pub2.publish(0)
-        # r2.sleep()
-        #r1.sleep()
+        r1.sleep(0.1)
         pos.x = 120
         pos.y = 0
         pos.z = 35

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

@@ -68,7 +68,7 @@ def keyboardLoop():
         finally :
             termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
         # ch代表获取的键盘按键
-        if ch == 'g':
+        if ch == 'e':
             if can_grasp:
                 msg=String()
                 msg.data='1'
@@ -76,7 +76,7 @@ def keyboardLoop():
                 can_grasp=False
                 speed = 0
                 turn = 0
-        elif ch == 'h':
+        elif ch == '1':
             if can_release:
                 msg=String()
                 msg.data='0'
@@ -84,6 +84,23 @@ def keyboardLoop():
                 can_release=False
                 speed = 0
                 turn = 0
+        elif ch == '2':
+            if can_release:
+                msg=String()
+                msg.data='2'
+                grasp_pub.publish(msg)
+                can_release=False
+                speed = 0
+                turn = 0
+        elif ch == '3':
+            if can_release:
+                msg=String()
+                msg.data='3'
+                grasp_pub.publish(msg)
+                can_release=False
+                speed = 0
+                turn = 0
+        
         elif ch == 'z':
             if can_release:
                 msg=String()