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

ate/behaviors/approach.py

@@ -0,0 +1,399 @@
+"""
+Approach behaviors for targeting detected objects.
+
+These behaviors work with any locomotion interface (quadruped, wheeled, etc.)
+rather than requiring NavigationInterface. They use:
+- Visual servoing (centering target in camera view)
+- Distance sensing (ultrasonic, visual estimation, depth camera)
+
+This is how FoodforThought makes skills portable across different robots.
+"""
+
+import time
+from dataclasses import dataclass
+from typing import Optional, Protocol, Union, Tuple
+from enum import Enum, auto
+
+from .tree import BehaviorNode, BehaviorStatus
+
+from ..interfaces import (
+    QuadrupedLocomotion,
+    WheeledLocomotion,
+    BipedLocomotion,
+    LocomotionInterface,
+    CameraInterface,
+    Vector3,
+    ActionResult,
+)
+from ..interfaces.sensors import (
+    DistanceSensorInterface,
+    DistanceReading,
+    VisualDistanceEstimator,
+)
+from ..interfaces.detection import Detection, BoundingBox
+
+
+class ApproachState(Enum):
+    """State machine for approach behavior."""
+    SEARCHING = auto()      # Looking for target
+    CENTERING = auto()      # Turning to center target
+    APPROACHING = auto()    # Moving toward target
+    REACHED = auto()        # Close enough to target
+    LOST = auto()           # Lost sight of target
+
+
+@dataclass
+class ApproachConfig:
+    """Configuration for approach behavior."""
+    # Target distance to stop at (meters)
+    target_distance: float = 0.15
+
+    # Image centering tolerance (fraction of image width from center)
+    center_tolerance: float = 0.15
+
+    # Speed settings
+    approach_speed: float = 0.3
+    turn_speed: float = 0.4
+    slow_distance: float = 0.3  # Slow down when this close
+
+    # Timeout settings
+    max_approach_time: float = 30.0
+    lost_target_timeout: float = 3.0
+
+    # Visual estimation (when no hardware distance sensor)
+    use_visual_distance: bool = True
+    default_object_size: float = 0.10  # meters
+
+
+class ApproachTarget(BehaviorNode):
+    """
+    Approach a detected target using visual servoing.
+
+    Works with ANY locomotion interface by using:
+    1. Camera to track target position in image
+    2. Distance sensor OR visual estimation for range
+    3. Locomotion commands (walk/drive forward, turn)
+
+    Blackboard:
+    - Reads: "target_detection" or "target_bbox" or "target_position"
+    - Writes: "approach_state", "target_distance", "approach_complete"
+
+    Example:
+        # Works with quadruped
+        approach = ApproachTarget(
+            locomotion=mechdog,
+            camera=mechdog,  # Same driver implements both
+            distance_sensor=mechdog,  # Has ultrasonic
+        )
+
+        # Works with wheeled robot
+        approach = ApproachTarget(
+            locomotion=diff_drive,
+            camera=usb_camera,
+            distance_sensor=None,  # Will use visual estimation
+        )
+    """
+
+    def __init__(
+        self,
+        locomotion: LocomotionInterface,
+        camera: Optional[CameraInterface] = None,
+        distance_sensor: Optional[DistanceSensorInterface] = None,
+        config: Optional[ApproachConfig] = None,
+        name: str = "",
+    ):
+        super().__init__(name or "ApproachTarget")
+        self.locomotion = locomotion
+        self.camera = camera
+        self.distance_sensor = distance_sensor
+        self.config = config or ApproachConfig()
+
+        # Visual distance estimator (fallback when no hardware sensor)
+        self._visual_estimator: Optional[VisualDistanceEstimator] = None
+        if camera and self.config.use_visual_distance:
+            res = camera.get_resolution()
+            if res[0] > 0:
+                self._visual_estimator = VisualDistanceEstimator(
+                    image_width=res[0],
+                    image_height=res[1],
+                )
+
+        # State
+        self._state = ApproachState.SEARCHING
+        self._start_time: Optional[float] = None
+        self._last_detection_time: Optional[float] = None
+        self._current_distance: float = float('inf')
+
+    def tick(self) -> BehaviorStatus:
+        """Execute one tick of the approach behavior."""
+        now = time.time()
+
+        # Initialize on first tick
+        if self._start_time is None:
+            self._start_time = now
+            self._state = ApproachState.SEARCHING
+
+        # Check timeout
+        if now - self._start_time > self.config.max_approach_time:
+            self._stop_locomotion()
+            return BehaviorStatus.FAILURE
+
+        # Get current target info from blackboard
+        target_info = self._get_target_info()
+
+        if target_info is None:
+            # No target info available
+            if self._last_detection_time is None:
+                return BehaviorStatus.FAILURE  # Never saw target
+
+            # Check if we lost target recently
+            if now - self._last_detection_time > self.config.lost_target_timeout:
+                self._state = ApproachState.LOST
+                self._stop_locomotion()
+                return BehaviorStatus.FAILURE
+
+            # Keep approaching based on last known info
+            return BehaviorStatus.RUNNING
+
+        # Update last detection time
+        self._last_detection_time = now
+
+        bbox, object_type = target_info
+
+        # Get distance to target
+        distance = self._get_distance(bbox, object_type)
+        self._current_distance = distance
+
+        if self.blackboard:
+            self.blackboard.set("target_distance", distance)
+            self.blackboard.set("approach_state", self._state.name)
+
+        # Check if we've reached the target
+        if distance <= self.config.target_distance:
+            self._state = ApproachState.REACHED
+            self._stop_locomotion()
+            if self.blackboard:
+                self.blackboard.set("approach_complete", True)
+            return BehaviorStatus.SUCCESS
+
+        # Get target position in image (for centering)
+        center_offset = self._get_center_offset(bbox)
+
+        # State machine
+        if abs(center_offset) > self.config.center_tolerance:
+            # Need to turn to center target
+            self._state = ApproachState.CENTERING
+            self._turn_toward_target(center_offset)
+            return BehaviorStatus.RUNNING
+        else:
+            # Target is centered, approach
+            self._state = ApproachState.APPROACHING
+            self._move_toward_target(distance)
+            return BehaviorStatus.RUNNING
+
+    def _get_target_info(self) -> Optional[Tuple[BoundingBox, str]]:
+        """Get target bounding box and type from blackboard."""
+        if not self.blackboard:
+            return None
+
+        # Try detection first
+        detection = self.blackboard.get("target_detection")
+        if detection and isinstance(detection, Detection):
+            return (detection.bbox, detection.class_name)
+
+        # Try raw bbox
+        bbox = self.blackboard.get("target_bbox")
+        if bbox and isinstance(bbox, BoundingBox):
+            return (bbox, "unknown")
+
+        # Try detection list (get first)
+        detections = self.blackboard.get("detections")
+        if detections and len(detections) > 0:
+            det = detections[0]
+            if isinstance(det, Detection):
+                return (det.bbox, det.class_name)
+
+        return None
+
+    def _get_center_offset(self, bbox: BoundingBox) -> float:
+        """
+        Get offset from image center as fraction of image width.
+
+        Returns:
+            Negative = target is left of center
+            Positive = target is right of center
+            0 = centered
+        """
+        if not self.camera:
+            return 0.0
+
+        res = self.camera.get_resolution()
+        if res[0] == 0:
+            return 0.0
+
+        image_width = res[0]
+        image_center_x = image_width / 2
+
+        # Get bbox center
+        bbox_center_x = bbox.x + bbox.width / 2
+
+        # Calculate offset as fraction of image width
+        offset = (bbox_center_x - image_center_x) / image_width
+
+        return offset
+
+    def _get_distance(self, bbox: BoundingBox, object_type: str) -> float:
+        """
+        Get distance to target using best available method.
+
+        Priority:
+        1. Hardware distance sensor (ultrasonic, lidar)
+        2. Visual estimation from bbox size
+        3. Default large distance
+        """
+        # Try hardware sensor first
+        if self.distance_sensor:
+            reading = self.distance_sensor.get_distance()
+            if reading.valid:
+                return reading.distance
+
+        # Fall back to visual estimation
+        if self._visual_estimator and bbox.width > 0:
+            reading = self._visual_estimator.estimate_from_detection(
+                bbox_width=int(bbox.width),
+                object_type=object_type,
+                known_size=self.config.default_object_size,
+            )
+            if reading.valid:
+                return reading.distance
+
+        # Default: assume we need to approach
+        return 1.0
+
+    def _turn_toward_target(self, offset: float) -> None:
+        """Turn to center the target in view."""
+        # Positive offset = target is right, turn right (negative angular)
+        # Negative offset = target is left, turn left (positive angular)
+
+        # Scale turn based on offset magnitude
+        turn_rate = -offset * self.config.turn_speed * 2
+
+        # Use appropriate locomotion method
+        if hasattr(self.locomotion, 'turn_continuous'):
+            self.locomotion.turn_continuous(turn_rate)
+        elif hasattr(self.locomotion, 'walk'):
+            # For quadrupeds, walk in place while turning
+            direction = Vector3(0, -offset, 0)  # y component controls turn
+            self.locomotion.walk(direction, speed=0.1)
+        elif hasattr(self.locomotion, 'drive'):
+            # For wheeled, differential drive
+            self.locomotion.drive(0, turn_rate)
+
+    def _move_toward_target(self, distance: float) -> None:
+        """Move toward the target."""
+        # Calculate speed based on distance
+        speed = self.config.approach_speed
+        if distance < self.config.slow_distance:
+            # Slow down as we get close
+            speed *= distance / self.config.slow_distance
+            speed = max(speed, 0.1)  # Minimum speed
+
+        # Use appropriate locomotion method
+        if hasattr(self.locomotion, 'walk'):
+            self.locomotion.walk(Vector3.forward(), speed=speed)
+        elif hasattr(self.locomotion, 'drive'):
+            self.locomotion.drive(speed, 0)
+
+    def _stop_locomotion(self) -> None:
+        """Stop all locomotion."""
+        if hasattr(self.locomotion, 'stop'):
+            self.locomotion.stop()
+
+    def reset(self) -> None:
+        """Reset behavior state."""
+        super().reset()
+        self._state = ApproachState.SEARCHING
+        self._start_time = None
+        self._last_detection_time = None
+        self._current_distance = float('inf')
+
+
+class VisualServoApproach(BehaviorNode):
+    """
+    Simpler approach using only visual feedback.
+
+    Approaches until the bounding box reaches a target size in the image.
+    No distance sensor required - uses bbox size as proxy for distance.
+
+    Blackboard:
+    - Reads: "target_detection" or "target_bbox"
+    - Writes: "approach_complete"
+    """
+
+    def __init__(
+        self,
+        locomotion: LocomotionInterface,
+        target_bbox_height_fraction: float = 0.6,  # Target height as fraction of image
+        center_tolerance: float = 0.1,
+        approach_speed: float = 0.3,
+        image_height: int = 480,
+        name: str = "",
+    ):
+        super().__init__(name or "VisualServoApproach")
+        self.locomotion = locomotion
+        self.target_fraction = target_bbox_height_fraction
+        self.center_tolerance = center_tolerance
+        self.approach_speed = approach_speed
+        self.image_height = image_height
+
+    def tick(self) -> BehaviorStatus:
+        """Execute visual servo approach."""
+        if not self.blackboard:
+            return BehaviorStatus.FAILURE
+
+        # Get target bbox
+        detection = self.blackboard.get("target_detection")
+        bbox = None
+        if detection and isinstance(detection, Detection):
+            bbox = detection.bbox
+        else:
+            bbox = self.blackboard.get("target_bbox")
+
+        if not bbox:
+            # No target visible
+            if hasattr(self.locomotion, 'stop'):
+                self.locomotion.stop()
+            return BehaviorStatus.FAILURE
+
+        # Check if bbox is large enough (close enough)
+        height_fraction = bbox.height / self.image_height
+        if height_fraction >= self.target_fraction:
+            if hasattr(self.locomotion, 'stop'):
+                self.locomotion.stop()
+            if self.blackboard:
+                self.blackboard.set("approach_complete", True)
+            return BehaviorStatus.SUCCESS
+
+        # Calculate center offset
+        image_center_x = self.image_height * (4/3) / 2  # Assume 4:3 aspect
+        bbox_center_x = bbox.x + bbox.width / 2
+        offset = (bbox_center_x - image_center_x) / image_center_x
+
+        # Turn or approach
+        if abs(offset) > self.center_tolerance:
+            # Turn toward target
+            turn_rate = -offset * 0.5
+            if hasattr(self.locomotion, 'turn_continuous'):
+                self.locomotion.turn_continuous(turn_rate)
+            elif hasattr(self.locomotion, 'walk'):
+                self.locomotion.walk(Vector3(0, -offset, 0), speed=0.1)
+        else:
+            # Approach
+            speed = self.approach_speed * (1 - height_fraction / self.target_fraction)
+            speed = max(speed, 0.1)
+            if hasattr(self.locomotion, 'walk'):
+                self.locomotion.walk(Vector3.forward(), speed=speed)
+            elif hasattr(self.locomotion, 'drive'):
+                self.locomotion.drive(speed, 0)
+
+        return BehaviorStatus.RUNNING