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

ate/robot/discovery.py

@@ -0,0 +1,339 @@
+"""
+Auto-discovery of robots on network and USB.
+
+Scans for:
+- Serial devices matching known patterns
+- Network cameras (ESP32-CAM, IP cameras)
+- ROS2 topics (if ROS2 available)
+- mDNS services
+"""
+
+import os
+import glob
+import time
+import platform
+import concurrent.futures
+from dataclasses import dataclass, field
+from typing import List, Optional, Dict, Any
+from enum import Enum, auto
+
+try:
+    import serial
+    import serial.tools.list_ports
+    HAS_SERIAL = True
+except ImportError:
+    HAS_SERIAL = False
+
+try:
+    import requests
+    HAS_REQUESTS = True
+except ImportError:
+    HAS_REQUESTS = False
+
+from .registry import KNOWN_ROBOTS, RobotType, ConnectionType
+
+
+class DiscoveryStatus(Enum):
+    """Status of discovered device."""
+    FOUND = auto()        # Device found
+    IDENTIFIED = auto()   # Device identified as known robot
+    CONNECTED = auto()    # Successfully connected
+    ERROR = auto()        # Error during discovery
+
+
+@dataclass
+class DiscoveredRobot:
+    """A robot discovered on the network or USB."""
+    robot_type: Optional[str] = None    # ID of matched robot type
+    name: str = ""                       # Display name
+    status: DiscoveryStatus = DiscoveryStatus.FOUND
+
+    # Connection info
+    connection: Optional[ConnectionType] = None
+    port: Optional[str] = None           # Serial port
+    ip: Optional[str] = None             # Network IP
+    ports: Dict[str, int] = field(default_factory=dict)  # Network ports
+
+    # Additional info
+    manufacturer: Optional[str] = None
+    model: Optional[str] = None
+    firmware: Optional[str] = None
+
+    # Raw data
+    raw_data: Dict[str, Any] = field(default_factory=dict)
+
+
+def discover_robots(
+    timeout: float = 5.0,
+    scan_serial: bool = True,
+    scan_network: bool = True,
+    network_subnet: Optional[str] = None,
+) -> List[DiscoveredRobot]:
+    """
+    Discover all robots on network and USB.
+
+    Args:
+        timeout: Timeout for network scans
+        scan_serial: Scan USB serial devices
+        scan_network: Scan network for cameras/robots
+        network_subnet: Subnet to scan (e.g., "192.168.1")
+
+    Returns:
+        List of discovered robots
+    """
+    discovered = []
+
+    if scan_serial:
+        discovered.extend(discover_serial_robots())
+
+    if scan_network:
+        discovered.extend(discover_network_cameras(
+            timeout=timeout,
+            subnet=network_subnet
+        ))
+
+    return discovered
+
+
+def discover_serial_robots() -> List[DiscoveredRobot]:
+    """
+    Discover robots connected via USB serial.
+
+    Matches connected serial devices against known robot patterns.
+    """
+    if not HAS_SERIAL:
+        return []
+
+    discovered = []
+    ports = serial.tools.list_ports.comports()
+
+    for port in ports:
+        robot = DiscoveredRobot(
+            connection=ConnectionType.SERIAL,
+            port=port.device,
+            raw_data={
+                "vid": port.vid,
+                "pid": port.pid,
+                "serial_number": port.serial_number,
+                "manufacturer": port.manufacturer,
+                "product": port.product,
+                "description": port.description,
+            }
+        )
+
+        # Try to identify robot type by matching patterns
+        for robot_type in KNOWN_ROBOTS.values():
+            if ConnectionType.SERIAL not in robot_type.connection_types:
+                continue
+
+            for pattern in robot_type.serial_patterns:
+                # Convert glob pattern to check
+                if _matches_pattern(port.device, pattern):
+                    robot.robot_type = robot_type.id
+                    robot.name = f"{robot_type.name} ({port.device})"
+                    robot.manufacturer = robot_type.manufacturer
+                    robot.model = robot_type.name
+                    robot.status = DiscoveryStatus.IDENTIFIED
+                    break
+
+            if robot.robot_type:
+                break
+
+        if not robot.robot_type:
+            robot.name = f"Unknown Device ({port.device})"
+            robot.manufacturer = port.manufacturer
+            robot.model = port.product
+
+        discovered.append(robot)
+
+    return discovered
+
+
+def discover_network_cameras(
+    timeout: float = 2.0,
+    subnet: Optional[str] = None,
+    max_workers: int = 50,
+) -> List[DiscoveredRobot]:
+    """
+    Discover network cameras (ESP32-CAM, IP cameras).
+
+    Scans common camera endpoints on the local subnet.
+    """
+    if not HAS_REQUESTS:
+        return []
+
+    # Determine subnet to scan
+    if subnet is None:
+        subnet = _get_local_subnet()
+        if subnet is None:
+            return []
+
+    discovered = []
+    ips_to_scan = [f"{subnet}.{i}" for i in range(1, 255)]
+
+    def check_camera(ip: str) -> Optional[DiscoveredRobot]:
+        """Check if IP has a camera endpoint."""
+        try:
+            # Try ESP32-CAM status endpoint
+            response = requests.get(
+                f"http://{ip}/status",
+                timeout=timeout
+            )
+            if response.status_code == 200:
+                return DiscoveredRobot(
+                    robot_type=None,  # Camera only, not a robot
+                    name=f"ESP32-CAM ({ip})",
+                    status=DiscoveryStatus.FOUND,
+                    connection=ConnectionType.WIFI,
+                    ip=ip,
+                    ports={"camera_port": 80, "camera_stream_port": 81},
+                    raw_data={"type": "esp32cam", "response": response.text[:200]},
+                )
+        except requests.RequestException:
+            pass
+
+        try:
+            # Try generic camera snapshot
+            response = requests.get(
+                f"http://{ip}/capture",
+                timeout=timeout,
+                stream=True
+            )
+            if response.status_code == 200:
+                content_type = response.headers.get("Content-Type", "")
+                if "image" in content_type:
+                    return DiscoveredRobot(
+                        name=f"Network Camera ({ip})",
+                        status=DiscoveryStatus.FOUND,
+                        connection=ConnectionType.WIFI,
+                        ip=ip,
+                        ports={"camera_port": 80},
+                        raw_data={"type": "generic_camera"},
+                    )
+        except requests.RequestException:
+            pass
+
+        return None
+
+    # Parallel scan
+    with concurrent.futures.ThreadPoolExecutor(max_workers=max_workers) as executor:
+        futures = {executor.submit(check_camera, ip): ip for ip in ips_to_scan}
+
+        try:
+            for future in concurrent.futures.as_completed(futures, timeout=timeout * 3):
+                try:
+                    result = future.result(timeout=0.1)
+                    if result:
+                        discovered.append(result)
+                except Exception:
+                    pass
+        except TimeoutError:
+            # Some futures didn't complete in time, that's ok
+            pass
+
+    return discovered
+
+
+def probe_serial_device(port: str, baud_rate: int = 115200) -> Optional[Dict[str, Any]]:
+    """
+    Probe a serial device to identify what type of robot it is.
+
+    Sends identification commands and parses response.
+    """
+    if not HAS_SERIAL:
+        return None
+
+    try:
+        with serial.Serial(port, baud_rate, timeout=2) as ser:
+            time.sleep(0.5)
+
+            # Try MicroPython REPL identification
+            ser.write(b'\x03')  # Ctrl+C
+            time.sleep(0.2)
+            ser.write(b'\x02')  # Ctrl+B for friendly REPL
+            time.sleep(0.3)
+
+            # Check for MicroPython
+            ser.write(b'import sys; print(sys.implementation)\r\n')
+            time.sleep(0.5)
+            response = ser.read(1000).decode('utf-8', errors='ignore')
+
+            if 'micropython' in response.lower():
+                info = {"type": "micropython", "response": response}
+
+                # Try to detect MechDog
+                ser.write(b'from HW_MechDog import MechDog\r\n')
+                time.sleep(0.3)
+                response2 = ser.read(500).decode('utf-8', errors='ignore')
+
+                if 'Error' not in response2 and 'Traceback' not in response2:
+                    info["robot"] = "hiwonder_mechdog"
+
+                return info
+
+    except Exception as e:
+        return {"error": str(e)}
+
+    return None
+
+
+def _matches_pattern(device: str, pattern: str) -> bool:
+    """Check if device matches a glob pattern."""
+    import fnmatch
+    return fnmatch.fnmatch(device, pattern)
+
+
+def _get_local_subnet() -> Optional[str]:
+    """Get the local network subnet (e.g., '192.168.1')."""
+    system = platform.system()
+
+    try:
+        import socket
+        s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        s.connect(("8.8.8.8", 80))
+        ip = s.getsockname()[0]
+        s.close()
+
+        # Extract subnet
+        parts = ip.split(".")
+        if len(parts) == 4:
+            return ".".join(parts[:3])
+    except Exception:
+        pass
+
+    return None
+
+
+def quick_scan() -> Dict[str, Any]:
+    """
+    Quick scan for common robot configurations.
+
+    Returns summary of what was found.
+    """
+    result = {
+        "serial_ports": [],
+        "network_cameras": [],
+        "identified_robots": [],
+    }
+
+    # Scan serial
+    serial_robots = discover_serial_robots()
+    for robot in serial_robots:
+        result["serial_ports"].append({
+            "port": robot.port,
+            "type": robot.robot_type,
+            "name": robot.name,
+        })
+        if robot.robot_type:
+            result["identified_robots"].append(robot)
+
+    # Quick network scan (just local subnet, fast timeout)
+    network_cameras = discover_network_cameras(timeout=1.0)
+    for camera in network_cameras:
+        result["network_cameras"].append({
+            "ip": camera.ip,
+            "name": camera.name,
+            "ports": camera.ports,
+        })
+
+    return result

