#!/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()