infinicode 2.8.97 → 2.8.99

package/packages/robopark/scheduler/robot_supervisor.py

@@ -403,7 +403,7 @@ def _post_supervisor_output(identity, kind: str, service: Optional[str], payload
         logger.debug(f"supervisor-output POST failed: {e}")
 
 
-def _handle_shell_command(identity, cmd: dict) -> None:
+def _handle_shell_command(identity, cmd: dict) -> None:
     """Process a single shell/log request returned by the scheduler.
 
     `cmd` is {id, kind: "shell_run"|"tail_logs"|"speaker_test", service,
@@ -425,11 +425,89 @@ def _handle_shell_command(identity, cmd: dict) -> None:
         result = _tail_log_file(log_path, int(params.get("lines", 200)))
     elif kind == "shell_run":
         result = _run_shell_command(params.get("name", ""), params)
-    elif kind == "speaker_test":
-        result = _speaker_roundtrip_test(params)
+    elif kind == "speaker_test":
+        result = _speaker_roundtrip_test(params)
+    elif kind == "motor_sequence":
+        result = _run_motor_sequence(params)
     else:
         result = {"ok": False, "error": f"unknown shell command kind: {kind!r}"}
-    _post_supervisor_output(identity, kind, service, result, request_id)
+    _post_supervisor_output(identity, kind, service, result, request_id)
+
+
+def _run_motor_sequence(params: dict) -> dict:
+    """Run one validated scheduler sequence against the robot-local motor API."""
+    import httpx
+    base = str(params.get("motor_server_url") or "http://127.0.0.1:8001").rstrip("/")
+    if not (base.startswith("http://127.0.0.1:") or base.startswith("http://localhost:")):
+        return {"ok": False, "error": "motor server must be robot-local"}
+    registry = {str(item.get("id")): item for item in (params.get("registry") or [])}
+    steps = params.get("steps") or []
+    started = time.monotonic()
+    completed = []
+    try:
+        with httpx.Client(timeout=8.0) as client:
+            existing = client.get(f"{base}/list-motors").json().get("motors", [])
+            existing_names = {str(item.get("name")) for item in existing}
+            for motor_id, motor in registry.items():
+                body = {"name": motor_id, "gpio": int(motor["gpio"]), "active_high": bool(motor.get("active_high", True))}
+                response = (client.put(f"{base}/update-motor/{motor_id}", json=body)
+                            if motor_id in existing_names else client.post(f"{base}/add-motor", json=body))
+                response.raise_for_status()
+            if params.get("test_all"):
+                duration_ms = max(50, min(1000, int(params.get("duration_ms", 300))))
+                pause_ms = max(0, min(2000, int(params.get("pause_ms", 200))))
+                response = client.post(f"{base}/test", json={
+                    "seconds_on": duration_ms / 1000.0,
+                    "seconds_pause": pause_ms / 1000.0,
+                    "pins": [int(motor["gpio"]) for motor in registry.values()],
+                })
+                response.raise_for_status()
+                deadline = time.monotonic() + len(registry) * ((duration_ms + pause_ms) / 1000.0) + 3.0
+                while time.monotonic() < deadline:
+                    status = client.get(f"{base}/status").json()
+                    if status.get("status") == "idle":
+                        if status.get("error"):
+                            raise RuntimeError(f"registered GPIO test failed: {status['error']}")
+                        completed = [{"motor_id": motor_id, "gpio": int(motor["gpio"])}
+                                     for motor_id, motor in registry.items()]
+                        break
+                    time.sleep(0.05)
+                else:
+                    raise TimeoutError("registered GPIO test did not return to idle")
+            for index, step in enumerate(steps):
+                motor_id = str(step.get("motor_id"))
+                motor = registry.get(motor_id)
+                if not motor:
+                    raise ValueError(f"step {index + 1} references unknown motor {motor_id}")
+                delay_ms = max(0, min(30000, int(step.get("delay_ms", 0))))
+                duration_ms = max(50, min(int(motor.get("max_duration_ms", 3000)), int(step.get("duration_ms", 500))))
+                if delay_ms:
+                    time.sleep(delay_ms / 1000.0)
+                response = client.post(f"{base}/trigger-motor", json={"motor_name": motor_id, "seconds": duration_ms / 1000.0})
+                response.raise_for_status()
+                deadline = time.monotonic() + duration_ms / 1000.0 + 2.0
+                while time.monotonic() < deadline:
+                    status_response = client.get(f"{base}/status")
+                    status_response.raise_for_status()
+                    status = status_response.json()
+                    if status.get("status") == "idle":
+                        if status.get("error"):
+                            raise RuntimeError(f"motor {motor_id} failed: {status['error']}")
+                        break
+                    time.sleep(0.05)
+                else:
+                    raise TimeoutError(f"motor {motor_id} did not return to idle")
+                completed.append({"motor_id": motor_id, "duration_ms": duration_ms})
+    except Exception as exc:
+        try:
+            httpx.post(f"{base}/stop-motors", json={}, timeout=3.0)
+        except Exception:
+            pass
+        return {"ok": False, "error": f"{type(exc).__name__}: {exc}", "completed_steps": completed,
+                "duration_ms": int((time.monotonic() - started) * 1000), "session_id": params.get("session_id")}
+    return {"ok": True, "pass": True, "sequence_id": params.get("sequence_id"), "completed_steps": completed,
+            "duration_ms": int((time.monotonic() - started) * 1000), "motor_server_url": base,
+            "session_id": params.get("session_id")}
 
 
 # ── Speaker roundtrip test (C2 gap-fill) ──
@@ -1021,7 +1099,7 @@ def run(config_path: Path) -> None:
                             _execute_command(target, cmd.get("action", "restart"))
                         elif kind == "supervisor_action":
                             logger.warning(f"remote command for unknown service {cmd.get('service_name')!r} ignored")
-                        elif kind in ("shell_run", "tail_logs", "speaker_test"):
+                        elif kind in ("shell_run", "tail_logs", "speaker_test", "motor_sequence"):
                             # run on a background thread so we don't block
                             # the 2s poll loop on a slow command
                             import threading

package/packages/robopark/vision/motor_server.py

@@ -1,11 +1,16 @@
 from fastapi import FastAPI, HTTPException
 from fastapi.middleware.cors import CORSMiddleware
-from pydantic import BaseModel
+from pydantic import BaseModel
 from typing import Optional, List
 import time
 import threading
 import json
-import os
+import os
+
+try:
+    import lgpio
+except ImportError:
+    lgpio = None
 
 app = FastAPI(title="Motor Control API", version="1.0.0")
 
@@ -27,7 +32,11 @@ active_motor = None
 remaining_seconds = 0
 last_action = "None"
 motor_thread = None
-stop_flag = False
+stop_flag = False
+motor_error = None
+gpio_handle = None
+claimed_pins = set()
+motor_lock = threading.Lock()
 
 # Load motors from file on startup
 def load_motors():
@@ -35,8 +44,16 @@ def load_motors():
     global motors
     try:
         if os.path.exists(MOTORS_FILE):
-            with open(MOTORS_FILE, 'r') as f:
-                motors = json.load(f)
+            with open(MOTORS_FILE, 'r') as f:
+                motors = json.load(f)
+            motors = {
+                str(name): ({"name": str(name), "gpio": int(value), "active_high": True}
+                            if not isinstance(value, dict) else {
+                                "name": str(value.get("name") or name), "gpio": int(value["gpio"]),
+                                "active_high": bool(value.get("active_high", True)),
+                            })
+                for name, value in motors.items()
+            }
             print(f"[LOAD] Loaded {len(motors)} motors from {MOTORS_FILE}")
             for name, gpio in motors.items():
                 print(f"  - {name}: GPIO {gpio}")
@@ -59,48 +76,92 @@ def save_motors():
 load_motors()
 
 # Request Models
-class MotorConfig(BaseModel):
-    name: str
-    gpio: int
+class MotorConfig(BaseModel):
+    name: str
+    gpio: int
+    active_high: bool = True
 
-class UpdateMotorConfig(BaseModel):
-    name: str
-    gpio: int
+class UpdateMotorConfig(BaseModel):
+    name: str
+    gpio: int
+    active_high: bool = True
 
-class TriggerMotor(BaseModel):
-    motor_name: str
-    seconds: int = 5
+class TriggerMotor(BaseModel):
+    motor_name: str
+    seconds: float = 1.0
 
 class StopMotors(BaseModel):
     motor_name: Optional[str] = None
 
-class TestConfig(BaseModel):
-    seconds_on: int = 2
-    seconds_pause: int = 1
+class TestConfig(BaseModel):
+    seconds_on: float = 0.3
+    seconds_pause: float = 0.2
     pins: Optional[List[int]] = None
 
 # Helper Functions
-def run_motor_thread(motor_name: str, duration: int):
-    global active_motor, remaining_seconds, last_action, stop_flag
-
-    active_motor = motor_name
-    remaining_seconds = duration
-    last_action = f"Triggered {motor_name}"
-
-    print(f"[MOTOR] Starting {motor_name} for {duration} seconds")
-
-    # Simulate motor running
-    for i in range(duration):
-        if stop_flag:
-            print(f"[MOTOR] {motor_name} stopped by user")
-            break
-        remaining_seconds = duration - i
-        time.sleep(1)
-
-    active_motor = None
-    remaining_seconds = 0
-    stop_flag = False
-    print(f"[MOTOR] {motor_name} completed")
+def _motor_value(motor: dict, active: bool) -> int:
+    return int(active == bool(motor.get("active_high", True)))
+
+def _claim_motor(motor: dict) -> None:
+    global gpio_handle
+    pin = int(motor["gpio"])
+    if pin < 2 or pin > 27:
+        raise ValueError(f"BCM GPIO {pin} is outside 2..27")
+    if lgpio is None:
+        raise RuntimeError("lgpio is not installed; refusing simulated production actuation")
+    if gpio_handle is None:
+        gpio_handle = lgpio.gpiochip_open(0)
+    if pin not in claimed_pins:
+        lgpio.gpio_claim_output(gpio_handle, pin, _motor_value(motor, False))
+        claimed_pins.add(pin)
+
+def _write_motor(motor: dict, active: bool) -> None:
+    _claim_motor(motor)
+    lgpio.gpio_write(gpio_handle, int(motor["gpio"]), _motor_value(motor, active))
+
+def _all_off() -> None:
+    for motor in motors.values():
+        try:
+            if isinstance(motor, dict):
+                _write_motor(motor, False)
+        except Exception as exc:
+            print(f"[GPIO] failed to de-energize {motor}: {exc}")
+
+def run_motor_thread(motor_name: str, duration: float):
+    global active_motor, remaining_seconds, last_action, stop_flag, motor_error
+
+    motor = motors[motor_name]
+    started = time.monotonic()
+    print(f"[MOTOR] Starting {motor_name} for {duration:.3f} seconds")
+    try:
+        _write_motor(motor, True)
+        while not stop_flag and time.monotonic() - started < duration:
+            remaining_seconds = max(0, duration - (time.monotonic() - started))
+            time.sleep(min(0.05, remaining_seconds or 0.01))
+    except Exception as exc:
+        motor_error = f"{type(exc).__name__}: {exc}"
+        last_action = f"Failed {motor_name}: {motor_error}"
+        print(f"[ERROR] Motor {motor_name} failed: {motor_error}")
+    finally:
+        try:
+            _write_motor(motor, False)
+        finally:
+            with motor_lock:
+                active_motor = None
+                remaining_seconds = 0
+                stop_flag = False
+            print(f"[MOTOR] {motor_name} completed and GPIO is OFF")
+
+@app.on_event("shutdown")
+def shutdown_gpio() -> None:
+    global gpio_handle
+    _all_off()
+    if lgpio is not None and gpio_handle is not None:
+        try:
+            lgpio.gpiochip_close(gpio_handle)
+        finally:
+            gpio_handle = None
+            claimed_pins.clear()
 
 # API Endpoints
 @app.get("/")
@@ -118,18 +179,24 @@ async def get_status():
         "status": status,
         "active_motor": active_motor,
         "remaining_seconds": remaining_seconds,
-        "last_action": last_action,
+        "last_action": last_action,
+        "error": motor_error,
         "total_motors": len(motors)
     }
 
 @app.post("/add-motor")
-async def add_motor(config: MotorConfig):
-    if config.name in motors:
-        raise HTTPException(status_code=400, detail=f"Motor '{config.name}' already exists")
-
-    motors[config.name] = {
-        "name": config.name,
-        "gpio": config.gpio
+async def add_motor(config: MotorConfig):
+    if config.gpio < 2 or config.gpio > 27:
+        raise HTTPException(status_code=422, detail="BCM GPIO must be in range 2..27")
+    if config.name in motors:
+        raise HTTPException(status_code=400, detail=f"Motor '{config.name}' already exists")
+    if any(int(item["gpio"]) == config.gpio for item in motors.values()):
+        raise HTTPException(status_code=409, detail=f"BCM GPIO {config.gpio} is already registered")
+
+    motors[config.name] = {
+        "name": config.name,
+        "gpio": config.gpio,
+        "active_high": config.active_high,
     }
     save_motors()
 
@@ -151,22 +218,27 @@ async def list_motors():
     }
 
 @app.put("/update-motor/{motor_name}")
-async def update_motor(motor_name: str, config: UpdateMotorConfig):
+async def update_motor(motor_name: str, config: UpdateMotorConfig):
+    if config.gpio < 2 or config.gpio > 27:
+        raise HTTPException(status_code=422, detail="BCM GPIO must be in range 2..27")
     if motor_name not in motors:
         raise HTTPException(status_code=404, detail=f"Motor '{motor_name}' not found")
 
     if active_motor == motor_name:
         raise HTTPException(status_code=409, detail=f"Cannot update motor '{motor_name}' while it is running")
 
-    if config.name != motor_name and config.name in motors:
-        raise HTTPException(status_code=400, detail=f"Motor name '{config.name}' already exists")
+    if config.name != motor_name and config.name in motors:
+        raise HTTPException(status_code=400, detail=f"Motor name '{config.name}' already exists")
+    if any(name != motor_name and int(item["gpio"]) == config.gpio for name, item in motors.items()):
+        raise HTTPException(status_code=409, detail=f"BCM GPIO {config.gpio} is already registered")
 
     if config.name != motor_name:
         del motors[motor_name]
 
-    motors[config.name] = {
-        "name": config.name,
-        "gpio": config.gpio
+    motors[config.name] = {
+        "name": config.name,
+        "gpio": config.gpio,
+        "active_high": config.active_high,
     }
     save_motors()
 
@@ -198,18 +270,30 @@ async def delete_motor(motor_name: str):
     }
 
 @app.post("/trigger-motor")
-async def trigger_motor(config: TriggerMotor):
-    global motor_thread, stop_flag
+async def trigger_motor(config: TriggerMotor):
+    global motor_thread, stop_flag, active_motor, remaining_seconds, last_action, motor_error
 
     if config.motor_name not in motors:
         raise HTTPException(status_code=404, detail=f"Motor '{config.motor_name}' not found")
 
-    if active_motor:
-        raise HTTPException(status_code=409, detail=f"Motor '{active_motor}' is already running")
-
-    stop_flag = False
-    motor_thread = threading.Thread(target=run_motor_thread, args=(config.motor_name, config.seconds))
-    motor_thread.start()
+    if config.seconds < 0.05 or config.seconds > 10.0:
+        raise HTTPException(status_code=422, detail="Pulse duration must be 0.05..10 seconds")
+
+    try:
+        _claim_motor(motors[config.motor_name])
+    except Exception as exc:
+        raise HTTPException(status_code=503, detail=f"GPIO is unavailable: {exc}") from exc
+
+    with motor_lock:
+        if active_motor:
+            raise HTTPException(status_code=409, detail=f"Motor '{active_motor}' is already running")
+        stop_flag = False
+        motor_error = None
+        active_motor = config.motor_name
+        remaining_seconds = config.seconds
+        last_action = f"Triggered {config.motor_name}"
+        motor_thread = threading.Thread(target=run_motor_thread, args=(config.motor_name, config.seconds), daemon=True)
+        motor_thread.start()
 
     return {
         "status": "success",
@@ -223,18 +307,20 @@ async def stop_motors(config: Optional[StopMotors] = None):
     global stop_flag, last_action
 
     try:
-        if not active_motor:
-            return {
+        if not active_motor:
+            _all_off()
+            return {
                 "status": "success",
                 "message": "No motors are currently running"
             }
 
-        stop_flag = True
-        last_action = f"Stopped {active_motor}"
+        stop_flag = True
+        last_action = f"Stopped {active_motor}"
 
         # Wait for thread to finish
-        if motor_thread and motor_thread.is_alive():
-            motor_thread.join(timeout=2)
+        if motor_thread and motor_thread.is_alive():
+            motor_thread.join(timeout=2)
+        _all_off()
 
         return {
             "status": "success",
@@ -252,13 +338,30 @@ async def stop_motors(config: Optional[StopMotors] = None):
         }
 
 @app.post("/test")
-async def run_test(config: TestConfig):
-    global last_action
-
-    if active_motor:
-        raise HTTPException(status_code=409, detail="A motor is already running")
-
-    pins = config.pins if config.pins else list(range(1, 28))
+async def run_test(config: TestConfig):
+    global last_action, active_motor, motor_error
+
+    if config.seconds_on < 0.05 or config.seconds_on > 10.0:
+        raise HTTPException(status_code=422, detail="Test pulse duration must be 0.05..10 seconds")
+    if config.seconds_pause < 0 or config.seconds_pause > 10.0:
+        raise HTTPException(status_code=422, detail="Test pause must be 0..10 seconds")
+
+    registered_pins = {int(m["gpio"]) for m in motors.values() if isinstance(m, dict)}
+    pins = config.pins if config.pins else sorted(registered_pins)
+    if not pins or any(pin not in registered_pins for pin in pins):
+        raise HTTPException(status_code=422, detail="Relay tests are limited to registered GPIO pins")
+    try:
+        for motor in motors.values():
+            if int(motor["gpio"]) in pins:
+                _claim_motor(motor)
+    except Exception as exc:
+        raise HTTPException(status_code=503, detail=f"GPIO is unavailable: {exc}") from exc
+
+    with motor_lock:
+        if active_motor:
+            raise HTTPException(status_code=409, detail="A motor is already running")
+        active_motor = "relay-test"
+        motor_error = None
 
     print(f"[TEST] Running test sequence:")
     print(f"  - Seconds ON: {config.seconds_on}")
@@ -267,19 +370,32 @@ async def run_test(config: TestConfig):
 
     last_action = f"Test sequence started"
 
-    # Simulate test sequence
-    def test_sequence():
-        global last_action
-        for pin in pins:
-            last_action = f"test:gpio:{pin}"
-            print(f">>> TESTING GPIO {pin}")
-            time.sleep(config.seconds_on)
-            print(f"[TEST] Pin {pin} OFF, pausing {config.seconds_pause}s")
-            time.sleep(config.seconds_pause)
-        last_action = "Test sequence completed"
-        print("[TEST] Test sequence completed")
-
-    test_thread = threading.Thread(target=test_sequence)
+    def test_sequence():
+        global last_action, active_motor, motor_error
+        try:
+            for pin in pins:
+                last_action = f"test:gpio:{pin}"
+                motor = next(m for m in motors.values() if int(m["gpio"]) == pin)
+                print(f">>> TESTING GPIO {pin}")
+                try:
+                    _write_motor(motor, True)
+                    time.sleep(config.seconds_on)
+                finally:
+                    _write_motor(motor, False)
+                print(f"[TEST] Pin {pin} OFF, pausing {config.seconds_pause}s")
+                time.sleep(config.seconds_pause)
+            last_action = "Test sequence completed"
+            print("[TEST] Test sequence completed")
+        except Exception as exc:
+            motor_error = f"{type(exc).__name__}: {exc}"
+            last_action = f"Relay test failed: {motor_error}"
+            print(f"[ERROR] Relay test failed: {motor_error}")
+        finally:
+            _all_off()
+            with motor_lock:
+                active_motor = None
+
+    test_thread = threading.Thread(target=test_sequence, daemon=True)
     test_thread.start()
 
     return {
@@ -297,8 +413,8 @@ if __name__ == "__main__":
     print("=" * 60)
     print("Motor Control API Server")
     print("=" * 60)
-    print("Starting server on http://0.0.0.0:8001")
+    print("Starting server on http://127.0.0.1:8001")
     print("API Documentation: http://localhost:8001/docs")
     print("ReDoc: http://localhost:8001/redoc")
     print("=" * 60)
-    uvicorn.run(app, host="0.0.0.0", port=8001)
+    uvicorn.run(app, host=os.getenv("ROBOPARK_MOTOR_HOST", "127.0.0.1"), port=8001)

package/packages/robopark/vision/requirements_vision_agent.txt

@@ -13,6 +13,7 @@ python-multipart>=0.0.12
 sounddevice>=0.4.6
 soundfile>=0.12.1
 groq>=0.11
+lgpio==0.2.2.0; platform_system == 'Linux'
 # Python 3.13 has no compatible NumPy 1.24 wheel. On older Pi images, use the
 # distro NumPy provided through system-site-packages instead.
 numpy>=2.1; python_version >= '3.13'