Fix cup uprighting auto execution

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,19 +228,21 @@ 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):
     run_node(YoloCupUprightingNode)
 
 
 if __name__ == "__main__":
-    main()
+    main()

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,17 +236,13 @@ 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,
     so each variant carries the scale needed to map corners back to the source
     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(