diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py index d79b21f..4379a59 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py @@ -169,8 +169,8 @@ def detect_and_pick(self, frame: np.ndarray): self.picking = True try: # feature 브랜치의 핵심 목표인 홈 복귀 시퀀스 직접 호출 - self._pick_and_return_home(bx, by, bz, cup_theta) - 诚然: + return self._pick_and_return_home(bx, by, bz, cup_theta) + finally: self.picking = False def _pick_and_return_home(self, bx, by, bz, cup_theta): @@ -205,19 +205,19 @@ def _pick_and_return_home(self, bx, by, bz, cup_theta): # 1-1 단계 실패 시 예외 처리 및 탈출 if not self.plan_pose(bx, by, safe_z, current_ori): log.error("[1-1] 상공 진입 실패. 시퀀스 중단.") - return + return False time.sleep(1.0) log.info("[1-2] 상공에서 파지 방향 정렬") if not self.plan_pose(bx, by, safe_z, target_ori): log.error("[1-2] 방향 정렬 실패. 시퀀스 중단.") - return + return False time.sleep(1.0) log.info("[2] 컵 파지 위치 하강") if not self.plan_pose(bx, by, pick_z, target_ori): log.error("[2] 파지 위치 하강 실패. 시퀀스 중단.") - return + return False self.gripper.close_gripper() log.info("[2] 그리퍼 클로즈 완료") @@ -228,14 +228,16 @@ def _pick_and_return_home(self, bx, by, bz, cup_theta): log.error("[3] 리프트업 실패. 물체 탈락 위험으로 인한 안전 복구 가동.") self.gripper.open_gripper() log.info("=> 그리퍼 비상 강제 릴리즈 완료.") - return + return False time.sleep(1.0) log.info("[4] 홈 위치로 복귀 (파지 유지)") if self.go_home_pose(): log.info("=> 홈 복귀 성공. 전체 구출 시퀀스 완수.") + return True else: log.error("=> [치명적] 파지는 완료했으나 관절 한계 혹은 충돌 궤적으로 인해 홈 복귀 실패.") + return False def main(args=None): @@ -243,4 +245,4 @@ def main(args=None): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/src/azas_perception/azas_perception/lid_marker.py b/src/azas_perception/azas_perception/lid_marker.py index 1dbbda4..aefcf69 100644 --- a/src/azas_perception/azas_perception/lid_marker.py +++ b/src/azas_perception/azas_perception/lid_marker.py @@ -144,13 +144,13 @@ def detect_aruco_marker( gray = cv2.cvtColor(patch, cv2.COLOR_BGR2GRAY) dictionary = _create_aruco_dictionary(dictionary_id) -<<<<<<< refactor/rg2_collision2 if dictionary is None: return None parameters = _create_aruco_detector_parameters() # The lid marker appears small and oblique in the wrist-camera view. Try - # conservative contrast/scale variants while keeping dictionary/id strict. + # conservative contrast/scale variants, but keep the dictionary/id filter + # strict so a noisy table feature cannot become a false lid marker. best: ArucoMarker | None = None best_score = -1.0 for candidate_gray, scale in _aruco_detection_images(gray): @@ -159,17 +159,6 @@ def detect_aruco_marker( dictionary, parameters, ) -======= - parameters = _create_aruco_detector_parameters() - - # The lid marker appears small and oblique in the wrist-camera view. Try - # conservative contrast/scale variants, but keep the dictionary/id filter - # strict so a noisy table feature cannot become a false lid marker. - best: ArucoMarker | None = None - best_score = -1.0 - for candidate_gray, scale in _aruco_detection_images(gray): - corners_list, ids, _rejected = _detect_aruco_markers(candidate_gray, dictionary, parameters) ->>>>>>> develop candidate = _select_aruco_marker_from_detections( corners_list, ids, @@ -184,18 +173,14 @@ def detect_aruco_marker( def _aruco_dictionary_id(dictionary_name: str) -> int | None: -<<<<<<< refactor/rg2_collision2 aruco = getattr(cv2, "aruco", None) if aruco is None: return None -======= ->>>>>>> develop name = str(dictionary_name).strip().upper() if not name: return None if not name.startswith("DICT_"): name = f"DICT_{name}" -<<<<<<< refactor/rg2_collision2 return getattr(aruco, name, None) @@ -220,33 +205,16 @@ def _create_aruco_detector_parameters(): parameters = aruco.DetectorParameters_create() else: return None -======= - return getattr(cv2.aruco, name, None) - - -def _create_aruco_dictionary(dictionary_id: int): - if hasattr(cv2.aruco, "getPredefinedDictionary"): - return cv2.aruco.getPredefinedDictionary(dictionary_id) - return cv2.aruco.Dictionary_get(dictionary_id) - - -def _create_aruco_detector_parameters(): - if hasattr(cv2.aruco, "DetectorParameters"): - parameters = cv2.aruco.DetectorParameters() - else: - parameters = cv2.aruco.DetectorParameters_create() ->>>>>>> develop return _tune_lid_aruco_detector_parameters(parameters) def _tune_lid_aruco_detector_parameters(parameters): -<<<<<<< refactor/rg2_collision2 aruco = getattr(cv2, "aruco", None) -======= # The lid marker is small in the wrist-camera overview image and often seen # at an angle. Keep the expected marker-id filter strict, but make candidate # extraction and perspective sampling tolerant enough for the measured setup. ->>>>>>> develop + if parameters is None: + return None tuned_values = { "adaptiveThreshWinSizeMin": 3, "adaptiveThreshWinSizeMax": 53, @@ -258,11 +226,7 @@ def _tune_lid_aruco_detector_parameters(parameters): "perspectiveRemovePixelPerCell": 8, "perspectiveRemoveIgnoredMarginPerCell": 0.20, "errorCorrectionRate": 0.75, -<<<<<<< refactor/rg2_collision2 "cornerRefinementMethod": getattr(aruco, "CORNER_REFINE_SUBPIX", 1), -======= - "cornerRefinementMethod": getattr(cv2.aruco, "CORNER_REFINE_SUBPIX", 1), ->>>>>>> develop "cornerRefinementWinSize": 3, } for name, value in tuned_values.items(): @@ -272,9 +236,6 @@ def _tune_lid_aruco_detector_parameters(parameters): def _aruco_detection_images(gray: np.ndarray) -> list[tuple[np.ndarray, float]]: -<<<<<<< refactor/rg2_collision2 - """Return grayscale variants for small/low-contrast lid ArUco detection.""" -======= """Return grayscale variants for small/low-contrast lid ArUco detection. OpenCV returns corners in the coordinate system of the image it receives, @@ -282,7 +243,6 @@ def _aruco_detection_images(gray: np.ndarray) -> list[tuple[np.ndarray, float]]: ROI. Variants are intentionally limited to deterministic contrast/scale transforms; no dictionary or marker-id relaxation is performed. """ ->>>>>>> develop variants: list[tuple[np.ndarray, float]] = [(gray, 1.0)] equalized = cv2.equalizeHist(gray) variants.append((equalized, 1.0)) @@ -291,23 +251,17 @@ def _aruco_detection_images(gray: np.ndarray) -> list[tuple[np.ndarray, float]]: sharpened = cv2.addWeighted(gray, 1.6, blur, -0.6, 0) variants.append((sharpened, 1.0)) -<<<<<<< refactor/rg2_collision2 + # Upscaling materially helps when the marker body is only a few tens of + # pixels wide in the RealSense overview frame. for source in (gray, equalized, sharpened): variants.append(( cv2.resize(source, None, fx=2.0, fy=2.0, interpolation=cv2.INTER_CUBIC), 2.0, )) -======= - # Upscaling materially helps when the marker body is only a few tens of - # pixels wide in the RealSense overview frame. - for source in (gray, equalized, sharpened): - variants.append((cv2.resize(source, None, fx=2.0, fy=2.0, interpolation=cv2.INTER_CUBIC), 2.0)) ->>>>>>> develop return variants def _detect_aruco_markers(gray: np.ndarray, dictionary, parameters): -<<<<<<< refactor/rg2_collision2 aruco = getattr(cv2, "aruco", None) if aruco is None: return [], None, [] @@ -318,12 +272,6 @@ def _detect_aruco_markers(gray: np.ndarray, dictionary, parameters): kwargs = {"parameters": parameters} if parameters is not None else {} return aruco.detectMarkers(gray, dictionary, **kwargs) return [], None, [] -======= - if hasattr(cv2.aruco, "ArucoDetector"): - detector = cv2.aruco.ArucoDetector(dictionary, parameters) - return detector.detectMarkers(gray) - return cv2.aruco.detectMarkers(gray, dictionary, parameters=parameters) ->>>>>>> develop def _select_aruco_marker_from_detections(