update pointing client side

lf-zhao · lf-zhao · commit a5ca5dd762a8 · 2025-11-07T16:29:14.000-05:00
diff --git a/predicators/spot_utils/perception/object_pointing.py b/predicators/spot_utils/perception/object_pointing.py
@@ -0,0 +1,199 @@
+"""High-level helpers for Gemini-based object pointing/detection."""
+
+from __future__ import annotations
+
+import argparse
+import logging
+from dataclasses import dataclass
+from pathlib import Path
+from typing import List, Optional, Sequence, Tuple, TypeVar, Union
+
+import numpy as np
+from PIL import Image
+
+from predicators.spot_utils.perception.pointing_gemini_sam2_client import \
+    PointingGeminiSAM2Client
+from predicators.spot_utils.perception.utils.gemini_parsing import \
+    denormalize_box, denormalize_point
+
+
+ImageLike = Union[str, Image.Image, np.ndarray]
+
+
+@dataclass
+class GeminiPointPrediction:
+    """Pixel selected by Gemini's pointing response."""
+    label: str
+    pixel: Tuple[int, int]
+
+
+@dataclass
+class GeminiBoxPrediction:
+    """Bounding-box detection returned by Gemini."""
+    label: str
+    box: Tuple[float, float, float, float]
+
+    @property
+    def centroid(self) -> Tuple[int, int]:
+        x1, y1, x2, y2 = self.box
+        return (int(round((x1 + x2) / 2.0)), int(round((y1 + y2) / 2.0)))
+
+
+@dataclass
+class GeminiPointingResult:
+    """Aggregated pointing/detection results for an image/prompt pair."""
+    image_index: int
+    prompt: str
+    points: List[GeminiPointPrediction]
+    boxes: List[GeminiBoxPrediction]
+    image_path: Optional[str] = None
+
+    def primary_pixel(self) -> Optional[Tuple[int, int]]:
+        """Return the best-guess pixel (points first, detection centroid second)."""
+        if self.points:
+            return self.points[0].pixel
+        if self.boxes:
+            return self.boxes[0].centroid
+        return None
+
+
+def _to_pil(image: ImageLike) -> Image.Image:
+    if isinstance(image, Image.Image):
+        return image
+    if isinstance(image, np.ndarray):
+        if image.dtype != np.uint8:
+            raise ValueError("numpy image must have dtype uint8")
+        if image.ndim == 2:
+            return Image.fromarray(image)
+        if image.ndim == 3:
+            return Image.fromarray(image)
+        raise ValueError(f"Unsupported ndarray shape {image.shape}")
+    if isinstance(image, str):
+        return Image.open(image)
+    raise TypeError(f"Unsupported image type: {type(image)}")
+
+
+T = TypeVar("T")
+
+
+def _coerce_sequence(value: Union[T, Sequence[T]]) -> List[T]:
+    if isinstance(value, (list, tuple)):
+        return list(value)
+    return [value]
+
+
+def _denormalize_points(points: List[dict], width: int,
+                        height: int) -> List[GeminiPointPrediction]:
+    results: List[GeminiPointPrediction] = []
+    for entry in points:
+        y_norm, x_norm = entry["point"]
+        y, x = denormalize_point([y_norm, x_norm], (width, height))
+        pixel = (int(round(x)), int(round(y)))
+        results.append(GeminiPointPrediction(entry.get("label", ""), pixel))
+    return results
+
+
+def _denormalize_boxes(boxes: List[List[float]], width: int, height: int,
+                       label: str) -> List[GeminiBoxPrediction]:
+    results: List[GeminiBoxPrediction] = []
+    for box in boxes:
+        x1, y1, x2, y2 = denormalize_box(box, (width, height))
+        results.append(GeminiBoxPrediction(label, (x1, y1, x2, y2)))
+    return results
+
+
+def point_objects_with_gemini(
+        images: Union[ImageLike, Sequence[ImageLike]],
+        prompts: Union[str, Sequence[str]],
+        *,
+        host: str = "localhost",
+        port: int = 7100,
+        detection: bool = False,
+        segmentation: bool = False) -> List[GeminiPointingResult]:
+    """Call the Gemini pointing service and parse the response."""
+    image_list = [_to_pil(img) for img in _coerce_sequence(images)]
+    prompt_list = [str(p) for p in _coerce_sequence(prompts)]
+    client = PointingGeminiSAM2Client(host=host, port=port)
+    response = client.predict(image_list,
+                              prompt_list,
+                              points=not detection,
+                              segmentation=segmentation,
+                              detection=detection)
+
+    raw_results = response.get("results", [])
+    parsed: List[GeminiPointingResult] = []
+    for entry in raw_results:
+        width = entry.get("image_width")
+        height = entry.get("image_height")
+        if width is None or height is None:
+            logging.warning("[GeminiPointing] Missing image size in response; "
+                            "skipping entry.")
+            continue
+        points_data = entry.get("points") or []
+        boxes_data = entry.get("boxes") or []
+        label = entry.get("prompt", "")
+        points = _denormalize_points(points_data, width, height)
+        boxes = _denormalize_boxes(boxes_data, width, height, label)
+        parsed.append(
+            GeminiPointingResult(
+                image_index=entry.get("image_index", 0),
+                prompt=entry.get("prompt", ""),
+                points=points,
+                boxes=boxes,
+                image_path=None,
+            ))
+    return parsed
+
+
+def point_single_image(
+        image: ImageLike,
+        prompt: str,
+        *,
+        host: str = "localhost",
+        port: int = 7100,
+        detection: bool = False) -> Optional[GeminiPointingResult]:
+    """Convenience wrapper for a single image/prompt pair."""
+    results = point_objects_with_gemini(image, prompt, host=host, port=port,
+                                        detection=detection)
+    return results[0] if results else None
+
+
+def _cli() -> None:
+    parser = argparse.ArgumentParser(description="Gemini object pointing CLI")
+    parser.add_argument("image",
+                        type=Path,
+                        help="Path to the image to analyze.")
+    parser.add_argument("prompt", type=str, help="Prompt describing the object.")
+    parser.add_argument("--host",
+                        default="localhost",
+                        help="Gemini pointing server host.")
+    parser.add_argument("--port",
+                        default=7100,
+                        type=int,
+                        help="Gemini pointing server port.")
+    parser.add_argument("--detection",
+                        action="store_true",
+                        help="Request detection boxes instead of direct points.")
+    args = parser.parse_args()
+
+    result = point_single_image(args.image,
+                                args.prompt,
+                                host=args.host,
+                                port=args.port,
+                                detection=args.detection)
+    if result is None:
+        print("No pointing result returned.")
+        return
+    print(f"Prompt: {result.prompt}")
+    if result.points:
+        print("Points:")
+        for point in result.points:
+            print(f"  {point.label}: {point.pixel}")
+    if result.boxes:
+        print("Detections:")
+        for box in result.boxes:
+            print(f"  {box.label}: {box.box} (centroid={box.centroid})")
+
+
+if __name__ == "__main__":  # pragma: no cover - CLI utility
+    _cli()
diff --git a/predicators/spot_utils/perception/pointing_gemini_sam2_client.py b/predicators/spot_utils/perception/pointing_gemini_sam2_client.py
@@ -145,10 +145,31 @@ async def predict_async(
                         },
                         timeout=120.0
                     )
