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

ate/robot/perception.py

@@ -0,0 +1,502 @@
+"""
+Perception integration for robot behaviors.
+
+Connects camera drivers and detectors to the behavior executor,
+enabling closed-loop control with real vision.
+"""
+
+import time
+from dataclasses import dataclass
+from typing import Optional, List, Tuple, Dict, Any
+
+from ..drivers.wifi_camera import WiFiCamera, WiFiCameraConfig
+from ..detection.color_detector import ColorDetector
+from ..detection.base import Detection, BoundingBox
+
+
+@dataclass
+class PerceptionResult:
+    """Result from perception system."""
+    found: bool
+    label: str = ""
+    confidence: float = 0.0
+    x: float = 0.0  # Normalized x position (0-1, center is 0.5)
+    y: float = 0.0  # Normalized y position (0-1)
+    width: float = 0.0  # Normalized width
+    height: float = 0.0  # Normalized height
+    distance: Optional[float] = None  # Estimated distance in meters
+    raw_detection: Optional[Detection] = None  # Original detection
+
+    @property
+    def center_offset(self) -> float:
+        """How far from center (negative=left, positive=right)."""
+        return self.x - 0.5
+
+    @property
+    def is_centered(self) -> bool:
+        """Is target roughly centered?"""
+        return abs(self.center_offset) < 0.15
+
+    @property
+    def is_close(self) -> bool:
+        """Is target within pickup range?"""
+        return self.distance is not None and self.distance < 0.2
+
+
+class PerceptionSystem:
+    """
+    Unified perception system for robot behaviors.
+
+    Coordinates:
+    - Camera capture
+    - Object detection
+    - Distance estimation
+    - Target tracking
+    """
+
+    def __init__(
+        self,
+        camera_ip: str = "192.168.4.1",  # MechDog default AP
+        camera_port: int = 80,
+    ):
+        """
+        Initialize perception system.
+
+        Args:
+            camera_ip: WiFi camera IP address
+            camera_port: HTTP port
+        """
+        self.camera_config = WiFiCameraConfig(
+            ip=camera_ip,
+            port=camera_port,
+        )
+        self.camera: Optional[WiFiCamera] = None
+        self.detector = ColorDetector(
+            min_area=300,    # Smaller objects
+            max_area=150000, # Allow large close objects
+            merge_distance=30,
+        )
+
+        # Tracking state
+        self.last_detection: Optional[PerceptionResult] = None
+        self.detection_history: List[PerceptionResult] = []
+        self.frame_size: Tuple[int, int] = (640, 480)
+
+        # Distance estimation calibration
+        # Based on object appearing X% of frame at Y meters
+        self.reference_size = 0.15  # Reference: object at 15% of frame width
+        self.reference_distance = 0.30  # is 30cm away
+
+    def connect(self) -> bool:
+        """Connect to camera."""
+        try:
+            self.camera = WiFiCamera(self.camera_config)
+            if self.camera.is_connected():
+                print(f"Camera connected: {self.camera_config.ip}")
+                return True
+            else:
+                print(f"Camera not responding: {self.camera_config.ip}")
+                return False
+        except Exception as e:
+            print(f"Camera connection failed: {e}")
+            return False
+
+    def disconnect(self):
+        """Disconnect from camera."""
+        if self.camera:
+            self.camera.stop_streaming()
+            self.camera = None
+
+    def detect(
+        self,
+        target_type: str = "any",
+        color: Optional[str] = None,
+    ) -> PerceptionResult:
+        """
+        Detect objects in current camera view.
+
+        Args:
+            target_type: Object type to find ("any", "ball", "cube", etc.)
+            color: Color filter ("green", "red", "blue", etc.)
+
+        Returns:
+            PerceptionResult with detection info
+        """
+        if not self.camera:
+            # Simulation mode
+            return self._simulate_detection(target_type, color)
+
+        # Capture frame
+        image = self.camera.get_image()
+        if image.width == 0:
+            return PerceptionResult(found=False)
+
+        self.frame_size = (image.width, image.height)
+
+        # Run detection
+        target_colors = [color] if color else None
+        detections = self.detector.detect(image, target_colors=target_colors)
+
+        if not detections:
+            return PerceptionResult(found=False)
+
+        # Find best detection (largest + most centered)
+        best = self._select_best_detection(detections)
+
+        # Convert to normalized coordinates
+        result = self._detection_to_result(best)
+
+        # Track history
+        self.last_detection = result
+        self.detection_history.append(result)
+        if len(self.detection_history) > 10:
+            self.detection_history.pop(0)
+
+        return result
+
+    def _select_best_detection(self, detections: List[Detection]) -> Detection:
+        """
+        Select the best detection from candidates.
+
+        Prefers: larger area + more centered
+        """
+        if len(detections) == 1:
+            return detections[0]
+
+        def score(det: Detection) -> float:
+            # Normalize center position
+            cx = det.bbox.center[0] / self.frame_size[0]
+            center_offset = abs(cx - 0.5)
+
+            # Score: large area, centered position
+            area_score = det.bbox.area / (self.frame_size[0] * self.frame_size[1])
+            center_score = 1.0 - (center_offset * 2)  # 1.0 when centered, 0 at edges
+
+            return area_score * 0.7 + center_score * 0.3 + det.confidence * 0.2
+
+        return max(detections, key=score)
+
+    def _detection_to_result(self, detection: Detection) -> PerceptionResult:
+        """Convert Detection to PerceptionResult with normalized coords."""
+        w, h = self.frame_size
+
+        # Normalize coordinates
+        norm_x = (detection.bbox.x + detection.bbox.width / 2) / w
+        norm_y = (detection.bbox.y + detection.bbox.height / 2) / h
+        norm_w = detection.bbox.width / w
+        norm_h = detection.bbox.height / h
+
+        # Estimate distance from apparent size
+        apparent_size = max(norm_w, norm_h)
+        distance = self._estimate_distance(apparent_size)
+
+        return PerceptionResult(
+            found=True,
+            label=detection.label,
+            confidence=detection.confidence,
+            x=norm_x,
+            y=norm_y,
+            width=norm_w,
+            height=norm_h,
+            distance=distance,
+            raw_detection=detection,
+        )
+
+    def _estimate_distance(self, apparent_size: float) -> float:
+        """
+        Estimate distance from apparent object size.
+
+        Uses inverse relationship: distance = k / size
+        Calibrated from reference measurement.
+        """
+        if apparent_size < 0.01:
+            return 2.0  # Very far (or detection error)
+
+        # k = reference_distance * reference_size
+        k = self.reference_distance * self.reference_size
+
+        distance = k / apparent_size
+
+        # Clamp to reasonable range
+        return max(0.05, min(2.0, distance))
+
+    def _simulate_detection(
+        self,
+        target_type: str,
+        color: Optional[str],
+    ) -> PerceptionResult:
+        """Simulate detection for testing without camera."""
+        # Simulate a green ball slightly right of center
+        return PerceptionResult(
+            found=True,
+            label=f"{color or ''} {target_type}".strip(),
+            confidence=0.85,
+            x=0.55,  # Slightly right
+            y=0.6,   # Lower half
+            width=0.12,
+            height=0.12,
+            distance=0.25,  # 25cm away
+        )
+
+    def get_alignment_correction(self, target: PerceptionResult) -> str:
+        """
+        Get alignment correction direction.
+
+        Returns:
+            "left", "right", "centered", or "lost"
+        """
+        if not target.found:
+            return "lost"
+
+        offset = target.center_offset
+        threshold = 0.1
+
+        if abs(offset) < threshold:
+            return "centered"
+        elif offset > 0:
+            return "right"  # Target is right, need to turn right
+        else:
+            return "left"   # Target is left, need to turn left
+
+    def is_target_stable(self, min_frames: int = 3) -> bool:
+        """
+        Check if target detection is stable across frames.
+
+        Helps filter out noise and false positives.
+        """
+        if len(self.detection_history) < min_frames:
+            return False
+
+        recent = self.detection_history[-min_frames:]
+
+        # All must be found
+        if not all(r.found for r in recent):
+            return False
+
+        # Position should be consistent (within 20% of frame)
+        x_coords = [r.x for r in recent]
+        y_coords = [r.y for r in recent]
+
+        x_range = max(x_coords) - min(x_coords)
+        y_range = max(y_coords) - min(y_coords)
+
+        return x_range < 0.2 and y_range < 0.2
+
+
+class LivePerceptionExecutor:
+    """
+    Executes behaviors with live perception feedback.
+
+    Replaces simulated detection in behaviors.py with real camera input.
+    """
+
+    def __init__(
+        self,
+        robot_interface,
+        camera_ip: str = "192.168.4.1",
+    ):
+        """
+        Initialize live perception executor.
+
+        Args:
+            robot_interface: Robot driver (MechDogDriver)
+            camera_ip: WiFi camera IP
+        """
+        self.robot = robot_interface
+        self.perception = PerceptionSystem(camera_ip=camera_ip)
+
+        # Import primitives for skill execution
+        from .primitives import PrimitiveLibrary
+        self.primitives = PrimitiveLibrary(robot_interface)
+
+        # Configuration
+        self.approach_distance = 0.15  # meters
+        self.alignment_threshold = 0.12
+        self.max_approach_steps = 20
+        self.step_delay = 0.4
+
+    def connect(self) -> bool:
+        """Connect to camera."""
+        return self.perception.connect()
+
+    def disconnect(self):
+        """Disconnect from camera."""
+        self.perception.disconnect()
+
+    def pickup_green_ball(self) -> bool:
+        """
+        Find and pick up a green ball.
+
+        Full behavior with real perception:
+        1. Detect green ball
+        2. Align to center it in view
+        3. Approach until close
+        4. Execute pickup sequence
+        5. Verify success
+
+        Returns:
+            True if successful
+        """
+        print("\n" + "=" * 50)
+        print("BEHAVIOR: Pickup Green Ball (Live Perception)")
+        print("=" * 50)
+
+        # Step 1: Initial detection
+        print("\n[1] Scanning for green ball...")
+        target = self.perception.detect("ball", "green")
+
+        if not target.found:
+            print("    No green ball found!")
+            return False
+
+        print(f"    Found! x={target.x:.2f}, dist={target.distance:.2f}m")
+
+        # Step 2: Align
+        print("\n[2] Aligning to target...")
+        for i in range(10):
+            target = self.perception.detect("ball", "green")
+            if not target.found:
+                print("    Lost target during alignment!")
+                return False
+
+            direction = self.perception.get_alignment_correction(target)
+
+            if direction == "centered":
+                print("    Aligned!")
+                break
+            elif direction == "left":
+                print(f"    Turning left (offset: {target.center_offset:.2f})")
+                self._turn_left()
+            elif direction == "right":
+                print(f"    Turning right (offset: {target.center_offset:.2f})")
+                self._turn_right()
+
+            time.sleep(0.3)
+        else:
+            print("    Failed to align after 10 attempts")
+            return False
+
+        # Step 3: Approach
+        print("\n[3] Approaching target...")
+        for step in range(self.max_approach_steps):
+            target = self.perception.detect("ball", "green")
+
+            if not target.found:
+                print("    Lost target during approach!")
+                return False
+
+            if target.distance <= self.approach_distance:
+                print(f"    Close enough! ({target.distance:.2f}m)")
+                break
+
+            print(f"    Step {step+1}: dist={target.distance:.2f}m")
+            self._step_forward()
+
+            # Re-align if needed
+            if abs(target.center_offset) > self.alignment_threshold:
+                if target.center_offset > 0:
+                    self._turn_right(angle=5)
+                else:
+                    self._turn_left(angle=5)
+
+            time.sleep(self.step_delay)
+        else:
+            print("    Too many steps, giving up")
+            return False
+
+        # Step 4: Execute pickup sequence
+        print("\n[4] Executing pickup sequence...")
+
+        print("    Tilting forward...")
+        self.primitives.execute("tilt_forward")
+        time.sleep(0.3)
+
+        print("    Ready to grab...")
+        self.primitives.execute("ready_to_grab")
+        time.sleep(0.3)
+
+        print("    Closing gripper...")
+        self.primitives.execute("gripper_close")
+        time.sleep(0.3)
+
+        print("    Lifting...")
+        self.primitives.execute("arm_carry")
+        time.sleep(0.2)
+
+        print("    Standing...")
+        self.primitives.execute("stand")
+        time.sleep(0.3)
+
+        # Step 5: Verify (visual check - ball should not be visible on ground)
+        print("\n[5] Verifying pickup...")
+        time.sleep(0.5)
+
+        verify_result = self.perception.detect("ball", "green")
+
+        if not verify_result.found or verify_result.distance > 0.5:
+            print("    SUCCESS! Ball no longer visible on ground.")
+            return True
+        else:
+            print("    Ball still visible - pickup may have failed")
+            return False
+
+    def _step_forward(self):
+        """Take one step forward."""
+        if hasattr(self.robot, 'walk'):
+            self.robot.walk('forward', 1)
+        else:
+            print("    [Simulated: step forward]")
+
+    def _turn_left(self, angle: int = 15):
+        """Turn left."""
+        if hasattr(self.robot, 'turn'):
+            self.robot.turn('left', angle)
+        else:
+            print(f"    [Simulated: turn left {angle}deg]")
+
+    def _turn_right(self, angle: int = 15):
+        """Turn right."""
+        if hasattr(self.robot, 'turn'):
+            self.robot.turn('right', angle)
+        else:
+            print(f"    [Simulated: turn right {angle}deg]")
+
+
+def run_perception_demo():
+    """Demo perception system."""
+    print("=" * 50)
+    print("PERCEPTION SYSTEM DEMO")
+    print("=" * 50)
+
+    # Create perception system in simulation mode (no camera)
+    perception = PerceptionSystem()
+
+    print("\nTesting simulated detection...")
+    result = perception.detect("ball", "green")
+
+    print(f"\nDetection result:")
+    print(f"  Found: {result.found}")
+    print(f"  Label: {result.label}")
+    print(f"  Position: ({result.x:.2f}, {result.y:.2f})")
+    print(f"  Distance: {result.distance:.2f}m")
+    print(f"  Centered: {result.is_centered}")
+    print(f"  Alignment: {perception.get_alignment_correction(result)}")
+
+    # Test live executor in simulation
+    print("\n" + "-" * 50)
+    print("Testing LivePerceptionExecutor (simulation)...")
+
+    executor = LivePerceptionExecutor(
+        robot_interface=None,  # No real robot
+        camera_ip="192.168.4.1",
+    )
+
+    # Run pickup behavior (will use simulated perception)
+    success = executor.pickup_green_ball()
+
+    print("\n" + "-" * 50)
+    print(f"Pickup result: {'SUCCESS' if success else 'FAILED'}")
+
+
+if __name__ == "__main__":
+    run_perception_demo()