From 167368c72420b2c8a3db422494b16699a7525e66 Mon Sep 17 00:00:00 2001 From: rise0chen Date: Thu, 21 Mar 2024 10:34:30 +0800 Subject: [PATCH] feat: apriltag pose --- apriltag/bin/src/apriltag.py | 14 +++++++------- apriltag/bin/src/wait_signal.py | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/apriltag/bin/src/apriltag.py b/apriltag/bin/src/apriltag.py index 9dd6a9a..5aff6a9 100644 --- a/apriltag/bin/src/apriltag.py +++ b/apriltag/bin/src/apriltag.py @@ -55,17 +55,17 @@ def main(): lebai_real = lebai_sdk.connect("127.0.0.1", False) flange_pose = (lebai_real.get_kin_data())["actual_flange_pose"] cam2flange = json.loads((lebai.get_item("plugin_camera_calibrater_data"))['value']) - cam = pose_times(flange_pose, cam2flange) + cam = lebai_real.pose_trans(flange_pose, cam2flange) for tag in tags: pos = tag.pose_t rot = rotation.rotationMatrixToEulerZyx(tag.pose_R) - tag = {"x":pos[0][0],"y":pos[1][0],"z":pos[2][0], "rx":rot[2],"ry":rot[1],"rz":rot[0]} - if (tag["x"]**2+tag["y"]**2+tag["z"]**2)**0.5 > 0.8: + offset = {"x":pos[0][0],"y":pos[1][0],"z":pos[2][0], "rx":rot[2],"ry":rot[1],"rz":rot[0]} + if (offset["x"]**2+offset["y"]**2+offset["z"]**2)**0.5 > 0.8: continue - tag_pos_offset = {tag.x, tag.y, tag.z, 0, 0, 0} - tag_rot_offset = {0, 0, 0, tag.rz, tag.ry, tag.rx} - tag_pose = pose_times(cam, tag_pos_offset) - tag_pose = pose_times(tag_pose, tag_rot_offset) + tag_pos_offset = {"x":offset["x"], "y":offset["y"], "z":offset["z"], "rz":0, "ry":0, "rx":0} + tag_rot_offset = {"x":0,"y": 0,"z": 0, "rz":offset["rz"], "ry":offset["ry"], "rx":offset["rx"]} + tag_pose = lebai_real.pose_trans(cam, tag_pos_offset) + tag_pose = lebai_real.pose_trans(tag_pose, tag_rot_offset) ret[tag.tag_id] = tag_pose for corner in tag.corners: cv2.line(img, tuple(corner.astype(int)), tuple(tag.center.astype(int)), 0, 3) diff --git a/apriltag/bin/src/wait_signal.py b/apriltag/bin/src/wait_signal.py index 9aef055..c4228ee 100644 --- a/apriltag/bin/src/wait_signal.py +++ b/apriltag/bin/src/wait_signal.py @@ -19,12 +19,12 @@ def main(): time.sleep(1) apriltag_id = lebai.get_signal(apriltag_signal) if apriltag_id >= 0: - lebai.save_pose("apriltag"+apriltag_id, None) + lebai.save_pose("apriltag"+str(apriltag_id), None) apriltag_data = subprocess.check_output(['lpy', apriltag_plugin]) apriltag_data = json.loads(apriltag_data) apriltag_pose = apriltag_data.get(str(apriltag_id), None) print(apriltag_id, apriltag_data) - lebai.save_pose("apriltag"+apriltag_id, apriltag_pose) + lebai.save_pose("apriltag"+str(apriltag_id), apriltag_pose) lebai.set_signal(apriltag_signal, -1)