foodforthought-cli 0.2.7__py3-none-any.whl → 0.3.0__py3-none-any.whl

ate/drivers/wifi_camera.py

@@ -0,0 +1,477 @@
+"""
+WiFi Camera driver for network-connected cameras.
+
+Supports common protocols:
+- MJPEG stream over HTTP (ESP32-CAM, many IP cameras)
+- Snapshot endpoint (REST API)
+- RTSP stream (professional IP cameras)
+
+Hiwonder MechDog visual module typically uses ESP32-CAM
+which serves MJPEG at http://<ip>:81/stream
+and snapshots at http://<ip>/capture
+"""
+
+import io
+import time
+import threading
+from typing import Optional, Tuple, Callable, Dict, Any
+from dataclasses import dataclass
+import requests
+from urllib.parse import urljoin
+
+from ..interfaces import (
+    CameraInterface,
+    Image,
+    ActionResult,
+)
+
+
+@dataclass
+class WiFiCameraConfig:
+    """Configuration for WiFi camera."""
+    ip: str                          # Camera IP address
+    port: int = 80                   # HTTP port
+    stream_port: int = 81            # MJPEG stream port (ESP32-CAM uses 81)
+    snapshot_path: str = "/capture"  # Snapshot endpoint
+    stream_path: str = "/stream"     # MJPEG stream endpoint
+    username: str = ""               # Auth username (if required)
+    password: str = ""               # Auth password (if required)
+    timeout: float = 5.0             # Request timeout
+    protocol: str = "esp32cam"       # Protocol: esp32cam, generic, rtsp
+
+
+class WiFiCamera(CameraInterface):
+    """
+    WiFi camera implementation.
+
+    Connects to network cameras over HTTP.
+    Tested with ESP32-CAM (common in hobby robots like MechDog).
+    """
+
+    def __init__(self, config: WiFiCameraConfig):
+        """
+        Initialize WiFi camera.
+
+        Args:
+            config: Camera configuration
+        """
+        self.config = config
+        self._base_url = f"http://{config.ip}:{config.port}"
+        self._stream_url = f"http://{config.ip}:{config.stream_port}{config.stream_path}"
+        self._snapshot_url = f"{self._base_url}{config.snapshot_path}"
+
+        self._session = requests.Session()
+        if config.username and config.password:
+            self._session.auth = (config.username, config.password)
+
+        self._streaming = False
+        self._stream_thread: Optional[threading.Thread] = None
+        self._stream_callback: Optional[Callable[[Image], None]] = None
+        self._last_frame: Optional[Image] = None
+        self._frame_count = 0
+        self._fps = 0.0
+        self._resolution = (640, 480)  # Default, updated on first capture
+
+        # Intrinsics (approximate for typical ESP32-CAM with OV2640)
+        self._intrinsics = {
+            "fx": 500.0,
+            "fy": 500.0,
+            "cx": 320.0,
+            "cy": 240.0,
+            "distortion": [0.0, 0.0, 0.0, 0.0, 0.0],
+        }
+
+    def get_image(self) -> Image:
+        """
+        Capture current frame from camera.
+
+        Returns:
+            Image with RGB data
+        """
+        try:
+            response = self._session.get(
+                self._snapshot_url,
+                timeout=self.config.timeout,
+                stream=True
+            )
+            response.raise_for_status()
+
+            # Read image data
+            image_data = response.content
+
+            # Try to decode with PIL if available
+            try:
+                from PIL import Image as PILImage
+                pil_image = PILImage.open(io.BytesIO(image_data))
+                width, height = pil_image.size
+                self._resolution = (width, height)
+
+                # Convert to RGB if needed
+                if pil_image.mode != "RGB":
+                    pil_image = pil_image.convert("RGB")
+
+                # Get raw bytes
+                rgb_data = pil_image.tobytes()
+
+                return Image(
+                    data=rgb_data,
+                    width=width,
+                    height=height,
+                    encoding="rgb8",
+                    timestamp=time.time(),
+                )
+            except ImportError:
+                # Return raw JPEG if PIL not available
+                return Image(
+                    data=image_data,
+                    width=self._resolution[0],
+                    height=self._resolution[1],
+                    encoding="jpeg",
+                    timestamp=time.time(),
+                )
+
+        except requests.RequestException as e:
+            # Return empty image on error
+            return Image(
+                data=b"",
+                width=0,
+                height=0,
+                encoding="error",
+                timestamp=time.time(),
+            )
+
+    def get_resolution(self) -> Tuple[int, int]:
+        """Get camera resolution."""
+        return self._resolution
+
+    def get_intrinsics(self) -> dict:
+        """Get camera intrinsic parameters."""
+        return self._intrinsics.copy()
+
+    def get_frame_id(self) -> str:
+        """Get coordinate frame ID."""
+        return "camera_link"
+
+    def set_resolution(self, width: int, height: int) -> ActionResult:
+        """
+        Set camera resolution.
+
+        ESP32-CAM supports: QQVGA(160x120), QVGA(320x240),
+        CIF(400x296), VGA(640x480), SVGA(800x600), XGA(1024x768),
+        SXGA(1280x1024), UXGA(1600x1200)
+        """
+        # Map resolution to ESP32-CAM framesize
+        resolution_map = {
+            (160, 120): 0,    # QQVGA
+            (320, 240): 5,    # QVGA
+            (640, 480): 8,    # VGA
+            (800, 600): 9,    # SVGA
+            (1024, 768): 10,  # XGA
+            (1280, 1024): 11, # SXGA
+            (1600, 1200): 13, # UXGA
+        }
+
+        framesize = resolution_map.get((width, height))
+        if framesize is None:
+            return ActionResult.error(f"Unsupported resolution: {width}x{height}")
+
+        try:
+            # ESP32-CAM control endpoint
+            url = f"{self._base_url}/control?var=framesize&val={framesize}"
+            response = self._session.get(url, timeout=self.config.timeout)
+            response.raise_for_status()
+            self._resolution = (width, height)
+            return ActionResult.success(f"Resolution set to {width}x{height}")
+        except requests.RequestException as e:
+            return ActionResult.error(f"Failed to set resolution: {e}")
+
+    def set_exposure(self, exposure_ms: float) -> ActionResult:
+        """Set exposure time (ESP32-CAM auto-exposure control)."""
+        try:
+            # Enable/disable auto exposure
+            # aec: 0=manual, 1=auto
+            # aec_value: exposure value (0-1200)
+            aec_value = int(exposure_ms * 10)  # Approximate mapping
+            aec_value = max(0, min(1200, aec_value))
+
+            # Disable auto exposure
+            self._session.get(
+                f"{self._base_url}/control?var=aec&val=0",
+                timeout=self.config.timeout
+            )
+            # Set exposure value
+            response = self._session.get(
+                f"{self._base_url}/control?var=aec_value&val={aec_value}",
+                timeout=self.config.timeout
+            )
+            response.raise_for_status()
+            return ActionResult.success(f"Exposure set to {exposure_ms}ms")
+        except requests.RequestException as e:
+            return ActionResult.error(f"Failed to set exposure: {e}")
+
+    # =========================================================================
+    # Streaming
+    # =========================================================================
+
+    def start_streaming(self, callback: Callable[[Image], None]) -> ActionResult:
+        """Start continuous image streaming."""
+        if self._streaming:
+            return ActionResult.error("Already streaming")
+
+        self._stream_callback = callback
+        self._streaming = True
+        self._stream_thread = threading.Thread(target=self._stream_loop, daemon=True)
+        self._stream_thread.start()
+
+        return ActionResult.success("Streaming started")
+
+    def stop_streaming(self) -> ActionResult:
+        """Stop image streaming."""
+        if not self._streaming:
+            return ActionResult.error("Not streaming")
+
+        self._streaming = False
+        if self._stream_thread:
+            self._stream_thread.join(timeout=2.0)
+            self._stream_thread = None
+
+        return ActionResult.success("Streaming stopped")
+
+    def _stream_loop(self):
+        """Background thread for MJPEG streaming."""
+        try:
+            response = self._session.get(
+                self._stream_url,
+                stream=True,
+                timeout=self.config.timeout
+            )
+
+            if response.status_code != 200:
+                return
+
+            bytes_buffer = b""
+            frame_start = time.time()
+            frame_count = 0
+
+            for chunk in response.iter_content(chunk_size=1024):
+                if not self._streaming:
+                    break
+
+                bytes_buffer += chunk
+
+                # Find JPEG start and end markers
+                start = bytes_buffer.find(b"\xff\xd8")  # JPEG start
+                end = bytes_buffer.find(b"\xff\xd9")    # JPEG end
+
+                if start != -1 and end != -1 and end > start:
+                    # Extract JPEG frame
+                    jpeg_data = bytes_buffer[start:end + 2]
+                    bytes_buffer = bytes_buffer[end + 2:]
+
+                    # Decode frame
+                    try:
+                        from PIL import Image as PILImage
+                        pil_image = PILImage.open(io.BytesIO(jpeg_data))
+                        width, height = pil_image.size
+
+                        if pil_image.mode != "RGB":
+                            pil_image = pil_image.convert("RGB")
+
+                        frame = Image(
+                            data=pil_image.tobytes(),
+                            width=width,
+                            height=height,
+                            format="rgb8",
+                            encoding="raw",
+                            timestamp=time.time(),
+                        )
+                    except ImportError:
+                        frame = Image(
+                            data=jpeg_data,
+                            width=self._resolution[0],
+                            height=self._resolution[1],
+                            format="jpeg",
+                            encoding="jpeg",
+                            timestamp=time.time(),
+                        )
+
+                    self._last_frame = frame
+                    frame_count += 1
+
+                    # Calculate FPS
+                    elapsed = time.time() - frame_start
+                    if elapsed >= 1.0:
+                        self._fps = frame_count / elapsed
+                        frame_count = 0
+                        frame_start = time.time()
+
+                    # Call callback
+                    if self._stream_callback:
+                        self._stream_callback(frame)
+
+        except Exception as e:
+            print(f"Stream error: {e}")
+        finally:
+            self._streaming = False
+
+    def get_fps(self) -> float:
+        """Get current frame rate."""
+        return self._fps
+
+    def get_last_frame(self) -> Optional[Image]:
+        """Get the last captured frame (for streaming mode)."""
+        return self._last_frame
+
+    # =========================================================================
+    # Camera controls (ESP32-CAM specific)
+    # =========================================================================
+
+    def set_quality(self, quality: int) -> ActionResult:
+        """
+        Set JPEG quality (4-63, lower is better quality).
+
+        Args:
+            quality: JPEG quality (4-63)
+        """
+        quality = max(4, min(63, quality))
+        try:
+            response = self._session.get(
+                f"{self._base_url}/control?var=quality&val={quality}",
+                timeout=self.config.timeout
+            )
+            response.raise_for_status()
+            return ActionResult.success(f"Quality set to {quality}")
+        except requests.RequestException as e:
+            return ActionResult.error(f"Failed to set quality: {e}")
+
+    def set_brightness(self, brightness: int) -> ActionResult:
+        """Set brightness (-2 to 2)."""
+        brightness = max(-2, min(2, brightness))
+        try:
+            response = self._session.get(
+                f"{self._base_url}/control?var=brightness&val={brightness}",
+                timeout=self.config.timeout
+            )
+            response.raise_for_status()
+            return ActionResult.success(f"Brightness set to {brightness}")
+        except requests.RequestException as e:
+            return ActionResult.error(f"Failed to set brightness: {e}")
+
+    def set_contrast(self, contrast: int) -> ActionResult:
+        """Set contrast (-2 to 2)."""
+        contrast = max(-2, min(2, contrast))
+        try:
+            response = self._session.get(
+                f"{self._base_url}/control?var=contrast&val={contrast}",
+                timeout=self.config.timeout
+            )
+            response.raise_for_status()
+            return ActionResult.success(f"Contrast set to {contrast}")
+        except requests.RequestException as e:
+            return ActionResult.error(f"Failed to set contrast: {e}")
+
+    def flip_horizontal(self, flip: bool) -> ActionResult:
+        """Flip image horizontally."""
+        try:
+            response = self._session.get(
+                f"{self._base_url}/control?var=hmirror&val={1 if flip else 0}",
+                timeout=self.config.timeout
+            )
+            response.raise_for_status()
+            return ActionResult.success(f"Horizontal flip: {flip}")
+        except requests.RequestException as e:
+            return ActionResult.error(f"Failed to flip: {e}")
+
+    def flip_vertical(self, flip: bool) -> ActionResult:
+        """Flip image vertically."""
+        try:
+            response = self._session.get(
+                f"{self._base_url}/control?var=vflip&val={1 if flip else 0}",
+                timeout=self.config.timeout
+            )
+            response.raise_for_status()
+            return ActionResult.success(f"Vertical flip: {flip}")
+        except requests.RequestException as e:
+            return ActionResult.error(f"Failed to flip: {e}")
+
+    def enable_face_detection(self, enable: bool) -> ActionResult:
+        """Enable/disable face detection (ESP32-CAM feature)."""
+        try:
+            response = self._session.get(
+                f"{self._base_url}/control?var=face_detect&val={1 if enable else 0}",
+                timeout=self.config.timeout
+            )
+            response.raise_for_status()
+            return ActionResult.success(f"Face detection: {enable}")
+        except requests.RequestException as e:
+            return ActionResult.error(f"Failed to set face detection: {e}")
+
+    # =========================================================================
+    # Connection management
+    # =========================================================================
+
+    def is_connected(self) -> bool:
+        """Check if camera is reachable."""
+        try:
+            response = self._session.get(
+                f"{self._base_url}/status",
+                timeout=2.0
+            )
+            return response.status_code == 200
+        except requests.RequestException:
+            return False
+
+    def get_status(self) -> Dict[str, Any]:
+        """Get camera status."""
+        try:
+            response = self._session.get(
+                f"{self._base_url}/status",
+                timeout=self.config.timeout
+            )
+            if response.status_code == 200:
+                return response.json()
+        except Exception:
+            pass
+        return {"connected": False}
+
+    def __repr__(self) -> str:
+        return f"WiFiCamera({self.config.ip}:{self.config.port})"
+
+
+def discover_cameras(subnet: str = "192.168.1", timeout: float = 1.0) -> list:
+    """
+    Discover cameras on the local network.
+
+    Args:
+        subnet: Subnet to scan (e.g., "192.168.1")
+        timeout: Timeout per IP
+
+    Returns:
+        List of discovered camera IPs
+    """
+    import concurrent.futures
+
+    cameras = []
+
+    def check_ip(ip):
+        try:
+            response = requests.get(
+                f"http://{ip}/status",
+                timeout=timeout
+            )
+            if response.status_code == 200:
+                return ip
+        except:
+            pass
+        return None
+
+    with concurrent.futures.ThreadPoolExecutor(max_workers=50) as executor:
+        ips = [f"{subnet}.{i}" for i in range(1, 255)]
+        futures = {executor.submit(check_ip, ip): ip for ip in ips}
+
+        for future in concurrent.futures.as_completed(futures):
+            result = future.result()
+            if result:
+                cameras.append(result)
+
+    return cameras

