feat: add PnP pose estimation, laser world coordinate solving, and new 0xA5 protocol

- Replace hardcoded arrays with JSON string format for calibration and laser
  parameters, matching scripts/calibrate/laser_pnp.py
- Apply IMAGE_SCALE to resize factor instead of hardcoded 0.5
- Add rectangle corner ordering (TL/BL/BR/TR) and PnP solving with SOLVEPNP_IPPE
- Compute laser world coordinates on Z=0 plane, normalize by RECT_L/RECT_W
  as measurement values; reference values set to rectangle center (0.5, 0.5)
- Publish Float32MultiArray [measure_x, ref_x, measure_y, ref_y] on /vision/target
- Remove /vision/binary_image publisher
- Add ENABLE_CUDA toggle and CUDA detection log with pipeline breakdown

workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import json
 import numpy as np
 import cv2
 
@@ -8,17 +9,28 @@ from sensor_msgs.msg import Image
 from std_msgs.msg import Float32MultiArray
 from cv_bridge import CvBridge
 
-# Camera calibration (from calibrate.py)
+# Camera calibration (paste from calibrate.py JSON output)
-K = np.array([
+CALIB_JSON = """
+{
+  "mtx": [
     [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]])
+  "dist": [[0.0, 0.0, 0.0, 0.0, 0.0]],
+  "img_size": [1440, 1080],
+  "square_size": 15.0,
+  "error": 0.021690239575344137
+}
+"""
 
-# Laser line parameters (from laser_calibrate.py)
+# Laser line parameters (paste from laser_calibrate.py JSON output)
-D_CAM = np.array([-0.00989344622939825, 0.015559548512101173, 0.9998300075531006])
+LASER_JSON = """
-O_CAM = np.array([-26.1895751953125, -3.379796028137207, 0.0])
+{
+  "direction": [-0.00989344622939825, 0.015559548512101173, 0.9998300075531006],
+  "origin": [-26.1895751953125, -3.379796028137207, 0.0]
+}
+"""
 
 # Rectangle physical dimensions (mm)
 RECT_L = 305.0
@@ -26,9 +38,12 @@ RECT_W = 215.0
 if RECT_W > RECT_L:
     RECT_L, RECT_W = RECT_W, RECT_L
 
+# CUDA switch — set to False to force CPU pipeline
+ENABLE_CUDA = False
+
 # Image preprocessing
 IMAGE_SCALE = 0.5
-IMAGE_AREA = 1080 * 1440 / 4
+IMAGE_AREA = 1080 * 1440 * IMAGE_SCALE * IMAGE_SCALE
 MIN_AREA_RATIO = 0.01
 THRESHOLD_AREA = IMAGE_AREA * MIN_AREA_RATIO
 
@@ -37,6 +52,24 @@ REF_X = (RECT_L / 2.0) / RECT_L  # 0.5
 REF_Y = (RECT_W / 2.0) / RECT_W  # 0.5
 
 
+def _parse_calib():
+    data = json.loads(CALIB_JSON)
+    mtx = np.array(data["mtx"])
+    dist = np.array(data["dist"])
+    return mtx, dist
+
+
+def _parse_laser():
+    data = json.loads(LASER_JSON)
+    d = np.array(data["direction"], dtype=np.float64)
+    o = np.array(data["origin"], dtype=np.float64)
+    return d, o
+
+
+K, DIST = _parse_calib()
+D_CAM, O_CAM = _parse_laser()
+
+
 class VisionNode(Node):
     def __init__(self):
         super().__init__('vision_node')
@@ -55,38 +88,38 @@ class VisionNode(Node):
         self._result_publisher = self.create_publisher(
             Image, '/vision/result_image', 10
         )
-        self._binary_publisher = self.create_publisher(
-            Image, '/vision/binary_image', 10
-        )
         self._target_publisher = self.create_publisher(
             Float32MultiArray, '/vision/target', 10
         )
 
         # GPU acceleration setup
         self._use_cuda = False
-        try:
+        if ENABLE_CUDA:
-            if cv2.cuda.getCudaEnabledDeviceCount() > 0:
+            try:
-                self._use_cuda = True
+                if cv2.cuda.getCudaEnabledDeviceCount() > 0:
-                self._gpu_frame = cv2.cuda_GpuMat()
+                    self._use_cuda = True
-                self._gpu_blurred = cv2.cuda_GpuMat()
+                    self._gpu_frame = cv2.cuda_GpuMat()
-                self._gaussian_filter = cv2.cuda.createGaussianFilter(
+                    self._gpu_blurred = cv2.cuda_GpuMat()
-                    cv2.CV_8UC1, cv2.CV_8UC1, (5, 5), 1)
+                    self._gaussian_filter = cv2.cuda.createGaussianFilter(
-        except AttributeError:
+                        cv2.CV_8UC1, cv2.CV_8UC1, (5, 5), 1)
-            pass
+                    self.get_logger().info(
+                        'CUDA GPU detected — '
+                        'resize + cvtColor + GaussianBlur on GPU, '
+                        'threshold + morphology + PnP on CPU')
+            except AttributeError:
+                pass
 
         self.get_logger().info(
-            'VisionNode ready - %s, subscribing to /camera/image_raw'
+            'VisionNode ready, subscribing to /camera/image_raw')
-            % ('CUDA GPU acceleration enabled'
-               if self._use_cuda else 'CPU pipeline')
-        )
 
         self.start_time = 0
 
     def _preprocess(self, cv_image):
-        """Return blurred grayscale at half resolution."""
+        """Return blurred grayscale at IMAGE_SCALE resolution."""
         if self._use_cuda:
             return self._gpu_preprocess(cv_image)
-        scaled = cv2.resize(cv_image, None, fx=0.5, fy=0.5,
+        scaled = cv2.resize(cv_image, None,
+                            fx=IMAGE_SCALE, fy=IMAGE_SCALE,
                             interpolation=cv2.INTER_AREA)
         gray = cv2.cvtColor(scaled, cv2.COLOR_BGR2GRAY)
         return cv2.GaussianBlur(gray, (5, 5), 1)
@@ -94,7 +127,7 @@ class VisionNode(Node):
     def _gpu_preprocess(self, cv_image):
         self._gpu_frame.upload(cv_image)
         gpu_scaled = cv2.cuda.resize(
-            self._gpu_frame, None, fx=0.5, fy=0.5,
+            self._gpu_frame, None, fx=IMAGE_SCALE, fy=IMAGE_SCALE,
             interpolation=cv2.INTER_AREA)
         gpu_gray = cv2.cuda.cvtColor(gpu_scaled, cv2.COLOR_BGR2GRAY)
         self._gpu_blurred.create(gpu_gray.size(), gpu_gray.type())
@@ -104,7 +137,7 @@ class VisionNode(Node):
     def _detect_rectangle(self, blurred):
         """Otsu threshold + morphology close + contour fitting.
 
-        Returns ordered corners (4, 2) float32 or None.
+        Returns corners (4,2) float32 or None.
         """
         _, binary = cv2.threshold(
             blurred, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
@@ -127,7 +160,7 @@ class VisionNode(Node):
                     best_area = area
                     best = approx.reshape(4, 2)
 
-        return best.astype(np.float32) if best is not None else None, closed
+        return best.astype(np.float32) if best is not None else None
 
     def _order_corners(self, image_corners):
         """Order corners as TL, BL, BR, TR and build world points."""
@@ -199,16 +232,8 @@ class VisionNode(Node):
 
             gauss_image = self._preprocess(cv_image)
 
-            corners, binary = self._detect_rectangle(gauss_image)
+            corners = 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)
-
                 # Publish result image with FPS overlay
                 result_image = cv_image.copy()
                 fps = cv2.getTickFrequency() / max(
@@ -315,14 +340,6 @@ class VisionNode(Node):
             result_msg.header.frame_id = msg.header.frame_id
             self._result_publisher.publish(result_msg)
 
-            # Publish binary image
-            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)
-
         except Exception as e:
             self.get_logger().error(f'Vision processing error: {e}')