ate/robot/introspection.py

@@ -0,0 +1,330 @@
+"""
+Robot capability introspection.
+
+Inspect what a robot can do by examining its interfaces and methods.
+"""
+
+import inspect
+from typing import Dict, List, Set, Any, Optional, Type
+from dataclasses import dataclass
+
+from ..interfaces import (
+    RobotInterface,
+    SafetyInterface,
+    QuadrupedLocomotion,
+    BipedLocomotion,
+    WheeledLocomotion,
+    AerialLocomotion,
+    ArmInterface,
+    GripperInterface,
+    CameraInterface,
+    DepthCameraInterface,
+    LidarInterface,
+    IMUInterface,
+    BodyPoseInterface,
+    NavigationInterface,
+    ObjectDetectionInterface,
+)
+
+
+@dataclass
+class MethodInfo:
+    """Information about a robot method."""
+    name: str
+    interface: str
+    description: str
+    parameters: List[str]
+    return_type: str
+
+
+@dataclass
+class CapabilityInfo:
+    """Information about a robot capability."""
+    name: str
+    interface_class: str
+    description: str
+    methods: List[MethodInfo]
+    available: bool = True
+
+
+# Map interfaces to capability names
+INTERFACE_CAPABILITIES = {
+    QuadrupedLocomotion: "quadruped_locomotion",
+    BipedLocomotion: "bipedal_locomotion",
+    WheeledLocomotion: "wheeled_locomotion",
+    AerialLocomotion: "aerial_locomotion",
+    ArmInterface: "arm",
+    GripperInterface: "gripper",
+    CameraInterface: "camera",
+    DepthCameraInterface: "depth_camera",
+    LidarInterface: "lidar",
+    IMUInterface: "imu",
+    BodyPoseInterface: "body_pose",
+    SafetyInterface: "safety",
+    NavigationInterface: "navigation",
+    ObjectDetectionInterface: "object_detection",
+}
+
+
+def get_capabilities(robot: Any) -> Dict[str, CapabilityInfo]:
+    """
+    Get all capabilities of a robot instance.
+
+    Args:
+        robot: Robot instance
+
+    Returns:
+        Dict mapping capability name to CapabilityInfo
+    """
+    capabilities = {}
+
+    for interface, cap_name in INTERFACE_CAPABILITIES.items():
+        if isinstance(robot, interface):
+            methods = get_interface_methods(interface)
+            capabilities[cap_name] = CapabilityInfo(
+                name=cap_name,
+                interface_class=interface.__name__,
+                description=interface.__doc__ or "",
+                methods=methods,
+                available=True,
+            )
+
+    return capabilities
+
+
+def get_methods(robot: Any) -> Dict[str, List[MethodInfo]]:
+    """
+    Get all available methods grouped by capability.
+
+    Args:
+        robot: Robot instance
+
+    Returns:
+        Dict mapping capability name to list of methods
+    """
+    result = {}
+
+    for interface, cap_name in INTERFACE_CAPABILITIES.items():
+        if isinstance(robot, interface):
+            methods = get_interface_methods(interface)
+            if methods:
+                result[cap_name] = methods
+
+    return result
+
+
+def get_interface_methods(interface: Type) -> List[MethodInfo]:
+    """
+    Get methods defined by an interface.
+
+    Args:
+        interface: Interface class
+
+    Returns:
+        List of MethodInfo
+    """
+    methods = []
+
+    for name, method in inspect.getmembers(interface, predicate=inspect.isfunction):
+        if name.startswith("_"):
+            continue
+
+        # Get signature
+        try:
+            sig = inspect.signature(method)
+            params = [
+                p.name for p in sig.parameters.values()
+                if p.name != "self"
+            ]
+            return_type = str(sig.return_annotation) if sig.return_annotation != inspect.Signature.empty else "None"
+        except (ValueError, TypeError):
+            params = []
+            return_type = "Unknown"
+
+        # Clean up return type
+        return_type = return_type.replace("typing.", "").replace("<class '", "").replace("'>", "")
+
+        methods.append(MethodInfo(
+            name=name,
+            interface=interface.__name__,
+            description=method.__doc__ or "",
+            parameters=params,
+            return_type=return_type,
+        ))
+
+    return methods
+
+
+def test_capability(robot: Any, capability: str) -> Dict[str, Any]:
+    """
+    Test if a capability is working.
+
+    Args:
+        robot: Robot instance
+        capability: Capability name to test
+
+    Returns:
+        Test result with status and details
+    """
+    result = {
+        "capability": capability,
+        "status": "unknown",
+        "tests": [],
+    }
+
+    # Find the interface for this capability
+    interface = None
+    for iface, cap_name in INTERFACE_CAPABILITIES.items():
+        if cap_name == capability:
+            interface = iface
+            break
+
+    if interface is None:
+        result["status"] = "error"
+        result["error"] = f"Unknown capability: {capability}"
+        return result
+
+    if not isinstance(robot, interface):
+        result["status"] = "not_available"
+        result["error"] = f"Robot does not have capability: {capability}"
+        return result
+
+    # Run capability-specific tests
+    try:
+        if capability == "camera":
+            result["tests"].append(_test_camera(robot))
+        elif capability == "quadruped_locomotion":
+            result["tests"].append(_test_locomotion(robot))
+        elif capability == "body_pose":
+            result["tests"].append(_test_body_pose(robot))
+        elif capability == "safety":
+            result["tests"].append(_test_safety(robot))
+        else:
+            result["tests"].append({
+                "name": "basic_check",
+                "status": "passed",
+                "message": f"Robot implements {interface.__name__}",
+            })
+
+        # Determine overall status
+        if all(t.get("status") == "passed" for t in result["tests"]):
+            result["status"] = "passed"
+        elif any(t.get("status") == "failed" for t in result["tests"]):
+            result["status"] = "failed"
+        else:
+            result["status"] = "partial"
+
+    except Exception as e:
+        result["status"] = "error"
+        result["error"] = str(e)
+
+    return result
+
+
+def _test_camera(robot: Any) -> Dict[str, Any]:
+    """Test camera capability."""
+    test = {"name": "camera_capture", "status": "unknown"}
+
+    try:
+        image = robot.get_image()
+        if image.width > 0 and image.height > 0:
+            test["status"] = "passed"
+            test["message"] = f"Captured {image.width}x{image.height} image"
+        else:
+            test["status"] = "failed"
+            test["message"] = "Image capture returned empty"
+    except Exception as e:
+        test["status"] = "failed"
+        test["error"] = str(e)
+
+    return test
+
+
+def _test_locomotion(robot: Any) -> Dict[str, Any]:
+    """Test locomotion capability (non-destructive)."""
+    test = {"name": "locomotion_status", "status": "unknown"}
+
+    try:
+        if hasattr(robot, "is_moving"):
+            is_moving = robot.is_moving()
+            test["status"] = "passed"
+            test["message"] = f"Robot is {'moving' if is_moving else 'stationary'}"
+        else:
+            test["status"] = "passed"
+            test["message"] = "Locomotion interface available"
+    except Exception as e:
+        test["status"] = "failed"
+        test["error"] = str(e)
+
+    return test
+
+
+def _test_body_pose(robot: Any) -> Dict[str, Any]:
+    """Test body pose capability."""
+    test = {"name": "body_pose_read", "status": "unknown"}
+
+    try:
+        height = robot.get_body_height()
+        test["status"] = "passed"
+        test["message"] = f"Body height: {height:.3f}m"
+    except Exception as e:
+        test["status"] = "failed"
+        test["error"] = str(e)
+
+    return test
+
+
+def _test_safety(robot: Any) -> Dict[str, Any]:
+    """Test safety capability."""
+    test = {"name": "safety_status", "status": "unknown"}
+
+    try:
+        estopped = robot.is_estopped()
+        test["status"] = "passed"
+        test["message"] = f"E-stop: {'active' if estopped else 'inactive'}"
+    except Exception as e:
+        test["status"] = "failed"
+        test["error"] = str(e)
+
+    return test
+
+
+def format_capabilities(capabilities: Dict[str, CapabilityInfo]) -> str:
+    """Format capabilities for CLI display."""
+    lines = []
+
+    for name, info in capabilities.items():
+        lines.append(f"\n{name.upper()}")
+        lines.append("-" * len(name))
+
+        if info.description:
+            first_line = info.description.strip().split("\n")[0]
+            lines.append(f"  {first_line}")
+
+        lines.append("  Methods:")
+        for method in info.methods[:5]:  # Show first 5 methods
+            params = ", ".join(method.parameters[:3])  # Show first 3 params
+            if len(method.parameters) > 3:
+                params += ", ..."
+            lines.append(f"    - {method.name}({params})")
+
+        if len(info.methods) > 5:
+            lines.append(f"    ... and {len(info.methods) - 5} more")
+
+    return "\n".join(lines)
+
+
+def format_methods(methods: Dict[str, List[MethodInfo]]) -> str:
+    """Format methods for CLI display."""
+    lines = []
+
+    for capability, method_list in methods.items():
+        lines.append(f"\n[{capability}]")
+        for method in method_list:
+            params = ", ".join(method.parameters)
+            lines.append(f"  {method.name}({params}) -> {method.return_type}")
+            if method.description:
+                first_line = method.description.strip().split("\n")[0][:60]
+                lines.append(f"    {first_line}")
+
+    return "\n".join(lines)