diff --git a/dimensional-hackathon/README.md b/dimensional-hackathon/README.md new file mode 100644 index 0000000000..5ed3ece145 --- /dev/null +++ b/dimensional-hackathon/README.md @@ -0,0 +1,64 @@ +# Go2 Minimal Demo + +This is a stripped-down demo app for the Unitree Go2 Air. + +What it does: + +- connects to the Go2 over DimensionalOS WebRTC +- serves the live FPV stream on a local dashboard +- runs a simple sequential waypoint patrol +- accepts `/patrol`, `/stop`, and `/status` via Telegram +- runs YOLO-only tool detection, detects persons, chairs, boxes etc +- sends Telegram alerts with a JPG snapshot +- plays a WAV locally and, if available, through the Go2 speaker + + +## Layout + +- `config.yaml`: runtime config +- `.env.example`: required environment variables +- `run.sh`: start script +- `demo_app/`: source-only application package + +## Setup + +From the workspace root: + +```bash +cd dimos/demo +pip install -r requirements.txt +cp .env.example .env +``` + +Set values in `.env`: + +```bash +TELEGRAM_BOT_TOKEN=... +TELEGRAM_OWNER_CHAT_ID=... +ROBOT_IP=192.168.12.1 +``` + +If you prefer shell exports, that also works. + +## Run + +```bash +cd dimos/demo +./run.sh +``` + +Then open: + +- dashboard: [http://localhost:8080](http://localhost:8080) + +Telegram commands: + +- `/start` +- `/patrol` +- `/stop` +- `/status` + +## Notes + +- The app is intended to run on your laptop or another host on the same Wi-Fi as the robot. +- Detection is YOLO-only. Any matching detection above the configured threshold can trigger an alert. diff --git a/dimensional-hackathon/config.yaml b/dimensional-hackathon/config.yaml new file mode 100644 index 0000000000..caaeb2a294 --- /dev/null +++ b/dimensional-hackathon/config.yaml @@ -0,0 +1,56 @@ +robot: + ip: "192.168.12.1" + obstacle_avoidance: true + camera_resize: [640, 480] + connect_timeout_sec: 15 + +waypoints: + - {id: W1, pos: [0.5, 0.0, 0.0], yaw: 0} + - {id: W2, pos: [0.5, 0.5, 0.0], yaw: 90} + +patrol: + loop_forever: true + scan_turns: 1 + scan_pause_sec: 0.15 + motion_settle_sec: 0.05 + forward_steps_per_cycle: 5 + forward_speed_mps: 0.60 + forward_step_duration_sec: 1.25 + sweep_yaw_radps: 0.24 + sweep_turn_duration_sec: 0.28 + +detection: + enabled: true + model_name: yolo11n.pt + interval_sec: 0.5 + conf_threshold: 0.25 + cooldown_sec: 0 + detection_classes: [chair, office chair, box, container] + +aisle: + enabled: true + x_min_m: 0.4 + x_max_m: 2.0 + half_width_m: 0.42 + min_points_in_zone: 6 + min_z_m: -0.35 + max_z_m: 1.20 + cell_size_m: 0.12 + min_occupied_cells: 2 + alert_repeat_sec: 3.0 + closeup_stop_distance_m: 0.8 + approach_step_m: 0.25 + max_approach_steps: 3 + turn_step_deg: 12 + reclear_consecutive_frames: 2 + +alert: + audio_file: ../deploy_agentics_real/assets/alert.wav + clip_duration_sec: 5 + buffer_seconds: 10 + capture_fps: 15 + +web: + host: 0.0.0.0 + port: 8080 + stream_fps: 10 diff --git a/dimensional-hackathon/demo_app/__init__.py b/dimensional-hackathon/demo_app/__init__.py new file mode 100644 index 0000000000..f33080ad59 --- /dev/null +++ b/dimensional-hackathon/demo_app/__init__.py @@ -0,0 +1 @@ +"""Minimal Go2 patrol demo.""" diff --git a/dimensional-hackathon/demo_app/aisle.py b/dimensional-hackathon/demo_app/aisle.py new file mode 100644 index 0000000000..2cff8bd0cb --- /dev/null +++ b/dimensional-hackathon/demo_app/aisle.py @@ -0,0 +1,114 @@ +from __future__ import annotations + +import numpy as np + +from demo_app.types import AisleObservation + + +class AisleCorridorDetector: + def __init__( + self, + x_min_m: float, + x_max_m: float, + half_width_m: float, + min_points_in_zone: int, + min_z_m: float, + max_z_m: float, + cell_size_m: float, + min_occupied_cells: int, + ) -> None: + self._x_min = x_min_m + self._x_max = x_max_m + self._half_width = half_width_m + self._min_points = min_points_in_zone + self._min_z = min_z_m + self._max_z = max_z_m + self._cell_size = max(cell_size_m, 0.01) + self._min_cells = max(min_occupied_cells, 1) + + def analyze(self, points: np.ndarray) -> AisleObservation: + if points.size == 0: + return AisleObservation( + corridor_clear=True, + obstruction_distance_m=None, + obstruction_direction="center", + obstruction_point_count=0, + occupied_cell_count=0, + obstruction_center_xy=None, + ) + + pts = points.astype(np.float32, copy=False) + finite = np.isfinite(pts).all(axis=1) + pts = pts[finite] + if pts.size == 0: + return AisleObservation( + corridor_clear=True, + obstruction_distance_m=None, + obstruction_direction="center", + obstruction_point_count=0, + occupied_cell_count=0, + obstruction_center_xy=None, + ) + + if pts.shape[1] >= 3: + height_mask = (pts[:, 2] >= self._min_z) & (pts[:, 2] <= self._max_z) + pts = pts[height_mask] + if pts.size == 0: + return AisleObservation( + corridor_clear=True, + obstruction_distance_m=None, + obstruction_direction="center", + obstruction_point_count=0, + occupied_cell_count=0, + obstruction_center_xy=None, + ) + + corridor_mask = ( + (pts[:, 0] >= self._x_min) + & (pts[:, 0] <= self._x_max) + & (np.abs(pts[:, 1]) <= self._half_width) + ) + zone = pts[corridor_mask] + point_count = int(zone.shape[0]) + if point_count < self._min_points: + return AisleObservation( + corridor_clear=True, + obstruction_distance_m=None, + obstruction_direction="center", + obstruction_point_count=point_count, + occupied_cell_count=0, + obstruction_center_xy=None, + ) + + cell_xy = np.floor(zone[:, :2] / self._cell_size).astype(np.int32) + occupied_cells = np.unique(cell_xy, axis=0) + occupied_count = int(occupied_cells.shape[0]) + if occupied_count < self._min_cells: + return AisleObservation( + corridor_clear=True, + obstruction_distance_m=None, + obstruction_direction="center", + obstruction_point_count=point_count, + occupied_cell_count=occupied_count, + obstruction_center_xy=None, + ) + + nearest_idx = int(np.argmin(zone[:, 0])) + nearest_distance = float(zone[nearest_idx, 0]) + center_y = float(np.mean(zone[:, 1])) + if center_y > 0.12: + direction = "left" + elif center_y < -0.12: + direction = "right" + else: + direction = "center" + + center_x = float(np.mean(zone[:, 0])) + return AisleObservation( + corridor_clear=False, + obstruction_distance_m=nearest_distance, + obstruction_direction=direction, + obstruction_point_count=point_count, + occupied_cell_count=occupied_count, + obstruction_center_xy=(center_x, center_y), + ) diff --git a/dimensional-hackathon/demo_app/audio.py b/dimensional-hackathon/demo_app/audio.py new file mode 100644 index 0000000000..b5d7b1ac6d --- /dev/null +++ b/dimensional-hackathon/demo_app/audio.py @@ -0,0 +1,30 @@ +from __future__ import annotations + +import asyncio +import logging +from pathlib import Path +from typing import Any + +from demo_app.types import AlertEvent + +logger = logging.getLogger(__name__) + + +class AudioAlert: + def __init__(self, audio_file: str, runner: Any = None): + self._audio_file = str(Path(audio_file)) + self._runner = runner + + async def play(self, event: AlertEvent) -> None: + tasks = [asyncio.to_thread(self._play_local_blocking)] + if self._runner is not None and hasattr(self._runner, "play_alert_on_robot"): + tasks.append(asyncio.create_task(self._runner.play_alert_on_robot(self._audio_file))) + await asyncio.gather(*tasks, return_exceptions=True) + + def _play_local_blocking(self) -> None: + try: + import playsound3 + + playsound3.playsound(self._audio_file, block=True) + except Exception as e: + logger.warning("Local audio playback failed: %s", e) diff --git a/dimensional-hackathon/demo_app/capture.py b/dimensional-hackathon/demo_app/capture.py new file mode 100644 index 0000000000..586227a66d --- /dev/null +++ b/dimensional-hackathon/demo_app/capture.py @@ -0,0 +1,98 @@ +from __future__ import annotations + +import asyncio +from collections import deque +from pathlib import Path + +import cv2 +import numpy as np + +from demo_app.types import AlertEvent + + +class CaptureBuffer: + def __init__(self, buffer_seconds: int, fps: int, output_dir: Path): + self._buffer: deque[tuple[float, np.ndarray]] = deque(maxlen=buffer_seconds * fps) + self._fps = fps + self._output_dir = output_dir + self._output_dir.mkdir(parents=True, exist_ok=True) + + def push(self, frame: np.ndarray, timestamp: float) -> None: + self._buffer.append((timestamp, frame.copy())) + + async def snapshot(self, event: AlertEvent) -> Path: + return await asyncio.to_thread(self._encode_snapshot, event) + + async def snapshot_and_clip(self, event: AlertEvent, duration_sec: float) -> tuple[Path, Path]: + return await asyncio.to_thread(self._encode, event, duration_sec) + + def _encode_snapshot(self, event: AlertEvent) -> Path: + ts_label = int(event.timestamp * 1000) + jpg_path = self._output_dir / f"obstruction_{ts_label}.jpg" + annotated = event.frame.copy() + for idx, line in enumerate([ + "AISLE OBSTRUCTION DETECTED", + f"dir={event.obstruction_direction or 'center'} " + f"dist={event.obstruction_distance_m:.2f}m" if event.obstruction_distance_m is not None else "dist=unknown", + f"points={event.obstruction_point_count}", + ]): + y = 28 + idx * 28 + cv2.putText( + annotated, + line, + (12, y), + cv2.FONT_HERSHEY_SIMPLEX, + 0.75, + (0, 0, 255), + 2, + ) + + detections = event.evidence_detections or [] + if detections: + for det in detections: + x1, y1, x2, y2 = det.bbox + cv2.rectangle(annotated, (x1, y1), (x2, y2), (0, 255, 255), 2) + label = f"{det.class_name} {det.confidence:.2f}" + cv2.putText( + annotated, + label, + (x1, max(90, y1 - 8)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (0, 255, 255), + 2, + ) + elif event.bbox is not None: + x1, y1, x2, y2 = event.bbox + cv2.rectangle(annotated, (x1, y1), (x2, y2), (0, 255, 0), 2) + + cv2.imwrite(str(jpg_path), annotated) + return jpg_path + + def _encode(self, event: AlertEvent, duration_sec: float) -> tuple[Path, Path]: + jpg_path = self._encode_snapshot(event) + ts_label = int(event.timestamp * 1000) + mp4_path = self._output_dir / f"anomaly_{ts_label}.mp4" + annotated = event.frame.copy() + + half = duration_sec / 2.0 + window = [ + (ts, frame) + for ts, frame in list(self._buffer) + if event.timestamp - half <= ts <= event.timestamp + half + ] + if not window: + fallback = list(self._buffer)[-int(duration_sec * self._fps):] + window = fallback or [(event.timestamp, annotated)] + + h, w = window[0][1].shape[:2] + writer = cv2.VideoWriter( + str(mp4_path), + cv2.VideoWriter_fourcc(*"mp4v"), + self._fps, + (w, h), + ) + for _, frame in window: + writer.write(frame) + writer.release() + return jpg_path, mp4_path diff --git a/dimensional-hackathon/demo_app/config.py b/dimensional-hackathon/demo_app/config.py new file mode 100644 index 0000000000..ec466fc60a --- /dev/null +++ b/dimensional-hackathon/demo_app/config.py @@ -0,0 +1,98 @@ +from __future__ import annotations + +import os +from pathlib import Path + +import yaml +from dotenv import load_dotenv +from pydantic import BaseModel + + +class RobotConfig(BaseModel): + ip: str = "" + obstacle_avoidance: bool = True + camera_resize: tuple[int, int] = (640, 480) + connect_timeout_sec: float = 15.0 + + +class WaypointConfig(BaseModel): + id: str + pos: tuple[float, float, float] + yaw: float + + +class PatrolConfig(BaseModel): + loop_forever: bool = True + scan_turns: int = 1 + scan_pause_sec: float = 1.0 + motion_settle_sec: float = 0.05 + forward_steps_per_cycle: int = 5 + forward_speed_mps: float = 0.60 + forward_step_duration_sec: float = 1.25 + sweep_yaw_radps: float = 0.24 + sweep_turn_duration_sec: float = 0.28 + + +class DetectionConfig(BaseModel): + enabled: bool = True + model_name: str = "yolo11n.pt" + interval_sec: float = 0.5 + conf_threshold: float = 0.25 + cooldown_sec: int = 300 + detection_classes: list[str] + + +class AlertConfig(BaseModel): + audio_file: str + clip_duration_sec: int = 5 + buffer_seconds: int = 10 + capture_fps: int = 15 + + +class WebConfig(BaseModel): + host: str = "0.0.0.0" + port: int = 8080 + stream_fps: int = 10 + + +class AisleConfig(BaseModel): + enabled: bool = True + x_min_m: float = 0.4 + x_max_m: float = 2.0 + half_width_m: float = 0.45 + min_points_in_zone: int = 12 + min_z_m: float = -0.35 + max_z_m: float = 1.20 + cell_size_m: float = 0.12 + min_occupied_cells: int = 2 + alert_repeat_sec: float = 3.0 + closeup_stop_distance_m: float = 0.8 + approach_step_m: float = 0.25 + max_approach_steps: int = 3 + turn_step_deg: float = 12.0 + reclear_consecutive_frames: int = 2 + + +class Settings(BaseModel): + robot: RobotConfig + waypoints: list[WaypointConfig] + patrol: PatrolConfig + aisle: AisleConfig + detection: DetectionConfig + alert: AlertConfig + web: WebConfig + telegram_bot_token: str + telegram_owner_chat_id: int + + +def load_config(yaml_path: Path = Path("config.yaml")) -> Settings: + load_dotenv() + data = yaml.safe_load(yaml_path.read_text()) + data["telegram_bot_token"] = os.environ["TELEGRAM_BOT_TOKEN"].strip() + data["telegram_owner_chat_id"] = int(os.environ["TELEGRAM_OWNER_CHAT_ID"]) + + robot_ip = os.environ.get("ROBOT_IP", "").strip() + if robot_ip: + data["robot"]["ip"] = robot_ip + + return Settings(**data) diff --git a/dimensional-hackathon/demo_app/dashboard.py b/dimensional-hackathon/demo_app/dashboard.py new file mode 100644 index 0000000000..2ec60a5239 --- /dev/null +++ b/dimensional-hackathon/demo_app/dashboard.py @@ -0,0 +1,131 @@ +from __future__ import annotations + +import asyncio +import base64 +import json +import time +from pathlib import Path +from typing import Any, Awaitable, Callable + +import cv2 +from fastapi import FastAPI, HTTPException, WebSocket, WebSocketDisconnect +from fastapi.responses import HTMLResponse +from fastapi.staticfiles import StaticFiles + +_DETECTION_TTL_SEC = 1.5 + + +def create_app( + stream_fps: int, + command_handlers: dict[str, Callable[[], Awaitable[Any]]] | None = None, +) -> tuple[FastAPI, dict]: + app = FastAPI(title="Go2 Demo Dashboard") + state: dict[str, Any] = { + "waypoints": [], + "visited": [], + "robot_pose": None, + "pose_history": [], + "corridor_clear": True, + "obstruction_distance_m": None, + "obstruction_direction": "center", + "obstruction_point_count": 0, + "anomalies": [], + "mode": "IDLE", + "latest_frame": None, + "latest_detections": [], + "event_log": [], + "planned_path": [], + } + + static_dir = Path(__file__).parent / "static" + app.mount("/static", StaticFiles(directory=static_dir), name="static") + + @app.get("/", response_class=HTMLResponse) + async def root() -> str: + return (static_dir / "index.html").read_text() + + @app.get("/api/map") + async def get_map() -> dict: + return { + "waypoints": state["waypoints"], + "visited": state["visited"], + "robot_pose": state["robot_pose"], + "pose_history": state["pose_history"], + "corridor_clear": state["corridor_clear"], + "obstruction_distance_m": state["obstruction_distance_m"], + "obstruction_direction": state["obstruction_direction"], + "obstruction_point_count": state["obstruction_point_count"], + "anomalies": state["anomalies"], + "mode": state["mode"], + "planned_path": state["planned_path"], + } + + @app.post("/api/command/{name}") + async def post_command(name: str) -> dict: + if not command_handlers or name not in command_handlers: + raise HTTPException(status_code=404, detail=f"Unknown command: {name}") + try: + result = await command_handlers[name]() + state.setdefault("event_log", []).append( + {"type": "command", "payload": {"name": name, "result": result}} + ) + return {"ok": True, "command": name, "result": result} + except Exception as e: + state.setdefault("event_log", []).append( + {"type": "command_error", "payload": {"name": name, "error": str(e)}} + ) + raise HTTPException(status_code=500, detail=str(e)) from e + + @app.websocket("/ws/stream") + async def ws_stream(ws: WebSocket) -> None: + await ws.accept() + interval = 1.0 / max(stream_fps, 1) + try: + while True: + frame = state.get("latest_frame") + if frame is not None: + rendered = _overlay_detections(frame, state) + _, buf = cv2.imencode(".jpg", rendered, [cv2.IMWRITE_JPEG_QUALITY, 70]) + await ws.send_text(base64.b64encode(buf).decode()) + await asyncio.sleep(interval) + except WebSocketDisconnect: + return + + @app.websocket("/ws/events") + async def ws_events(ws: WebSocket) -> None: + await ws.accept() + last_seen = 0 + try: + while True: + events = state.get("event_log", []) + if len(events) > last_seen: + for event in events[last_seen:]: + await ws.send_text(json.dumps(event)) + last_seen = len(events) + await asyncio.sleep(0.2) + except WebSocketDisconnect: + return + + return app, state + + +def _overlay_detections(frame, state) -> Any: + detections = state.get("latest_detections") or [] + if not detections: + return frame + now = time.time() + out = frame.copy() + for detection in detections: + if now - detection.get("timestamp", 0) > _DETECTION_TTL_SEC: + continue + bbox = detection.get("bbox") + if not bbox or len(bbox) != 4: + continue + x1, y1, x2, y2 = (int(v) for v in bbox) + label = f"{detection.get('class_name', '?')} {detection.get('confidence', 0.0):.2f}" + cv2.rectangle(out, (x1, y1), (x2, y2), (0, 255, 255), 2) + (tw, th), _ = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.55, 1) + ly = max(0, y1 - 6) + cv2.rectangle(out, (x1, ly - th - 4), (x1 + tw + 6, ly + 2), (0, 255, 255), -1) + cv2.putText(out, label, (x1 + 3, ly - 2), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 0, 0), 1) + return out diff --git a/dimensional-hackathon/demo_app/detector.py b/dimensional-hackathon/demo_app/detector.py new file mode 100644 index 0000000000..54db4d3dec --- /dev/null +++ b/dimensional-hackathon/demo_app/detector.py @@ -0,0 +1,105 @@ +from __future__ import annotations +import logging +from pathlib import Path + +import numpy as np + +from demo_app.types import Detection + +logger = logging.getLogger(__name__) + + +class YoloToolDetector: + INFER_SIZE = 1280 + + def __init__(self, prompts: list[str], conf_threshold: float, model_name: str) -> None: + from ultralytics import YOLO + model_path = Path(model_name) + if not model_path.is_absolute(): + candidate = Path.cwd() / model_name + if candidate.exists(): + model_path = candidate + logger.info( + "YOLO init step 1/4: model_path=%s exists=%s", + model_path, + model_path.exists(), + ) + self._model = YOLO(str(model_path)) + logger.info("YOLO init step 2/4: closed-set YOLO model object created") + self._prompts = list(prompts) + logger.warning( + "Using closed-set YOLO for this demo. It will detect only built-in classes " + "and then filter them against prompts=%s", + self._prompts, + ) + self._conf = conf_threshold + + def detect(self, frame: np.ndarray, timestamp: float) -> list[Detection]: + results = self._model.predict(frame, conf=self._conf, imgsz=self.INFER_SIZE, verbose=False) + if not results: + return [] + result = results[0] + if result.boxes is None: + return [] + + detections: list[Detection] = [] + for box in result.boxes: + xyxy = box.xyxy[0].cpu().numpy().astype(int) + cls_idx = int(box.cls[0].item()) + conf = float(box.conf[0].item()) + class_name = self._class_name_from_result(result, cls_idx) + matched_prompt = self._match_prompt(class_name) + if matched_prompt is None: + continue + class_name = matched_prompt + detections.append( + Detection( + bbox=(int(xyxy[0]), int(xyxy[1]), int(xyxy[2]), int(xyxy[3])), + class_name=class_name, + confidence=conf, + frame=frame, + timestamp=timestamp, + ) + ) + return detections + + def _class_name_from_result(self, result, cls_idx: int) -> str: + names = getattr(result, "names", None) + if isinstance(names, dict): + return str(names.get(cls_idx, "unknown")) + if isinstance(names, list) and 0 <= cls_idx < len(names): + return str(names[cls_idx]) + model_names = getattr(self._model, "names", None) + if isinstance(model_names, dict): + return str(model_names.get(cls_idx, "unknown")) + if isinstance(model_names, list) and 0 <= cls_idx < len(model_names): + return str(model_names[cls_idx]) + return "unknown" + + def _match_prompt(self, detected_name: str) -> str | None: + detected_norm = self._normalize(detected_name) + for prompt in self._prompts: + prompt_norm = self._normalize(prompt) + if prompt_norm == detected_norm: + return prompt + if prompt_norm in detected_norm or detected_norm in prompt_norm: + return prompt + + aliases = { + "person": {"human", "man", "woman", "boy", "girl", "pedestrian"}, + "chair": {"seat", "stool", "armchair"}, + "office chair": {"desk chair", "rolling chair", "swivel chair"}, + "couch": {"sofa"}, + "bench": {"park bench"}, + "suitcase": {"luggage", "case"}, + "backpack": {"bag", "rucksack"}, + } + for prompt in self._prompts: + alias_set = aliases.get(prompt, set()) + if detected_norm in alias_set: + return prompt + return None + + @staticmethod + def _normalize(value: str) -> str: + return " ".join(value.lower().replace("_", " ").replace("-", " ").split()) diff --git a/dimensional-hackathon/demo_app/main.py b/dimensional-hackathon/demo_app/main.py new file mode 100644 index 0000000000..397980afcd --- /dev/null +++ b/dimensional-hackathon/demo_app/main.py @@ -0,0 +1,553 @@ +from __future__ import annotations + +import asyncio +import logging +import signal +import time +from pathlib import Path +from typing import Any + +import numpy as np +import uvicorn + +from demo_app.aisle import AisleCorridorDetector +from demo_app.audio import AudioAlert +from demo_app.capture import CaptureBuffer +from demo_app.config import load_config +from demo_app.dashboard import create_app +from demo_app.detector import YoloToolDetector +from demo_app.patrol import PatrolController, nearest_waypoint +from demo_app.robot import Go2Runner +from demo_app.telegram_bot import TelegramBot +from demo_app.types import AisleObservation, AlertEvent, Detection, Waypoint +from demo_app.vision_obstruction import CenterLaneVisionDetector + +logger = logging.getLogger(__name__) + + +async def run() -> None: + logging.basicConfig( + level=logging.INFO, + format="%(asctime)s %(levelname)s %(name)s: %(message)s", + ) + + logger.info("Loading config from config.yaml") + cfg = load_config(Path("config.yaml")) + logger.info("Config loaded") + waypoints = [Waypoint(id=w.id, pos=tuple(w.pos), yaw_deg=w.yaw) for w in cfg.waypoints] + + logger.info("Creating dashboard state") + web_state: dict[str, Any] + command_handlers: dict[str, Any] = {} + app, web_state = create_app(stream_fps=cfg.web.stream_fps, command_handlers=command_handlers) + web_state["waypoints"] = [{"id": w.id, "pos": list(w.pos), "yaw": w.yaw_deg} for w in waypoints] + + runner = Go2Runner( + robot_ip=cfg.robot.ip, + obstacle_avoidance=cfg.robot.obstacle_avoidance, + camera_resize=tuple(cfg.robot.camera_resize), + connect_timeout_sec=cfg.robot.connect_timeout_sec, + ) + logger.info("Starting Go2 runner") + runner.start() + logger.info("Go2 runner started") + + patrol = PatrolController( + runner=runner, + waypoints=waypoints, + web_state=web_state, + loop_forever=cfg.patrol.loop_forever, + scan_turns=cfg.patrol.scan_turns, + scan_pause_sec=cfg.patrol.scan_pause_sec, + motion_settle_sec=cfg.patrol.motion_settle_sec, + forward_steps_per_cycle=cfg.patrol.forward_steps_per_cycle, + forward_speed_mps=cfg.patrol.forward_speed_mps, + forward_step_duration_sec=cfg.patrol.forward_step_duration_sec, + sweep_yaw_radps=cfg.patrol.sweep_yaw_radps, + sweep_turn_duration_sec=cfg.patrol.sweep_turn_duration_sec, + ) + + capture = CaptureBuffer( + buffer_seconds=cfg.alert.buffer_seconds, + fps=cfg.alert.capture_fps, + output_dir=Path("captures"), + ) + audio = AudioAlert(audio_file=cfg.alert.audio_file, runner=runner) + + detector: YoloToolDetector | None = None + latest_lidar_points: np.ndarray | None = None + latest_observation: AisleObservation | None = None + obstruction_task: asyncio.Task | None = None + obstruction_latched = False + approach_abort_requested = False + last_obstruction_alert_ts = 0.0 + last_pose_record = 0.0 + last_no_lidar_log = 0.0 + last_lidar_parse_log = 0.0 + last_aisle_debug_log = 0.0 + clear_streak = 0 + last_vision_debug_log = 0.0 + + aisle_detector = AisleCorridorDetector( + x_min_m=cfg.aisle.x_min_m, + x_max_m=cfg.aisle.x_max_m, + half_width_m=cfg.aisle.half_width_m, + min_points_in_zone=cfg.aisle.min_points_in_zone, + min_z_m=cfg.aisle.min_z_m, + max_z_m=cfg.aisle.max_z_m, + cell_size_m=cfg.aisle.cell_size_m, + min_occupied_cells=cfg.aisle.min_occupied_cells, + ) + vision_detector = CenterLaneVisionDetector() + logger.info( + "Aisle detector: x=[%.2f, %.2f] half_width=%.2f z=[%.2f, %.2f] min_points=%d min_cells=%d repeat=%.2fs", + cfg.aisle.x_min_m, + cfg.aisle.x_max_m, + cfg.aisle.half_width_m, + cfg.aisle.min_z_m, + cfg.aisle.max_z_m, + cfg.aisle.min_points_in_zone, + cfg.aisle.min_occupied_cells, + cfg.aisle.alert_repeat_sec, + ) + + def push_event(event_type: str, payload: dict[str, Any]) -> None: + web_state.setdefault("event_log", []).append({"type": event_type, "payload": payload}) + + def set_mode(mode: str) -> None: + if web_state.get("mode") == mode: + return + web_state["mode"] = mode + push_event("state", {"mode": mode}) + + async def stop_patrol_and_abort() -> None: + nonlocal approach_abort_requested + approach_abort_requested = True + await patrol.stop(return_home=False) + + async def on_patrol() -> None: + nonlocal approach_abort_requested + approach_abort_requested = False + await patrol.start() + + async def on_stop() -> None: + await stop_patrol_and_abort() + if web_state.get("mode") == "APPROACHING": + set_mode("IDLE") + + async def on_status() -> str: + return await patrol.status() + + async def command_forward() -> str: + await stop_patrol_and_abort() + logger.info("Dashboard command: forward") + ok = await patrol.manual_drive(forward_mps=0.25, duration_sec=1.0) + return "ok" if ok else "failed" + + async def command_back() -> str: + await stop_patrol_and_abort() + logger.info("Dashboard command: back") + ok = await patrol.manual_drive(forward_mps=-0.20, duration_sec=1.0) + return "ok" if ok else "failed" + + async def command_left() -> str: + await stop_patrol_and_abort() + logger.info("Dashboard command: left") + ok = await patrol.manual_drive(left_mps=0.15, duration_sec=1.0) + return "ok" if ok else "failed" + + async def command_right() -> str: + await stop_patrol_and_abort() + logger.info("Dashboard command: right") + ok = await patrol.manual_drive(left_mps=-0.15, duration_sec=1.0) + return "ok" if ok else "failed" + + async def command_turn_left() -> str: + await stop_patrol_and_abort() + logger.info("Dashboard command: turn_left") + ok = await patrol.manual_drive(yaw_radps=0.50, duration_sec=0.8) + return "ok" if ok else "failed" + + async def command_turn_right() -> str: + await stop_patrol_and_abort() + logger.info("Dashboard command: turn_right") + ok = await patrol.manual_drive(yaw_radps=-0.50, duration_sec=0.8) + return "ok" if ok else "failed" + + command_handlers.update( + { + "patrol": on_patrol, + "stop": on_stop, + "status": on_status, + "forward": command_forward, + "back": command_back, + "left": command_left, + "right": command_right, + "turn_left": command_turn_left, + "turn_right": command_turn_right, + } + ) + + telegram = TelegramBot( + token=cfg.telegram_bot_token, + owner_chat_id=cfg.telegram_owner_chat_id, + on_patrol_command=on_patrol, + on_stop_command=on_stop, + on_status_query=on_status, + ) + + def on_frame(frame: Any) -> None: + ts = time.time() + web_state["latest_frame"] = frame + capture.push(frame, ts) + + def on_lidar(msg: Any) -> None: + nonlocal latest_lidar_points + nonlocal last_lidar_parse_log + try: + if hasattr(msg, "points_f32"): + points = msg.points_f32() + elif hasattr(msg, "pointcloud_tensor"): + points = msg.pointcloud_tensor.point["positions"].numpy() + else: + raise TypeError(f"Unsupported lidar message type: {type(msg)!r}") + arr = np.asarray(points, dtype=np.float32) + if arr.ndim != 2 or arr.shape[1] < 2: + raise ValueError(f"Unexpected lidar shape: {arr.shape!r}") + latest_lidar_points = arr[:, :3] + except Exception as exc: + now = time.time() + if now - last_lidar_parse_log >= 3.0: + logger.warning("Failed to parse lidar frame: %s", exc) + last_lidar_parse_log = now + + video_sub = runner.video_stream().subscribe(on_next=on_frame) + lidar_sub = runner.lidar_stream().subscribe(on_next=on_lidar) + + stop_event = asyncio.Event() + + async def ensure_detector() -> YoloToolDetector: + nonlocal detector + if detector is None: + logger.info("Initializing YOLO evidence detector: %s", cfg.detection.model_name) + detector = await asyncio.to_thread( + YoloToolDetector, + cfg.detection.detection_classes, + cfg.detection.conf_threshold, + cfg.detection.model_name, + ) + logger.info( + "YOLO evidence detector ready. Watching classes=%s conf_threshold=%.2f", + cfg.detection.detection_classes, + cfg.detection.conf_threshold, + ) + return detector + + async def detect_evidence(frame: np.ndarray, timestamp: float) -> list[Detection]: + if not cfg.detection.enabled: + web_state["latest_detections"] = [] + return [] + model = await ensure_detector() + detections = await asyncio.to_thread(model.detect, frame, timestamp) + if detections: + logger.info( + "YOLO evidence detections: %s", + ", ".join(f"{det.class_name}@{det.confidence:.2f}" for det in detections), + ) + else: + logger.info("YOLO evidence detector saw 0 watched objects in obstruction frame") + web_state["latest_detections"] = [ + { + "bbox": list(det.bbox), + "class_name": det.class_name, + "confidence": det.confidence, + "timestamp": det.timestamp, + } + for det in detections + ] + return detections + + async def handle_obstruction(initial_observation: AisleObservation) -> None: + nonlocal obstruction_latched + observation = initial_observation + waypoint_id = patrol.current_waypoint() or nearest_waypoint(runner.get_pose(), waypoints) + push_event( + "corridor_blocked", + { + "distance_m": observation.obstruction_distance_m, + "direction": observation.obstruction_direction, + "point_count": observation.obstruction_point_count, + "waypoint": waypoint_id, + }, + ) + logger.info( + "Corridor blocked: distance=%s direction=%s points=%d waypoint=%s", + f"{observation.obstruction_distance_m:.2f}m" + if observation.obstruction_distance_m is not None + else "unknown", + observation.obstruction_direction, + observation.obstruction_point_count, + waypoint_id, + ) + set_mode("OBSTRUCTION") + push_event( + "approach_started", + { + "distance_m": observation.obstruction_distance_m, + "direction": observation.obstruction_direction, + "mode": "capture_only", + }, + ) + + frame = web_state.get("latest_frame") + if frame is None: + logger.warning("No camera frame available for obstruction capture") + set_mode("OBSTRUCTION") + return + + timestamp = time.time() + evidence = await detect_evidence(frame, timestamp) + pose = runner.get_pose() + waypoint_id = patrol.current_waypoint() or nearest_waypoint(pose, waypoints) + primary = evidence[0] if evidence else None + event = AlertEvent( + bbox=primary.bbox if primary is not None else None, + class_name=primary.class_name if primary is not None else "obstruction", + confidence=primary.confidence if primary is not None else 1.0, + frame=frame.copy(), + robot_pose=(pose.x, pose.y), + nearest_waypoint=waypoint_id, + timestamp=timestamp, + obstruction_distance_m=observation.obstruction_distance_m, + obstruction_direction=observation.obstruction_direction, + obstruction_point_count=observation.obstruction_point_count, + evidence_detections=evidence or None, + ) + web_state["anomalies"].append( + { + "class_name": event.class_name, + "robot_pose": list(event.robot_pose), + "nearest_waypoint": event.nearest_waypoint, + "timestamp": event.timestamp, + } + ) + jpg_path = await capture.snapshot(event) + logger.info("Saved obstruction JPG: %s", jpg_path) + push_event( + "capture_saved", + { + "class_name": event.class_name, + "jpg": str(jpg_path), + }, + ) + + sent = await telegram.send_photo_alert(event, jpg_path) + push_event( + "telegram_sent" if sent else "telegram_failed", + { + "class_name": event.class_name, + "jpg": str(jpg_path), + }, + ) + asyncio.create_task(audio.play(event)) + if patrol.is_running(): + set_mode("PATROLLING") + else: + set_mode("IDLE") + + async def monitor_loop() -> None: + nonlocal clear_streak + nonlocal last_no_lidar_log + nonlocal last_pose_record + nonlocal latest_observation + nonlocal obstruction_latched + nonlocal obstruction_task + nonlocal last_obstruction_alert_ts + nonlocal last_aisle_debug_log + nonlocal last_vision_debug_log + + loop_interval = 0.25 + while not stop_event.is_set(): + await asyncio.sleep(loop_interval) + + pose = runner.get_pose() + web_state["robot_pose"] = {"x": pose.x, "y": pose.y, "yaw_deg": pose.yaw_deg} + now = time.time() + if now - last_pose_record >= 0.5: + history = web_state.setdefault("pose_history", []) + history.append([pose.x, pose.y]) + if len(history) > 400: + del history[: len(history) - 400] + last_pose_record = now + + if obstruction_task is not None and obstruction_task.done(): + try: + await obstruction_task + except asyncio.CancelledError: + pass + except Exception: + logger.exception("Obstruction task failed") + obstruction_task = None + + if not cfg.aisle.enabled: + continue + + patrol_phase = str(web_state.get("patrol_phase", "IDLE")) + if patrol_phase not in {"FORWARD_STEP", "PATROLLING"}: + latest_observation = None + clear_streak = 0 + obstruction_latched = False + web_state["corridor_clear"] = True + web_state["obstruction_distance_m"] = None + web_state["obstruction_direction"] = "center" + web_state["obstruction_point_count"] = 0 + continue + + points = latest_lidar_points + if points is None: + if now - last_no_lidar_log >= 3.0: + logger.info("Aisle monitor waiting for lidar frames") + last_no_lidar_log = now + continue + + latest_observation = await asyncio.to_thread(aisle_detector.analyze, points) + vision_observation = None + frame = web_state.get("latest_frame") + if frame is not None: + vision_observation = await asyncio.to_thread(vision_detector.detect, frame) + web_state["corridor_clear"] = latest_observation.corridor_clear + web_state["obstruction_distance_m"] = latest_observation.obstruction_distance_m + web_state["obstruction_direction"] = latest_observation.obstruction_direction + web_state["obstruction_point_count"] = latest_observation.obstruction_point_count + if now - last_aisle_debug_log >= 2.0: + logger.info( + "Aisle sample: clear=%s points=%d cells=%d dist=%s dir=%s", + latest_observation.corridor_clear, + latest_observation.obstruction_point_count, + latest_observation.occupied_cell_count, + f"{latest_observation.obstruction_distance_m:.2f}m" + if latest_observation.obstruction_distance_m is not None + else "-", + latest_observation.obstruction_direction, + ) + last_aisle_debug_log = now + if vision_observation is not None and now - last_vision_debug_log >= 2.0: + logger.info( + "Vision lane sample: blocked=%s area=%.0f bbox=%s", + vision_observation.blocked, + vision_observation.contour_area, + vision_observation.bbox, + ) + last_vision_debug_log = now + + effective_observation = latest_observation + if latest_observation.corridor_clear and vision_observation is not None and vision_observation.blocked: + effective_observation = AisleObservation( + corridor_clear=False, + obstruction_distance_m=1.0, + obstruction_direction="center", + obstruction_point_count=max(1, int(vision_observation.contour_area // 400)), + occupied_cell_count=1, + obstruction_center_xy=(1.0, 0.0), + ) + web_state["corridor_clear"] = False + web_state["obstruction_distance_m"] = effective_observation.obstruction_distance_m + web_state["obstruction_direction"] = effective_observation.obstruction_direction + web_state["obstruction_point_count"] = effective_observation.obstruction_point_count + + if effective_observation.corridor_clear: + clear_streak += 1 + if clear_streak >= cfg.aisle.reclear_consecutive_frames and obstruction_latched: + obstruction_latched = False + if web_state.get("mode") == "OBSTRUCTION": + set_mode("PATROLLING" if patrol.is_running() else "IDLE") + push_event("corridor_cleared", {"point_count": 0}) + continue + + clear_streak = 0 + if obstruction_task is not None: + continue + if not patrol.is_running(): + continue + if obstruction_latched and now - last_obstruction_alert_ts < cfg.aisle.alert_repeat_sec: + continue + + obstruction_latched = True + last_obstruction_alert_ts = now + obstruction_task = asyncio.create_task(handle_obstruction(effective_observation)) + + logger.info("Starting Telegram bot") + await telegram.start() + logger.info("Telegram bot started") + push_event("telegram", {"message": f"owner_chat_id={cfg.telegram_owner_chat_id}"}) + + server = uvicorn.Server( + uvicorn.Config(app, host=cfg.web.host, port=cfg.web.port, log_level="info", loop="asyncio") + ) + logger.info("Starting web dashboard on http://%s:%s", cfg.web.host, cfg.web.port) + server_task = asyncio.create_task(server.serve()) + monitor_task = asyncio.create_task(monitor_loop()) + + shutdown = asyncio.Event() + loop = asyncio.get_running_loop() + + def request_shutdown() -> None: + shutdown.set() + + for sig in (signal.SIGINT, signal.SIGTERM): + try: + loop.add_signal_handler(sig, request_shutdown) + except (NotImplementedError, RuntimeError): + signal.signal(sig, lambda *_: request_shutdown()) + + await shutdown.wait() + + stop_event.set() + monitor_task.cancel() + try: + await monitor_task + except asyncio.CancelledError: + pass + except Exception: + pass + + if obstruction_task is not None: + obstruction_task.cancel() + try: + await obstruction_task + except asyncio.CancelledError: + pass + except Exception: + pass + + try: + await patrol.stop(return_home=False) + except Exception: + pass + + try: + video_sub.dispose() + except Exception: + pass + + try: + lidar_sub.dispose() + except Exception: + pass + + try: + await telegram.stop() + except Exception: + pass + + try: + server.should_exit = True + await server_task + except Exception: + pass + + runner.stop() + + +if __name__ == "__main__": + asyncio.run(run()) diff --git a/dimensional-hackathon/demo_app/patrol.py b/dimensional-hackathon/demo_app/patrol.py new file mode 100644 index 0000000000..3c87bef322 --- /dev/null +++ b/dimensional-hackathon/demo_app/patrol.py @@ -0,0 +1,239 @@ +from __future__ import annotations + +import asyncio +import logging +import math +from typing import Any + +from demo_app.types import Pose, Waypoint + +logger = logging.getLogger(__name__) + + +class PatrolController: + SPEED_MPS = 0.5 + + def __init__( + self, + runner, + waypoints: list[Waypoint], + web_state: dict[str, Any], + loop_forever: bool = True, + scan_turns: int = 4, + scan_pause_sec: float = 1.0, + motion_settle_sec: float = 0.05, + forward_steps_per_cycle: int = 3, + forward_speed_mps: float = 0.30, + forward_step_duration_sec: float = 1.25, + sweep_yaw_radps: float = 0.24, + sweep_turn_duration_sec: float = 0.28, + ) -> None: + self._runner = runner + self._waypoints = waypoints + self._web_state = web_state + self._loop_forever = loop_forever + self._scan_turns = scan_turns + self._scan_pause_sec = scan_pause_sec + self._motion_settle_sec = motion_settle_sec + self._forward_steps_per_cycle = forward_steps_per_cycle + self._forward_speed_mps = forward_speed_mps + self._forward_step_duration_sec = forward_step_duration_sec + self._sweep_yaw_radps = sweep_yaw_radps + self._sweep_turn_duration_sec = sweep_turn_duration_sec + self._task: asyncio.Task | None = None + self._stop_requested = False + self._return_home_on_stop = False + self._current_wp: str | None = None + self._web_state["patrol_phase"] = "IDLE" + + async def start(self) -> None: + if self._task is not None and not self._task.done(): + return + self._stop_requested = False + self._return_home_on_stop = False + self._task = asyncio.create_task(self._run()) + + async def stop(self, return_home: bool = False) -> None: + self._stop_requested = True + self._return_home_on_stop = return_home + task = self._task + if task is not None: + await task + + async def move_relative(self, forward: float, left: float, degrees: float) -> bool: + pose = self._runner.get_pose() + yaw_rad = math.radians(pose.yaw_deg) + cos_y, sin_y = math.cos(yaw_rad), math.sin(yaw_rad) + target_x = pose.x + cos_y * forward - sin_y * left + target_y = pose.y + sin_y * forward + cos_y * left + target_yaw = pose.yaw_deg + degrees + dist = math.hypot(target_x - pose.x, target_y - pose.y) + duration = max(0.75, dist / self.SPEED_MPS) if dist > 0.05 else max(0.75, abs(degrees) / 90.0) + self._web_state["planned_path"] = [[pose.x, pose.y], [target_x, target_y]] + self._push_event( + "move", + {"forward": forward, "left": left, "degrees": degrees}, + ) + return await asyncio.to_thread( + self._runner.move_to, + target_x, + target_y, + target_yaw, + duration, + lambda: self._stop_requested, + ) + + async def manual_drive( + self, + *, + forward_mps: float = 0.0, + left_mps: float = 0.0, + yaw_radps: float = 0.0, + duration_sec: float = 0.75, + ) -> bool: + self._push_event( + "move", + { + "forward_mps": forward_mps, + "left_mps": left_mps, + "yaw_radps": yaw_radps, + "duration_sec": duration_sec, + }, + ) + return await asyncio.to_thread( + self._runner.drive_for_duration, + forward_mps, + left_mps, + yaw_radps, + duration_sec, + ) + + async def status(self) -> str: + mode = self._web_state.get("mode", "IDLE") + visited = len(self._web_state.get("visited", [])) + current = self._current_wp or "-" + return f"mode={mode} current={current} visited={visited}" + + def is_running(self) -> bool: + return self._task is not None and not self._task.done() + + def current_waypoint(self) -> str: + return self._current_wp or "" + + async def _run(self) -> None: + self._set_mode("PATROLLING") + self._web_state["visited"] = [] + self._push_event("state", {"mode": "PATROLLING"}) + + try: + cycle_index = 0 + while not self._stop_requested: + cycle_index += 1 + self._current_wp = f"AISLE_{cycle_index}" + self._web_state["planned_path"] = [] + + for step_idx in range(self._forward_steps_per_cycle): + if self._stop_requested: + break + await self._forward_step(step_idx + 1) + + if self._stop_requested: + break + + await self._scan_in_place() + + if not self._loop_forever: + break + + if self._return_home_on_stop: + self._set_mode("RETURNING") + self._push_event("state", {"mode": "RETURNING"}) + await self._return_home() + finally: + self._current_wp = None + self._web_state["planned_path"] = [] + self._set_mode("IDLE") + self._set_phase("IDLE") + self._push_event("state", {"mode": "IDLE"}) + + async def _move_to_waypoint(self, waypoint: Waypoint) -> bool: + pose = self._runner.get_pose() + self._web_state["planned_path"] = [[pose.x, pose.y], [waypoint.pos[0], waypoint.pos[1]]] + dist = math.hypot(waypoint.pos[0] - pose.x, waypoint.pos[1] - pose.y) + duration = max(1.0, dist / self.SPEED_MPS) + logger.info("Moving to %s", waypoint.id) + return await asyncio.to_thread( + self._runner.move_to, + waypoint.pos[0], + waypoint.pos[1], + waypoint.yaw_deg, + duration, + lambda: self._stop_requested, + ) + + async def _scan_in_place(self) -> None: + if self._scan_turns <= 0: + return + + for _ in range(self._scan_turns): + for phase_name, yaw_radps, duration_sec in ( + ("SWEEP_RIGHT", -self._sweep_yaw_radps, self._sweep_turn_duration_sec), + ("CENTER_FROM_RIGHT", self._sweep_yaw_radps, self._sweep_turn_duration_sec), + ("SWEEP_LEFT", self._sweep_yaw_radps, self._sweep_turn_duration_sec), + ("CENTER_FROM_LEFT", -self._sweep_yaw_radps, self._sweep_turn_duration_sec), + ): + if self._stop_requested: + return + self._set_phase(phase_name) + await self.manual_drive(yaw_radps=yaw_radps, duration_sec=duration_sec) + await self._settle_motion() + await asyncio.sleep(self._scan_pause_sec) + self._set_phase("PATROLLING") + + async def _forward_step(self, step_number: int) -> None: + if self._stop_requested: + return + self._set_phase("FORWARD_STEP") + pose = self._runner.get_pose() + yaw_rad = math.radians(pose.yaw_deg) + step_distance = self._forward_speed_mps * self._forward_step_duration_sec + target_x = pose.x + math.cos(yaw_rad) * step_distance + target_y = pose.y + math.sin(yaw_rad) * step_distance + self._web_state["planned_path"] = [[pose.x, pose.y], [target_x, target_y]] + await self.manual_drive( + forward_mps=self._forward_speed_mps, + duration_sec=self._forward_step_duration_sec, + ) + await self._settle_motion() + await asyncio.sleep(0.20) + self._set_phase("PATROLLING") + + async def _return_home(self) -> None: + pose = self._runner.get_pose() + self._web_state["planned_path"] = [[pose.x, pose.y], [0.0, 0.0]] + await asyncio.to_thread(self._runner.move_to, 0.0, 0.0, 0.0, 5.0, lambda: self._stop_requested) + + def _set_mode(self, mode: str) -> None: + self._web_state["mode"] = mode + + def _set_phase(self, phase: str) -> None: + self._web_state["patrol_phase"] = phase + + def _push_event(self, event_type: str, payload: dict[str, Any]) -> None: + self._web_state.setdefault("event_log", []).append({"type": event_type, "payload": payload}) + + async def _settle_motion(self) -> None: + if self._stop_requested or self._motion_settle_sec <= 0: + return + await self.manual_drive(duration_sec=self._motion_settle_sec) + + +def nearest_waypoint(pose: Pose, waypoints: list[Waypoint]) -> str: + best_id = "" + best_dist = float("inf") + for waypoint in waypoints: + dist = (pose.x - waypoint.pos[0]) ** 2 + (pose.y - waypoint.pos[1]) ** 2 + if dist < best_dist: + best_dist = dist + best_id = waypoint.id + return best_id diff --git a/dimensional-hackathon/demo_app/robot.py b/dimensional-hackathon/demo_app/robot.py new file mode 100644 index 0000000000..1b399e92af --- /dev/null +++ b/dimensional-hackathon/demo_app/robot.py @@ -0,0 +1,350 @@ +from __future__ import annotations + +import logging +import math +import threading +import time +from typing import Any, Callable, Protocol + +import cv2 +import numpy as np +from reactivex.subject import Subject + +from demo_app.types import Pose + +logger = logging.getLogger(__name__) + + +class _ConnectionProtocol(Protocol): + def lidar_stream(self): ... + def video_stream(self): ... + def odom_stream(self): ... + def move(self, twist, duration: float = 0.0) -> bool: ... + def standup(self) -> bool: ... + def liedown(self) -> bool: ... + def balance_stand(self) -> bool: ... + def set_obstacle_avoidance(self, enabled: bool = True) -> None: ... + def stop(self) -> None: ... + + +def _default_connection_factory(ip: str, mode: str = "ai"): + from dimos.robot.unitree.connection import UnitreeWebRTCConnection + return UnitreeWebRTCConnection(ip=ip, mode=mode) + + +class _Twist: + class _Vec: + def __init__(self, x: float = 0.0, y: float = 0.0, z: float = 0.0) -> None: + self.x, self.y, self.z = x, y, z + + def __init__(self, vx: float, vy: float, vyaw: float) -> None: + self.linear = self._Vec(vx, vy, 0.0) + self.angular = self._Vec(0.0, 0.0, vyaw) + + +class Go2Runner: + MAX_VX = 0.8 + MAX_VY = 0.3 + MAX_VYAW = 0.8 + ARRIVE_DIST = 0.25 + ARRIVE_YAW = 5.0 + + def __init__( + self, + robot_ip: str, + obstacle_avoidance: bool = True, + camera_resize: tuple[int, int] = (640, 480), + connection_factory: Callable[..., _ConnectionProtocol] = _default_connection_factory, + standup_delay_sec: float = 2.0, + connect_timeout_sec: float = 15.0, + ) -> None: + self._ip = robot_ip + self._obstacle_avoidance = obstacle_avoidance + self._resize_w, self._resize_h = camera_resize + self._connection_factory = connection_factory + self._standup_delay = standup_delay_sec + self._connect_timeout = connect_timeout_sec + + self._conn: _ConnectionProtocol | None = None + self._video_subject: Subject = Subject() + self._lidar_subject: Subject = Subject() + self._pose = Pose(0.0, 0.0, 0.0) + self._pose_lock = threading.Lock() + self._video_sub = None + self._lidar_sub = None + self._odom_sub = None + self._audio_hub: Any = None + self._alert_uuid: str | None = None + + def start(self) -> None: + if self._conn is not None: + return + logger.info("Connecting to Go2 at %s", self._ip) + result: dict[str, Any] = {} + error: dict[str, BaseException] = {} + + def _build_connection() -> None: + try: + result["conn"] = self._connection_factory(self._ip, mode="ai") + except BaseException as exc: # noqa: BLE001 - propagate exact startup failure + error["exc"] = exc + + connect_thread = threading.Thread(target=_build_connection, daemon=True) + connect_thread.start() + connect_thread.join(self._connect_timeout) + if connect_thread.is_alive(): + raise RuntimeError( + f"Timed out after {self._connect_timeout:.0f}s connecting to Go2 at " + f"{self._ip!r}. Check Wi-Fi, IP, and whether the robot is powered on." + ) + if "exc" in error: + exc = error["exc"] + raise RuntimeError( + f"Cannot reach Go2 at {self._ip!r}. Check Wi-Fi and IP. " + f"({type(exc).__name__}: {exc})" + ) from exc + self._conn = result["conn"] + + logger.info("Subscribing to video and odometry streams") + self._video_sub = self._conn.video_stream().subscribe(self._on_video) + self._lidar_sub = self._conn.lidar_stream().subscribe(self._on_lidar) + self._odom_sub = self._conn.odom_stream().subscribe(self._on_odom) + logger.info("Sending standup and balance commands") + self._conn.standup() + time.sleep(self._standup_delay) + self._conn.balance_stand() + if hasattr(self._conn, "free_walk"): + try: + logger.info("Attempting to activate FreeWalk mode") + self._conn.free_walk() + logger.info("FreeWalk mode activated") + except Exception as e: + logger.warning("FreeWalk activation failed: %s", e) + self._conn.set_obstacle_avoidance(self._obstacle_avoidance) + logger.info("Connected to Go2 at %s", self._ip) + + try: + from unitree_webrtc_connect.webrtc_audiohub import WebRTCAudioHub + + legion_conn = getattr(self._conn, "conn", None) + if legion_conn is not None: + self._audio_hub = WebRTCAudioHub(connection=legion_conn, logger=logger) + logger.info("WebRTCAudioHub initialized") + except Exception as e: + logger.warning("Audio hub init failed: %s", e) + self._audio_hub = None + + def stop(self) -> None: + if self._conn is None: + return + try: + if self._video_sub is not None: + self._video_sub.dispose() + except Exception: + pass + try: + if self._lidar_sub is not None: + self._lidar_sub.dispose() + except Exception: + pass + try: + if self._odom_sub is not None: + self._odom_sub.dispose() + except Exception: + pass + try: + self._conn.liedown() + except Exception as e: + logger.warning("Go2 lie down failed: %s", e) + try: + self._conn.stop() + except Exception: + pass + self._conn = None + + def video_stream(self) -> Subject: + return self._video_subject + + def lidar_stream(self) -> Subject: + return self._lidar_subject + + def _on_video(self, frame) -> None: + try: + if hasattr(frame, "data") and isinstance(getattr(frame, "data", None), np.ndarray): + arr = frame.data + elif hasattr(frame, "to_ndarray"): + arr = frame.to_ndarray(format="rgb24") + elif isinstance(frame, np.ndarray): + arr = frame + else: + arr = np.asarray(frame) + + if arr.ndim != 3: + return + + if arr.shape[:2] != (self._resize_h, self._resize_w): + arr = cv2.resize(arr, (self._resize_w, self._resize_h), interpolation=cv2.INTER_AREA) + self._video_subject.on_next(arr) + except Exception as e: + logger.warning("Video frame handler error: %s", e) + + def _on_lidar(self, msg) -> None: + try: + self._lidar_subject.on_next(msg) + except Exception as e: + logger.warning("Lidar handler error: %s", e) + + def _on_odom(self, msg) -> None: + try: + x = getattr(msg, "x", None) + y = getattr(msg, "y", None) + if x is None and hasattr(msg, "position"): + x = msg.position.x + y = msg.position.y + qw = getattr(msg, "qw", None) + qx = getattr(msg, "qx", 0.0) + qy = getattr(msg, "qy", 0.0) + qz = getattr(msg, "qz", None) + if qw is None and hasattr(msg, "orientation"): + qw, qx, qy, qz = ( + msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z + ) + yaw_rad = math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz)) + with self._pose_lock: + self._pose = Pose(float(x), float(y), math.degrees(yaw_rad)) + except Exception as e: + logger.warning("Odom handler error: %s", e) + + def get_pose(self) -> Pose: + with self._pose_lock: + return self._pose + + def move_to( + self, + x: float, + y: float, + yaw_deg: float, + duration_sec: float = 5.0, + should_stop: Callable[[], bool] | None = None, + ) -> bool: + if self._conn is None: + return False + deadline = time.monotonic() + duration_sec + tick = 0.05 + kp_lin = 0.8 + kp_yaw = 1.2 + + try: + interrupted = False + while time.monotonic() < deadline: + conn = self._conn + if conn is None: + return False + if should_stop is not None and should_stop(): + logger.info("move_to interrupted before reaching target") + interrupted = True + break + + pose = self.get_pose() + dx, dy = x - pose.x, y - pose.y + dist = math.hypot(dx, dy) + yaw_err = ((yaw_deg - pose.yaw_deg + 180.0) % 360.0) - 180.0 + if dist < self.ARRIVE_DIST and abs(yaw_err) < self.ARRIVE_YAW: + break + + if dist > self.ARRIVE_DIST: + yaw_rad = math.radians(pose.yaw_deg) + cy, sy = math.cos(yaw_rad), math.sin(yaw_rad) + fwd = cy * dx + sy * dy + lat = -sy * dx + cy * dy + vx = max(-self.MAX_VX, min(self.MAX_VX, kp_lin * fwd)) + vy = max(-self.MAX_VY, min(self.MAX_VY, kp_lin * lat)) + else: + vx, vy = 0.0, 0.0 + + vyaw = max(-self.MAX_VYAW, min(self.MAX_VYAW, kp_yaw * math.radians(yaw_err))) + conn.move(_Twist(vx, vy, vyaw)) + time.sleep(tick) + finally: + try: + conn = self._conn + if conn is not None: + conn.move(_Twist(0.0, 0.0, 0.0)) + except Exception: + pass + return not interrupted + + def drive_for_duration( + self, + forward_mps: float = 0.0, + left_mps: float = 0.0, + yaw_radps: float = 0.0, + duration_sec: float = 0.75, + ) -> bool: + if self._conn is None: + return False + logger.info( + "Manual drive: forward=%.2f left=%.2f yaw=%.2f duration=%.2f", + forward_mps, + left_mps, + yaw_radps, + duration_sec, + ) + try: + return bool(self._conn.move(_Twist(forward_mps, left_mps, yaw_radps), duration_sec)) + except Exception as e: + logger.warning("Manual drive failed: %s", e) + return False + + async def play_alert_on_robot(self, audio_file: str) -> bool: + if self._audio_hub is None: + return False + path = str(audio_file) + try: + if self._alert_uuid is None: + await self._audio_hub.upload_audio_file(path) + listing = await self._audio_hub.get_audio_list() + records = self._extract_audio_records(listing) + target_name = path.rsplit("/", 1)[-1].rsplit(".", 1)[0] + for rec in records: + if rec.get("file_name") == target_name or rec.get("name") == target_name: + self._alert_uuid = rec.get("unique_id") or rec.get("uuid") + break + if self._alert_uuid is None and records: + self._alert_uuid = records[-1].get("unique_id") or records[-1].get("uuid") + + if self._alert_uuid: + await self._audio_hub.play_by_uuid(self._alert_uuid) + return True + return await self._megaphone_play(path) + except Exception as e: + logger.warning("Robot alert playback failed: %s", e) + return False + + async def _megaphone_play(self, path: str) -> bool: + try: + await self._audio_hub.enter_megaphone() + await self._audio_hub.upload_megaphone(path) + await self._audio_hub.exit_megaphone() + return True + except Exception as e: + logger.warning("Megaphone playback failed: %s", e) + return False + + @staticmethod + def _extract_audio_records(listing) -> list[dict]: + if listing is None: + return [] + if isinstance(listing, list): + return listing + if isinstance(listing, dict): + for key in ("data", "audio_list", "list", "records", "items"): + value = listing.get(key) + if isinstance(value, list): + return value + if isinstance(value, dict): + for nested in ("audio_list", "list", "records", "items"): + nested_value = value.get(nested) + if isinstance(nested_value, list): + return nested_value + return [] diff --git a/dimensional-hackathon/demo_app/static/index.html b/dimensional-hackathon/demo_app/static/index.html new file mode 100644 index 0000000000..b8c03861bc --- /dev/null +++ b/dimensional-hackathon/demo_app/static/index.html @@ -0,0 +1,321 @@ + + + + +Go2 Demo Dashboard + + + +
+ Go2 Demo ยท State: IDLE +
+
+
+

