From d5e69d2274f971c5216c6fde29b245dae9b13c0b Mon Sep 17 00:00:00 2001 From: rise0chen Date: Thu, 1 Aug 2024 14:59:44 +0800 Subject: [PATCH] rm modbus --- camera_calibrater_modbus/.gitignore | 2 - camera_calibrater_modbus/bin/daemon.exe | 4 - camera_calibrater_modbus/bin/disable.exe | 1 - camera_calibrater_modbus/bin/enable.exe | 4 - .../bin/src/camera_calibrater.py | 128 ------------ .../bin/src/requirements.txt | 3 - .../bin/src/utils/rotation.py | 57 ------ camera_calibrater_modbus/plugin.json | 6 - camera_calibrater_modbus/web/index.html | 186 ------------------ 9 files changed, 391 deletions(-) delete mode 100644 camera_calibrater_modbus/.gitignore delete mode 100644 camera_calibrater_modbus/bin/daemon.exe delete mode 100644 camera_calibrater_modbus/bin/disable.exe delete mode 100644 camera_calibrater_modbus/bin/enable.exe delete mode 100644 camera_calibrater_modbus/bin/src/camera_calibrater.py delete mode 100644 camera_calibrater_modbus/bin/src/requirements.txt delete mode 100644 camera_calibrater_modbus/bin/src/utils/rotation.py delete mode 100644 camera_calibrater_modbus/plugin.json delete mode 100644 camera_calibrater_modbus/web/index.html diff --git a/camera_calibrater_modbus/.gitignore b/camera_calibrater_modbus/.gitignore deleted file mode 100644 index c258dd0..0000000 --- a/camera_calibrater_modbus/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -/.plugin -x86_64/ diff --git a/camera_calibrater_modbus/bin/daemon.exe b/camera_calibrater_modbus/bin/daemon.exe deleted file mode 100644 index 26622bf..0000000 --- a/camera_calibrater_modbus/bin/daemon.exe +++ /dev/null @@ -1,4 +0,0 @@ -#!/usr/bin/env bash -cd "$(dirname "$0")" -#./`uname -m`/camera_calibrater -lpy ./src/camera_calibrater.py diff --git a/camera_calibrater_modbus/bin/disable.exe b/camera_calibrater_modbus/bin/disable.exe deleted file mode 100644 index f1f641a..0000000 --- a/camera_calibrater_modbus/bin/disable.exe +++ /dev/null @@ -1 +0,0 @@ -#!/usr/bin/env bash diff --git a/camera_calibrater_modbus/bin/enable.exe b/camera_calibrater_modbus/bin/enable.exe deleted file mode 100644 index f9a15a5..0000000 --- a/camera_calibrater_modbus/bin/enable.exe +++ /dev/null @@ -1,4 +0,0 @@ -#!/usr/bin/env bash -cd "$(dirname "$0")" -#chmod 755 ./`uname -m`/camera_calibrater -lpip install -r ./src/requirements.txt diff --git a/camera_calibrater_modbus/bin/src/camera_calibrater.py b/camera_calibrater_modbus/bin/src/camera_calibrater.py deleted file mode 100644 index e6aaa28..0000000 --- a/camera_calibrater_modbus/bin/src/camera_calibrater.py +++ /dev/null @@ -1,128 +0,0 @@ -# encoding: utf-8 - -### 视觉标定 - -import os -import shutil -import numpy as np -from contextlib import suppress -import cv2 -import json -import time -import math -import lebai_sdk -import utils.rotation as rotation - -lebai_sdk.init() -lebai = lebai_sdk.connect("127.0.0.1", True) - -def get_tag(): - tag = (lebai.get_item("plugin_camera_calibrater_tag"))['value'] - if not tag: - tag = "0" - return tag -def get_size(): - size = (lebai.get_item("plugin_camera_calibrater_size"))['value'] - if not size: - size = "120" - size = int(size) - return size -def get_i(): - i = (lebai.get_item("plugin_camera_calibrater_i"))['value'] - if not i: - i = "0" - i = int(i) - return i -def get_cmd(): - cmd = (lebai.get_item("plugin_camera_calibrater_cmd_clear"))['value'] - if cmd and cmd != "": - return "clear" - cmd = (lebai.get_item("plugin_camera_calibrater_cmd_calibrate"))['value'] - if cmd and cmd != "": - return "calibrate" - cmd = (lebai.get_item("plugin_camera_calibrater_cmd_record"))['value'] - if cmd and cmd != "": - return "record" - cmd = (lebai.get_item("plugin_camera_calibrater_cmd_preview"))['value'] - if cmd and cmd != "": - return "preview" - - return "" -def get_pose(i): - robot_pose = (lebai.get_item("plugin_camera_calibrater_pose_{}".format(i)))['value'] - if not robot_pose: - return None - tag_pose = (lebai.get_item("plugin_camera_calibrater_pose_tag_{}".format(i)))['value'] - if not tag_pose: - return None - return {"robot": json.loads(robot_pose), "tag": json.loads(tag_pose)} - -def find_tags(lebai_real): - lebai_real.write_single_register("openmv", "306", get_size()) - lebai_real.write_single_register("openmv", "300", 1) - while lebai_real.read_holding_registers("openmv", "300", 1)[0]!=0: - time.sleep(0.1) - num = lebai_real.read_holding_registers("openmv", "301", 1)[0] - tags = {} - for i in range(num): - tag_data = lebai_real.read_holding_registers("openmv", str(311+10*i), 7) - x = tag_data[1] - if x > 32767: - x = x - 65536 - y = tag_data[2] - if y > 32767: - y = y - 65536 - z = tag_data[3] - if z > 32767: - z = z - 65536 - tags[str(tag_data[0])] = {"x":x/1000,"y":y/1000,"z":z/1000,"rz":tag_data[4]/65536*2*math.pi,"ry":tag_data[5]/65536*2*math.pi,"rx":tag_data[6]/65536*2*math.pi} - return tags - -def main(): - while True: - time.sleep(0.1) - cmd = get_cmd() - if not cmd or cmd == "": - continue - if cmd == "record": - lebai_real = lebai_sdk.connect("127.0.0.1", False) - tag_id = get_tag() - tags = find_tags(lebai_real) - if tag_id not in tags: - print(tags) - lebai.set_item("plugin_camera_calibrater_cmd_record", "") - continue - - i = get_i()+1 - lebai.set_item("plugin_camera_calibrater_i", str(i)) - lebai.set_item("plugin_camera_calibrater_pose_tag_{}".format(i), json.dumps(tags[tag_id])) - pose = (lebai_real.get_kin_data())["actual_flange_pose"] - lebai.set_item("plugin_camera_calibrater_pose_{}".format(i), json.dumps(pose)) - - if cmd == "clear": - # 清除数据 - lebai.set_item("plugin_camera_calibrater_i", str(0)) - if cmd == "calibrate": - R_end2base = [] # 法兰相对于基座的姿态 - T_end2base = [] # 法兰相对于基座的位置 - R_chess2cam = [] # 标签相对于摄像头的姿态 - T_chess2cam = [] # 标签相对于摄像头的位置 - # 手眼标定 - for i in range(1, get_i()+1): - pose = get_pose(i) - p = pose["robot"] - R_end2base.append(rotation.eulerZyzToRotationMatrix([p["rz"], p["ry"], p["rx"]])) - T_end2base.append(np.array([p["x"], p["y"], p["z"]])) - p = pose["tag"] - R_chess2cam.append(rotation.eulerZyzToRotationMatrix([p["rz"], p["ry"], p["rx"]])) - T_chess2cam.append(np.array([p["x"], p["y"], p["z"]])) - R, T = cv2.calibrateHandEye(R_end2base, T_end2base, R_chess2cam, T_chess2cam, cv2.CALIB_HAND_EYE_TSAI) - r = rotation.rotationMatrixToEulerZyx(R) - data = {"x":T[0][0],"y":T[1][0],"z":T[2][0],"rz":r[0],"ry":r[1],"rx":r[2]} - lebai.set_item("plugin_camera_calibrater_data", json.dumps(data)) - - lebai.set_item("plugin_camera_calibrater_i", str(0)) - lebai.set_item("plugin_camera_calibrater_cmd_{}".format(cmd), "") - -if __name__ == '__main__': - main() diff --git a/camera_calibrater_modbus/bin/src/requirements.txt b/camera_calibrater_modbus/bin/src/requirements.txt deleted file mode 100644 index 8748e8f..0000000 --- a/camera_calibrater_modbus/bin/src/requirements.txt +++ /dev/null @@ -1,3 +0,0 @@ -lebai_sdk>=0.2.0 -numpy>=1.0.0 -opencv-python-headless>=4.0.0 diff --git a/camera_calibrater_modbus/bin/src/utils/rotation.py b/camera_calibrater_modbus/bin/src/utils/rotation.py deleted file mode 100644 index 75812f5..0000000 --- a/camera_calibrater_modbus/bin/src/utils/rotation.py +++ /dev/null @@ -1,57 +0,0 @@ -import numpy as np -import math - -def eulerZyzToRotationMatrix(theta) : - # 分别构建三个轴对应的旋转矩阵 - R_x = np.array([[1, 0, 0 ], - [0, math.cos(theta[2]), -math.sin(theta[2]) ], - [0, math.sin(theta[2]), math.cos(theta[2]) ] - ]) - R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ], - [0, 1, 0 ], - [-math.sin(theta[1]), 0, math.cos(theta[1]) ] - ]) - R_z = np.array([[math.cos(theta[0]), -math.sin(theta[0]), 0], - [math.sin(theta[0]), math.cos(theta[0]), 0], - [0, 0, 1] - ]) - # 将三个矩阵相乘，得到最终的旋转矩阵 - R = np.dot(R_z, np.dot( R_y, R_x )) - return R - -# 检查一个旋转矩阵是否有效 -def isRotationMatrix(R) : - # 得到该矩阵的转置 - Rt = np.transpose(R) - # 旋转矩阵的一个性质是，相乘后为单位阵 - shouldBeIdentity = np.dot(Rt, R) - # 构建一个三维单位阵 - I = np.identity(3, dtype = R.dtype) - # 将单位阵和旋转矩阵相乘后的值做差 - n = np.linalg.norm(I - shouldBeIdentity) - # 如果小于一个极小值，则表示该矩阵为旋转矩阵 - return n < 1e-6 - -# 这部分的代码输出与Matlab里边的rotm2euler一致 -def rotationMatrixToEulerZyx(R) : - # 断言判断是否为有效的旋转矩阵 - assert(isRotationMatrix(R)) - sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) - singular = sy < 1e-6 - if not singular : - x = math.atan2(R[2,1] , R[2,2]) - y = math.atan2(-R[2,0], sy) - z = math.atan2(R[1,0], R[0,0]) - else : - x = math.atan2(-R[1,2], R[1,1]) - y = math.atan2(-R[2,0], sy) - z = 0 - return np.array([z, y, x]) - - -if __name__ == "__main__": - mat = np.array([[9.444320517616471289e-01 ,1.642640750095582525e-01 ,2.847198856183729143e-01], - [-2.962673613057895672e-01, 5.017864051458875707e-02, 9.537860109379947549e-01 ], - [1.423859200559868809e-01 ,-9.851392884751688506e-01, 9.605640047622143740e-02] - ]) - print(rotationMatrixToEulerZyx(mat)) \ No newline at end of file diff --git a/camera_calibrater_modbus/plugin.json b/camera_calibrater_modbus/plugin.json deleted file mode 100644 index 55e27d9..0000000 --- a/camera_calibrater_modbus/plugin.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "name": "camera_calibrater_modbus", - "description": "相机标定", - "homepage": "lebai.ltd", - "auto_restart": true -} \ No newline at end of file diff --git a/camera_calibrater_modbus/web/index.html b/camera_calibrater_modbus/web/index.html deleted file mode 100644 index fe20c63..0000000 --- a/camera_calibrater_modbus/web/index.html +++ /dev/null @@ -1,186 +0,0 @@ - - - - - - 相机标定 - - - -
- 标签ID
- -
- 标签尺寸(单位 毫米mm)
- -
-
- - -
-
-
-
- 采集至少6个不同位置后再点击“标定” -
- - - -
- 相机位置
- - - - - - \ No newline at end of file