Sedra/X2_dashboard
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
X2_dashboard/x2_dashboard_agent/x2_agent.py
Sidra 164d537e4d Update 2026-08-13 16:23
2026-08-13 16:23:18 +04:00

1478 lines
61 KiB
Python
Raw Permalink Blame History

#!/usr/bin/env python3
"""
AGIBOT X2 dashboard agent - runs ON the robot (PC2).
Why this exists
---------------
A browser cannot speak ROS 2 DDS, and the dashboard is meant to stay reachable
even when the robot is switched off (so it can show "power the robot on" rather
than a dead link). So the dashboard server lives on the operator's machine and
this small agent lives on the robot, bridging ROS 2 to a plain TCP socket.
Protocol: newline-delimited JSON, no dependencies beyond rclpy.
-> {"type":"hello","data":{...}} (agent, on connect)
-> {"type":"state","data":{...}} (agent, ~10 Hz)
<- {"type":"cmd","id":7,"name":"set_mode","args":{}} (client)
-> {"type":"result","id":7,"ok":true,"message":"..."} (agent)
Run it with ROS 2 and the AimDK workspace sourced:
source /opt/ros/humble/setup.bash
source ~/aimdk/install/setup.bash
python3 x2_agent.py
"""
from __future__ import annotations
import argparse
import asyncio
import base64
import importlib
import json
import math
import os
import signal
import socket
import threading
import time
import traceback
os.environ.setdefault("RCUTILS_LOGGING_SEVERITY", "ERROR")
import rclpy # noqa: E402
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup # noqa: E402
from rclpy.executors import MultiThreadedExecutor # noqa: E402
from rclpy.qos import ( # noqa: E402
QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy,
)
VERSION = "1.1.0"
DEFAULT_PORT = 8781
# --------------------------------------------------------------------------
# Interface constants - these mirror backend/x2_spec.py and were read off this
# robot's own aimdk_msgs package.
# --------------------------------------------------------------------------
SRV = {
"get_mode": ("aimdk_msgs/srv/GetMcAction", "/aimdk_5Fmsgs/srv/GetMcAction"),
"set_mode": ("aimdk_msgs/srv/SetMcAction", "/aimdk_5Fmsgs/srv/SetMcAction"),
"preset": ("aimdk_msgs/srv/SetMcPresetMotion", "/aimdk_5Fmsgs/srv/SetMcPresetMotion"),
"set_source": ("aimdk_msgs/srv/SetMcInputSource", "/aimdk_5Fmsgs/srv/SetMcInputSource"),
"get_source": ("aimdk_msgs/srv/GetCurrentInputSource", "/aimdk_5Fmsgs/srv/GetCurrentInputSource"),
"tts": ("aimdk_msgs/srv/PlayTts", "/aimdk_5Fmsgs/srv/PlayTts"),
"emoji": ("aimdk_msgs/srv/PlayEmoji", "/aimdk_5Fmsgs/srv/PlayEmoji"),
"led": ("aimdk_msgs/srv/SetPmuLed", "/aimdk_5Fmsgs/srv/SetPmuLed"),
"set_volume": ("aimdk_msgs/srv/SetVolume", "/aimdk_5Fmsgs/srv/SetVolume"),
"get_volume": ("aimdk_msgs/srv/GetVolume", "/aimdk_5Fmsgs/srv/GetVolume"),
"set_mute": ("aimdk_msgs/srv/SetMute", "/aimdk_5Fmsgs/srv/SetMute"),
"get_mute": ("aimdk_msgs/srv/GetMute", "/aimdk_5Fmsgs/srv/GetMute"),
"hand_type": ("aimdk_msgs/srv/GetHandType", "/aimdk_5Fmsgs/srv/GetHandType"),
}
TOPIC_PMU = "/aima/hal/pmu/state"
TOPIC_IMU_CHEST = "/aima/hal/imu/chest/state"
TOPIC_IMU_TORSO = "/aima/hal/imu/torso/state"
TOPIC_TOUCH = "/aima/hal/sensor/touch_head"
TOPIC_HAND_STATE = "/aima/hal/joint/hand/state"
TOPIC_HAND_CMD = "/aima/hal/joint/hand/command"
TOPIC_VELOCITY = "/aima/mc/locomotion/velocity"
TOPIC_ODOM = "/aima/mc/leg_odometry"
TOPIC_FACE = "/face_ui_proxy/status"
JOINT_GROUPS = {
"head": ("/aima/hal/joint/head/state", "/aima/hal/joint/head/command"),
"waist": ("/aima/hal/joint/waist/state", "/aima/hal/joint/waist/command"),
"arm": ("/aima/hal/joint/arm/state", "/aima/hal/joint/arm/command"),
"leg": ("/aima/hal/joint/leg/state", "/aima/hal/joint/leg/command"),
}
# Every camera on this robot, verified against the live ROS graph rather than
# the datasheet. All of them are *on demand*: nothing is subscribed until the
# operator switches a feed on. A compressed frame off this robot is 170-430 KB
# and the six RGB feeds together publish at ~60 Hz, so subscribing to the lot at
# startup would push ~15 MB/s through the DDS stack for pictures nobody is
# looking at - on the same Wi-Fi the robot uses to walk.
#
# `flip` is a per-feed default the browser can override. Every value below was
# set by pulling a real frame and looking at it, not by guessing:
# * both head cameras and the stereo pair are mounted upright
# * the Orbbec RGB-D module is mounted UPSIDE DOWN - its colour frame shows
# the floor across the top and the chairs hanging from it - so its colour
# and depth feeds both default to a 180 rotation.
CAMERAS = {
"rgb_head_front_center": {
"topic": "/aima/hal/sensor/rgb_head_front_center/rgb_image/compressed",
"type": "sensor_msgs/msg/CompressedImage", "kind": "rgb", "flip": False,
},
"rgb_head_rear": {
"topic": "/aima/hal/sensor/rgb_head_rear/rgb_image/compressed",
"type": "sensor_msgs/msg/CompressedImage", "kind": "rgb", "flip": False,
},
"rgbd_head_front": {
"topic": "/aima/hal/sensor/rgbd_head_front/rgb_image/compressed",
"type": "sensor_msgs/msg/CompressedImage", "kind": "rgb", "flip": True,
},
"stereo_head_front_left": {
"topic": "/aima/hal/sensor/stereo_head_front_left/rgb_image/compressed",
"type": "sensor_msgs/msg/CompressedImage", "kind": "rgb", "flip": False,
},
"stereo_head_front_right": {
"topic": "/aima/hal/sensor/stereo_head_front_right/rgb_image/compressed",
"type": "sensor_msgs/msg/CompressedImage", "kind": "rgb", "flip": False,
},
"depth_front": {
"topic": "/camera/depth/image_raw/compressedDepth",
"type": "sensor_msgs/msg/CompressedImage", "kind": "depth", "flip": True,
},
"perception_input": {
"topic": "/mono_perception/debug/input_image",
"type": "sensor_msgs/msg/CompressedImage", "kind": "debug", "flip": False,
},
"perception_seg": {
"topic": "/mono_perception/debug/seg_mask",
"type": "sensor_msgs/msg/CompressedImage", "kind": "debug", "flip": False,
},
"perception_lines": {
"topic": "/mono_perception/debug/line_color_map",
"type": "sensor_msgs/msg/CompressedImage", "kind": "debug", "flip": False,
},
}
# Chest LiDAR. 25.5k points per scan at 2 Hz, 816 KB a message - far too much to
# hold open for a tab nobody has opened, so it is on demand like the cameras.
LIDAR_KEY = "lidar_chest_front"
LIDAR = {
"topic": "/aima/hal/sensor/lidar_chest_front/lidar_pointcloud_down_sampling",
"type": "sensor_msgs/msg/PointCloud2", "kind": "lidar",
}
# Points kept per scan after decimation. The browser draws these as a point
# cloud; beyond a few thousand the JSON hop costs more than the picture gains.
LIDAR_MAX_POINTS = 4000
# compressedDepth frames carry a 12-byte ConfigHeader before the PNG payload
# (an int32 format enum plus two float32 quantisation params). Verified by
# decoding a real frame off this robot: strip 12 and a 720x1280 uint16 image
# decodes; strip 8, 16 or 20 and it does not.
DEPTH_HEADER_BYTES = 12
# Depth arrives in millimetres. Measured maximum return on this unit is ~4.2 m,
# so the ramp covers 4.5 m: stretching it to 6 m spent a third of the colour
# range on distances the sensor never reports, and flattened the near field
# everyone actually looks at into a single shade of blue.
DEPTH_RANGE_MM = 4500
MC_ACTION_VALUES = {
"PASSIVE_DEFAULT": 1, "SOFT_EMERGENCY_STOP": 2, "DAMPING_DEFAULT": 3,
"ZERO_TORQUE_DEFAULT": 4, "JOINT_DEFAULT": 100, "JOINT_FREEZE": 101,
"STAND_DEFAULT": 200, "STAND_BODY_CONTROL": 201, "LOCOMOTION_DEFAULT": 300,
"RUN_DEFAULT": 301, "LOCOMOTION_STEP": 302, "VR_REMOTE_CONTROLLER": 400,
"SIT_DOWN_DEFAULT": 2000, "CROUCH_DOWN_DEFAULT": 2002, "LIE_DOWN_DEFAULT": 2004,
"STAND_UP_DEFAULT": 2005, "ASCEND_STAIRS": 2006, "DESCEND_STAIRS": 2008,
}
MC_VALUE_TO_NAME = {v: k for k, v in MC_ACTION_VALUES.items()}
PMU_RAILS = [
("bus_48v", "bus_48v_voltage", "bus_48v_current", 48.0),
("output_48v", "output_48v_voltage", "output_48v_current", 48.0),
("output_12v", "output_12v_voltage", "output_12v_current", 12.0),
("head_power", "head_power_voltage", "head_power_current", 24.0),
("orin", "orin_voltage", "orin_current", 19.0),
("rk3588", "rk3588_voltage", "rk3588_current", 12.0),
("fan", "fan_voltage", None, 12.0),
("bus_48v_pmos", "bus_48v_pmos_voltage", None, 48.0),
]
DRIVEABLE_MODES = {"LOCOMOTION_DEFAULT", "LOCOMOTION_STEP", "RUN_DEFAULT",
"STAND_DEFAULT", "STAND_BODY_CONTROL"}
DEADMAN_S = 0.5
VELOCITY_HZ = 20
def load_type(path: str):
pkg, kind, name = path.split("/")
return getattr(importlib.import_module(f"{pkg}.{kind}"), name)
def seq(value):
"""
Normalise a ROS array field to a plain list.
Fixed-size array fields come back as numpy arrays, where `value or default`
raises ValueError instead of testing emptiness. Everything that touches an
array field goes through here.
"""
if value is None:
return []
try:
return list(value)
except TypeError:
return []
def quat_to_rpy(x, y, z, w):
roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
s = 2 * (w * y - z * x)
pitch = math.copysign(math.pi / 2, s) if abs(s) >= 1 else math.asin(s)
yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
return roll, pitch, yaw
def flatten(msg, depth=0):
"""ROS message -> JSON-safe dict."""
if depth > 3:
return None
try:
fields = msg.get_fields_and_field_types()
except AttributeError:
return msg if isinstance(msg, (int, float, str, bool)) else str(msg)[:120]
out = {}
for name in fields:
value = getattr(msg, name, None)
if isinstance(value, (int, float, str, bool)):
out[name] = value
elif hasattr(value, "get_fields_and_field_types"):
out[name] = flatten(value, depth + 1)
else:
try:
out[name] = [flatten(v, depth + 1) for v in list(value)[:32]]
except TypeError:
pass
return out
class Agent:
def __init__(self, node):
self.node = node
self.lock = threading.Lock()
self.state = {
"agent_version": VERSION,
"hostname": socket.gethostname(),
"mode": "UNKNOWN", "mode_desc": "", "mode_value": None, "mode_status": "",
"battery_pct": None, "battery_voltage": None, "battery_current": None,
"battery_temp": None, "battery_cycles": None, "battery_capacity_mah": None,
"battery_power": None, "charging": False,
"pmu_temp": None, "fan_rpm": None, "fan_pct": None,
"rails": {}, "pmu_info": {}, "pmu_raw": {},
"imu": {}, "joints": {}, "hand_type": "None", "hand_state": {},
"touch_head": {"touched": False, "zones": []},
"velocity": {"forward": 0.0, "lateral": 0.0, "angular": 0.0},
"velocity_command": {"forward": 0.0, "lateral": 0.0, "angular": 0.0},
"odom": {"x": 0.0, "y": 0.0, "yaw": 0.0},
"volume": None, "muted": False, "emoji_id": None, "face_status": None,
"led": {"mode": 0, "r": 0, "g": 0, "b": 0},
"input_source": "", "source_registered": False,
"topic_stats": {}, "cameras": {}, "streams": {},
}
self.frames = {} # camera key -> (ts, bytes, format)
self.clients = set()
self.publishers = {}
self.service_clients = {}
self.last_velocity_cmd = 0.0
self.led_keepalive = None
self.loop = None
# -- on-demand streams ------------------------------------------------
# Cameras and the LiDAR are not subscribed until asked for. `_streams`
# is the live registry; `_stream_wanted` is what the operator has asked
# for. The two are reconciled on the executor thread (see
# _reconcile_streams) because rclpy entity creation is not thread-safe.
self._streams = {} # key -> {"sub": obj, "spec": dict}
self._stream_wanted = {} # key -> bool
self._stream_lock = threading.Lock()
# Callback groups matter enormously here, and getting them wrong fails
# silently. Three separate groups, for three separate reasons:
#
# * Everything defaults into ONE MutuallyExclusiveCallbackGroup, which
# runs one callback at a time for the whole node. That group already
# carries two 500 Hz IMUs, four 100 Hz joint arrays and 100 Hz touch.
# Adding six camera feeds on top - each callback copying 170-430 KB -
# starved the reconcile timer within seconds of the first camera
# delivering data: switching feeds on and off stopped working while
# telemetry carried on looking perfectly healthy.
#
# * The timer therefore gets a group to itself, so nothing can delay it.
#
# * The streams get their own MutuallyExclusive group - deliberately NOT
# Reentrant. A reentrant group has no concurrency limit, so seven live
# feeds happily occupied all six executor threads and starved the
# timer all over again, just more slowly. Serialising them costs
# nothing (each callback is a memcpy - 60 frames/s measured at about
# 3% of one thread) and bounds their thread use at exactly one.
self._stream_group = MutuallyExclusiveCallbackGroup()
self._control_group = MutuallyExclusiveCallbackGroup()
self.cloud = None # (ts, [[x,y,z,intensity], ...])
for key in list(CAMERAS) + [LIDAR_KEY]:
self.state["streams"][key] = {
"key": key, "active": False, "frames": 0, "last": None,
"since": None, "error": "",
}
self._setup()
# -- ROS wiring ---------------------------------------------------------
def _qos(self, reliable=True, transient=False, depth=5):
return QoSProfile(
reliability=ReliabilityPolicy.RELIABLE if reliable else ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.TRANSIENT_LOCAL if transient else DurabilityPolicy.VOLATILE,
history=HistoryPolicy.KEEP_LAST, depth=depth,
)
def _sub(self, type_path, topic, handler, depth=2):
"""
Subscribe with BEST_EFFORT + VOLATILE.
In DDS a reader matches a writer when the writer *offers at least* what
the reader *requests*. BEST_EFFORT/VOLATILE requests the least of
anything, so this one profile matches every publisher on the robot -
the RELIABLE ones and the TRANSIENT_LOCAL ones alike. Requesting more
(an earlier version subscribed twice, once TRANSIENT_LOCAL) buys nothing
for live telemetry and makes Fast DDS retain history it then complains
about overflowing on the high-rate topics.
"""
try:
cls = load_type(type_path)
except Exception as exc:
print(f"[agent] cannot load {type_path} for {topic}: {exc}")
return
try:
self.node.create_subscription(
cls, topic, self._guard(topic, handler),
self._qos(reliable=False, transient=False, depth=depth))
except Exception as exc:
print(f"[agent] subscribe failed {topic}: {exc}")
def _guard(self, topic, handler):
"""
Wrap a subscription callback so it can never kill the executor.
rclpy lets an exception raised inside a callback propagate out of
MultiThreadedExecutor.spin(), which terminates the spin thread outright:
every subscription, timer and pending service response stops, silently,
for the life of the process. One malformed message must not be able to
take the whole bridge down, so each callback swallows and reports
instead.
"""
state = {"errors": 0}
def wrapped(msg):
try:
handler(msg)
except Exception as exc:
state["errors"] += 1
if state["errors"] <= 3:
print(f"[agent] callback error on {topic}: "
f"{type(exc).__name__}: {exc}")
traceback.print_exc()
elif state["errors"] == 4:
print(f"[agent] further errors on {topic} suppressed")
self.state.setdefault("callback_errors", {})[topic] = state["errors"]
return wrapped
def _setup(self):
self._sub("aimdk_msgs/msg/PmuState", TOPIC_PMU, self.on_pmu)
self._sub("sensor_msgs/msg/Imu", TOPIC_IMU_CHEST, lambda m: self.on_imu("chest", m))
self._sub("sensor_msgs/msg/Imu", TOPIC_IMU_TORSO, lambda m: self.on_imu("torso", m))
self._sub("aimdk_msgs/msg/TouchState", TOPIC_TOUCH, self.on_touch)
self._sub("aimdk_msgs/msg/HandStateArray", TOPIC_HAND_STATE, self.on_hand)
self._sub("nav_msgs/msg/Odometry", TOPIC_ODOM, self.on_odom)
self._sub("aimdk_msgs/msg/FaceEmojiStatus", TOPIC_FACE, self.on_face)
self._sub("aimdk_msgs/msg/McLocomotionVelocity", TOPIC_VELOCITY, self.on_velocity)
for key, (state_topic, _) in JOINT_GROUPS.items():
self._sub("aimdk_msgs/msg/JointStateArray", state_topic,
(lambda k: (lambda m: self.on_joints(k, m)))(key))
# Cameras and the LiDAR are deliberately NOT subscribed here. They are
# created and destroyed at runtime by _reconcile_streams below, so a
# feed costs nothing until somebody switches it on.
# Every publisher and service client is created here, before the
# executor starts spinning. rclpy is not thread-safe for entity
# creation: making a client from a worker thread while the executor is
# spinning wedges the executor, and subscriptions silently stop firing.
for alias in SRV:
try:
type_path, name = SRV[alias]
self.service_clients[alias] = self.node.create_client(
load_type(type_path), name)
except Exception as exc:
print(f"[agent] cannot create client for {alias}: {exc}")
for topic, type_path in (
(TOPIC_VELOCITY, "aimdk_msgs/msg/McLocomotionVelocity"),
(TOPIC_HAND_CMD, "aimdk_msgs/msg/HandCommandArray"),
*[(cmd_topic, "aimdk_msgs/msg/JointCommandArray")
for _, cmd_topic in JOINT_GROUPS.values()],
):
try:
self.publishers[topic] = self.node.create_publisher(
load_type(type_path), topic, self._qos(depth=10))
except Exception as exc:
print(f"[agent] cannot create publisher for {topic}: {exc}")
# Heartbeat. If this stops incrementing the executor has wedged, which
# is otherwise invisible - subscriptions just quietly stop arriving.
self.node.create_timer(1.0, self._on_heartbeat)
# Reconciles requested streams with live subscriptions. This has to be a
# timer rather than something the command handler does directly:
# commands run on an asyncio worker thread (see serve()), and creating
# or destroying a subscription off the executor thread while the
# executor is spinning wedges it - every subscription and timer stops,
# silently, for the life of the process. Timer callbacks run *on* the
# executor, which is exactly where this work is safe.
#
# _control_group, not the default one: see the note in __init__ about
# this timer being starved by the high-rate telemetry callbacks.
self.node.create_timer(0.2, self._reconcile_streams,
callback_group=self._control_group)
def _on_heartbeat(self):
try:
self.state["spin_ticks"] = self.state.get("spin_ticks", 0) + 1
except Exception:
pass
# -- on-demand streams --------------------------------------------------
def want_stream(self, key: str, active: bool) -> None:
"""Record a request. The executor timer does the actual ROS work."""
with self._stream_lock:
self._stream_wanted[key] = active
def stop_all_streams(self, why: str = "") -> None:
with self._stream_lock:
if not any(self._stream_wanted.values()):
return
for key in self._stream_wanted:
self._stream_wanted[key] = False
print(f"[agent] stopping all streams{f' ({why})' if why else ''}")
def _reconcile_streams(self):
"""Bring live subscriptions in line with what has been asked for.
Runs on the executor thread, ~5 Hz. Cheap when nothing has changed.
"""
with self._stream_lock:
wanted = dict(self._stream_wanted)
for key, active in wanted.items():
live = key in self._streams
if active == live:
continue
if active:
self._open_stream(key)
else:
self._close_stream(key)
def _open_stream(self, key: str):
spec = LIDAR if key == LIDAR_KEY else CAMERAS.get(key)
if spec is None:
return
entry = self.state["streams"].setdefault(key, {"key": key})
try:
cls = load_type(spec["type"])
except Exception as exc:
entry.update(active=False, error=f"cannot load {spec['type']}: {exc}")
with self._stream_lock:
self._stream_wanted[key] = False
return
handler = (self.on_cloud if key == LIDAR_KEY
else (lambda k: (lambda m: self.on_frame(k, m)))(key))
try:
sub = self.node.create_subscription(
cls, spec["topic"], self._guard(spec["topic"], handler),
# depth=1: only the newest frame or scan is ever served, so
# queueing older ones is pure memory for data that is already
# stale by the time anyone asks.
self._qos(reliable=False, transient=False, depth=1),
callback_group=self._stream_group)
except Exception as exc:
entry.update(active=False, error=str(exc))
with self._stream_lock:
self._stream_wanted[key] = False
return
self._streams[key] = {"sub": sub, "spec": spec}
entry.update(active=True, error="", since=time.time(), frames=0, last=None)
print(f"[agent] stream on: {key} -> {spec['topic']}")
def _close_stream(self, key: str):
entry = self._streams.pop(key, None)
if entry is not None:
try:
self.node.destroy_subscription(entry["sub"])
except Exception as exc:
print(f"[agent] destroy_subscription failed for {key}: {exc}")
# Drop the retained payload too - a stopped feed must not keep serving
# the last picture it saw as though it were live.
self.frames.pop(key, None)
if key == LIDAR_KEY:
self.cloud = None
self.state["cameras"].pop(key, None)
status = self.state["streams"].setdefault(key, {"key": key})
status.update(active=False, since=None, last=None)
print(f"[agent] stream off: {key}")
def mark(self, topic):
stats = self.state["topic_stats"]
now = time.time()
entry = stats.get(topic)
if entry is None:
stats[topic] = {"topic": topic, "count": 1, "last": now, "hz": 0.0}
return
gap = now - entry["last"]
if gap > 0:
inst = 1.0 / gap
entry["hz"] = round(inst if entry["count"] < 2 else entry["hz"] * 0.85 + inst * 0.15, 2)
entry["count"] += 1
entry["last"] = now
# -- subscription handlers ---------------------------------------------
def on_pmu(self, msg):
s = self.state
g = lambda f, d=None: getattr(msg, f, d) # noqa: E731
s["battery_pct"] = float(g("battery_remaining_capacity_percentage", 0) or 0)
s["battery_voltage"] = g("battery_voltage")
s["battery_current"] = g("battery_current")
s["battery_temp"] = g("battery_temperature")
s["battery_cycles"] = g("battery_cycle_count")
s["battery_capacity_mah"] = g("battery_remaining_capacity")
s["battery_power"] = g("battery_output_power")
s["pmu_temp"] = g("pmu_temperature")
s["fan_rpm"] = g("fan_speed")
s["fan_pct"] = g("fan_pecentage")
current = s["battery_current"]
s["charging"] = bool(current is not None and current > 0.05)
rails = {}
for key, vfield, cfield, nominal in PMU_RAILS:
voltage = g(vfield)
current = g(cfield) if cfield else None
rails[key] = {
"voltage": voltage, "current": current, "nominal": nominal,
"ok": voltage is None or voltage > nominal * 0.8,
}
s["rails"] = rails
s["pmu_info"] = {f: g(f) for f in (
"bms_manufacturer", "bms_serial_number", "bms_hardware_version",
"bms_software_version", "pmu_software_version", "pmu_hardware_version",
"pmu_protocol_version")}
s["pmu_raw"] = flatten(msg)
self.mark(TOPIC_PMU)
def on_imu(self, key, msg):
q = msg.orientation
roll, pitch, yaw = quat_to_rpy(q.x, q.y, q.z, q.w)
self.state["imu"][key] = {
"roll": round(roll, 5), "pitch": round(pitch, 5), "yaw": round(yaw, 5),
"accel_x": round(msg.linear_acceleration.x, 4),
"accel_y": round(msg.linear_acceleration.y, 4),
"accel_z": round(msg.linear_acceleration.z, 4),
"gyro_x": round(msg.angular_velocity.x, 5),
"gyro_y": round(msg.angular_velocity.y, 5),
"gyro_z": round(msg.angular_velocity.z, 5),
}
self.mark(TOPIC_IMU_CHEST if key == "chest" else TOPIC_IMU_TORSO)
def on_touch(self, msg):
# TouchState's fixed-size arrays arrive as numpy arrays, and
# `numpy_array or []` raises ValueError ("truth value ... is
# ambiguous"). Convert explicitly; never lean on truthiness here.
touched = [bool(v) for v in seq(getattr(msg, "is_touched", None))]
self.state["touch_head"] = {
"touched": any(touched),
"zones": touched,
"data": [int(v) for v in seq(getattr(msg, "data", None))],
"threshold": [int(v) for v in seq(getattr(msg, "threshold", None))],
"event_type": int(getattr(msg, "event_type", 0) or 0),
}
self.mark(TOPIC_TOUCH)
def on_hand(self, msg):
def side(prefix):
return [{
"name": getattr(h, "name", "") or f"j{i}",
"position": round(float(getattr(h, "position", 0.0)), 4),
"velocity": round(float(getattr(h, "velocity", 0.0)), 4),
"effort": round(float(getattr(h, "effort", 0.0)), 4),
"fault": int(getattr(h, "faultcode", 0) or 0),
} for i, h in enumerate(seq(getattr(msg, f"{prefix}_hands", None)))]
left_type = getattr(getattr(msg, "left_hand_type", None), "value", 0)
right_type = getattr(getattr(msg, "right_hand_type", None), "value", 0)
self.state["hand_state"] = {
"left": side("left"), "right": side("right"),
"left_type": int(left_type), "right_type": int(right_type),
}
self.mark(TOPIC_HAND_STATE)
def on_joints(self, key, msg):
rows = []
for j in seq(getattr(msg, "joints", None)):
rows.append({
"name": getattr(j, "name", ""),
"position": round(float(getattr(j, "position", 0.0)), 5),
"velocity": round(float(getattr(j, "velocity", 0.0)), 5),
"effort": round(float(getattr(j, "effort", 0.0)), 4),
"error": int(getattr(j, "error_code", 0) or 0),
})
self.state["joints"][key] = rows
self.mark(JOINT_GROUPS[key][0])
def on_odom(self, msg):
p = msg.pose.pose.position
q = msg.pose.pose.orientation
_, _, yaw = quat_to_rpy(q.x, q.y, q.z, q.w)
self.state["odom"] = {"x": round(p.x, 4), "y": round(p.y, 4), "yaw": round(yaw, 4)}
t = msg.twist.twist
self.state["velocity"] = {
"forward": round(t.linear.x, 4),
"lateral": round(t.linear.y, 4),
"angular": round(t.angular.z, 4),
}
self.mark(TOPIC_ODOM)
def on_velocity(self, msg):
# Someone (possibly us) is commanding velocity; reflect it so the UI can
# show the RC or app driving even when the dashboard is not.
self.state["velocity_command"] = {
"forward": round(float(getattr(msg, "forward_velocity", 0.0)), 4),
"lateral": round(float(getattr(msg, "lateral_velocity", 0.0)), 4),
"angular": round(float(getattr(msg, "angular_velocity", 0.0)), 4),
"source": getattr(msg, "source", ""),
}
self.mark(TOPIC_VELOCITY)
def on_face(self, msg):
self.state["emoji_id"] = int(getattr(msg, "e_id", 0) or 0)
self.state["face_status"] = int(getattr(msg, "status", 0) or 0)
self.mark(TOPIC_FACE)
def on_frame(self, key, msg):
fmt = (getattr(msg, "format", "") or "jpeg").lower()
data = bytes(msg.data)
now = time.time()
# Kept encoded exactly as it arrived. Depth is decoded lazily in
# cmd_camera_frame instead of here: this callback runs on the executor
# thread at the publish rate (~13 Hz for depth), while the browser asks
# for 1-5 frames a second, so decoding here would burn CPU on the
# robot's own control machine for frames nobody collects.
self.frames[key] = (now, data, "png" if "png" in fmt else "jpeg")
self.state["cameras"][key] = {"bytes": len(data), "ts": now, "format": fmt}
status = self.state["streams"].setdefault(key, {"key": key})
status["frames"] = status.get("frames", 0) + 1
status["last"] = now
self.mark(CAMERAS[key]["topic"])
def on_cloud(self, msg):
"""Decimate a PointCloud2 to something a browser can draw.
Fields on this robot: x,y,z float32 at offsets 0/4/8 and intensity
float32 at 16, point_step 32 (read off the live topic, not assumed).
A scan is ~25.5k points; sending them all as JSON would be several MB a
second for a picture that looks identical at a tenth of the points.
"""
import numpy as np
step = int(msg.point_step)
raw = np.frombuffer(bytes(msg.data), dtype=np.uint8)
count = raw.size // step
if not count:
return
rows = raw[:count * step].reshape(count, step)
# Stride rather than random choice: it keeps the scan's angular
# structure, so walls stay walls instead of dissolving into noise.
if count > LIDAR_MAX_POINTS:
rows = rows[:: max(1, count // LIDAR_MAX_POINTS)][:LIDAR_MAX_POINTS]
xyz = np.frombuffer(rows[:, 0:12].tobytes(), dtype=np.float32).reshape(-1, 3)
try:
intensity = np.frombuffer(rows[:, 16:20].tobytes(), dtype=np.float32)
except ValueError:
intensity = np.zeros(len(xyz), dtype=np.float32)
good = np.isfinite(xyz).all(axis=1)
xyz, intensity = xyz[good], intensity[good]
points = np.column_stack([xyz.round(3), intensity.round(1)])
self.cloud = (time.time(), points.tolist())
status = self.state["streams"].setdefault(LIDAR_KEY, {"key": LIDAR_KEY})
status["frames"] = status.get("frames", 0) + 1
status["last"] = time.time()
status["points"] = int(len(points))
status["scan_points"] = int(count)
self.mark(LIDAR["topic"])
# -- service helpers ----------------------------------------------------
def client_for(self, alias):
"""Clients are all pre-created in _setup - never build one here."""
type_path, _ = SRV[alias]
cli = self.service_clients.get(alias)
if cli is None:
raise RuntimeError(f"no client for '{alias}' (creation failed at startup)")
return cli, load_type(type_path)
def call(self, alias, build=None, timeout=6.0):
"""
Blocking service call, made from a worker thread.
Deliberately does NOT use wait_for_service. That builds a second wait
set on the same context while the executor is already waiting on one,
which deadlocks the executor in rclpy/Humble - subscriptions fire once
and then never again. service_is_ready() is a plain graph query with no
wait set, so it is safe to call from here.
"""
try:
cli, cls = self.client_for(alias)
except Exception as exc:
return False, f"type unavailable: {exc}", None
deadline = time.time() + timeout
while not cli.service_is_ready():
if time.time() > deadline:
return False, f"service {SRV[alias][1]} unavailable", None
time.sleep(0.05)
req = cls.Request()
if build:
try:
build(req)
except Exception as exc:
return False, f"bad request: {exc}", None
future = cli.call_async(req)
deadline = time.time() + timeout
while not future.done() and time.time() < deadline:
time.sleep(0.01)
if not future.done():
return False, "service call timed out", None
resp = future.result()
return True, "ok", flatten(resp)
def publisher(self, topic, type_path):
pub = self.publishers.get(topic)
if pub is None:
# Only reached by publish_raw for a topic not known at startup.
# Creating it now is a calculated risk the typed commands avoid.
pub = self.node.create_publisher(load_type(type_path), topic, self._qos(depth=10))
self.publishers[topic] = pub
return pub
# -- polling ------------------------------------------------------------
def poll_slow(self):
"""Mode, volume, mute, hand type - not available as topics."""
ok, _, detail = self.call("get_mode", timeout=3.0)
if ok and detail:
info = detail.get("info") or {}
desc = info.get("action_desc") or ""
value = (info.get("current_action") or {}).get("value")
status = (info.get("status") or {}).get("value")
if desc:
self.state["mode"] = desc
elif value in MC_VALUE_TO_NAME:
self.state["mode"] = MC_VALUE_TO_NAME[value]
self.state["mode_value"] = value
self.state["mode_status"] = status
ok, _, detail = self.call("get_volume", timeout=3.0)
if ok and detail and "audio_volume" in detail:
self.state["volume"] = detail["audio_volume"]
ok, _, detail = self.call("get_mute", timeout=3.0)
if ok and detail and "is_mute" in detail:
self.state["muted"] = bool(detail["is_mute"])
ok, _, detail = self.call("hand_type", timeout=3.0)
if ok and detail:
left = (detail.get("left_hands_type") or {}).get("value", 0)
right = (detail.get("right_hands_type") or {}).get("value", 0)
names = {0: "None", 1: "Nimble hands", 2: "Claw gripper",
3: "Leisai nimble hands", 255: "Error"}
self.state["hand_type"] = names.get(right or left, "Unknown")
self.state["hand_left_type"] = int(left)
self.state["hand_right_type"] = int(right)
ok, _, detail = self.call("get_source", timeout=3.0)
if ok and detail:
src = (detail.get("input_source") or {}).get("name", "")
self.state["input_source"] = src
def publish_velocity(self, forward, lateral, angular):
msg = load_type("aimdk_msgs/msg/McLocomotionVelocity")()
msg.source = "x2_dashboard"
msg.forward_velocity = float(forward)
msg.lateral_velocity = float(lateral)
msg.angular_velocity = float(angular)
self.publisher(TOPIC_VELOCITY, "aimdk_msgs/msg/McLocomotionVelocity").publish(msg)
# -- command handlers ---------------------------------------------------
def cmd_set_mode(self, args):
mode = str(args.get("mode", ""))
if mode not in MC_ACTION_VALUES:
return {"ok": False, "message": f"unknown mode '{mode}'"}
def build(req):
req.source = "x2_dashboard"
req.command.action.value = MC_ACTION_VALUES[mode]
req.command.action_desc = mode
ok, msg, detail = self.call("set_mode", build)
if ok:
self.state["mode"] = mode
return {"ok": ok, "message": f"mode -> {mode}" if ok else msg, "detail": detail}
def cmd_get_mode(self, args):
self.poll_slow()
return {"ok": True, "message": self.state["mode"],
"detail": {"mode": self.state["mode"], "status": self.state["mode_status"]}}
def cmd_set_velocity(self, args):
mode = self.state.get("mode")
if mode not in DRIVEABLE_MODES:
return {"ok": False,
"message": f"mode {mode} does not accept velocity - enter Stable stand or Walk"}
if not self.state.get("source_registered"):
return {"ok": False, "message": "register an input source first"}
f = float(args.get("forward", 0.0))
l = float(args.get("lateral", 0.0))
a = float(args.get("angular", 0.0))
self.state["velocity_command"] = {"forward": f, "lateral": l, "angular": a}
self.last_velocity_cmd = time.time()
try:
self.publish_velocity(f, l, a)
except Exception as exc:
return {"ok": False, "message": f"publish failed: {exc}"}
return {"ok": True, "message": "velocity published",
"detail": {"forward": f, "lateral": l, "angular": a}}
def cmd_stop(self, args):
"""Never gated - an emergency stop that can be refused is not one."""
self.state["velocity_command"] = {"forward": 0.0, "lateral": 0.0, "angular": 0.0}
self.last_velocity_cmd = time.time()
try:
self.publish_velocity(0.0, 0.0, 0.0)
except Exception as exc:
return {"ok": False, "message": f"stop publish failed: {exc}"}
return {"ok": True, "message": "motion stopped"}
def cmd_preset(self, args):
motion = int(args.get("motion", 0))
area = int(args.get("area", 0))
interrupt = bool(args.get("interrupt", True))
def build(req):
req.area.value = area
req.motion.value = motion
req.interrupt = interrupt
req.ani_path = ""
req.play_timestamp = 0
ok, msg, detail = self.call("preset", build, timeout=8.0)
return {"ok": ok, "message": "preset started" if ok else msg, "detail": detail}
def cmd_register_source(self, args):
name = str(args.get("name", "x2_dashboard"))
priority = int(args.get("priority", 30))
timeout = int(args.get("timeout", 1000))
def build(req):
req.action.value = 1001 # INPUTACTION_ADD
req.input_source.name = name
req.input_source.priority = priority
req.input_source.timeout = timeout
ok, msg, detail = self.call("set_source", build)
if ok:
self.state["source_registered"] = True
self.state["input_source"] = name
return {"ok": ok, "message": f"registered '{name}'" if ok else msg, "detail": detail}
def cmd_set_joints(self, args):
group = str(args.get("group", ""))
if group not in JOINT_GROUPS:
return {"ok": False, "message": f"unknown joint group '{group}'"}
mode = str(args.get("mode", "position"))
targets = args.get("targets") or {}
stiffness = args.get("stiffness")
damping = args.get("damping")
try:
ArrayT = load_type("aimdk_msgs/msg/JointCommandArray")
CmdT = load_type("aimdk_msgs/msg/JointCommand")
except Exception as exc:
return {"ok": False, "message": f"aimdk_msgs unavailable: {exc}"}
live = self.state["joints"].get(group) or []
names = [j["name"] for j in live] or list(targets)
commands = []
for name in names:
c = CmdT()
c.name = name
value = float(targets.get(name, 0.0))
if mode == "position":
c.position = value
elif mode == "velocity":
c.velocity = value
else:
c.effort = value
if stiffness is not None:
c.stiffness = float(stiffness)
if damping is not None:
c.damping = float(damping)
commands.append(c)
arr = ArrayT()
arr.joints = commands
try:
self.publisher(JOINT_GROUPS[group][1],
"aimdk_msgs/msg/JointCommandArray").publish(arr)
except Exception as exc:
return {"ok": False, "message": f"publish failed: {exc}"}
return {"ok": True, "message": f"{len(commands)} joint command(s) published"}
def cmd_set_hand(self, args):
side = str(args.get("side", "right"))
positions = [float(v) for v in (args.get("positions") or [])]
try:
ArrayT = load_type("aimdk_msgs/msg/HandCommandArray")
CmdT = load_type("aimdk_msgs/msg/HandCommand")
except Exception as exc:
return {"ok": False, "message": f"aimdk_msgs unavailable: {exc}"}
arr = ArrayT()
cmds = []
for v in positions:
c = CmdT()
c.position = v
cmds.append(c)
if side == "left":
arr.left_hands = cmds
else:
arr.right_hands = cmds
try:
self.publisher(TOPIC_HAND_CMD, "aimdk_msgs/msg/HandCommandArray").publish(arr)
except Exception as exc:
return {"ok": False, "message": f"publish failed: {exc}"}
return {"ok": True, "message": f"{side} hand command published"}
def cmd_speak(self, args):
text = str(args.get("text", "")).strip()
if not text:
return {"ok": False, "message": "nothing to say"}
priority = int(args.get("priority", 6))
interrupt = bool(args.get("interrupt", False))
def build(req):
req.tts_req.text = text
req.tts_req.priority_level.value = priority
req.tts_req.priority_weight = 50
req.tts_req.domain = "dashboard"
req.tts_req.trace_id = f"dash-{int(time.time() * 1000)}"
req.tts_req.is_interrupted = interrupt
ok, msg, detail = self.call("tts", build, timeout=8.0)
return {"ok": ok, "message": "speaking" if ok else msg, "detail": detail}
def cmd_set_volume(self, args):
volume = max(0, min(100, int(args.get("volume", 50))))
ok, msg, detail = self.call("set_volume", lambda r: setattr(r, "audio_volume", volume))
if ok:
self.state["volume"] = volume
return {"ok": ok, "message": f"volume {volume}" if ok else msg, "detail": detail}
def cmd_set_mute(self, args):
muted = bool(args.get("muted", False))
ok, msg, detail = self.call("set_mute", lambda r: setattr(r, "is_mute", muted))
if ok:
self.state["muted"] = muted
return {"ok": ok, "message": "muted" if muted else "unmuted", "detail": detail}
def cmd_emoji(self, args):
emotion = int(args.get("emotion_id", 1))
mode = int(args.get("mode", 1))
priority = int(args.get("priority", 6))
def build(req):
req.emotion_id = emotion
req.mode = mode
req.priority = priority
ok, msg, detail = self.call("emoji", build)
return {"ok": ok, "message": "emoji sent" if ok else msg, "detail": detail}
def _send_led(self, mode, r, g, b, priority):
def build(req):
req.trace_id = f"dash-{int(time.time() * 1000)}"
req.led_strip_mode = int(mode)
req.r = int(r)
req.g = int(g)
req.b = int(b)
req.priority = int(priority)
req.reset_priority = False
return self.call("led", build)
def cmd_led(self, args):
mode = int(args.get("mode", 0))
r, g, b = (int(args.get(k, 0)) for k in ("r", "g", "b"))
priority = int(args.get("priority", 6))
keep = bool(args.get("keep", True))
ok, msg, detail = self._send_led(mode, r, g, b, priority)
if ok:
self.state["led"] = {"mode": mode, "r": r, "g": g, "b": b, "keep": keep}
# The robot's own task_manager drives this strip too and reclaims it
# after roughly a minute, which is why a colour set from here fades
# away on its own. Remembering the request lets the keepalive below
# put it back.
self.led_keepalive = (
{"mode": mode, "r": r, "g": g, "b": b, "priority": priority}
if keep else None
)
return {"ok": ok, "message": "led updated" if ok else msg, "detail": detail}
def cmd_camera_frame(self, args):
key = str(args.get("key", ""))
if key not in self.state["streams"]:
return {"ok": False, "message": f"unknown stream '{key}'"}
if not self.state["streams"][key].get("active"):
return {"ok": False, "message": "feed is off", "detail": {"off": True}}
entry = self.frames.get(key)
if not entry:
return {"ok": False, "message": "no frame received on this topic yet"}
ts, data, fmt = entry
if time.time() - ts > 5.0:
return {"ok": False, "message": "feed is stale"}
spec = CAMERAS.get(key) or {}
flip = bool(args.get("flip", spec.get("flip", False)))
if spec.get("kind") == "depth":
data, fmt = self._render_depth(data, flip)
if data is None:
return {"ok": False, "message": fmt}
elif flip:
rotated = self._rotate180(data)
if rotated is not None:
data, fmt = rotated, "jpeg"
return {"ok": True, "message": "frame",
"detail": {"format": fmt, "ts": ts, "bytes": len(data),
"b64": base64.b64encode(data).decode("ascii")}}
@staticmethod
def _rotate180(data):
try:
import cv2
import numpy as np
img = cv2.imdecode(np.frombuffer(data, np.uint8), cv2.IMREAD_COLOR)
if img is None:
return None
ok, jpeg = cv2.imencode(".jpg", cv2.rotate(img, cv2.ROTATE_180),
[cv2.IMWRITE_JPEG_QUALITY, 80])
return jpeg.tobytes() if ok else None
except Exception:
return None
@staticmethod
def _render_depth(data, flip=False):
"""Turn a compressedDepth payload into a viewable colour image.
The raw frame is a 16-bit millimetre map behind a 12-byte header - a
browser cannot show that, and scaling it by eye ruins the near field.
Mapping a fixed metric range to a colour ramp keeps distances readable
and, more importantly, comparable between frames.
"""
try:
import cv2
import numpy as np
except ImportError:
return None, "depth needs numpy and opencv on the robot"
try:
img = cv2.imdecode(np.frombuffer(data[DEPTH_HEADER_BYTES:], np.uint8),
cv2.IMREAD_UNCHANGED)
if img is None:
return None, "could not decode depth frame"
depth = img.astype(np.float32)
if img.dtype != np.uint16:
# 32FC1 variants carry metres, not millimetres.
depth *= 1000.0
scaled = np.clip(depth / DEPTH_RANGE_MM * 255.0, 0, 255).astype(np.uint8)
coloured = cv2.applyColorMap(scaled, cv2.COLORMAP_JET)
# 0 means "no return", not "touching the lens". Painting it black
# stops the sky and every reflective surface reading as closest.
coloured[depth <= 0] = 0
if flip:
coloured = cv2.rotate(coloured, cv2.ROTATE_180)
ok, jpeg = cv2.imencode(".jpg", coloured, [cv2.IMWRITE_JPEG_QUALITY, 80])
if not ok:
return None, "could not encode depth frame"
return jpeg.tobytes(), "jpeg"
except Exception as exc:
return None, f"depth render failed: {type(exc).__name__}: {exc}"
def cmd_stream_set(self, args):
"""Switch a camera or the LiDAR on or off."""
key = str(args.get("key", ""))
active = bool(args.get("active", False))
if key not in self.state["streams"]:
return {"ok": False, "message": f"unknown stream '{key}'"}
self.want_stream(key, active)
# Wait for the executor timer to apply it, so the caller gets a truthful
# answer rather than an optimistic one.
deadline = time.time() + 6.0
while time.time() < deadline:
status = self.state["streams"][key]
if bool(status.get("active")) == active:
return {"ok": True,
"message": f"{key} {'on' if active else 'off'}",
"detail": dict(status)}
if status.get("error"):
return {"ok": False, "message": status["error"], "detail": dict(status)}
time.sleep(0.05)
return {"ok": False, "message": "the agent did not apply that in time",
"detail": dict(self.state["streams"][key])}
def cmd_stream_list(self, args):
return {"ok": True, "message": "streams", "detail": {
"streams": self.state["streams"],
"cameras": {k: {"topic": v["topic"], "kind": v["kind"], "flip": v["flip"]}
for k, v in CAMERAS.items()},
"lidar": {"key": LIDAR_KEY, **LIDAR},
}}
def cmd_lidar_points(self, args):
status = self.state["streams"].get(LIDAR_KEY) or {}
if not status.get("active"):
return {"ok": False, "message": "LiDAR is off", "detail": {"off": True}}
if not self.cloud:
return {"ok": False, "message": "no scan received yet"}
ts, points = self.cloud
if time.time() - ts > 8.0:
return {"ok": False, "message": "scan is stale"}
return {"ok": True, "message": "points", "detail": {
"ts": ts, "count": len(points), "points": points,
"frame": "lidar_chest_front",
}}
def cmd_graph(self, args):
return {"ok": True, "message": "graph", "detail": {
"topics": [{"name": n, "types": list(t)}
for n, t in self.node.get_topic_names_and_types()],
"services": [{"name": n, "types": list(t)}
for n, t in self.node.get_service_names_and_types()],
"nodes": [f"{ns}{n}" for n, ns in self.node.get_node_names_and_namespaces()],
}}
def cmd_publish_raw(self, args):
topic = str(args.get("topic", ""))
type_path = str(args.get("type", ""))
fields = args.get("fields") or {}
try:
msg = load_type(type_path)()
for k, v in fields.items():
if hasattr(msg, k):
setattr(msg, k, v)
self.publisher(topic, type_path).publish(msg)
except Exception as exc:
return {"ok": False, "message": f"{type(exc).__name__}: {exc}"}
return {"ok": True, "message": f"published to {topic}"}
def cmd_ping(self, args):
return {"ok": True, "message": "pong", "detail": {"ts": time.time()}}
HANDLERS = {
"set_mode": cmd_set_mode, "get_mode": cmd_get_mode,
"set_velocity": cmd_set_velocity, "stop": cmd_stop,
"preset": cmd_preset, "register_source": cmd_register_source,
"set_joints": cmd_set_joints, "set_hand": cmd_set_hand,
"speak": cmd_speak, "set_volume": cmd_set_volume, "set_mute": cmd_set_mute,
"emoji": cmd_emoji, "led": cmd_led, "camera_frame": cmd_camera_frame,
"graph": cmd_graph, "publish_raw": cmd_publish_raw, "ping": cmd_ping,
"stream_set": cmd_stream_set, "stream_list": cmd_stream_list,
"lidar_points": cmd_lidar_points,
}
def dispatch(self, name, args):
handler = self.HANDLERS.get(name)
if handler is None:
return {"ok": False, "message": f"unknown command '{name}'"}
try:
return handler(self, args or {})
except Exception as exc:
traceback.print_exc()
return {"ok": False, "message": f"{type(exc).__name__}: {exc}"}
# --------------------------------------------------------------------------
# TCP server
# --------------------------------------------------------------------------
async def serve(agent: Agent, host: str, port: int):
async def handle(reader, writer):
peer = writer.get_extra_info("peername")
agent.clients.add(writer)
print(f"[agent] client connected: {peer}")
try:
hello = {"type": "hello", "data": {
"agent_version": VERSION,
"hostname": socket.gethostname(),
"ros_domain_id": os.environ.get("ROS_DOMAIN_ID", "0"),
"cameras": list(CAMERAS),
"camera_specs": {k: {"topic": v["topic"], "kind": v["kind"],
"flip": v["flip"]} for k, v in CAMERAS.items()},
"lidar": {"key": LIDAR_KEY, **LIDAR},
"on_demand": True,
"started": AGENT_STARTED,
}}
writer.write((json.dumps(hello) + "\n").encode())
await writer.drain()
while True:
line = await reader.readline()
if not line:
break
try:
msg = json.loads(line)
except json.JSONDecodeError:
continue
if msg.get("type") != "cmd":
continue
result = await asyncio.to_thread(
agent.dispatch, msg.get("name", ""), msg.get("args"))
result["type"] = "result"
result["id"] = msg.get("id")
writer.write((json.dumps(result) + "\n").encode())
await writer.drain()
except (ConnectionError, asyncio.IncompleteReadError):
pass
except Exception as exc:
print(f"[agent] client error: {exc}")
finally:
agent.clients.discard(writer)
try:
writer.close()
except Exception:
pass
# Nobody left to watch: drop every camera and the LiDAR. Otherwise
# closing the last browser leaves the robot pushing megabytes a
# second through DDS forever, and the operator has no way to tell.
if not agent.clients:
agent.stop_all_streams("no clients connected")
print(f"[agent] client disconnected: {peer}")
server = await asyncio.start_server(handle, host, port)
addrs = ", ".join(str(s.getsockname()) for s in server.sockets)
print(f"[agent] listening on {addrs}")
async with server:
await server.serve_forever()
async def broadcaster(agent: Agent, hz: float):
period = 1.0 / hz
while True:
await asyncio.sleep(period)
if not agent.clients:
continue
agent.state["ts"] = time.time()
payload = (json.dumps({"type": "state", "data": agent.state},
default=str) + "\n").encode()
for writer in list(agent.clients):
try:
writer.write(payload)
except Exception:
agent.clients.discard(writer)
async def velocity_keepalive(agent: Agent):
"""Republish the standing velocity command and zero it if the client goes quiet."""
period = 1.0 / VELOCITY_HZ
while True:
await asyncio.sleep(period)
cmd = agent.state["velocity_command"]
moving = any(abs(float(cmd.get(k, 0) or 0)) > 1e-6
for k in ("forward", "lateral", "angular"))
if not moving or not agent.last_velocity_cmd:
continue
if time.time() - agent.last_velocity_cmd > DEADMAN_S:
agent.state["velocity_command"] = {"forward": 0.0, "lateral": 0.0, "angular": 0.0}
try:
agent.publish_velocity(0.0, 0.0, 0.0)
except Exception:
pass
print("[agent] dead-man timeout - velocity zeroed")
continue
try:
agent.publish_velocity(cmd["forward"], cmd["lateral"], cmd["angular"])
except Exception:
pass
async def led_keepalive(agent: Agent):
"""
Keep re-asserting the requested light-strip setting.
The PMU animations (breathing, blinking, flowing) are cyclic and would run
forever on their own, but the robot's task_manager also owns this strip and
takes it back after roughly a minute, so a colour set from the dashboard
quietly disappears. Re-sending the same request wins it back.
The interval is 20 s on purpose: it is a whole multiple of all three
animation cycles (4 s breathing, 1 s blinking, 2 s flowing), so the restart
lands on a cycle boundary and is not visible as a stutter. A value like 15 s
would cut the 4 s breathing cycle mid-way and look like a glitch.
"""
INTERVAL = 20.0
misses = 0
while True:
await asyncio.sleep(INTERVAL)
wanted = agent.led_keepalive
if not wanted:
misses = 0
continue
try:
ok, msg, _ = await asyncio.to_thread(
agent._send_led, wanted["mode"], wanted["r"], wanted["g"],
wanted["b"], wanted["priority"])
if ok:
misses = 0
else:
misses += 1
if misses in (1, 5):
print(f"[agent] led keepalive failed: {msg}")
except asyncio.CancelledError:
raise
except Exception as exc:
print(f"[agent] led keepalive error: {exc}")
async def slow_poller(agent: Agent):
quiet_failures = 0
while True:
# Once rclpy is tearing down, every service call raises. Stop rather
# than filling the journal with the same error twice a second.
if not rclpy.ok():
return
try:
await asyncio.to_thread(agent.poll_slow)
quiet_failures = 0
except asyncio.CancelledError:
raise
except Exception as exc:
quiet_failures += 1
if quiet_failures <= 3:
print(f"[agent] poll error: {exc}")
elif quiet_failures == 4:
print("[agent] further poll errors suppressed")
await asyncio.sleep(2.0)
AGENT_STARTED = time.time()
def main():
parser = argparse.ArgumentParser(description="AGIBOT X2 dashboard agent")
parser.add_argument("--host", default="0.0.0.0")
parser.add_argument("--port", type=int, default=DEFAULT_PORT)
parser.add_argument("--hz", type=float, default=10.0, help="state broadcast rate")
args = parser.parse_args()
rclpy.init()
node = rclpy.create_node("x2_dashboard_agent")
agent = Agent(node)
# Six threads, not four: there are now three callback groups (core
# telemetry, the reentrant stream group, and the reconcile timer), and the
# stream group wants room to copy several large frames at once.
executor = MultiThreadedExecutor(num_threads=6)
executor.add_node(node)
spin_thread = threading.Thread(target=executor.spin, daemon=True, name="ros-spin")
spin_thread.start()
print(f"[agent] x2_agent {VERSION} up on ROS domain {os.environ.get('ROS_DOMAIN_ID', '0')}")
async def run():
loop = asyncio.get_running_loop()
agent.loop = loop
stopping = asyncio.Event()
# systemd stops the unit with SIGTERM, which never surfaces as
# KeyboardInterrupt. Without this the loop keeps running after rclpy is
# torn down, the unit sits in "deactivating" until it is killed, and the
# journal fills with invalid-context errors.
for sig in (signal.SIGTERM, signal.SIGINT):
try:
loop.add_signal_handler(sig, stopping.set)
except NotImplementedError:
pass
tasks = [
asyncio.create_task(serve(agent, args.host, args.port), name="serve"),
asyncio.create_task(broadcaster(agent, args.hz), name="broadcast"),
asyncio.create_task(velocity_keepalive(agent), name="velocity"),
asyncio.create_task(led_keepalive(agent), name="led"),
asyncio.create_task(slow_poller(agent), name="poller"),
]
await stopping.wait()
print("[agent] stop requested, shutting down")
# Leave the robot safe: never hand back control with a velocity standing.
try:
agent.publish_velocity(0.0, 0.0, 0.0)
except Exception:
pass
for task in tasks:
task.cancel()
await asyncio.gather(*tasks, return_exceptions=True)
try:
asyncio.run(run())
except KeyboardInterrupt:
print("\n[agent] interrupted")
finally:
try:
executor.shutdown()
except Exception:
pass
try:
node.destroy_node()
except Exception:
pass
try:
if rclpy.ok():
rclpy.shutdown()
except Exception:
pass
print("[agent] stopped")
if __name__ == "__main__":
main()
Reference in New Issue View Git Blame Copy Permalink