Robot FPV

+
+ + + + + + + + + +
+ +
+
+

Map

+
+
CorridorCLEAR
+
Distance-
+
Directioncenter
+
Point Count0
+
+ +
+
+

Events

+
+
+
+ + + diff --git a/dimensional-hackathon/demo_app/telegram_bot.py b/dimensional-hackathon/demo_app/telegram_bot.py new file mode 100644 index 0000000000..091e45f12a --- /dev/null +++ b/dimensional-hackathon/demo_app/telegram_bot.py @@ -0,0 +1,175 @@ +from __future__ import annotations + +import asyncio +import datetime +import json +import logging +from pathlib import Path +from typing import Any, Awaitable, Callable + +from demo_app.types import AlertEvent + +logger = logging.getLogger(__name__) + + +def format_alert_caption(event: AlertEvent) -> str: + ts = datetime.datetime.utcfromtimestamp(event.timestamp).strftime("%Y-%m-%d %H:%M:%S") + x, y = event.robot_pose + wp_part = f" near {event.nearest_waypoint}" if event.nearest_waypoint else "" + labels = "" + if event.evidence_detections: + unique = sorted({det.class_name for det in event.evidence_detections}) + labels = f"\nEvidence: {', '.join(unique)}" + lines = ["AISLE OBSTRUCTION DETECTED"] + if event.obstruction_distance_m is not None: + lines.append(f"Distance: {event.obstruction_distance_m:.2f} m") + else: + lines.append("Distance: unknown") + lines.extend( + [ + f"Direction: {event.obstruction_direction or 'center'}", + f"Point count: {event.obstruction_point_count}", + f"Robot pose: ~({x:.1f}, {y:.1f}) m{wp_part}", + f"Time: {ts}", + ] + ) + if labels: + lines.append(labels.removeprefix("\n")) + return "\n".join(lines) + + +class TelegramBot: + def __init__( + self, + token: str, + owner_chat_id: int, + on_patrol_command: Callable[[], Awaitable[None]], + on_stop_command: Callable[[], Awaitable[None]], + on_status_query: Callable[[], Awaitable[str]], + failed_dir: Path | None = None, + ) -> None: + self._token = token + self._owner = owner_chat_id + self._on_patrol = on_patrol_command + self._on_stop = on_stop_command + self._on_status = on_status_query + self._app: Any = None + self._failed_dir = failed_dir or Path("failed_alerts") + self._failed_dir.mkdir(parents=True, exist_ok=True) + + def _is_owner(self, update: Any) -> bool: + chat_id = update.effective_chat.id if update.effective_chat is not None else None + allowed = chat_id == self._owner + if not allowed: + logger.warning("Ignoring Telegram command from chat_id=%s; expected owner_chat_id=%s", chat_id, self._owner) + return allowed + + async def _cmd_start(self, update: Any, ctx: Any) -> None: + if not self._is_owner(update): + return + await update.message.reply_text( + "Go2 demo ready.\n/patrol - start patrol\n/stop - stop patrol\n/status - current state" + ) + + async def _cmd_patrol(self, update: Any, ctx: Any) -> None: + if not self._is_owner(update): + return + await self._on_patrol() + await update.message.reply_text("Patrol started.") + + async def _cmd_stop(self, update: Any, ctx: Any) -> None: + if not self._is_owner(update): + return + await self._on_stop() + await update.message.reply_text("Stopping patrol.") + + async def _cmd_status(self, update: Any, ctx: Any) -> None: + if not self._is_owner(update): + return + await update.message.reply_text(await self._on_status()) + + async def start(self) -> None: + try: + from telegram.ext import Application, CommandHandler + except Exception as e: + raise RuntimeError( + "python-telegram-bot is required for Telegram controls. " + "Install it with: pip install python-telegram-bot[all]" + ) from e + + self._app = Application.builder().token(self._token).build() + self._app.add_handler(CommandHandler("start", self._cmd_start)) + self._app.add_handler(CommandHandler("patrol", self._cmd_patrol)) + self._app.add_handler(CommandHandler("stop", self._cmd_stop)) + self._app.add_handler(CommandHandler("status", self._cmd_status)) + await self._app.initialize() + await self._app.start() + await self._app.updater.start_polling() + logger.info("Telegram bot polling started for owner_chat_id=%s", self._owner) + try: + await self._app.bot.send_message( + self._owner, + "Go2 demo bot connected. Available commands: /patrol /stop /status", + ) + except Exception as e: + logger.warning("Telegram startup message failed: %s", e) + + async def stop(self) -> None: + if self._app is None: + return + await self._app.updater.stop() + await self._app.stop() + await self._app.shutdown() + self._app = None + + async def send_photo_alert(self, event: AlertEvent, jpg_path: Path) -> bool: + if self._app is None: + self._dead_letter(event, jpg_path, None) + return False + + caption = format_alert_caption(event) + for delay in (0, 1, 2, 4): + if delay: + await asyncio.sleep(delay) + try: + with open(jpg_path, "rb") as photo: + await self._app.bot.send_photo(self._owner, photo=photo, caption=caption) + return True + except Exception as e: + logger.warning("Telegram send failed after %ss: %s", delay, e) + + self._dead_letter(event, jpg_path, None) + return False + + async def send_alert(self, event: AlertEvent, jpg_path: Path, mp4_path: Path) -> None: + if self._app is None: + self._dead_letter(event, jpg_path, mp4_path) + return + + caption = format_alert_caption(event) + for delay in (0, 1, 2, 4): + if delay: + await asyncio.sleep(delay) + try: + with open(jpg_path, "rb") as photo: + await self._app.bot.send_photo(self._owner, photo=photo, caption=caption) + with open(mp4_path, "rb") as video: + await self._app.bot.send_video(self._owner, video=video) + return + except Exception as e: + logger.warning("Telegram send failed after %ss: %s", delay, e) + + self._dead_letter(event, jpg_path, mp4_path) + + def _dead_letter(self, event: AlertEvent, jpg: Path, mp4: Path | None) -> None: + path = self._failed_dir / f"alert_{int(event.timestamp * 1000)}.json" + path.write_text(json.dumps({ + "timestamp": event.timestamp, + "class_name": event.class_name, + "confidence": event.confidence, + "robot_pose": list(event.robot_pose), + "nearest_waypoint": event.nearest_waypoint, + "jpg": str(jpg), + "mp4": str(mp4) if mp4 is not None else "", + })) + logger.error("Telegram alert saved to %s", path) diff --git a/dimensional-hackathon/demo_app/types.py b/dimensional-hackathon/demo_app/types.py new file mode 100644 index 0000000000..f753dbaeaa --- /dev/null +++ b/dimensional-hackathon/demo_app/types.py @@ -0,0 +1,53 @@ +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + + +@dataclass(frozen=True) +class Pose: + x: float + y: float + yaw_deg: float + + +@dataclass(frozen=True) +class Waypoint: + id: str + pos: tuple[float, float, float] + yaw_deg: float + + +@dataclass(frozen=True) +class Detection: + bbox: tuple[int, int, int, int] + class_name: str + confidence: float + frame: np.ndarray + timestamp: float + + +@dataclass(frozen=True) +class AisleObservation: + corridor_clear: bool + obstruction_distance_m: float | None + obstruction_direction: str + obstruction_point_count: int + occupied_cell_count: int + obstruction_center_xy: tuple[float, float] | None + + +@dataclass(frozen=True) +class AlertEvent: + bbox: tuple[int, int, int, int] | None + class_name: str + confidence: float + frame: np.ndarray + robot_pose: tuple[float, float] + nearest_waypoint: str + timestamp: float + obstruction_distance_m: float | None = None + obstruction_direction: str = "" + obstruction_point_count: int = 0 + evidence_detections: list[Detection] | None = None diff --git a/dimensional-hackathon/demo_app/vision_obstruction.py b/dimensional-hackathon/demo_app/vision_obstruction.py new file mode 100644 index 0000000000..661a96229b --- /dev/null +++ b/dimensional-hackathon/demo_app/vision_obstruction.py @@ -0,0 +1,63 @@ +from __future__ import annotations + +from dataclasses import dataclass + +import cv2 +import numpy as np + + +@dataclass(frozen=True) +class VisionObstruction: + blocked: bool + contour_area: float + bbox: tuple[int, int, int, int] | None + + +class CenterLaneVisionDetector: + def __init__( + self, + roi_x_frac: tuple[float, float] = (0.28, 0.72), + roi_y_frac: tuple[float, float] = (0.42, 0.95), + min_contour_area: float = 2200.0, + ) -> None: + self._roi_x_frac = roi_x_frac + self._roi_y_frac = roi_y_frac + self._min_contour_area = min_contour_area + + def detect(self, frame: np.ndarray) -> VisionObstruction: + if frame is None or frame.size == 0: + return VisionObstruction(False, 0.0, None) + + h, w = frame.shape[:2] + x1 = int(w * self._roi_x_frac[0]) + x2 = int(w * self._roi_x_frac[1]) + y1 = int(h * self._roi_y_frac[0]) + y2 = int(h * self._roi_y_frac[1]) + if x2 <= x1 or y2 <= y1: + return VisionObstruction(False, 0.0, None) + + roi = frame[y1:y2, x1:x2] + gray = cv2.cvtColor(roi, cv2.COLOR_RGB2GRAY) + blur = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blur, 50, 140) + kernel = np.ones((5, 5), np.uint8) + closed = cv2.morphologyEx(edges, cv2.MORPH_CLOSE, kernel, iterations=2) + contours, _ = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + best_area = 0.0 + best_bbox: tuple[int, int, int, int] | None = None + for contour in contours: + area = float(cv2.contourArea(contour)) + if area < self._min_contour_area: + continue + rx, ry, rw, rh = cv2.boundingRect(contour) + # Bias toward closer/lower-center obstacles and ignore thin floor reflections. + if rh < 24 or rw < 24: + continue + if (ry + rh) < int((y2 - y1) * 0.58): + continue + if area > best_area: + best_area = area + best_bbox = (x1 + rx, y1 + ry, x1 + rx + rw, y1 + ry + rh) + + return VisionObstruction(best_bbox is not None, best_area, best_bbox) diff --git a/dimensional-hackathon/requirements.txt b/dimensional-hackathon/requirements.txt new file mode 100644 index 0000000000..1d50352895 --- /dev/null +++ b/dimensional-hackathon/requirements.txt @@ -0,0 +1,10 @@ +python-dotenv +pyyaml +pydantic +fastapi +uvicorn[standard] +opencv-python +reactivex +ultralytics +python-telegram-bot[all] +playsound3 diff --git a/dimensional-hackathon/run.sh b/dimensional-hackathon/run.sh new file mode 100644 index 0000000000..9f0b714c43 --- /dev/null +++ b/dimensional-hackathon/run.sh @@ -0,0 +1,5 @@ +#!/usr/bin/env bash +set -euo pipefail +cd "$(dirname "$0")" +export PYTHONUNBUFFERED=1 +exec python -m demo_app.main