feat: apriltag pose

lebai-robotics · Mar 21, 2024 · 167368c · 167368c
1 parent cdea5f9
commit 167368c
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 9 deletions.
diff --git 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
@@ -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)