feat(right_arm): /detect/wrist con YOLO + SAM21 + centroide camera

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

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