feat: add PnP laser solving and update 0xA5 frame to 4-float protocol

- Integrate PnP rectangle pose estimation + laser world coordinate computation from scripts/calibrate/laser_pnp.py - Publish normalized measurement (laser/RECT) and reference (rect center) via Float32MultiArray - Update gimbal 0xA5 frame to 18-byte format: measure_x, ref_x, measure_y, ref_y - Add camera calibration, laser line params, and RECT_L/W constants - Add corner ordering (TL/BL/BR/TR), SOLVEPNP_IPPE, and pose/laser visualization overlay - Remove standalone vision_example.py (superseded by ROS2 nodes) - Add scripts/calibrate/ with calibration and PnP tools
2026-06-03 22:38:51 +08:00
parent 0340d40b84
commit fd3d6b7ab5
10 changed files with 2549 additions and 81 deletions
@@ -5,12 +5,36 @@ import cv2
 import rclpy
 from rclpy.node import Node
 from sensor_msgs.msg import Image
-from geometry_msgs.msg import Point
+from std_msgs.msg import Float32MultiArray
 from cv_bridge import CvBridge

-# 配置项（与 vision_example.py 保持一致）
+# Camera calibration (from calibrate.py)
+K = np.array([
+    [1247.0565667653996, 0.0, 753.8809622002652],
+    [0.0, 1256.6168638051222, 528.9954978706462],
+    [0.0, 0.0, 1.0]
+])
+DIST = np.array([[0.0, 0.0, 0.0, 0.0, 0.0]])
+
+# Laser line parameters (from laser_calibrate.py)
+D_CAM = np.array([-0.00989344622939825, 0.015559548512101173, 0.9998300075531006])
+O_CAM = np.array([-26.1895751953125, -3.379796028137207, 0.0])
+
+# Rectangle physical dimensions (mm)
+RECT_L = 305.0
+RECT_W = 215.0
+if RECT_W > RECT_L:
+    RECT_L, RECT_W = RECT_W, RECT_L
+
+# Image preprocessing
+IMAGE_SCALE = 0.5
 IMAGE_AREA = 1080 * 1440 / 4
-THRESHOLD_AREA = IMAGE_AREA * 0.01
+MIN_AREA_RATIO = 0.01
+THRESHOLD_AREA = IMAGE_AREA * MIN_AREA_RATIO
+
+# Reference values (center of rectangle, normalized)
+REF_X = (RECT_L / 2.0) / RECT_L  # 0.5
+REF_Y = (RECT_W / 2.0) / RECT_W  # 0.5


 class VisionNode(Node):
@@ -18,6 +42,13 @@ class VisionNode(Node):
        super().__init__('vision_node')
        self._bridge = CvBridge()

+        # Scale intrinsics to match IMAGE_SCALE
+        self._K_scaled = K.copy()
+        self._K_scaled[0, 0] *= IMAGE_SCALE
+        self._K_scaled[1, 1] *= IMAGE_SCALE
+        self._K_scaled[0, 2] *= IMAGE_SCALE
+        self._K_scaled[1, 2] *= IMAGE_SCALE
+
        self._subscription = self.create_subscription(
            Image, '/camera/image_raw', self._image_callback, 10
        )
@@ -28,7 +59,7 @@ class VisionNode(Node):
            Image, '/vision/binary_image', 10
        )
        self._target_publisher = self.create_publisher(
-            Point, '/vision/target', 10
+            Float32MultiArray, '/vision/target', 10
        )

        # GPU acceleration setup
@@ -70,6 +101,93 @@ class VisionNode(Node):
        self._gaussian_filter.apply(gpu_gray, self._gpu_blurred)
        return self._gpu_blurred.download()

