UPDATE Vision 修复Z轴抓取

get_position_test.py

@@ -11,6 +11,7 @@ import cv2
 import os
 
 from Vision.camera_coordinate_dete import Detection
+from Vision.camera_coordinate_dete_planevison import Detection_plane_vsion
 from Trace.handeye_calibration import *
 from Vision.tool.utils import get_disk_space
 from Vision.tool.utils import remove_nan_mean_value
@@ -41,6 +42,23 @@ def detectionPosition_test():
             cv2.imshow('img', img)
             cv2.waitKey(0)
 
+def Detection_plane_vsion_test():
+    detection = Detection_plane_vsion()
+    while True:
+        ret, img, xyz, nx_ny_nz, box = detection.get_position(Point_isVision=True, save_img_point=1)
+        if ret==1:
+            print('xyz点云坐标：', xyz)
+            print('nx_ny_nz法向量：', nx_ny_nz)
+            print('矩形框四顶点：', box)
+            # img = cv2.resize(img,(720, 540))
+            if xyz!=None:
+                transformation_matrix = R_matrix(521.534, 0.705, 850.03, 0.0, 90.0, 0.0)   # (x, y, z, u, v, w)
+                target_position, noraml_base = getPosition(xyz[0], xyz[1], xyz[2], nx_ny_nz[0], nx_ny_nz[1], nx_ny_nz[2], transformation_matrix, box)
+                print("target_position:", target_position)
+                print("noraml_base", noraml_base)
+            cv2.imshow('img', img)
+            cv2.waitKey(0)
+
 def take_photo_position_test():
     detection = Detection()
     while True:
@@ -115,4 +133,4 @@ def bag_collection_test():
 
 
 if __name__ == '__main__':
-    detectionPosition_test()
+    Detection_plane_vsion_test()