242 lines
8.9 KiB
Python
242 lines
8.9 KiB
Python
"""
|
|
The contract every robot bridge implements.
|
|
|
|
Two bridges exist: bridge_ros2 (real rclpy over the network) and bridge_mock
|
|
(a physics-lite simulation). The server and the whole frontend only ever talk to
|
|
this interface, so the UI is identical whether or not a robot is present.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import time
|
|
from dataclasses import dataclass, field, asdict
|
|
from typing import Any
|
|
|
|
from . import x2_spec
|
|
|
|
|
|
@dataclass
|
|
class CommandResult:
|
|
ok: bool
|
|
message: str = ""
|
|
detail: Any = None
|
|
|
|
def as_dict(self) -> dict:
|
|
return {"ok": self.ok, "message": self.message, "detail": self.detail}
|
|
|
|
@classmethod
|
|
def failure(cls, message: str, detail: Any = None) -> "CommandResult":
|
|
return cls(False, message, detail)
|
|
|
|
@classmethod
|
|
def success(cls, message: str = "OK", detail: Any = None) -> "CommandResult":
|
|
return cls(True, message, detail)
|
|
|
|
|
|
@dataclass
|
|
class Connection:
|
|
online: bool = False
|
|
transport: str = "none" # "ros2" | "mock" | "none"
|
|
host: str = ""
|
|
ros_domain_id: int = 0
|
|
since: float | None = None
|
|
error: str = ""
|
|
simulated: bool = True
|
|
|
|
@property
|
|
def uptime_s(self) -> float:
|
|
return (time.time() - self.since) if self.since else 0.0
|
|
|
|
|
|
@dataclass
|
|
class RobotState:
|
|
"""Everything the dashboard knows about the robot right now."""
|
|
|
|
connection: Connection = field(default_factory=Connection)
|
|
|
|
mode: str = "UNKNOWN"
|
|
mode_desc: str = ""
|
|
mode_status: str = "unknown"
|
|
|
|
# Power (from /aima/hal/pmu/state)
|
|
battery_pct: float | None = None
|
|
battery_voltage: float | None = None
|
|
battery_current: float | None = None
|
|
battery_temp: float | None = None
|
|
battery_cycles: int | None = None
|
|
charging: bool = False
|
|
pmu_temp: float | None = None
|
|
fan_rpm: float | None = None
|
|
fan_pct: float | None = None
|
|
rails: dict[str, dict] = field(default_factory=dict)
|
|
|
|
# IMU (from /aima/hal/imu/*/state)
|
|
imu: dict[str, dict] = field(default_factory=dict)
|
|
|
|
# Joints, keyed by group ("head" | "waist" | "arm" | "leg")
|
|
joints: dict[str, list] = field(default_factory=dict)
|
|
|
|
# End effector
|
|
hand_type: str = "unknown"
|
|
hand_state: dict[str, list] = field(default_factory=dict)
|
|
|
|
# Sensors
|
|
touch_head: dict = field(default_factory=dict)
|
|
|
|
# Motion
|
|
velocity: dict = field(default_factory=lambda: {"forward": 0.0, "lateral": 0.0, "angular": 0.0})
|
|
velocity_command: dict = field(default_factory=lambda: {"forward": 0.0, "lateral": 0.0, "angular": 0.0})
|
|
odom: dict = field(default_factory=lambda: {"x": 0.0, "y": 0.0, "yaw": 0.0})
|
|
|
|
# Interaction
|
|
volume: int = 60
|
|
muted: bool = False
|
|
emoji_id: int | None = None
|
|
led: dict = field(default_factory=lambda: {"mode": 0, "r": 0, "g": 0, "b": 0})
|
|
|
|
# Arbitration
|
|
input_source: str = ""
|
|
input_sources: list = field(default_factory=list)
|
|
source_registered: bool = False
|
|
|
|
# Topic liveness, keyed by topic name
|
|
topic_stats: dict[str, dict] = field(default_factory=dict)
|
|
|
|
# Anything a plugin wants to publish into the shared state
|
|
custom: dict[str, Any] = field(default_factory=dict)
|
|
|
|
updated: float = field(default_factory=time.time)
|
|
|
|
def as_dict(self) -> dict:
|
|
data = asdict(self)
|
|
data["connection"]["uptime_s"] = round(self.connection.uptime_s, 1)
|
|
return data
|
|
|
|
def mark_topic(self, topic: str, expected_hz: float | None = None) -> None:
|
|
now = time.time()
|
|
stat = self.topic_stats.get(topic)
|
|
if stat is None:
|
|
stat = {"topic": topic, "count": 0, "last": now, "hz": 0.0,
|
|
"expected_hz": expected_hz, "first": now}
|
|
self.topic_stats[topic] = stat
|
|
gap = now - stat["last"]
|
|
if gap > 0:
|
|
# Exponential moving average keeps the rate readable rather than jittery.
|
|
instant = 1.0 / gap
|
|
stat["hz"] = round(instant if stat["count"] == 0 else stat["hz"] * 0.8 + instant * 0.2, 2)
|
|
stat["count"] += 1
|
|
stat["last"] = now
|
|
if expected_hz is not None:
|
|
stat["expected_hz"] = expected_hz
|
|
|
|
|
|
class Bridge:
|
|
"""Abstract robot bridge. Subclasses override what they can support."""
|
|
|
|
name = "base"
|
|
simulated = True
|
|
|
|
def __init__(self, hub, config: dict):
|
|
self.hub = hub
|
|
self.config = config
|
|
self.state = RobotState()
|
|
|
|
# -- lifecycle ----------------------------------------------------------
|
|
|
|
async def start(self) -> None:
|
|
raise NotImplementedError
|
|
|
|
async def stop(self) -> None:
|
|
raise NotImplementedError
|
|
|
|
async def reconfigure(self, config: dict) -> None:
|
|
self.config = config
|
|
|
|
# -- introspection ------------------------------------------------------
|
|
|
|
def snapshot(self) -> dict:
|
|
return self.state.as_dict()
|
|
|
|
async def list_topics(self) -> list[dict]:
|
|
return list(self.state.topic_stats.values())
|
|
|
|
# -- commands -----------------------------------------------------------
|
|
|
|
async def set_mode(self, mode_id: str) -> CommandResult:
|
|
return CommandResult.failure("set_mode not supported by this bridge")
|
|
|
|
async def get_mode(self) -> CommandResult:
|
|
return CommandResult.success(detail={"mode": self.state.mode})
|
|
|
|
async def set_velocity(self, forward: float, lateral: float, angular: float) -> CommandResult:
|
|
return CommandResult.failure("set_velocity not supported by this bridge")
|
|
|
|
async def stop_motion(self) -> CommandResult:
|
|
"""
|
|
Zero the velocity command unconditionally.
|
|
|
|
Deliberately not routed through set_velocity: every precondition that
|
|
method enforces (right mode, registered input source) is a reason to
|
|
refuse *starting* motion, never a reason to refuse stopping it. An
|
|
emergency stop that can be declined is not an emergency stop.
|
|
"""
|
|
self.state.velocity_command = {"forward": 0.0, "lateral": 0.0, "angular": 0.0}
|
|
return CommandResult.success("Motion stopped")
|
|
|
|
async def play_preset(self, motion: int, area: int, interrupt: bool = True) -> CommandResult:
|
|
return CommandResult.failure("play_preset not supported by this bridge")
|
|
|
|
async def set_joints(self, group: str, mode: str, targets: dict[str, float],
|
|
stiffness: float | None = None,
|
|
damping: float | None = None) -> CommandResult:
|
|
return CommandResult.failure("set_joints not supported by this bridge")
|
|
|
|
async def set_hand(self, side: str, positions: list[float]) -> CommandResult:
|
|
return CommandResult.failure("set_hand not supported by this bridge")
|
|
|
|
async def speak(self, text: str, priority: int = 6, interrupt: bool = False) -> CommandResult:
|
|
return CommandResult.failure("speak not supported by this bridge")
|
|
|
|
async def set_volume(self, volume: int) -> CommandResult:
|
|
return CommandResult.failure("set_volume not supported by this bridge")
|
|
|
|
async def set_mute(self, muted: bool) -> CommandResult:
|
|
return CommandResult.failure("set_mute not supported by this bridge")
|
|
|
|
async def play_emoji(self, emotion_id: int, mode: int = 1, priority: int = 6) -> CommandResult:
|
|
return CommandResult.failure("play_emoji not supported by this bridge")
|
|
|
|
async def set_led(self, mode: int, r: int, g: int, b: int,
|
|
priority: int = x2_spec.LED_DEFAULT_PRIORITY,
|
|
keep: bool = True) -> CommandResult:
|
|
return CommandResult.failure("set_led not supported by this bridge")
|
|
|
|
async def register_input_source(self, name: str, priority: int, timeout: int) -> CommandResult:
|
|
return CommandResult.failure("register_input_source not supported by this bridge")
|
|
|
|
async def camera_frame(self, camera_key: str, stream: str = "rgb",
|
|
flip: bool | None = None) -> bytes | None:
|
|
"""Latest frame as JPEG bytes, or None if unavailable."""
|
|
return None
|
|
|
|
# -- on-demand streams --------------------------------------------------
|
|
# Cameras and the LiDAR stay unsubscribed on the robot until switched on
|
|
# here, so an unopened tab costs no bandwidth.
|
|
|
|
async def set_stream(self, key: str, active: bool) -> CommandResult:
|
|
return CommandResult.failure("set_stream not supported by this bridge")
|
|
|
|
async def list_streams(self) -> CommandResult:
|
|
return CommandResult.failure("list_streams not supported by this bridge")
|
|
|
|
async def lidar_points(self) -> CommandResult:
|
|
return CommandResult.failure("lidar_points not supported by this bridge")
|
|
|
|
# -- escape hatch for plugins ------------------------------------------
|
|
|
|
async def publish_raw(self, topic: str, message_type: str, payload: dict) -> CommandResult:
|
|
return CommandResult.failure("publish_raw not supported by this bridge")
|
|
|
|
async def call_service(self, service: str, service_type: str, payload: dict) -> CommandResult:
|
|
return CommandResult.failure("call_service not supported by this bridge")
|