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

ate/transports/hybrid.py

@@ -0,0 +1,444 @@
+"""
+Hybrid Transport Implementation.
+
+Combines multiple transports (BLE + Serial) to provide unified access
+to all robot capabilities. This is the key abstraction that solves
+the "different capabilities on different transports" problem.
+
+Example: MechDog
+- BLE: Locomotion (walk, turn, stand), sensors (ultrasonic, battery)
+- USB Serial: Arm control (MicroPython REPL), gripper, raw servo access
+
+The hybrid transport automatically routes commands to the right transport.
+"""
+
+import asyncio
+from typing import Optional
+from dataclasses import dataclass, field
+
+from .base import (
+    RobotTransport,
+    TransportState,
+    TransportCapability,
+    ConnectionInfo,
+    CommandResult,
+)
+from .ble import BLETransport
+from .serial import SerialTransport
+
+
+@dataclass
+class HybridConfig:
+    """Configuration for hybrid transport."""
+    prefer_ble: bool = True  # Prefer BLE when both support a capability
+    auto_connect_serial: bool = False  # Auto-connect serial if BLE connected
+    parallel_discovery: bool = True
+
+
+# Mapping of capabilities to preferred transport
+CAPABILITY_TRANSPORT_MAP = {
+    # BLE is preferred for these (wireless, lower latency for movement)
+    TransportCapability.WALK: "ble",
+    TransportCapability.TURN: "ble",
+    TransportCapability.STAND: "ble",
+    TransportCapability.SIT: "ble",
+    TransportCapability.POSTURE: "ble",
+    TransportCapability.ACTION_GROUPS: "ble",
+    TransportCapability.ULTRASONIC: "ble",
+    TransportCapability.BATTERY: "ble",
+    TransportCapability.RGB_LED: "ble",
+
+    # Serial is REQUIRED for these (BLE can't do them on MechDog)
+    TransportCapability.ARM_POSITION: "serial",
+    TransportCapability.GRIPPER: "serial",
+    TransportCapability.RAW_SERVO: "serial",
+    TransportCapability.IMU: "serial",
+}
+
+
+class HybridTransport(RobotTransport):
+    """
+    Hybrid transport that combines BLE and Serial connections.
+
+    Automatically routes commands to the appropriate transport based on:
+    1. Required capability for the command
+    2. Which transports have that capability
+    3. User preference (prefer_ble config)
+
+    Example:
+        async with HybridTransport() as robot:
+            # Connect to both transports
+            await robot.connect_all(
+                ble_address="FAF198B6-...",
+                serial_port="/dev/cu.usbmodem1234"
+            )
+
+            # These automatically route to the right transport
+            await robot.walk_forward()      # -> BLE
+            await robot.gripper_open()      # -> Serial
+            await robot.get_battery()       # -> BLE
+            await robot.arm_move([(1, 90)]) # -> Serial
+    """
+
+    def __init__(self, config: Optional[HybridConfig] = None):
+        super().__init__()
+
+        self.config = config or HybridConfig()
+        self._ble: Optional[BLETransport] = None
+        self._serial: Optional[SerialTransport] = None
+
+    @property
+    def ble(self) -> Optional[BLETransport]:
+        """Access underlying BLE transport."""
+        return self._ble
+
+    @property
+    def serial_transport(self) -> Optional[SerialTransport]:
+        """Access underlying serial transport."""
+        return self._serial
+
+    @property
+    def is_connected(self) -> bool:
+        """True if at least one transport is connected."""
+        ble_connected = self._ble and self._ble.is_connected
+        serial_connected = self._serial and self._serial.is_connected
+        return ble_connected or serial_connected
+
+    async def connect(self, address: str, **kwargs) -> bool:
+        """
+        Connect using the appropriate transport based on address format.
+
+        Args:
+            address: Either BLE address (UUID/MAC) or serial port path
+        """
+        if address.startswith("/dev") or address.startswith("COM"):
+            return await self.connect_serial(address, **kwargs)
+        else:
+            return await self.connect_ble(address, **kwargs)
+
+    async def connect_ble(self, address: str, **kwargs) -> bool:
+        """Connect BLE transport."""
+        self._ble = BLETransport()
+        success = await self._ble.connect(address, **kwargs)
+
+        if success:
+            self._state = TransportState.CONNECTED
+            self._update_capabilities()
+
+        return success
+
+    async def connect_serial(self, port: str, **kwargs) -> bool:
+        """Connect serial transport."""
+        self._serial = SerialTransport()
+        success = await self._serial.connect(port, **kwargs)
+
+        if success:
+            self._state = TransportState.CONNECTED
+            self._update_capabilities()
+
+        return success
+
+    async def connect_all(
+        self,
+        ble_address: Optional[str] = None,
+        serial_port: Optional[str] = None,
+        **kwargs
+    ) -> dict[str, bool]:
+        """
+        Connect to both transports.
+
+        Args:
+            ble_address: BLE device address
+            serial_port: Serial port path
+
+        Returns:
+            Dict with connection status for each transport
+        """
+        results = {}
+
+        if ble_address:
+            results["ble"] = await self.connect_ble(ble_address, **kwargs)
+
+        if serial_port:
+            results["serial"] = await self.connect_serial(serial_port, **kwargs)
+
+        return results
+
+    async def disconnect(self) -> None:
+        """Disconnect all transports."""
+        if self._ble:
+            await self._ble.disconnect()
+            self._ble = None
+
+        if self._serial:
+            await self._serial.disconnect()
+            self._serial = None
+
+        self._state = TransportState.DISCONNECTED
+
+    async def send(self, data: bytes) -> CommandResult:
+        """
+        Send raw data using BLE transport (preferred for raw commands).
+
+        For capability-specific commands, use the high-level methods instead.
+        """
+        if self._ble and self._ble.is_connected:
+            return await self._ble.send(data)
+        elif self._serial and self._serial.is_connected:
+            return await self._serial.send(data)
+        else:
+            return CommandResult(success=False, error="No transport connected")
+
+    async def send_command(self, command: str) -> CommandResult:
+        """
+        Send a command string, routing to appropriate transport.
+
+        Tries to infer the right transport from command format.
+        """
+        # HiWonder BLE commands start with "CMD|"
+        if command.startswith("CMD|") and self._ble and self._ble.is_connected:
+            return await self._ble.send_command(command)
+
+        # Python-like commands go to serial REPL
+        if self._serial and self._serial.is_connected:
+            return await self._serial.send_command(command)
+
+        # Fall back to BLE
+        if self._ble and self._ble.is_connected:
+            return await self._ble.send_command(command)
+
+        return CommandResult(success=False, error="No transport connected")
+
+    @classmethod
+    async def discover(cls, timeout: float = 5.0) -> list[ConnectionInfo]:
+        """Discover all available robots across all transports."""
+        results = []
+
+        # Discover in parallel
+        ble_task = asyncio.create_task(BLETransport.discover(timeout))
+        serial_task = asyncio.create_task(SerialTransport.discover(timeout))
+
+        ble_results, serial_results = await asyncio.gather(
+            ble_task, serial_task,
+            return_exceptions=True
+        )
+
+        if isinstance(ble_results, list):
+            results.extend(ble_results)
+        if isinstance(serial_results, list):
+            results.extend(serial_results)
+
+        return results
+
+    async def probe_capabilities(self) -> set[TransportCapability]:
+        """Probe capabilities from all connected transports."""
+        caps = set()
+
+        if self._ble and self._ble.is_connected:
+            caps.update(await self._ble.probe_capabilities())
+
+        if self._serial and self._serial.is_connected:
+            caps.update(await self._serial.probe_capabilities())
+
+        self._capabilities = caps
+        return caps
+
+    def _update_capabilities(self) -> None:
+        """Update combined capabilities from all transports."""
+        caps = set()
+
+        if self._ble:
+            caps.update(self._ble.capabilities)
+
+        if self._serial:
+            caps.update(self._serial.capabilities)
+
+        self._capabilities = caps
+
+    def _get_transport_for_capability(
+        self,
+        cap: TransportCapability
+    ) -> Optional[RobotTransport]:
+        """Get the appropriate transport for a capability."""
+        preferred = CAPABILITY_TRANSPORT_MAP.get(cap)
+
+        # Check preferred transport first
+        if preferred == "ble" and self._ble and self._ble.has_capability(cap):
+            return self._ble
+        if preferred == "serial" and self._serial and self._serial.has_capability(cap):
+            return self._serial
+
+        # Fall back to any transport that has the capability
+        if self._ble and self._ble.has_capability(cap):
+            return self._ble
+        if self._serial and self._serial.has_capability(cap):
+            return self._serial
+
+        return None
+
+    # ========== High-Level Robot Commands ==========
+    # These automatically route to the correct transport
+
+    async def walk_forward(self, speed: int = 100) -> CommandResult:
+        """Walk forward - routes to BLE."""
+        if self._ble and self._ble.is_connected:
+            return await self._ble.walk_forward(speed)
+        return CommandResult(success=False, error="BLE not connected")
+
+    async def walk_backward(self, speed: int = 100) -> CommandResult:
+        """Walk backward - routes to BLE."""
+        if self._ble and self._ble.is_connected:
+            return await self._ble.walk_backward(speed)
+        return CommandResult(success=False, error="BLE not connected")
+
+    async def stop(self) -> CommandResult:
+        """Stop movement - routes to BLE."""
+        if self._ble and self._ble.is_connected:
+            return await self._ble.stop()
+        return CommandResult(success=False, error="BLE not connected")
+
+    async def stand(self) -> CommandResult:
+        """Stand up - routes to BLE."""
+        if self._ble and self._ble.is_connected:
+            return await self._ble.stand()
+        return CommandResult(success=False, error="BLE not connected")
+
+    async def sit(self) -> CommandResult:
+        """Sit down - routes to BLE."""
+        if self._ble and self._ble.is_connected:
+            return await self._ble.sit()
+        return CommandResult(success=False, error="BLE not connected")
+
+    async def get_battery(self) -> Optional[float]:
+        """Get battery voltage - routes to BLE."""
+        if self._ble and self._ble.is_connected:
+            return await self._ble.get_battery()
+        return None
+
+    async def get_ultrasonic(self) -> Optional[int]:
+        """Get ultrasonic distance - routes to BLE."""
+        if self._ble and self._ble.is_connected:
+            return await self._ble.get_ultrasonic()
+        return None
+
+    async def run_action_group(self, group_id: int) -> CommandResult:
+        """Run action group - routes to BLE."""
+        if self._ble and self._ble.is_connected:
+            return await self._ble.run_action_group(group_id)
+        return CommandResult(success=False, error="BLE not connected")
+
+    async def arm_move(self, positions: list[tuple[int, int]]) -> CommandResult:
+        """Move arm servos - routes to Serial."""
+        if self._serial and self._serial.is_connected:
+            return await self._serial.arm_move(positions)
+        return CommandResult(
+            success=False,
+            error="Serial not connected (arm control requires USB)"
+        )
+
+    async def gripper_open(self) -> CommandResult:
+        """Open gripper - routes to Serial."""
+        if self._serial and self._serial.is_connected:
+            return await self._serial.gripper_open()
+        return CommandResult(
+            success=False,
+            error="Serial not connected (gripper requires USB)"
+        )
+
+    async def gripper_close(self) -> CommandResult:
+        """Close gripper - routes to Serial."""
+        if self._serial and self._serial.is_connected:
+            return await self._serial.gripper_close()
+        return CommandResult(
+            success=False,
+            error="Serial not connected (gripper requires USB)"
+        )
+
+    async def execute_python(self, code: str) -> CommandResult:
+        """Execute Python code on robot - routes to Serial."""
+        if self._serial and self._serial.is_connected:
+            return await self._serial.execute_python(code)
+        return CommandResult(
+            success=False,
+            error="Serial not connected (Python execution requires USB)"
+        )
+
+    # ========== Ball Pickup Sequence ==========
+    # This demonstrates the power of hybrid transport
+
+    async def ball_pickup_sequence(
+        self,
+        approach_speed: int = 80,
+        max_distance_mm: int = 300
+    ) -> bool:
+        """
+        Execute ball pickup using both transports.
+
+        This is the canonical example of why hybrid transport matters:
+        1. Use BLE ultrasonic to detect ball
+        2. Use BLE locomotion to approach
+        3. Use Serial to control gripper
+        4. Use BLE to walk back
+
+        Returns:
+            True if pickup was successful
+        """
+        # Check we have both transports
+        if not (self._ble and self._ble.is_connected):
+            print("ERROR: BLE not connected (needed for locomotion/sensors)")
+            return False
+
+        if not (self._serial and self._serial.is_connected):
+            print("WARNING: Serial not connected (gripper may not work)")
+
+        print("=== Ball Pickup Sequence ===")
+
+        # Step 1: Detect ball with ultrasonic
+        print("1. Scanning for ball...")
+        distance = await self.get_ultrasonic()
+        if distance is None:
+            print("   ERROR: Ultrasonic not responding")
+            return False
+
+        print(f"   Distance: {distance}mm")
+        if distance > max_distance_mm:
+            print(f"   Ball too far (max {max_distance_mm}mm)")
+            return False
+
+        # Step 2: Approach
+        print("2. Approaching ball...")
+        while distance > 100:  # Stop when close
+            await self.walk_forward(approach_speed)
+            await asyncio.sleep(0.5)
+            distance = await self.get_ultrasonic() or distance
+            print(f"   Distance: {distance}mm")
+
+        await self.stop()
+        print("   Stopped at pickup position")
+
+        # Step 3: Lower and grab (uses Serial for gripper)
+        print("3. Grabbing ball...")
+        if self._serial and self._serial.is_connected:
+            await self.gripper_open()
+            await asyncio.sleep(0.5)
+            # Lower arm (example servo positions)
+            await self.arm_move([(1, 45), (2, 90), (3, 45)])
+            await asyncio.sleep(0.5)
+            await self.gripper_close()
+            await asyncio.sleep(0.5)
+            # Raise arm
+            await self.arm_move([(1, 90), (2, 45), (3, 90)])
+            print("   Grabbed!")
+        else:
+            print("   SKIP: No serial connection for gripper")
+
+        # Step 4: Walk back
+        print("4. Returning...")
+        for _ in range(5):
+            await self.walk_backward(approach_speed)
+            await asyncio.sleep(0.5)
+
+        await self.stop()
+        await self.stand()
+        print("=== Pickup Complete ===")
+        return True