diff --git a/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py b/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py index 92ca37f..ed30a3a 100755 --- a/workspace/src/algorithm_pkg/algorithm_pkg/vision_node.py +++ b/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( - closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + contours, hierarchy = cv2.findContours( + 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) - 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) + if not _is_rect(cnt): + continue + # 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,17 +324,18 @@ class VisionNode(Node): [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 not np.isnan(imgpts).any(): + 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) # Compute and publish normalized measurement + reference if laser_world is not None: @@ -312,10 +353,13 @@ class VisionNode(Node): 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) + 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), 2, + (0, 0, 255), -1) else: # PnP failed — still draw detected corners corners_scaled = (ordered_corners * scale_up).astype(np.int32)