+    def _detect_rectangle(self, blurred):
+        """Otsu threshold + morphology close + contour fitting.
+
+        Returns ordered corners (4, 2) float32 or None.
+        """
+        _, binary = cv2.threshold(
+            blurred, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
+        kernel = np.ones((3, 3), np.uint8)
+        closed = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel)
+
+        contours, _ = cv2.findContours(
+            closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+        best = None
+        best_area = 0
+        for cnt in contours:
+            area = cv2.contourArea(cnt)
+            if area < THRESHOLD_AREA:
+                continue
+            epsilon = 0.02 * cv2.arcLength(cnt, True)
+            approx = cv2.approxPolyDP(cnt, epsilon, True)
+            if len(approx) == 4 and cv2.isContourConvex(approx):
+                if area > best_area:
+                    best_area = area
+                    best = approx.reshape(4, 2)
+
+        return best.astype(np.float32) if best is not None else None, closed
+
+    def _order_corners(self, image_corners):
+        """Order corners as TL, BL, BR, TR and build world points."""
+        pts = image_corners.copy()
+
+        edge_info = []
+        for i in range(4):
+            j = (i + 1) % 4
+            edge_info.append((i, j, np.linalg.norm(pts[i] - pts[j])))
+
+        edge_info.sort(key=lambda e: e[2])
+        short_a, short_b = edge_info[0], edge_info[1]
+
+        def mid_x(edge):
+            i, j, _ = edge
+            return (pts[i][0] + pts[j][0]) / 2.0
+
+        if mid_x(short_a) < mid_x(short_b):
+            left_edge, right_edge = short_a, short_b
+        else:
+            left_edge, right_edge = short_b, short_a
+
+        li, lj, _ = left_edge
+        if pts[li][1] < pts[lj][1]:
+            TL_idx, BL_idx = li, lj
+        else:
+            TL_idx, BL_idx = lj, li
+
+        ri, rj, _ = right_edge
+        if pts[ri][1] < pts[rj][1]:
+            TR_idx, BR_idx = ri, rj
+        else:
+            TR_idx, BR_idx = rj, ri
+
+        ordered = pts[[TL_idx, BL_idx, BR_idx, TR_idx]]
+        world = np.array([
+            [0, 0],
+            [0, RECT_W],
+            [RECT_L, RECT_W],
+            [RECT_L, 0],
+        ], dtype=np.float32)
+
+        return ordered, world
+
+    def _compute_laser_world(self, rvec, tvec):
+        """Compute laser point world coordinates on Z=0 plane."""
+        R, _ = cv2.Rodrigues(rvec)
+        R_inv = R.T
+        t = tvec.ravel()
+
+        d_world = R_inv @ D_CAM
+        O_world = R_inv @ (O_CAM - t)
+
+        if abs(d_world[2]) < 1e-10:
+            return None
+
+        s = -O_world[2] / d_world[2]
+        P_world = O_world + s * d_world
+        return P_world[:2]
+
    def _image_callback(self, msg):
        try:
            encoding = msg.encoding
@@ -81,87 +199,126 @@ class VisionNode(Node):

            gauss_image = self._preprocess(cv_image)

-            # Otsu 二值化
-            _, binary = cv2.threshold(
-                gauss_image, 0, 255,
-                cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
+            corners, binary = self._detect_rectangle(gauss_image)
+            if corners is None:
+                # Publish binary image even when no rectangle found
+                binary_color = cv2.cvtColor(binary, cv2.COLOR_GRAY2BGR)
+                binary_msg = self._bridge.cv2_to_imgmsg(
+                    binary_color, encoding='bgr8')
+                binary_msg.header.stamp = msg.header.stamp
+                binary_msg.header.frame_id = msg.header.frame_id
+                self._binary_publisher.publish(binary_msg)

-            # 形态学闭运算
-            kernel = np.ones((3, 3), np.uint8)
-            dilated = cv2.dilate(binary, kernel, iterations=1)
-            eroded = cv2.erode(dilated, kernel, iterations=1)
+                # Publish result image with FPS overlay
+                result_image = cv_image.copy()
+                fps = cv2.getTickFrequency() / max(
+                    cv2.getTickCount() - self.start_time, 1)
+                cv2.putText(result_image, f'FPS: {fps:.2f}', (10, 30),
+                            cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
+                self.start_time = cv2.getTickCount()
+                result_msg = self._bridge.cv2_to_imgmsg(
+                    result_image, encoding='bgr8')
+                result_msg.header.stamp = msg.header.stamp
+                result_msg.header.frame_id = msg.header.frame_id
+                self._result_publisher.publish(result_msg)
+                return

-            # 查找轮廓
-            contours, _ = cv2.findContours(
-                eroded, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE
+            ordered_corners, world_pts = self._order_corners(corners)
+            world_pts_3d = np.hstack(
+                [world_pts, np.zeros((4, 1), dtype=np.float32)])
+
+            success, rvec, tvec = cv2.solvePnP(
+                world_pts_3d, ordered_corners, self._K_scaled, DIST,
+                flags=cv2.SOLVEPNP_IPPE
            )

            result_image = cv_image.copy()
-            best_target = None
-            max_area = 0
-            for cnt in contours:
-                # 绘制原始轮廓，调试用
-                cnt_scaled = (cnt / 0.5).astype(np.int32)
-                cv2.drawContours(result_image, [cnt_scaled], -1, (0, 255, 0), 1)
+            scale_up = 1.0 / IMAGE_SCALE

-                # 面积过滤
-                area = cv2.contourArea(cnt)
-                if area < THRESHOLD_AREA:
-                    continue
+            if success:
+                laser_world = self._compute_laser_world(rvec, tvec)

-                # 形状近似：四边形 + 凸多边形
-                epsilon = 0.02 * cv2.arcLength(cnt, True)
-                approx = cv2.approxPolyDP(cnt, epsilon, True)
+                # Draw ordered corners (scaled up to original resolution)
+                corners_scaled = (ordered_corners * scale_up).astype(np.int32)
+                labels = ["TL", "BL", "BR", "TR"]
+                colors = [(0, 0, 255), (255, 0, 0), (0, 255, 0), (255, 255, 0)]
+                for i, (pt, label) in enumerate(zip(corners_scaled, labels)):
+                    cv2.circle(result_image, tuple(pt), 6, colors[i], -1)
+                    cv2.putText(result_image, f"{label}", (pt[0] + 10, pt[1] - 10),
+                                cv2.FONT_HERSHEY_SIMPLEX, 0.6, colors[i], 2)

-                if len(approx) == 4 and cv2.isContourConvex(approx):
-                    # 坐标映射回原始分辨率
-                    approx_scaled = (approx / 0.5).astype(np.int32)
-                    cv2.drawContours(result_image, [approx_scaled], -1,
-                                     (0, 0, 255), 2)
-                    for point in approx_scaled:
-                        cv2.circle(result_image, tuple(point[0]), 5,
-                                   (255, 0, 0), -1)
+                for i in range(4):
+                    j = (i + 1) % 4
+                    cv2.line(result_image, tuple(corners_scaled[i]),
+                             tuple(corners_scaled[j]), (255, 255, 255), 2)

-                    if area > max_area:
-                        max_area = area
-                        best_target = approx
+                # Draw axis
+                axis_len = 50
+                axis_3d = np.float32(
+                    [[0, 0, 0], [axis_len, 0, 0], [0, axis_len, 0],
+                     [0, 0, axis_len]])
+                imgpts, _ = cv2.projectPoints(
+                    axis_3d, rvec, tvec, self._K_scaled, DIST)
+                origin = tuple(
+                    (imgpts[0].ravel() * scale_up).astype(int))
+                cv2.line(result_image, origin,
+                         tuple((imgpts[1].ravel() * scale_up).astype(int)),
+                         (0, 0, 255), 3)
+                cv2.line(result_image, origin,
+                         tuple((imgpts[2].ravel() * scale_up).astype(int)),
+                         (0, 255, 0), 3)
+                cv2.line(result_image, origin,
+                         tuple((imgpts[3].ravel() * scale_up).astype(int)),
+                         (255, 0, 0), 3)

-            if best_target is not None:
-                moments = cv2.moments(best_target)
-                if moments['m00'] > 0:
-                    cx = moments['m10'] / moments['m00']
-                    cy = moments['m01'] / moments['m00']
-                    h, w = gauss_image.shape[:2]
-                    error_x = (cx - w / 2) / (w / 2)
-                    error_y = (cy - h / 2) / (h / 2)
+                # Compute and publish normalized measurement + reference
+                if laser_world is not None:
+                    measure_x = float(laser_world[0] / RECT_L)
+                    measure_y = float(laser_world[1] / RECT_W)

-                    target_msg = Point()
-                    target_msg.x = error_x
-                    target_msg.y = -error_y
-                    target_msg.z = 0.0
+                    target_msg = Float32MultiArray()
+                    target_msg.data = [
+                        measure_x, REF_X, measure_y, REF_Y]
                    self._target_publisher.publish(target_msg)

-            # FPS 叠加
-            fps = cv2.getTickFrequency() / (cv2.getTickCount() - self.start_time)
-            cv2.putText(
-                result_image, f'FPS: {fps:.2f}',
-                (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2
-            )
+                    # Draw laser point (project to image)
+                    P_world_3d = np.array(
+                        [[laser_world[0], laser_world[1], 0]],
+                        dtype=np.float32)
+                    px_proj, _ = cv2.projectPoints(
+                        P_world_3d, rvec, tvec, self._K_scaled, DIST)
+                    lx = int(px_proj.ravel()[0] * scale_up)
+                    ly = int(px_proj.ravel()[1] * scale_up)
+                    cv2.circle(result_image, (lx, ly), 10, (0, 0, 255), 2)
+                    cv2.circle(result_image, (lx, ly), 2, (0, 0, 255), -1)
+            else:
+                # PnP failed — still draw detected corners
+                corners_scaled = (ordered_corners * scale_up).astype(np.int32)
+                for pt in corners_scaled:
+                    cv2.circle(result_image, tuple(pt), 6, (0, 165, 255), -1)
+                for i in range(4):
+                    j = (i + 1) % 4
+                    cv2.line(result_image, tuple(corners_scaled[i]),
+                             tuple(corners_scaled[j]), (0, 165, 255), 2)
+
+            # FPS overlay
+            fps = cv2.getTickFrequency() / max(
+                cv2.getTickCount() - self.start_time, 1)
+            cv2.putText(result_image, f'FPS: {fps:.2f}', (10, 30),
+                        cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
            self.start_time = cv2.getTickCount()

-            # 发布结果图像（带轮廓标注）
+            # Publish result image
            result_msg = self._bridge.cv2_to_imgmsg(
-                result_image, encoding='bgr8'
-            )
+                result_image, encoding='bgr8')
            result_msg.header.stamp = msg.header.stamp
            result_msg.header.frame_id = msg.header.frame_id
            self._result_publisher.publish(result_msg)

-            # 发布二值化图像（调试用）
-            binary_color = cv2.cvtColor(eroded, cv2.COLOR_GRAY2BGR)
+            # Publish binary image
+            binary_color = cv2.cvtColor(binary, cv2.COLOR_GRAY2BGR)
            binary_msg = self._bridge.cv2_to_imgmsg(
-                binary_color, encoding='bgr8'
-            )
+                binary_color, encoding='bgr8')
            binary_msg.header.stamp = msg.header.stamp
            binary_msg.header.frame_id = msg.header.frame_id
            self._binary_publisher.publish(binary_msg)
@@ -5,8 +5,8 @@ import json
 import serial
 import rclpy
 from rclpy.node import Node
-from geometry_msgs.msg import Twist, Point
-from std_msgs.msg import String
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float32MultiArray, String

 CMD_AUTO = 0x01
 CMD_MANUAL = 0x02
@@ -42,7 +42,7 @@ class GimbalNode(Node):
            Twist, '/cmd_gimbal', self._cmd_gimbal_callback, 10
        )
        self._target_sub = self.create_subscription(
-            Point, '/vision/target', self._target_callback, 10
+            Float32MultiArray, '/vision/target', self._target_callback, 10
        )
        self._pid_config_sub = self.create_subscription(
            String, '/pid_config', self._pid_config_callback, 10
@@ -80,15 +80,22 @@ class GimbalNode(Node):
        data.append(_checksum(data))
        self._write_frame(data)

-    def _target_callback(self, msg: Point):
+    def _target_callback(self, msg: Float32MultiArray):
        if self._current_mode != 'auto':
            return
+        if len(msg.data) < 4:
+            self.get_logger().error(
+                f'Target data requires 4 values, got {len(msg.data)}')
+            return
+        measure_x, ref_x, measure_y, ref_y = msg.data[:4]
        self.get_logger().debug(
-            f'Auto tracking: error_x={msg.x:.4f}, error_y={msg.y:.4f}'
+            f'Auto tracking: measure=({measure_x:.4f},{measure_y:.4f}) '
+            f'ref=({ref_x:.4f},{ref_y:.4f})'
        )
        data = bytearray()
        data.append(0xA5)
-        data.extend(struct.pack('<ff', float(msg.x), float(msg.y)))
+        data.extend(struct.pack(
+            '<ffff', measure_x, ref_x, measure_y, ref_y))
        data.append(_checksum(data))
        self._write_frame(data)