first commit

Vision/get_position_test.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+'''
+# @Time    : 2024/9/7 11:49
+# @Author  : hjw
+# @File    : get_position_test.py
+'''
+from camera_coordinate_dete import Detection
+from Trace.handeye_calibration import *
+
+import cv2
+detection = Detection()
+
+while True:
+    ret, img, xyz, nx_ny_nz, box = detection.get_position()
+    if ret==1:
+        print('xyz点云坐标：', xyz)
+        print('nx_ny_nz法向量：', nx_ny_nz)
+        print('矩形框四顶点：', box)
+        img = cv2.resize(img,(720, 540))
+        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)