diff --git a/src/presa_mano/api/right_arm_routes.py b/src/presa_mano/api/right_arm_routes.py index 8f07430..a59ec50 100644 --- a/src/presa_mano/api/right_arm_routes.py +++ b/src/presa_mano/api/right_arm_routes.py @@ -8,11 +8,15 @@ from fastapi import APIRouter, HTTPException from fastapi.responses import Response from pydantic import BaseModel +import numpy as np + from ..clients import G1Unavailable from ..config import update_config from ..geometry import quaternion_xyzw_to_rpy_deg +from ..perception.sam21_bridge import Sam21Fallback, segment_with_sam21 from ..timing import Stopwatch from . import _deps +from .perception_routes import _bbox_to_roi router = APIRouter() log = logging.getLogger(__name__) @@ -137,3 +141,49 @@ def detect_front(req: DetectFrontReq) -> dict[str, Any]: "image_size": [int(intr.get("width", 0)), int(intr.get("height", 0))], "timings_ms": sw.as_dict(), } + + +class DetectWristReq(BaseModel): + class_name: Optional[str] = None + conf: Optional[float] = None + + +def _detect_wrist_core(class_name: str | None, conf: float | None, sw: Stopwatch + ) -> dict[str, Any]: + """Funzione core riusata da /detect/wrist e da /move/toward.""" + nuwa = _deps.nuwa() + image_url = nuwa.rgb_url() + try: + with sw.step("yolo"): + yres = _deps.yolo().detect_url(image_url=image_url, conf=conf) + except Exception as e: + raise HTTPException(status_code=502, detail=f"YOLO unreachable: {e}") + top = _top_detection(yres, class_name) + if top is None: + raise HTTPException(status_code=404, detail=f"no '{class_name or 'object'}' detected") + bbox = _bbox_to_roi(top["bbox"]) + try: + seg = segment_with_sam21(nuwa, _deps.sam21(), bbox, sw) + except Sam21Fallback as e: + raise HTTPException(status_code=422, detail=f"SAM21 fallback: {e}") + pts = np.asarray(seg.points_cv_m, dtype=np.float64) + if pts.shape[0] < 10: + raise HTTPException(status_code=422, detail=f"too few sam21 points: {pts.shape[0]}") + with sw.step("centroid"): + centroid = pts.mean(axis=0) + distance = float(np.linalg.norm(centroid)) + return { + "detection": top, + "bbox": list(bbox), + "centroid_camera_m": [float(centroid[0]), float(centroid[1]), float(centroid[2])], + "distance_m": distance, + "n_sam21_points": int(pts.shape[0]), + } + + +@router.post("/detect/wrist") +def detect_wrist(req: DetectWristReq) -> dict[str, Any]: + sw = Stopwatch() + body = _detect_wrist_core(req.class_name, req.conf, sw) + body["timings_ms"] = sw.as_dict() + return body diff --git a/tests/test_right_arm_routes.py b/tests/test_right_arm_routes.py index 8d483aa..d23c71b 100644 --- a/tests/test_right_arm_routes.py +++ b/tests/test_right_arm_routes.py @@ -165,3 +165,64 @@ def test_detect_front_falls_back_to_proxy_url_on_yolo_failure(): assert len(attempts) == 2 assert "192.168" in attempts[0] assert attempts[1].endswith("/api/right_arm/snapshot/front") + + +def test_detect_wrist_returns_centroid_and_distance(): + # YOLO → bbox → SAM21 → centroide + import numpy as np + fake_points = np.array([ + [0.01, -0.02, 0.20], + [0.02, -0.01, 0.21], + [0.00, -0.03, 0.19], + ] * 5, dtype=np.float64) # 15 punti, > 10 minimo + + with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \ + patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \ + patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \ + patch("presa_mano.api.right_arm_routes._deps.sam21") as sam_f: + nuwa = MagicMock() + nuwa.rgb_url.return_value = "http://localhost:8877/api/rgb" + nuwa_f.return_value = nuwa + yolo = MagicMock() + yolo.detect_url.return_value = _fake_yolo_top() + yolo_f.return_value = yolo + sam_f.return_value = MagicMock() + seg = MagicMock() + seg.points_cv_m = fake_points + seg_f.return_value = seg + + r = client.post("/api/right_arm/detect/wrist", + json={"class_name": "pestello"}) + assert r.status_code == 200 + body = r.json() + assert body["detection"]["class"] == "pestello" + assert "bbox" in body + c = body["centroid_camera_m"] + assert abs(c[2] - 0.20) < 0.05 + assert body["distance_m"] > 0 + assert body["n_sam21_points"] == 15 + + +def test_detect_wrist_422_too_few_points(): + import numpy as np + with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \ + patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f, \ + patch("presa_mano.api.right_arm_routes.segment_with_sam21") as seg_f, \ + patch("presa_mano.api.right_arm_routes._deps.sam21") as sam_f: + nuwa = MagicMock(); nuwa.rgb_url.return_value = "http://x"; nuwa_f.return_value = nuwa + yolo = MagicMock(); yolo.detect_url.return_value = _fake_yolo_top(); yolo_f.return_value = yolo + sam_f.return_value = MagicMock() + seg = MagicMock(); seg.points_cv_m = np.zeros((3, 3)) + seg_f.return_value = seg + r = client.post("/api/right_arm/detect/wrist", + json={"class_name": "pestello"}) + assert r.status_code == 422 + + +def test_detect_wrist_404_when_yolo_no_match(): + with patch("presa_mano.api.right_arm_routes._deps.nuwa") as nuwa_f, \ + patch("presa_mano.api.right_arm_routes._deps.yolo") as yolo_f: + nuwa = MagicMock(); nuwa.rgb_url.return_value = "http://x"; nuwa_f.return_value = nuwa + yolo = MagicMock(); yolo.detect_url.return_value = {"detections": []}; yolo_f.return_value = yolo + r = client.post("/api/right_arm/detect/wrist", json={"class_name": "pestello"}) + assert r.status_code == 404