fix(vision_grasp): 根据 J5 状态处理相机旋转 180°

问题：
- 相机水平安装，但随 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

@@ -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)`
+
 ## 🚀 快速开始
 
 ### 编译

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]:
+    def get_current_tcp_pose(self) -> Tuple[float, float, float, float, int]:
-        """查询当前 TCP 位姿"""
+        """查询当前 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)
+            # 获取当前 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}°')
+
+            # 图像坐标到相机坐标系转换
+            # 相机水平安装，但会随 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})')
 
-            self.get_logger().info(f'转换后相机坐标: ({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)