"""Concrete :class:`RobotInterface` backed by the official Unitree Go2 SDK. This module talks to a physical Unitree Go2 over CycloneDDS using the ``unitree_sdk2py`` package. All heavy/optional dependencies (``unitree_sdk2py``, ``cv2``) are imported *lazily* inside :class:`Go2Robot.__init__`, so importing this module never fails on an off-robot machine that lacks the SDK -- only *instantiating* :class:`Go2Robot` requires it. Velocity convention (matches ``SportClient.Move``): vx forward (+) / backward (-) m/s, body frame vy left (+) / right (-) m/s, body frame vyaw turn-left/CCW (+) / right/CW (-) rad/s Driving is routed through one of two paths: * ``ObstaclesAvoidClient.Move`` -- the firmware LiDAR avoidance layer filters and hard-stops these commands (the *safe* path), used when ``cfg.safety.use_lidar_avoidance`` is enabled. * ``SportClient.Move`` -- the raw locomotion path, used otherwise. A background daemon thread continuously pulls + decodes camera frames so that :meth:`get_frame` is non-blocking and decoupled from the control rate. """ from __future__ import annotations import logging import threading import time from typing import TYPE_CHECKING, Callable, Dict, Optional, Tuple import numpy as np from gowelcome.robot.interface import GESTURES, RobotInterface if TYPE_CHECKING: # pragma: no cover - type hints only from config import GoWelcomeConfig logger = logging.getLogger(__name__) class Go2Robot(RobotInterface): """Drive a physical Unitree Go2 via the official ``unitree_sdk2py`` SDK. Args: cfg: The fully-populated :class:`~config.GoWelcomeConfig`. Raises: ImportError: If ``unitree_sdk2py`` (or ``cv2``) is not installed. """ def __init__(self, cfg: "GoWelcomeConfig") -> None: # --- Lazy heavy imports (kept out of module scope on purpose) -------- try: from unitree_sdk2py.core.channel import ChannelFactoryInitialize from unitree_sdk2py.go2.obstacles_avoid.obstacles_avoid_client import ( ObstaclesAvoidClient, ) from unitree_sdk2py.go2.sport.sport_client import SportClient from unitree_sdk2py.go2.video.video_client import VideoClient except ImportError as exc: # pragma: no cover - off-robot path raise ImportError( "Go2Robot requires the official Unitree SDK. Install it with: " "pip install unitree_sdk2py (or build from " "github.com/unitreerobotics/unitree_sdk2_python)." ) from exc try: import cv2 # noqa: F401 (imported for an early, clear failure) except ImportError as exc: # pragma: no cover - off-robot path raise ImportError( "Go2Robot requires OpenCV to decode camera frames. Install it " "with: pip install opencv-python" ) from exc self.cfg = cfg self._cv2 = cv2 # cache the module for the camera thread # --- DDS channel: initialise EXACTLY ONCE before any client --------- ChannelFactoryInitialize(cfg.network.domain_id, cfg.network.interface) # --- Sport (locomotion / posture / gestures) ------------------------ self._sport = SportClient() self._sport.SetTimeout(10.0) self._sport.Init() # --- Video (front camera) ------------------------------------------- self._video = VideoClient() self._video.SetTimeout(3.0) self._video.Init() # --- Obstacle avoidance (firmware LiDAR hard-stop layer) ------------ self._oa = ObstaclesAvoidClient() self._oa.SetTimeout(3.0) self._oa.Init() # --- Vui (Go2 on-board volume only) --------------------------------- self._vui = None try: from unitree_sdk2py.go2.vui.vui_client import VuiClient vui = VuiClient() vui.SetTimeout(3.0) vui.Init() self._vui = vui except Exception as exc: # pragma: no cover - firmware-dependent logger.warning("VuiClient unavailable (volume control disabled): %s", exc) # --- Audio backend (greeting playback) ------------------------------ from gowelcome.robot.audio import build_audio_backend self.audio = build_audio_backend(cfg) self._greet_volume_set = False # --- Gesture name -> SportClient method map ------------------------- self._gestures: Dict[str, Callable[[], int]] = { "hello": self._sport.Hello, "heart": self._sport.Heart, "stretch": self._sport.Stretch, "dance1": self._sport.Dance1, "dance2": self._sport.Dance2, "scrape": self._sport.Scrape, "content": self._sport.Content, "sit": self._sport.Sit, "rise_sit": self._sport.RiseSit, } # --- Camera frame buffer (written by the camera thread) ------------- self._frame: Optional[np.ndarray] = None self._frame_size: Tuple[int, int] = (0, 0) self._frame_lock = threading.Lock() # --- Avoidance routing state ---------------------------------------- self._avoidance_on = False if cfg.safety.use_lidar_avoidance: self.set_avoidance(True) # --- Shutdown guard (idempotent) ------------------------------------ self._shutdown = False self._shutdown_lock = threading.Lock() # --- Start the camera thread last, once everything is wired up ------ self._cam_stop = threading.Event() self._cam_thread = threading.Thread( target=self._camera_loop, name="Go2CameraThread", daemon=True ) self._cam_thread.start() logger.info( "Go2Robot ready (iface=%s domain=%d avoidance=%s dry_run=%s)", cfg.network.interface, cfg.network.domain_id, self._avoidance_on, cfg.dry_run, ) # ------------------------------------------------------------------ # # Camera thread # # ------------------------------------------------------------------ # def _camera_loop(self) -> None: """Continuously pull + decode camera frames into ``self._frame``. Runs on a daemon thread; failures are logged and retried so a transient decode error never kills the stream. Throttled to ~camera target_fps when frames are unavailable to avoid a busy-spin. """ cv2 = self._cv2 min_period = 1.0 / max(1.0, self.cfg.camera.target_fps) while not self._cam_stop.is_set(): start = time.monotonic() try: code, data = self._video.GetImageSample() except Exception as exc: # pragma: no cover - hardware path logger.debug("GetImageSample raised: %s", exc) self._cam_stop.wait(min_period) continue if code != 0 or not data: logger.debug("GetImageSample non-zero code=%s", code) self._cam_stop.wait(min_period) continue try: buf = np.frombuffer(bytes(data), dtype=np.uint8) image = cv2.imdecode(buf, cv2.IMREAD_COLOR) except Exception as exc: # pragma: no cover - hardware path logger.debug("Frame decode failed: %s", exc) self._cam_stop.wait(min_period) continue if image is None: self._cam_stop.wait(min_period) continue h, w = image.shape[:2] with self._frame_lock: self._frame = image self._frame_size = (int(w), int(h)) # Gentle pace so we don't outrun the camera / burn CPU. elapsed = time.monotonic() - start if elapsed < min_period: self._cam_stop.wait(min_period - elapsed) # ------------------------------------------------------------------ # # Perception input # # ------------------------------------------------------------------ # def get_frame(self) -> "Optional[np.ndarray]": """Return the latest BGR camera frame, or ``None`` if none yet.""" with self._frame_lock: return self._frame def frame_size(self) -> "tuple[int, int]": """Current ``(width, height)``; ``(0, 0)`` until the first frame.""" with self._frame_lock: return self._frame_size # ------------------------------------------------------------------ # # Locomotion # # ------------------------------------------------------------------ # def drive(self, vx: float, vy: float, vyaw: float) -> None: """Command a body-frame velocity for one tick. The :class:`~config.SafetyConfig` hard caps are applied first, then the command is routed through the active path (avoidance ``Move`` if enabled, else sport ``Move``). When ``cfg.dry_run`` is set the intended command is logged but only a zero velocity is actually sent. """ safety = self.cfg.safety cvx = _clamp(vx, -safety.max_vx, safety.max_vx) cvy = _clamp(vy, -safety.max_vy, safety.max_vy) cvyaw = _clamp(vyaw, -safety.max_vyaw, safety.max_vyaw) if self.cfg.dry_run: logger.info( "[dry_run] drive intent vx=%.3f vy=%.3f vyaw=%.3f (sending 0,0,0)", cvx, cvy, cvyaw, ) self._send_move(0.0, 0.0, 0.0) return self._send_move(cvx, cvy, cvyaw) def _send_move(self, vx: float, vy: float, vyaw: float) -> None: """Dispatch a velocity through the currently-active drive path.""" if self._avoidance_on: self._call("oa.Move", lambda: self._oa.Move(vx, vy, vyaw)) else: self._call("sport.Move", lambda: self._sport.Move(vx, vy, vyaw)) def stop(self) -> None: """Command zero velocity through the active path (stay standing).""" self._send_move(0.0, 0.0, 0.0) def set_avoidance(self, on: bool) -> None: """Enable/disable the on-robot LiDAR hard-stop avoidance layer. When enabling: poll ``SwitchSet/SwitchGet`` until the firmware reports the switch as on, then claim remote control via ``UseRemoteCommandFromApi(True)``. When disabling: release remote control and turn the switch off. ``drive`` routes through ``oa.Move`` only while enabled. """ if on: ok = self._enable_avoidance() self._avoidance_on = ok if not ok: logger.warning( "Avoidance could not be enabled; falling back to sport.Move path" ) else: self._call( "oa.UseRemoteCommandFromApi(False)", lambda: self._oa.UseRemoteCommandFromApi(False), ) self._call("oa.SwitchSet(False)", lambda: self._oa.SwitchSet(False)) self._avoidance_on = False logger.info("LiDAR avoidance disabled") def _enable_avoidance(self) -> bool: """Turn the firmware avoidance switch on and claim API control. Returns: ``True`` if the switch is confirmed on and API control was claimed. """ deadline = time.monotonic() + 5.0 enabled = False while time.monotonic() < deadline: try: self._oa.SwitchSet(True) code, enable = self._oa.SwitchGet() except Exception as exc: # pragma: no cover - hardware path logger.debug("Avoidance SwitchGet/Set raised: %s", exc) time.sleep(0.1) continue if code == 0 and enable: enabled = True break time.sleep(0.1) if not enabled: return False rc = self._call( "oa.UseRemoteCommandFromApi(True)", lambda: self._oa.UseRemoteCommandFromApi(True), ) if rc != 0: return False logger.info("LiDAR avoidance enabled (API remote control claimed)") return True # ------------------------------------------------------------------ # # Posture / expression # # ------------------------------------------------------------------ # def balance_stand(self) -> None: """Enter balanced standing (ready-to-move).""" self._call("sport.BalanceStand", self._sport.BalanceStand) def stand_up(self) -> None: """Stiff stand up.""" self._call("sport.StandUp", self._sport.StandUp) def damp(self) -> None: """Emergency soft-stop: limp the motors (safe failure posture).""" self._call("sport.Damp", self._sport.Damp) def gesture(self, name: str) -> None: """Perform an expressive gesture (see :data:`GESTURES`). Unknown names are logged and ignored (no-op). """ fn = self._gestures.get(name) if fn is None: logger.warning( "Unknown gesture %r (known: %s) -- ignoring", name, ", ".join(GESTURES), ) return self._call(f"sport.{name}", fn) # ------------------------------------------------------------------ # # Greeting payload # # ------------------------------------------------------------------ # def play_greeting(self) -> None: """Play the configured greeting clip (non-blocking). Best-effort sets the Go2 on-board volume once via ``VuiClient`` (the gesture sequence itself is driven by the state machine calling :meth:`gesture`). """ if self._vui is not None and not self._greet_volume_set: try: self._vui.SetVolume(int(self.cfg.greet.audio_volume)) except Exception as exc: # pragma: no cover - firmware-dependent logger.debug("VuiClient.SetVolume failed: %s", exc) finally: self._greet_volume_set = True try: self.audio.play(self.cfg.greet.wav_path, blocking=False) except Exception as exc: logger.warning("Greeting playback failed: %s", exc) # ------------------------------------------------------------------ # # Lifecycle # # ------------------------------------------------------------------ # def shutdown(self) -> None: """Stop motion, release the avoidance API, close camera + audio. Idempotent and safe to call from a signal handler / ``finally`` block. Every step is wrapped so one failure does not block the rest. """ with self._shutdown_lock: if self._shutdown: return self._shutdown = True # 1. Stop the camera thread. try: self._cam_stop.set() if self._cam_thread.is_alive(): self._cam_thread.join(timeout=2.0) except Exception as exc: logger.warning("Camera thread shutdown failed: %s", exc) # 2. Release the firmware avoidance path (zero velocity + drop API). if self._avoidance_on: try: self._oa.Move(0.0, 0.0, 0.0) except Exception as exc: logger.warning("oa.Move(0,0,0) on shutdown failed: %s", exc) try: self._oa.UseRemoteCommandFromApi(False) except Exception as exc: logger.warning("oa.UseRemoteCommandFromApi(False) failed: %s", exc) self._avoidance_on = False # 3. Stop locomotion. try: self._sport.StopMove() except Exception as exc: logger.warning("sport.StopMove() on shutdown failed: %s", exc) # 4. Close audio. try: self.audio.close() except Exception as exc: logger.warning("audio.close() on shutdown failed: %s", exc) logger.info("Go2Robot shutdown complete") # ------------------------------------------------------------------ # # Helpers # # ------------------------------------------------------------------ # def _call(self, what: str, fn: Callable[[], int]) -> int: """Invoke an SDK call, logging (never raising) on a non-zero/error. Args: what: Human-readable label for log messages. fn: A zero-arg callable returning an SDK int code (0 == ok). Returns: The SDK return code, or ``-1`` if the call raised. """ try: code = fn() except Exception as exc: # pragma: no cover - hardware path logger.error("%s raised: %s", what, exc) return -1 if code is not None and code != 0: logger.warning("%s returned non-zero code=%s", what, code) return code if code is not None else 0 def _clamp(value: float, lo: float, hi: float) -> float: """Clamp ``value`` into the inclusive ``[lo, hi]`` range.""" return lo if value < lo else hi if value > hi else value