83 lines
3.0 KiB
Python
83 lines
3.0 KiB
Python
"""R1 head video-service control via the robot_state DDS service (api_id 1001).
|
|
|
|
Phone-free equivalent of the Unitree app's Settings → Service Status toggle for
|
|
the stereo camera: `RobotStateClient.ServiceSwitch("stereo_patch_pc1", on)`.
|
|
VIDEO ONLY — this never touches motion/loco services.
|
|
|
|
Reuses the app's already-initialized DDS ChannelFactory (arm_controller.init()
|
|
calls ChannelFactoryInitialize once per process), so the client is created
|
|
lazily on first use — by then the arm subsystem has set the factory up. If the
|
|
SDK/DDS isn't available it degrades to a no-op (returns False) instead of raising.
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import threading
|
|
from typing import Optional
|
|
|
|
from Project.Sanad.core.logger import get_logger
|
|
|
|
log = get_logger("stream_service")
|
|
|
|
STEREO_SERVICE = "stereo_patch_pc1"
|
|
|
|
|
|
class StreamService:
|
|
"""Thin, thread-safe wrapper around RobotStateClient for the stereo stream."""
|
|
|
|
def __init__(self) -> None:
|
|
self._lock = threading.Lock()
|
|
self._client = None
|
|
self._unavailable = False
|
|
|
|
def _client_or_none(self):
|
|
if self._client is not None:
|
|
return self._client
|
|
if self._unavailable:
|
|
return None
|
|
try:
|
|
# Reuses the process-wide ChannelFactory (already initialized by the
|
|
# arm controller). Does NOT re-init it — that's the single-init rule.
|
|
from unitree_sdk2py.go2.robot_state.robot_state_client import (
|
|
RobotStateClient,
|
|
)
|
|
c = RobotStateClient()
|
|
c.Init()
|
|
c.SetTimeout(3.0)
|
|
self._client = c
|
|
log.info("robot_state client ready (stereo video-service control)")
|
|
return c
|
|
except Exception as exc:
|
|
# No SDK, DDS not up yet, or robot link down — mark unavailable so we
|
|
# don't retry-spam; a process restart re-attempts.
|
|
log.info("robot_state client unavailable (%s) — stream auto-control off", exc)
|
|
self._unavailable = True
|
|
return None
|
|
|
|
def set_stereo(self, on: bool) -> bool:
|
|
"""Start (on=True) / stop (on=False) the stereo stream. Idempotent —
|
|
starting an already-running service is harmless. Returns True on success."""
|
|
with self._lock:
|
|
c = self._client_or_none()
|
|
if c is None:
|
|
return False
|
|
try:
|
|
code = c.ServiceSwitch(STEREO_SERVICE, bool(on))
|
|
if code == 0:
|
|
log.info("stereo stream -> %s", "ON" if on else "OFF")
|
|
return True
|
|
log.warning("stereo ServiceSwitch(%s) returned code=%s", on, code)
|
|
return False
|
|
except Exception as exc:
|
|
log.warning("stereo ServiceSwitch(%s) failed: %s", on, exc)
|
|
return False
|
|
|
|
|
|
_instance: Optional[StreamService] = None
|
|
|
|
|
|
def get_stream_service() -> StreamService:
|
|
global _instance
|
|
if _instance is None:
|
|
_instance = StreamService()
|
|
return _instance
|