openarm/openarm_test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加标签id过滤

Browse Source
This commit is contained in:
shenchenyang 2026-03-18 15:34:59 +08:00
parent 0e267ce936
commit 049987f52d
1 changed files with 38 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
src/openarm_camera/openarm_camera/stereo_hand_depth_node.py
View File

@ -177,7 +177,10 @@ class StereoHandDepthNode(Node):
)
msg_out = PoseStamped()
if ids is not None:
detected_ids = ids.flatten().tolist()
cv2.aruco.drawDetectedMarkers(left_rect, corners, ids)
if 0 in detected_ids:
self.get_logger().info("Detected ArUco id=0")
# c = corners[0][0]
# center = c.mean(axis=0)
# u, v = int(center[0]), int(center[1])
@ -188,43 +191,43 @@ class StereoHandDepthNode(Node):
# left_gray, f"disp={d:.2f}", (u+5, v-5),
# cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1
# )
for e in corners:
ret, rvec, tvec = cv2.solvePnP(
objectPoints=np.array([
[-self.marker_size/2, self.marker_size/2, 0],
[ self.marker_size/2, self.marker_size/2, 0],
[ self.marker_size/2, -self.marker_size/2, 0],
[-self.marker_size/2, -self.marker_size/2, 0]
], dtype=np.float32),
imagePoints=e,
cameraMatrix=self.K1,
distCoeffs=self.D1
)
if(ret == True):
cv2.drawFrameAxes(left_rect, self.K1, self.D1, rvec, tvec, 0.1, 2)
R_cv, _ = cv2.Rodrigues(rvec)
R_cv_to_ros = np.array([
[ 0, 0, -1],
[ 1, 0, 0],
[ 0, -1, 0]
])
R_ros = R_cv_to_ros @ R_cv
t_ros = R_cv_to_ros @ tvec.reshape(3)
# t_ros = tvec.reshape(3)
rot = R.from_matrix(R_ros)
for e in corners:
ret, rvec, tvec = cv2.solvePnP(
objectPoints=np.array([
[-self.marker_size/2, self.marker_size/2, 0],
[ self.marker_size/2, self.marker_size/2, 0],
[ self.marker_size/2, -self.marker_size/2, 0],
[-self.marker_size/2, -self.marker_size/2, 0]
], dtype=np.float32),
imagePoints=e,
cameraMatrix=self.K1,
distCoeffs=self.D1
)
if(ret == True):
cv2.drawFrameAxes(left_rect, self.K1, self.D1, rvec, tvec, 0.1, 2)
R_cv, _ = cv2.Rodrigues(rvec)
R_cv_to_ros = np.array([
[ 0, 0, -1],
[ 1, 0, 0],
[ 0, -1, 0]
])
R_ros = R_cv_to_ros @ R_cv
t_ros = R_cv_to_ros @ tvec.reshape(3)
# t_ros = tvec.reshape(3)
rot = R.from_matrix(R_ros)
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})")
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})")
# 记录时间
self.last_marker_time = time.time()
# 记录时间
self.last_marker_time = time.time()
# 记录最后一次有效目标
self.last_valid_pos = np.array([t_ros[0] + 0.7, t_ros[1], t_ros[2]])
self.get_logger().info(f"Marker position (ROS frame): ({self.last_valid_pos[0]:.3f}, {self.last_valid_pos[1]:.3f}, {self.last_valid_pos[2]:.3f})")
self.last_valid_quat = np.array([qx, qy, qz, qw])
# 记录最后一次有效目标
self.last_valid_pos = np.array([t_ros[0] + 0.7, t_ros[1], t_ros[2]])
self.get_logger().info(f"Marker position (ROS frame): ({self.last_valid_pos[0]:.3f}, {self.last_valid_pos[1]:.3f}, {self.last_valid_pos[2]:.3f})")
self.last_valid_quat = np.array([qx, qy, qz, qw])
# u = int(u * (w / 2)) # 注意这里是左图宽度
# v = int(v * h)