-                    
+
                     progress.update(task, completed=True)
+
+                    if response.status_code != 200:
+                        raise RuntimeError(
+                            f"Pointing service returned status {response.status_code}:\n"
+                            f"{response.text}"
+                        )
+
                     result = response.json()
-                    console.print("[green]Server request completed successfully[/green]")
+                    entries = []
+                    count = "unknown"
+                    example = None
+                    if isinstance(result, dict):
+                        entries = result.get("results") or []
+                        count = len(entries)
+                        example = entries[0].get("prompt") if entries else None
+                        if not entries:
+                            logging.warning(
+                                "[PointingClient] No detections returned (HTTP 200)"
+                                "; check server logs for details.")
+                    extra = f", sample='{example}'" if example else ""
+                    console.print(
+                        f"[green]Server request completed successfully[/green]"
+                        f" (results={count}{extra})")
             timings["server_request"] = t.elapsed_time
 
         return {"results": result, "timings": timings}
@@ -235,4 +256,4 @@ def predict(
 
 
 if __name__ == "__main__":
-    app() 
+    app()
diff --git a/predicators/spot_utils/perception/vlm_pointing.py b/predicators/spot_utils/perception/vlm_pointing.py
@@ -0,0 +1,119 @@
+"""Adapter that proxies hand-view pointing requests to Gemini."""
+
+from __future__ import annotations
+
+import logging
+import time
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Dict, Optional, Tuple
+
+from bosdyn.client import math_helpers
+from PIL import Image, ImageDraw
+
+from predicators.settings import CFG
+from predicators.spot_utils.perception.object_pointing import \
+    GeminiPointingResult, point_single_image
+from predicators.spot_utils.perception.perception_structs import \
+    ObjectDetectionID, RGBDImageWithContext
+from predicators.structs import Object
+
+_POINTING_SERVICE_AVAILABLE = True
+_POINTING_WARNING_EMITTED = False
+
+
+@dataclass
+class VLMPointingResult:
+    """Record of a pixel selected for a target object."""
+    object_name: str
+    pixel: Tuple[int, int]
+    orientation: Optional[math_helpers.Quat]
+    source: str = "gemini"
+
+
+def compute_pointing_result(  # pylint: disable=unused-argument
+        target_object: Object,
+        images: Dict[str, RGBDImageWithContext],
+        detection_artifacts: Optional[Dict[str, Any]],
+        detection_id_to_obj: Dict[ObjectDetectionID, Object],
+        rng: Optional[Any],
+        camera_name: str = "hand_color_image") -> Optional[VLMPointingResult]:
+    """Select a pixel for the target object via Gemini pointing service."""
+    del detection_artifacts, detection_id_to_obj, rng  # unused in Gemini path
+    global _POINTING_SERVICE_AVAILABLE, _POINTING_WARNING_EMITTED  # pylint:disable=global-statement
+
+    if not CFG.spot_use_vlm_pointing:
+        return None
+    if not _POINTING_SERVICE_AVAILABLE:
+        if not _POINTING_WARNING_EMITTED:
+            logging.warning("Gemini pointing disabled after connection failure; "
+                            "skipping until service becomes reachable again.")
+            _POINTING_WARNING_EMITTED = True
+        return None
+    if camera_name not in images:
+        logging.debug("[GeminiPointing] Camera %s missing; cannot compute pixel.",
+                      camera_name)
+        return None
+    rgbd = images[camera_name]
+    try:
+        pil_image = Image.fromarray(rgbd.rgb)
+    except Exception as exc:  # pragma: no cover - defensive
+        logging.debug("Failed to convert image for Gemini pointing: %s", exc)
+        return None
+
+    detection_mode = CFG.spot_pointing_mode.lower().startswith("det")
+    prompt = f"Point to the {target_object.name}"
+    try:
+        result = point_single_image(pil_image,
+                                    prompt,
+                                    host=CFG.spot_pointing_host,
+                                    port=CFG.spot_pointing_port,
+                                    detection=detection_mode)
+    except Exception as exc:  # pragma: no cover - network failure
+        logging.warning("Gemini pointing failed: %s", exc)
+        _POINTING_SERVICE_AVAILABLE = False
+        _POINTING_WARNING_EMITTED = False
+        return None
+    if result is None:
+        return None
+    _POINTING_SERVICE_AVAILABLE = True
+    _POINTING_WARNING_EMITTED = False
+    if CFG.spot_pointing_debug_visuals:
+        _save_pointing_debug_visual(target_object.name, rgbd, result)
+    pixel = result.primary_pixel()
+    if pixel is None:
+        return None
+    return VLMPointingResult(target_object.name,
+                             pixel,
+                             orientation=None,
+                             source="gemini")
+
+
+def _save_pointing_debug_visual(target_name: str,
+                                rgbd: RGBDImageWithContext,
+                                result: GeminiPointingResult) -> None:
+    """Best-effort visualization of Gemini pointing results."""
+    try:
+        outdir = Path(CFG.spot_pointing_debug_dir)
+        outdir.mkdir(parents=True, exist_ok=True)
+        image = Image.fromarray(rgbd.rgb.copy())
+        draw = ImageDraw.Draw(image)
+        for point in result.points:
+            x, y = point.pixel
+            r = 8
+            draw.ellipse((x - r, y - r, x + r, y + r),
+                         outline="red",
+                         width=2)
+            draw.text((x + 5, y - 5), point.label or "", fill="red")
+        for box in result.boxes:
+            x1, y1, x2, y2 = box.box
+            draw.rectangle((x1, y1, x2, y2), outline="cyan", width=2)
+            draw.text((x1 + 5, y1 - 5), box.label or "", fill="cyan")
+        timestamp = time.strftime("%Y%m%d-%H%M%S")
+        filename = outdir / \
+            f"{timestamp}_{target_name}_{rgbd.camera_name}_pointing.png"
+        image.save(filename)
+        logging.info("[GeminiPointing] Saved debug visualization to %s",
+                     filename)
+    except Exception as exc:  # pragma: no cover - best-effort logging
+        logging.debug("Failed to save pointing debug visual: %s", exc)
