feat: add /vision/debug_image topic, inner-rect filter, and NaN guards

- Publish binary+contour debug image to /vision/debug_image
- Require nested rectangular contour with no deeper children
- Add NaN guards after solvePnP and projectPoints to prevent crashes

workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py

@@ -91,6 +91,9 @@ class VisionNode(Node):
         self._target_publisher = self.create_publisher(
             Float32MultiArray, '/vision/target', 10
         )
+        self._debug_publisher = self.create_publisher(
+            Image, '/vision/debug_image', 10
+        )
 
         # GPU acceleration setup
         self._use_cuda = False
@@ -144,23 +147,48 @@ class VisionNode(Node):
         kernel = np.ones((3, 3), np.uint8)
         closed = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel)
 
-        contours, _ = cv2.findContours(
+        contours, hierarchy = cv2.findContours(
-            closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+            closed, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+
+        def _is_rect(ct):
+            ep = 0.02 * cv2.arcLength(ct, True)
+            ap = cv2.approxPolyDP(ct, ep, True)
+            return len(ap) == 4 and cv2.isContourConvex(ap)
 
         best = None
         best_area = 0
-        for cnt in contours:
+        best_inner_idx = -1
+        for i, cnt in enumerate(contours):
             area = cv2.contourArea(cnt)
             if area < THRESHOLD_AREA:
                 continue
-            epsilon = 0.02 * cv2.arcLength(cnt, True)
+            if not _is_rect(cnt):
-            approx = cv2.approxPolyDP(cnt, epsilon, True)
+                continue
-            if len(approx) == 4 and cv2.isContourConvex(approx):
+            # Require an inner rectangular child contour
+            child_idx = hierarchy[0][i][2]
+            has_inner_rect = False
+            while child_idx != -1:
+                if _is_rect(contours[child_idx]) and hierarchy[0][child_idx][2] == -1:
+                    has_inner_rect = True
+                    break
+                child_idx = hierarchy[0][child_idx][0]  # next sibling
+            if not has_inner_rect:
+                continue
+            approx = cv2.approxPolyDP(cnt, 0.02 * cv2.arcLength(cnt, True), True)
             if area > best_area:
                 best_area = area
                 best = approx.reshape(4, 2)
+                best_inner_idx = child_idx
 
-        return best.astype(np.float32) if best is not None else None
+        # Draw all contours on closed for debugging
+        vis = cv2.cvtColor(closed, cv2.COLOR_GRAY2BGR)
+        cv2.drawContours(vis, contours, -1, (0, 255, 0), 1)
+        if best is not None:
+            cv2.drawContours(vis, [best.astype(np.int32)], -1, (0, 0, 255), 2)
+            if best_inner_idx != -1:
+                cv2.drawContours(vis, [contours[best_inner_idx]], -1, (255, 0, 0), 2)
+
+        return (best.astype(np.float32) if best is not None else None, vis)
 
     def _order_corners(self, image_corners):
         """Order corners as TL, BL, BR, TR and build world points."""
@@ -232,7 +260,16 @@ class VisionNode(Node):
 
             gauss_image = self._preprocess(cv_image)
 
-            corners = self._detect_rectangle(gauss_image)
+            corners, debug_image = self._detect_rectangle(gauss_image)
+
+            # Publish binary debug image at processing resolution
+            if debug_image is not None:
+                debug_msg = self._bridge.cv2_to_imgmsg(
+                    debug_image, encoding='bgr8')
+                debug_msg.header.stamp = msg.header.stamp
+                debug_msg.header.frame_id = msg.header.frame_id
+                self._debug_publisher.publish(debug_msg)
+
             if corners is None:
                 # Publish result image with FPS overlay
                 result_image = cv_image.copy()
@@ -257,6 +294,9 @@ class VisionNode(Node):
                 flags=cv2.SOLVEPNP_IPPE
             )
 
+            if success and (np.isnan(rvec).any() or np.isnan(tvec).any()):
+                success = False
+
             result_image = cv_image.copy()
             scale_up = 1.0 / IMAGE_SCALE
 
@@ -284,6 +324,7 @@ class VisionNode(Node):
                      [0, 0, axis_len]])
                 imgpts, _ = cv2.projectPoints(
                     axis_3d, rvec, tvec, self._K_scaled, DIST)
+                if not np.isnan(imgpts).any():
                     origin = tuple(
                         (imgpts[0].ravel() * scale_up).astype(int))
                     cv2.line(result_image, origin,
@@ -312,10 +353,13 @@ class VisionNode(Node):
                         dtype=np.float32)
                     px_proj, _ = cv2.projectPoints(
                         P_world_3d, rvec, tvec, self._K_scaled, DIST)
+                    if not np.isnan(px_proj).any():
                         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), 10,
-                    cv2.circle(result_image, (lx, ly), 2, (0, 0, 255), -1)
+                                   (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)