From a2fc8c1faf3e23ba74e09ef77125fffdff51c65b Mon Sep 17 00:00:00 2001 From: shenchenyang <664376944@qq.com> Date: Thu, 5 Feb 2026 19:27:23 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=9B=9B=E5=85=83=E6=95=B0?= =?UTF-8?q?=E5=92=8C=E6=AC=A7=E6=8B=89=E8=A7=92=E8=A7=A3=E7=AE=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../openarm_camera/stereo_hand_depth_node.py | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py b/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py index 3284a12..de8c482 100644 --- a/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py +++ b/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py @@ -9,6 +9,8 @@ from geometry_msgs.msg import PointStamped from cv_bridge import CvBridge from sensor_msgs.msg import CameraInfo import mediapipe as mp +from scipy.spatial.transform import Rotation as R +# from tf_transformations import quaternion_from_euler, euler_from_quaternion class StereoHandDepthNode(Node): @@ -181,8 +183,14 @@ class StereoHandDepthNode(Node): cameraMatrix=self.K1, distCoeffs=self.D1 ) - cv2.drawFrameAxes(left_gray, self.K1, self.D1, rvec, tvec, 0.1, 2) - # self.get_logger().info(f"rvec: {rvec}") + if(ret == True): + cv2.drawFrameAxes(left_gray, self.K1, self.D1, rvec, tvec, 0.1, 2) + R_ca, _ = cv2.Rodrigues(rvec) + rot = R.from_matrix(R_ca) + qx, qy, qz, qw = rot.as_quat() + roll, pitch, yaw = rot.as_euler('xyz') + self.get_logger().info(f"quat: ({qx}, {qy}, {qz}, {qw})") + self.get_logger().info(f"euler: ({roll*180/3.14:.1f}, {pitch*180/3.14:.1f}, {yaw*180/3.14:.1f})") # u = int(u * (w / 2)) # 注意这里是左图宽度 # v = int(v * h) p.header = msg.header