ate/interfaces/__init__.py

@@ -0,0 +1,187 @@
+"""
+FoodforThought Robot Interfaces
+
+Abstract interfaces that define capabilities for any robot.
+Implementations are hardware-specific, but skills are written against these interfaces.
+
+Architecture:
+    RobotInterface (base)
+    ├── Capabilities (mixins)
+    │   ├── Locomotion
+    │   │   ├── QuadrupedLocomotion
+    │   │   ├── BipedLocomotion
+    │   │   ├── WheeledLocomotion
+    │   │   └── AerialLocomotion
+    │   ├── Manipulation
+    │   │   ├── ArmInterface
+    │   │   └── GripperInterface
+    │   ├── Perception
+    │   │   ├── CameraInterface
+    │   │   ├── DepthCameraInterface
+    │   │   ├── LidarInterface
+    │   │   └── IMUInterface
+    │   └── Body
+    │       └── BodyPoseInterface
+    └── Safety
+        └── SafetyInterface
+
+Example:
+    # MechDog implements:
+    class MechDogDriver(QuadrupedLocomotion, BodyPoseInterface, SafetyInterface):
+        ...
+
+    # Spot with arm implements:
+    class SpotDriver(QuadrupedLocomotion, BodyPoseInterface, ArmInterface,
+                     GripperInterface, CameraInterface, LidarInterface, SafetyInterface):
+        ...
+
+    # Skills are written against interfaces:
+    def pick_and_place(robot: QuadrupedLocomotion & GripperInterface, target: Vector3):
+        robot.walk_to(target)
+        robot.lower_body(height=0.1)
+        robot.grasp()
+        ...
+"""
+
+from .types import (
+    Vector3,
+    Quaternion,
+    Pose,
+    Twist,
+    JointState,
+    JointLimits,
+    Image,
+    DepthImage,
+    PointCloud,
+    IMUReading,
+    ForceTorqueReading,
+    BatteryState,
+    RobotStatus,
+    GaitType,
+    GripperState,
+    GripperStatus,
+    ActionResult,
+)
+
+from .base import (
+    RobotInterface,
+    SafetyInterface,
+    Capability,
+    RobotInfo,
+)
+
+from .locomotion import (
+    LocomotionInterface,
+    QuadrupedLocomotion,
+    BipedLocomotion,
+    WheeledLocomotion,
+    AerialLocomotion,
+)
+
+from .manipulation import (
+    ArmInterface,
+    GripperInterface,
+    DualArmInterface,
+)
+
+from .perception import (
+    CameraInterface,
+    DepthCameraInterface,
+    LidarInterface,
+    IMUInterface,
+    ForceTorqueInterface,
+)
+
+from .body import (
+    BodyPoseInterface,
+)
+
+from .detection import (
+    BoundingBox,
+    Detection,
+    DetectionResult,
+    ObjectDetectionInterface,
+    TrashDetectionInterface,
+)
+
+from .navigation import (
+    NavigationState,
+    NavigationGoal,
+    NavigationStatus,
+    Waypoint,
+    NavigationInterface,
+    SimpleNavigationInterface,
+)
+
+from .sensors import (
+    DistanceSensorType,
+    DistanceReading,
+    DistanceSensorInterface,
+    VisualDistanceEstimator,
+    ProximitySensorReading,
+    ProximitySensorInterface,
+)
+
+__all__ = [
+    # Types
+    "Vector3",
+    "Quaternion",
+    "Pose",
+    "Twist",
+    "JointState",
+    "JointLimits",
+    "Image",
+    "DepthImage",
+    "PointCloud",
+    "IMUReading",
+    "ForceTorqueReading",
+    "BatteryState",
+    "RobotStatus",
+    "GaitType",
+    "GripperState",
+    "GripperStatus",
+    "ActionResult",
+    # Base
+    "RobotInterface",
+    "SafetyInterface",
+    "Capability",
+    "RobotInfo",
+    # Locomotion
+    "LocomotionInterface",
+    "QuadrupedLocomotion",
+    "BipedLocomotion",
+    "WheeledLocomotion",
+    "AerialLocomotion",
+    # Manipulation
+    "ArmInterface",
+    "GripperInterface",
+    "DualArmInterface",
+    # Perception
+    "CameraInterface",
+    "DepthCameraInterface",
+    "LidarInterface",
+    "IMUInterface",
+    "ForceTorqueInterface",
+    # Body
+    "BodyPoseInterface",
+    # Detection (higher-level)
+    "BoundingBox",
+    "Detection",
+    "DetectionResult",
+    "ObjectDetectionInterface",
+    "TrashDetectionInterface",
+    # Navigation (higher-level)
+    "NavigationState",
+    "NavigationGoal",
+    "NavigationStatus",
+    "Waypoint",
+    "NavigationInterface",
+    "SimpleNavigationInterface",
+    # Sensors (distance/proximity)
+    "DistanceSensorType",
+    "DistanceReading",
+    "DistanceSensorInterface",
+    "VisualDistanceEstimator",
+    "ProximitySensorReading",
+    "ProximitySensorInterface",
+]