From df436a9a312c3c9edf657c55ba72cf92a2238fbb Mon Sep 17 00:00:00 2001 From: FallenSigh Date: Tue, 16 Jun 2026 19:25:13 +0800 Subject: [PATCH] =?UTF-8?q?fix(vision=5Fgrasp):=20=E6=A0=B9=E6=8D=AE=20J5?= =?UTF-8?q?=20=E7=8A=B6=E6=80=81=E5=A4=84=E7=90=86=E7=9B=B8=E6=9C=BA?= =?UTF-8?q?=E6=97=8B=E8=BD=AC=20180=C2=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 问题： - 相机水平安装，但随 J5 状态旋转 180° - J5 张开（UP）：相机正向，图像正常 - J5 闭合（DOWN）：相机旋转 180°，图像上下颠倒 解决方案： - 从 GetPose 获取 J5 状态 - J5 > 0 (UP): (xc, yc, zc) = (x_img, -y_img, z) - J5 < 0 (DOWN): (xc, yc, zc) = (-x_img, y_img, z) - 自动处理相机旋转，无需手动调整 更新文档说明相机旋转机制 --- ros2/README.md | 5 +++ .../src/udp_teleop/udp_teleop/vision_grasp.py | 38 ++++++++++++------- 2 files changed, 29 insertions(+), 14 deletions(-) diff --git a/ros2/README.md b/ros2/README.md index bb9798d..60f66e6 100644 --- a/ros2/README.md +++ b/ros2/README.md @@ -31,9 +31,14 @@ CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。 **功能**： - 相机坐标到基坐标系的自动转换 +- **自动处理相机旋转**：根据 J5 状态自动调整图像坐标转换 - 抓取流程：释放 → 移动 → 抓取 → 回收 - 释放流程：移动 → 释放 → 回收 +**相机旋转说明**： +- J5 > 0°（张开/UP）：相机正向，`(xc, yc, zc) = (x_img, -y_img, z)` +- J5 < 0°（闭合/DOWN）：相机旋转 180°，`(xc, yc, zc) = (-x_img, y_img, z)` + ## 🚀 快速开始 ### 编译 diff --git a/ros2/src/udp_teleop/udp_teleop/vision_grasp.py b/ros2/src/udp_teleop/udp_teleop/vision_grasp.py index b6c72ca..8d4286d 100644 --- a/ros2/src/udp_teleop/udp_teleop/vision_grasp.py +++ b/ros2/src/udp_teleop/udp_teleop/vision_grasp.py @@ -183,8 +183,8 @@ class VisionGraspNode(Node): self.get_logger().info(' - /vision_grasp/grasp_target (geometry_msgs/Point)') self.get_logger().info(' - /vision_grasp/release_target (geometry_msgs/Point)') - def get_current_tcp_pose(self) -> Tuple[float, float, float, float]: - """查询当前 TCP 位姿""" + def get_current_tcp_pose(self) -> Tuple[float, float, float, float, int]: + """查询当前 TCP 位姿（包括 J5 状态）""" req = GetPose.Request() future = self.get_pose_cli.call_async(req) @@ -202,7 +202,7 @@ class VisionGraspNode(Node): if not result.success: raise RuntimeError(f"获取 TCP 位姿失败: {result.message}") - return result.x, result.y, result.z, result.phi + return result.x, result.y, result.z, result.phi, result.j5 def move_to(self, x: float, y: float, z: float, phi: float, duration: float, grip: bool = False, release: bool = False) -> bool: @@ -280,18 +280,28 @@ class VisionGraspNode(Node): def _execute_grasp_thread(self, x: float, y: float, z: float): """在独立线程中执行抓取流程""" try: - # 转换坐标：(x, y, z) -> (xc, yc, zc) = (x, -y, z) - xc = x - yc = -y - zc = z + # 获取当前 TCP 位姿（包括 J5 状态） + tcp_x, tcp_y, tcp_z, tcp_phi, j5 = self.get_current_tcp_pose() + self.get_logger().info(f'当前 TCP: ({tcp_x:.1f}, {tcp_y:.1f}, {tcp_z:.1f}), phi={tcp_phi:.1f}°, j5={j5}°') - self.get_logger().info(f'转换后相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})') + # 图像坐标到相机坐标系转换 + # 相机水平安装，但会随 J5 状态旋转 180° + if j5 > 0: + # J5 张开（UP）：相机正向 + # 图像 Y 向下 → 相机 -Y + xc = x + yc = -y + zc = z + self.get_logger().info(f'相机正向（UP），相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})') + else: + # J5 闭合（DOWN）：相机旋转 180° + # 图像 X,Y 都翻转（相对于 UP 状态） + xc = -x + yc = y + zc = z + self.get_logger().info(f'相机旋转 180°（DOWN），相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})') - # 获取当前 TCP 位姿 - tcp_x, tcp_y, tcp_z, tcp_phi = self.get_current_tcp_pose() - self.get_logger().info(f'当前 TCP: ({tcp_x:.1f}, {tcp_y:.1f}, {tcp_z:.1f}), phi={tcp_phi:.1f}°') - - # 坐标变换 + # 坐标变换：相机 → 基坐标系 target_x, target_y, target_z = camera_to_base( xc, yc, zc, tcp_x, tcp_y, tcp_z, tcp_phi, @@ -323,7 +333,7 @@ class VisionGraspNode(Node): """在独立线程中执行释放流程""" try: # 获取当前 TCP 位姿（用于获取 phi） - _, _, _, tcp_phi = self.get_current_tcp_pose() + _, _, _, tcp_phi, _ = self.get_current_tcp_pose() # 执行释放流程 self.execute_release(x, y, z, tcp_phi)