diff --git a/dimos/core/arduino_module.py b/dimos/core/arduino_module.py new file mode 100644 index 0000000000..abf8203de4 --- /dev/null +++ b/dimos/core/arduino_module.py @@ -0,0 +1,1036 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""ArduinoModule: DimOS module for Arduino-based hardware. + +An ArduinoModule generates a ``dimos_arduino.h`` header at build time, +compiles and flashes the user's Arduino sketch, then launches a generic +C++ bridge that relays structured data between the Arduino's USB serial +and the DimOS LCM bus. + +Example usage:: + + class MyArduinoBot(ArduinoModule): + config: MyArduinoBotConfig + imu_out: Out[Imu] + motor_cmd_in: In[Twist] + +See ``dimos/hardware/arduino/`` for the C headers, bridge binary, and +protocol documentation. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import errno +import functools +import glob +import inspect +import json +import math +import os +from pathlib import Path +import re +import shutil +import subprocess +import tempfile +import time +from typing import IO, Any, ClassVar, get_args, get_origin, get_type_hints + +from dimos.core.core import rpc +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.core.stream import In, Out +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# Path to the arduino hardware directory (relative to this file) +_ARDUINO_HW_DIR = Path(__file__).resolve().parent.parent / "hardware" / "arduino" +_COMMON_DIR = _ARDUINO_HW_DIR / "common" +_DSP_PROTOCOL_PATH = _COMMON_DIR / "dsp_protocol.h" + +# Lock file coordinating concurrent `nix build .#arduino_bridge` across +# ArduinoModule instances in the same blueprint. +_BRIDGE_BUILD_LOCK_PATH = _ARDUINO_HW_DIR / ".bridge_build.lock" + + +@functools.lru_cache(maxsize=1) +def _arduino_tools_bin_dir() -> Path: + """Resolve dimos_arduino_tools via ``nix build`` and return its bin/ path. + + Uses nix rather than $PATH so the module works without ``nix develop``. + Cached for the process lifetime; no-op after the first cold build. + """ + logger.info("Resolving dimos_arduino_tools via nix build") + try: + result = subprocess.run( + [ + "nix", + "build", + ".#dimos_arduino_tools", + "--print-out-paths", + "--no-link", + ], + cwd=str(_ARDUINO_HW_DIR), + capture_output=True, + text=True, + timeout=600, + ) + except FileNotFoundError as exc: + raise RuntimeError( + "`nix` not found on PATH. ArduinoModule resolves its Arduino " + "toolchain (arduino-cli, avrdude, qemu-system-avr) through the " + f"flake at {_ARDUINO_HW_DIR}, so the `nix` CLI must be " + "installed and on PATH. Install Nix (https://nixos.org/download) " + "and re-run." + ) from exc + if result.returncode != 0: + raise RuntimeError( + "Failed to materialize `dimos_arduino_tools` via " + "`nix build .#dimos_arduino_tools`:\n" + f"{result.stderr}\n{result.stdout}" + ) + # `--print-out-paths` prints one store path per output on stdout; + # we have a single output, so the last non-empty line is it. + out_paths = [line for line in result.stdout.splitlines() if line.strip()] + if not out_paths: + raise RuntimeError( + "`nix build .#dimos_arduino_tools --print-out-paths` returned " + "no paths on stdout. This should never happen; please file a " + "bug against the arduino flake." + ) + return Path(out_paths[-1]) / "bin" + + +def _arduino_msgs_dir() -> Path: + """Return the path to generated Arduino LCM message headers (share/arduino_msgs/).""" + tools_bin = _arduino_tools_bin_dir() + msgs_dir = tools_bin.parent / "share" / "arduino_msgs" + if not msgs_dir.is_dir(): + raise RuntimeError( + f"Expected Arduino message headers at {msgs_dir} but directory " + "does not exist. Rebuild dimos_arduino_tools: " + "nix build .#dimos_arduino_tools" + ) + return msgs_dir + + +def _arduino_cli_bin() -> str: + return str(_arduino_tools_bin_dir() / "arduino-cli") + + +def _avrdude_bin() -> str: + return str(_arduino_tools_bin_dir() / "avrdude") + + +def _qemu_system_avr_bin() -> str: + return str(_arduino_tools_bin_dir() / "qemu-system-avr") + + +@dataclass +class CTypeGenerator: + """Override for generating C struct/encode/decode for a message type.""" + + struct_create: Any # Callable[[str], str] — (type_name) -> C code + encode_create: Any | None = None # Callable[[str, str, int], str] + decode_create: Any | None = None # Callable[[str, str, int], str] + + +# Registry of known Arduino-compatible message type header paths. +# +# This list is kept in sync with two other places: +# - dimos/hardware/arduino/cpp/main.cpp :: init_hash_registry() +# - dimos-lcm :: generated/arduino_c_msgs/** +# `tests/test_arduino_msg_registry_sync.py` fails CI if any drift appears. +_KNOWN_TYPE_HEADERS: dict[str, str] = { + "std_msgs.Time": "std_msgs/Time.h", + "std_msgs.Bool": "std_msgs/Bool.h", + "std_msgs.Int32": "std_msgs/Int32.h", + "std_msgs.Float32": "std_msgs/Float32.h", + "std_msgs.Float64": "std_msgs/Float64.h", + "std_msgs.ColorRGBA": "std_msgs/ColorRGBA.h", + "geometry_msgs.Vector3": "geometry_msgs/Vector3.h", + "geometry_msgs.Point": "geometry_msgs/Point.h", + "geometry_msgs.Point32": "geometry_msgs/Point32.h", + "geometry_msgs.Quaternion": "geometry_msgs/Quaternion.h", + "geometry_msgs.Pose": "geometry_msgs/Pose.h", + "geometry_msgs.Pose2D": "geometry_msgs/Pose2D.h", + "geometry_msgs.Twist": "geometry_msgs/Twist.h", + "geometry_msgs.Accel": "geometry_msgs/Accel.h", + "geometry_msgs.Transform": "geometry_msgs/Transform.h", + "geometry_msgs.Wrench": "geometry_msgs/Wrench.h", + "geometry_msgs.Inertia": "geometry_msgs/Inertia.h", + "geometry_msgs.PoseWithCovariance": "geometry_msgs/PoseWithCovariance.h", + "geometry_msgs.TwistWithCovariance": "geometry_msgs/TwistWithCovariance.h", + "geometry_msgs.AccelWithCovariance": "geometry_msgs/AccelWithCovariance.h", +} + + +class ArduinoModuleConfig(NativeModuleConfig): + def to_cli_args(self) -> list[str]: + # Bridge CLI is built explicitly in start() — suppress generic emission. + return [] + + # Sketch + sketch_path: str = "sketch/sketch.ino" + board_fqbn: str = "arduino:avr:uno" + + # Bridge binary (generic, same for all modules) + executable: str = "result/bin/arduino_bridge" + build_command: str = "nix build .#arduino_bridge" + cwd: str | None = None + + # Connection + port: str | None = None + baudrate: int = 115200 + auto_detect: bool = True + auto_reconnect: bool = True + reconnect_interval: float = 2.0 + + # Virtual mode (QEMU emulator instead of real hardware) + virtual: bool = False + qemu_startup_timeout_s: float = 5.0 + + # Flash + auto_flash: bool = True + flash_timeout: float = 60.0 + + # Compile-time tuning (passed as -D flags to arduino-cli) + # These override the defaults in dimos_lcm_pubsub.h / dsp_protocol.h. + # Set to None (the default) to keep the header's built-in default. + max_subs: int | None = None # DIMOS_LCM_MAX_SUBS (AVR default: 4) + max_pending: int | None = None # DIMOS_LCM_MAX_PENDING (AVR default: 2) + max_msg_size: int | None = None # DIMOS_LCM_MAX_MSG_SIZE (AVR default: 64) + max_payload: int | None = None # DSP_MAX_PAYLOAD (AVR default: 256) + + # Arbitrary user-defined #defines emitted in dimos_arduino.h and + # passed as -D compiler flags. Example: + # arduino_defines={"MOTOR_PIN": 13, "SENSOR_THRESHOLD": 0.5} + # becomes: + # #define MOTOR_PIN 13 + # #define SENSOR_THRESHOLD 0.5f + arduino_defines: dict[str, int | float | str | bool] = {} + + # Subclass fields to exclude from the generated #define embedding. + # Framework fields are excluded automatically; this is for user fields + # that shouldn't reach the sketch. + arduino_config_exclude: frozenset[str] = frozenset() + + +# Framework fields that DO get embedded in the sketch header. +_ARDUINO_SKETCH_FIELDS: frozenset[str] = frozenset({"baudrate"}) + + +# Must match #ifdef __AVR__ in dsp_protocol.h. +_AVR_DEFAULT_DSP_MAX_PAYLOAD = 256 + +# Mapping from ArduinoModuleConfig field names to C preprocessor macros +# for the compile-time tuning knobs. +_TUNING_FIELD_TO_DEFINE: dict[str, str] = { + "max_subs": "DIMOS_LCM_MAX_SUBS", + "max_pending": "DIMOS_LCM_MAX_PENDING", + "max_msg_size": "DIMOS_LCM_MAX_MSG_SIZE", + "max_payload": "DSP_MAX_PAYLOAD", +} + + +def _c_literal(name: str, val: int | float | str | bool) -> str: + """Convert a Python value to a C literal string for ``#define``.""" + if isinstance(val, bool): + return "1" if val else "0" + if isinstance(val, int): + return str(val) + if isinstance(val, float): + if not math.isfinite(val): + raise ValueError( + f"Cannot embed non-finite float for arduino_defines key {name!r} (value={val!r})" + ) + return f"{val}f" + if isinstance(val, str): + return json.dumps(val) + raise TypeError( + f"arduino_defines key {name!r} has unsupported type " + f"{type(val).__name__}. Use int, float, str, or bool." + ) + + +_AVR_FQBN_PREFIXES: tuple[str, ...] = ("arduino:avr:",) + + +class ArduinoModule(NativeModule): + """Manages an Arduino board: generates header, compiles sketch, flashes, + and runs a C++ serial↔LCM bridge. + """ + + config: ArduinoModuleConfig + c_type_generators: ClassVar[dict[type, CTypeGenerator]] = {} + + _qemu_proc: subprocess.Popen[bytes] | None = None + _virtual_pty: str | None = None + _qemu_log_path: str | None = None + _qemu_log_fd: IO[bytes] | None = None + _bridge_bin: str | None = None + + @rpc + def build(self) -> None: + """Generate header, compile sketch, build bridge, and optionally flash.""" + if not self.config.virtual and self.config.auto_detect and not self.config.port: + self.config.port = self._detect_port() + logger.info("Auto-detected Arduino port", port=self.config.port) + + self._generate_header() + self._ensure_core_installed() + self._compile_sketch() + self._build_bridge() + self._bridge_bin = str(_ARDUINO_HW_DIR / "result" / "bin" / "arduino_bridge") + + if not self.config.virtual and self.config.auto_flash and self.config.port: + self._flash() + + def _build_bridge(self) -> None: + """Build the C++ bridge via nix. File-locked to handle concurrent modules.""" + import fcntl # POSIX-only; deferred here so the module can be imported on Windows + + bridge_bin = _ARDUINO_HW_DIR / "result" / "bin" / "arduino_bridge" + + # Ensure the lock file exists (nix flake dir is always present). + _BRIDGE_BUILD_LOCK_PATH.touch(exist_ok=True) + + with open(_BRIDGE_BUILD_LOCK_PATH, "w") as lock_fh: + fcntl.flock(lock_fh.fileno(), fcntl.LOCK_EX) + try: + logger.info("Building arduino_bridge via nix flake") + result = subprocess.run( + ["nix", "build", ".#arduino_bridge"], + cwd=str(_ARDUINO_HW_DIR), + capture_output=True, + text=True, + timeout=600, + ) + if result.returncode != 0: + raise RuntimeError( + f"arduino_bridge build failed:\n{result.stderr}\n{result.stdout}" + ) + if not bridge_bin.exists(): + raise RuntimeError( + f"arduino_bridge build succeeded but binary missing: {bridge_bin}" + ) + logger.info("arduino_bridge built successfully", path=str(bridge_bin)) + finally: + fcntl.flock(lock_fh.fileno(), fcntl.LOCK_UN) + + def _resolve_topics(self) -> dict[str, str]: + """Collect topics and validate they have the ``channel#msg_type`` shape + the bridge requires (only LCMTransport produces this). + """ + raw = super()._collect_topics() + bad: list[tuple[str, str]] = [] + for stream_name, channel in raw.items(): + if "#" not in channel: + bad.append((stream_name, channel)) + if bad: + bad_desc = ", ".join(f"{s!r}={c!r}" for s, c in bad) + raise RuntimeError( + f"ArduinoModule stream(s) {bad_desc} resolved to channel " + f"strings without a '#msg_type' suffix. The arduino_bridge " + f"binary needs typed channels to look up LCM fingerprint " + f"hashes. Declare these streams with LCMTransport (the " + f"default) rather than pLCMTransport / SHMTransport / etc., " + f"or remap them to use LCM." + ) + return raw + + def _build_topic_args(self) -> list[str]: + # The bridge uses its own CLI schema (--topic_in , + # --topic_out ) built in start(), so suppress the + # generic -- args from NativeModule. + return [] + + @rpc + def start(self) -> None: + """Launch the C++ bridge subprocess (and QEMU if virtual).""" + topics = self._resolve_topics() + topic_enum = self._build_topic_enum() + + if self.config.virtual: + serial_port = self._start_qemu() + else: + serial_port = self.config.port or "/dev/ttyACM0" + + bridge_args = [ + "--serial_port", + serial_port, + "--baudrate", + str(self.config.baudrate), + "--reconnect", + str(self.config.auto_reconnect).lower(), + "--reconnect_interval", + str(self.config.reconnect_interval), + ] + + for stream_name, topic_id in topic_enum.items(): + if stream_name not in topics: + continue + lcm_channel = topics[stream_name] + if stream_name in self.outputs: + bridge_args.extend(["--topic_out", str(topic_id), lcm_channel]) + elif stream_name in self.inputs: + bridge_args.extend(["--topic_in", str(topic_id), lcm_channel]) + + if self._bridge_bin is not None: + self.config.executable = self._bridge_bin + + # Save/restore extra_args so start()/stop() cycles don't accumulate. + user_extra = list(self.config.extra_args) + self.config.extra_args = user_extra + bridge_args + try: + super().start() + except BaseException: + # If the bridge itself failed to launch we still need to tear + # down any QEMU process we just brought up. + self._cleanup_qemu() + raise + finally: + self.config.extra_args = user_extra + + @rpc + def stop(self) -> None: + try: + super().stop() + finally: + self._cleanup_qemu() + + def _cleanup_qemu(self) -> None: + """Tear down QEMU state. Safe to call even if never started.""" + if self._qemu_proc is not None: + try: + if self._qemu_proc.poll() is None: + self._qemu_proc.terminate() + try: + self._qemu_proc.wait(timeout=5) + except subprocess.TimeoutExpired: + self._qemu_proc.kill() + try: + self._qemu_proc.wait(timeout=2) + except subprocess.TimeoutExpired: + logger.error( + "QEMU did not exit after SIGKILL", + pid=self._qemu_proc.pid, + ) + finally: + self._qemu_proc = None + + if self._qemu_log_fd is not None: + try: + self._qemu_log_fd.close() + except OSError: + pass + self._qemu_log_fd = None + + if self._qemu_log_path is not None: + try: + os.unlink(self._qemu_log_path) + except FileNotFoundError: + pass + except OSError as exc: + logger.warning( + "Failed to remove QEMU log file", + path=self._qemu_log_path, + error=str(exc), + ) + self._qemu_log_path = None + + if self._virtual_pty is not None: + logger.info("QEMU virtual Arduino stopped") + self._virtual_pty = None + + @rpc + def flash(self) -> None: + self._flash() + + def _get_stream_types(self) -> dict[str, type]: + hints = get_type_hints(type(self)) + result: dict[str, type] = {} + for name, hint in hints.items(): + origin = get_origin(hint) + if origin is In or origin is Out: + args = get_args(hint) + if args: + result[name] = args[0] + return result + + # Topic IDs are transmitted as 2 bytes on the wire (DSP protocol) + # with id 0 reserved for the debug channel, leaving 1..65534 usable. + MAX_TOPICS: ClassVar[int] = 65534 + + def _validate_inbound_payload_sizes(self, stream_types: dict[str, type]) -> None: + """Reject inbound streams that exceed AVR's DSP_MAX_PAYLOAD (256B). + + Only checks AVR targets — non-AVR boards use a 1024B buffer. + Only checks inbound (In[T]) — outbound is the Arduino's own buffer. + """ + if not self.config.board_fqbn.startswith(_AVR_FQBN_PREFIXES): + return + + limit = self.config.max_payload or _AVR_DEFAULT_DSP_MAX_PAYLOAD + offenders: list[tuple[str, str, int]] = [] + for name, msg_type in stream_types.items(): + if name not in self.inputs: + continue # outbound — Arduino owns the encoder, not our problem + size = _encoded_payload_size(msg_type) + if size is None: + continue # custom type via c_type_generators — trust the user + if size > limit: + offenders.append((name, msg_type.__name__, size)) + if offenders: + desc = "; ".join( + f"{name!r}: {type_name}={size}B" for name, type_name, size in offenders + ) + raise ValueError( + f"ArduinoModule inbound stream(s) exceed the AVR " + f"DSP_MAX_PAYLOAD limit of {limit} bytes ({desc}). The " + f"AVR-side parser would silently drop every frame. " + f"Either (a) split the message into smaller types, " + f"(b) target a non-AVR board with more SRAM (e.g. " + f"esp32:esp32:*), or (c) if you know your board has " + f"enough SRAM, override the buffer in your sketch " + f"via `-DDSP_MAX_PAYLOAD=` in compile flags " + f"and remove this check by subclassing " + f"`_validate_inbound_payload_sizes`." + ) + + def _warn_avr_sram_pressure(self, stream_types: dict[str, type]) -> None: + """Log a warning if the number of streams is likely to overflow AVR SRAM. + + Each stream adds a subscription entry (~9 bytes), a topic mapping + entry, and the type's encode/decode code. The LCM pubsub engine + itself uses ~256 bytes (decode buffer + outbox) on AVR. With + Arduino Uno's 2KB total SRAM, more than ~4 streams is risky. + """ + if not self.config.board_fqbn.startswith(_AVR_FQBN_PREFIXES): + return + n = len(stream_types) + threshold = self.config.max_subs or 4 + if n > threshold: + logger.warning( + "AVR SRAM pressure: %d streams declared (max_subs=%d) on a " + "board with ~2KB SRAM. Each stream adds subscriptions, type " + "descriptors, and encode/decode buffers. Compilation may fail " + "with 'data section exceeds available space'. Consider " + "reducing streams, increasing max_subs (if your board has " + "enough SRAM), or using a board with more SRAM.", + n, + threshold, + ) + + def _build_topic_enum(self) -> dict[str, int]: + stream_types = self._get_stream_types() + max_topics = 65534 + if len(stream_types) > max_topics: + raise ValueError( + f"{type(self).__name__} declares {len(stream_types)} streams, " + f"but ArduinoModule supports at most {max_topics} (topic " + f"IDs are uint16_t with 0 reserved for the debug channel). " + f"Split the module or drop streams." + ) + topic_enum: dict[str, int] = {} + topic_id = 1 + for name in sorted(stream_types.keys()): + topic_enum[name] = topic_id + topic_id += 1 + return topic_enum + + def _detect_port(self) -> str: + """Find port whose FQBN matches config.board_fqbn, or raise.""" + result = subprocess.run( + [_arduino_cli_bin(), "board", "list", "--format", "json"], + capture_output=True, + text=True, + timeout=10, + ) + + if result.returncode != 0: + raise RuntimeError(f"arduino-cli board list failed: {result.stderr}") + + try: + boards = json.loads(result.stdout) + except json.JSONDecodeError as exc: + raise RuntimeError( + f"arduino-cli board list returned invalid JSON: {exc}\n" + f"stdout was:\n{result.stdout[:4096]}" + ) from exc + + # Accept both old (bare list) and new ({"detected_ports": [...]}) formats. + if isinstance(boards, list): + entries = boards + elif isinstance(boards, dict): + entries = boards.get("detected_ports", []) + else: + entries = [] + + # Search for a port whose matching_boards contains our FQBN. + for entry in entries: + port_info = entry if isinstance(entry, dict) else {} + address = str(port_info.get("port", {}).get("address", "")) + matching_boards = port_info.get("matching_boards", []) + for board in matching_boards: + if board.get("fqbn", "") == self.config.board_fqbn: + return address + + raise RuntimeError( + f"No Arduino board found matching FQBN '{self.config.board_fqbn}'. " + f"Connected ports: {sorted(glob.glob('/dev/ttyACM*') + glob.glob('/dev/ttyUSB*'))}. " + f"Run 'arduino-cli board list' to see what arduino-cli can see, " + f"or set `port=...` explicitly on your module config." + ) + + def _generate_header(self) -> None: + """Generate dimos_arduino.h from stream declarations + config.""" + stream_types = self._get_stream_types() + topic_enum = self._build_topic_enum() + self._validate_inbound_payload_sizes(stream_types) + self._warn_avr_sram_pressure(stream_types) + + sections: list[str] = [] + + sections.append( + "/* Auto-generated by DimOS ArduinoModule — do not edit */\n" + "#ifndef DIMOS_ARDUINO_H\n" + "#define DIMOS_ARDUINO_H\n" + ) + + # Emit #defines for user-defined config fields + _ARDUINO_SKETCH_FIELDS. + # Framework-internal fields (executable, virtual, etc.) are skipped. + sections.append("/* --- Config --- */") + framework_fields = set(ArduinoModuleConfig.model_fields) + emit_framework = framework_fields & _ARDUINO_SKETCH_FIELDS + ignore_fields = (framework_fields - emit_framework) | set( + self.config.arduino_config_exclude + ) + for field_name in self.config.__class__.model_fields: + if field_name in ignore_fields: + continue + val = getattr(self.config, field_name) + if val is None: + continue + c_name = f"DIMOS_{field_name.upper()}" + if isinstance(val, bool): + sections.append(f"#define {c_name} {'1' if val else '0'}") + elif isinstance(val, int): + sections.append(f"#define {c_name} {val}") + elif isinstance(val, float): + if not math.isfinite(val): + raise ValueError( + f"Cannot embed non-finite float for config field " + f"'{field_name}' (value={val!r}) in dimos_arduino.h" + ) + sections.append(f"#define {c_name} {val}f") + elif isinstance(val, str): + # json.dumps produces a valid C string literal (escapes ", + # \, and non-printables; wraps in double quotes). + sections.append(f"#define {c_name} {json.dumps(val)}") + else: + raise TypeError( + f"Cannot embed config field '{field_name}' of type " + f"{type(val).__name__} in dimos_arduino.h. Add it to " + f"arduino_config_exclude or convert it to str/int/float/bool." + ) + sections.append("") + + # User-defined #defines from arduino_defines config. + if self.config.arduino_defines: + sections.append("/* --- User-defined constants --- */") + for def_name, def_val in self.config.arduino_defines.items(): + if not def_name.isidentifier(): + raise ValueError( + f"arduino_defines key {def_name!r} is not a valid C identifier." + ) + sections.append(f"#define {def_name} {_c_literal(def_name, def_val)}") + sections.append("") + + # Topic enum (still used by bridge CLI args and backward compat) + sections.append("/* --- Topic enum (shared with C++ bridge) --- */") + sections.append("enum dimos_topic {") + sections.append(" DIMOS_TOPIC_DEBUG = 0,") + for name, tid in topic_enum.items(): + direction = "Out" if name in self.outputs else "In" + msg_type = stream_types[name] + sections.append( + f" DIMOS_TOPIC__{name.upper()} = {tid}, /* {direction}[{msg_type.__name__}] */" + ) + sections.append("};") + sections.append("") + + # LCM pubsub layer (must come before message headers so type + # descriptors defined in the message headers can reference it) + sections.append("/* --- LCM pubsub layer --- */") + sections.append('#include "dimos_lcm_pubsub.h"') + sections.append("") + + # Message type includes + sections.append("/* --- Message type headers --- */") + included_types: set[str] = set() + for _name, msg_type in stream_types.items(): + msg_name = getattr(msg_type, "msg_name", None) + if msg_name is None: + msg_name = f"{msg_type.__module__}.{msg_type.__qualname__}" + + if msg_name in included_types: + continue + included_types.add(msg_name) + + header = _KNOWN_TYPE_HEADERS.get(msg_name) + if header: + sections.append(f'#include "{header}"') + elif msg_type in self.c_type_generators: + gen = self.c_type_generators[msg_type] + sections.append(gen.struct_create(msg_type.__name__)) + else: + raise TypeError( + f"No Arduino C header for message type '{msg_name}'. " + f"Either add it to arduino_msgs/ or set c_type_generators " + f"on your ArduinoModule subclass." + ) + sections.append("") + + # Topic mapping table (channel name ↔ topic ID) + # The bridge uses "topic_name#msg_type" as the LCM channel. + # For the Arduino side, we use just the topic_name part as the + # channel name for the subscribe API. + try: + topics = self._resolve_topics() + except Exception: + # During unit tests or when transports aren't wired yet, + # fall back to stream names as channel names. + topics = {name: name for name in topic_enum} + sections.append("/* --- Topic ↔ channel mapping --- */") + sections.append("#ifndef DIMOS_TOPIC_MAPPING_DEFINED") + sections.append("#define DIMOS_TOPIC_MAPPING_DEFINED") + sections.append("typedef struct {") + sections.append(" uint16_t topic_id;") + sections.append(" const char *channel;") + sections.append("} dimos_topic_mapping_t;") + sections.append("#endif") + sections.append(f"#define DIMOS_NUM_TOPICS {len(topic_enum)}") + sections.append("static const dimos_topic_mapping_t _dimos_topic_map[] = {") + for name, tid in topic_enum.items(): + if name in topics: + # Extract channel name (before #) for the Arduino-side API + lcm_channel = topics[name] + channel_name = lcm_channel.split("#")[0] + else: + channel_name = name + sections.append(f' {{ {tid}, "{channel_name}" }},') + sections.append("};") + sections.append("") + + # Channel name constants for user convenience + sections.append("/* --- Channel name constants --- */") + for name in topic_enum: + if name in topics: + channel_name = topics[name].split("#")[0] + else: + channel_name = name + sections.append(f'#define DIMOS_CHANNEL__{name.upper()} "{channel_name}"') + sections.append("") + + # DSP protocol + LCM serial adapter + sections.append("/* --- Serial transport + LCM integration --- */") + sections.append('#include "dimos_lcm_serial.h"') + sections.append("") + + # Close header guard + sections.append("#endif /* DIMOS_ARDUINO_H */") + + # Header must live in the sketch dir — arduino-cli's preprocessor + # ignores -I flags during its ctags pass. Wipe build dir to avoid + # stale includes.cache causing "not declared" errors. + sketch_dir = self._resolve_sketch_dir() + header_path = sketch_dir / "dimos_arduino.h" + header_path.write_text("\n".join(sections)) + build_dir = self._build_dir() + if build_dir.exists(): + shutil.rmtree(build_dir) + build_dir.mkdir(parents=True, exist_ok=True) + logger.info("Generated Arduino header", path=str(header_path)) + + def _resolve_sketch_dir(self) -> Path: + subclass_file = Path(inspect.getfile(type(self))) + base_dir = subclass_file.parent + if self.config.cwd: + base_dir = base_dir / self.config.cwd + sketch_path = base_dir / self.config.sketch_path + return sketch_path.parent + + def _build_dir(self) -> Path: + sketch_dir = self._resolve_sketch_dir() + return sketch_dir / "build" + + def _ensure_core_installed(self) -> None: + """Install the arduino-cli core for board_fqbn if not already present.""" + # board_fqbn is e.g. "arduino:avr:uno" — the core id is the first + # two colon-separated segments: "arduino:avr". + parts = self.config.board_fqbn.split(":") + if len(parts) < 2: + raise RuntimeError( + f"Invalid board_fqbn {self.config.board_fqbn!r}; " + f"expected 'vendor:architecture:board' (e.g. 'arduino:avr:uno')" + ) + core_id = f"{parts[0]}:{parts[1]}" + + arduino_cli = _arduino_cli_bin() + + # Cheap check first: `arduino-cli core list` prints installed cores. + # If our core is already there, skip the install step entirely. + list_result = subprocess.run( + [arduino_cli, "core", "list"], + capture_output=True, + text=True, + timeout=30, + ) + if list_result.returncode == 0 and any( + line.split()[0] == core_id for line in list_result.stdout.splitlines() if line.strip() + ): + return + + logger.info("Installing arduino core", core=core_id) + install_result = subprocess.run( + [arduino_cli, "core", "install", core_id], + capture_output=True, + text=True, + timeout=600, + ) + if install_result.returncode != 0: + raise RuntimeError( + f"arduino-cli core install {core_id} failed:\n" + f"{install_result.stderr}\n{install_result.stdout}" + ) + logger.info("Arduino core installed", core=core_id) + + def _compile_sketch(self) -> None: + sketch_dir = self._resolve_sketch_dir() + build_dir = self._build_dir() + build_dir.mkdir(parents=True, exist_ok=True) + + common = str(_COMMON_DIR) + msgs = str(_arduino_msgs_dir()) + extra_flags = f"-I{common} -I{msgs} -DF_CPU=16000000UL" + if self.config.virtual: + # QEMU AVR doesn't fire USART interrupts — use direct register I/O. + extra_flags += " -DDSP_DIRECT_USART" + + # Compile-time tuning knobs from config fields. + for field, macro in _TUNING_FIELD_TO_DEFINE.items(): + val = getattr(self.config, field) + if val is not None: + extra_flags += f" -D{macro}={val}" + + # User-defined #defines also passed as -D flags so they're + # visible in all translation units, not just via the header. + for def_name, def_val in self.config.arduino_defines.items(): + extra_flags += f" -D{def_name}={_c_literal(def_name, def_val)}" + + cmd = [ + _arduino_cli_bin(), + "compile", + "--fqbn", + self.config.board_fqbn, + "--build-property", + f"compiler.cpp.extra_flags={extra_flags}", + "--build-property", + f"compiler.c.extra_flags={extra_flags}", + "--build-path", + str(build_dir), + str(sketch_dir), + ] + + logger.info("Compiling Arduino sketch", cmd=" ".join(cmd)) + result = subprocess.run( + cmd, + capture_output=True, + text=True, + timeout=120, + ) + if result.returncode != 0: + raise RuntimeError( + f"Arduino sketch compilation failed:\n{result.stderr}\n{result.stdout}" + ) + logger.info("Arduino sketch compiled successfully", build_dir=str(build_dir)) + + def _start_qemu(self) -> str: + """Launch qemu-system-avr and return the PTY path. Cleans up on failure.""" + build_dir = self._build_dir() + sketch_name = Path(self.config.sketch_path).stem + elf_path = build_dir / f"{sketch_name}.ino.elf" + if not elf_path.exists(): + raise RuntimeError(f"Compiled sketch not found: {elf_path}") + + machine_map = { + "arduino:avr:uno": "uno", + "arduino:avr:mega": "mega", + "arduino:avr:mega2560": "mega2560", + } + machine = machine_map.get(self.config.board_fqbn, "uno") + + tmp_log = tempfile.NamedTemporaryFile( + prefix="dimos_qemu_", suffix=".log", delete=False, mode="w" + ) + self._qemu_log_path = tmp_log.name + tmp_log.close() + + cmd = [ + _qemu_system_avr_bin(), + "-machine", + machine, + "-bios", + str(elf_path), + "-serial", + "pty", + "-monitor", + "null", + "-nographic", + ] + + logger.info("Starting QEMU virtual Arduino", cmd=" ".join(cmd)) + try: + self._qemu_log_fd = open(self._qemu_log_path, "wb") + self._qemu_proc = subprocess.Popen( + cmd, + stdout=self._qemu_log_fd, + stderr=subprocess.STDOUT, + ) + + timeout = self.config.qemu_startup_timeout_s + deadline = time.monotonic() + timeout + pty: str | None = None + while time.monotonic() < deadline: + if self._qemu_proc.poll() is not None: + with open(self._qemu_log_path) as f: + raise RuntimeError( + f"QEMU exited unexpectedly before announcing a PTY:\n{f.read()}" + ) + with open(self._qemu_log_path) as f: + content = f.read() + # Match "char device redirected to /dev/pts/N" or /dev/ttysNNN + m = re.search( + r"char device redirected to (/dev/(?:pts/\d+|ttys\d+))", + content, + ) + if m: + pty = m.group(1) + break + time.sleep(0.1) + + if pty is None: + raise RuntimeError( + f"QEMU started but did not announce a PTY within {timeout:.1f}s. " + f"Increase qemu_startup_timeout_s in the module config if " + f"this is a loaded CI machine. Log tail:\n" + f"{_tail_text(self._qemu_log_path, 2048)}" + ) + + self._virtual_pty = pty + logger.info("QEMU virtual Arduino running", pty=pty, pid=self._qemu_proc.pid) + return pty + except BaseException: + self._cleanup_qemu() + raise + + def _flash(self) -> None: + sketch_dir = self._resolve_sketch_dir() + build_dir = self._build_dir() + port = self.config.port + if not port: + raise RuntimeError("No port configured for flashing") + + cmd = [ + _arduino_cli_bin(), + "upload", + "-p", + port, + "--fqbn", + self.config.board_fqbn, + "--input-dir", + str(build_dir), + str(sketch_dir), + ] + + logger.info("Flashing Arduino", cmd=" ".join(cmd), port=port) + result = subprocess.run( + cmd, + capture_output=True, + text=True, + timeout=self.config.flash_timeout, + ) + if result.returncode != 0: + combined = f"{result.stderr}\n{result.stdout}" + hint = "" + if "Permission denied" in combined and port: + import sys + + if sys.platform == "linux": + hint = ( + f"\n\nHint: the current user cannot access {port}. " + f"Quick fix:\n" + f" sudo chmod 666 {port}\n" + f"Permanent fix (requires re-login):\n" + f" sudo usermod -a -G dialout $USER" + ) + else: + hint = ( + f"\n\nHint: the current user cannot access {port}. " + f"Check that your user has read/write access to the " + f"serial device." + ) + raise RuntimeError(f"Arduino flash failed:\n{combined}{hint}") + logger.info("Arduino flashed successfully", port=port) + + +def _encoded_payload_size(msg_type: type) -> int | None: + """Return the full DSP wire payload size (fingerprint + data), or None if + the type can't be introspected. + """ + try: + instance = msg_type() + except Exception: + return None + encode = getattr(instance, "lcm_encode", None) + if encode is None: + return None + try: + encoded = encode() + except Exception: + return None + return len(encoded) + + +def _tail_text(path: str, max_bytes: int) -> str: + try: + with open(path, "rb") as f: + try: + f.seek(-max_bytes, os.SEEK_END) + except OSError as exc: + if exc.errno != errno.EINVAL: + raise + f.seek(0) + return f.read().decode(errors="replace") + except OSError: + return "" + + +__all__ = [ + "ArduinoModule", + "ArduinoModuleConfig", + "CTypeGenerator", +] diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py index 4e2ec0c699..1d192ca9e4 100644 --- a/dimos/core/native_module.py +++ b/dimos/core/native_module.py @@ -160,11 +160,8 @@ def start(self) -> None: self._maybe_build() - topics = self._collect_topics() - cmd = [self.config.executable] - for name, topic_str in topics.items(): - cmd.extend([f"--{name}", topic_str]) + cmd.extend(self._build_topic_args()) cmd.extend(self.config.to_cli_args()) cmd.extend(self.config.extra_args) @@ -334,6 +331,18 @@ def _maybe_build(self) -> None: f"Check that build_command produces the executable at the expected location." ) + def _build_topic_args(self) -> list[str]: + """Build CLI args that map stream names to LCM topics. + + Subclasses that construct their own topic arguments (e.g. + ``ArduinoModule`` with numeric topic IDs) can override this + to return ``[]``. + """ + args: list[str] = [] + for name, topic_str in self._collect_topics().items(): + args.extend([f"--{name}", topic_str]) + return args + def _collect_topics(self) -> dict[str, str]: """Extract LCM topic strings from blueprint-assigned stream transports.""" topics: dict[str, str] = {} diff --git a/dimos/core/tests/test_arduino_module.py b/dimos/core/tests/test_arduino_module.py new file mode 100644 index 0000000000..a9ed0a6165 --- /dev/null +++ b/dimos/core/tests/test_arduino_module.py @@ -0,0 +1,610 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Unit tests for dimos.core.arduino_module. + +Covers the pure/host-side logic — header generation, topic enum +assignment, the three-way registry sync, port detection with mocked +arduino-cli, and QEMU cleanup paths. These tests do not require a real +Arduino or QEMU. +""" + +from __future__ import annotations + +import json +from pathlib import Path +import re +import subprocess +from typing import Any +from unittest import mock + +import pytest + +# Captured at import time — the autouse fixture below patches the module +# attribute for every test, so tests that want to exercise the *real* +# resolver (e.g. its FileNotFoundError handling) reach it through this +# unpatched reference. +from dimos.core.arduino_module import ( + _ARDUINO_HW_DIR, + _KNOWN_TYPE_HEADERS, + ArduinoModule, + ArduinoModuleConfig, + _arduino_tools_bin_dir as _real_arduino_tools_bin_dir, +) +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.Twist import Twist + +# Fixtures / helpers + + +@pytest.fixture(autouse=True) +def _fake_arduino_tools_bin_dir( + tmp_path_factory: pytest.TempPathFactory, +) -> Any: + """Short-circuit ``_arduino_tools_bin_dir`` for every test in this file. + + Without this, every helper that shells out to ``arduino-cli`` / + ``qemu-system-avr`` (``_detect_port``, ``_ensure_core_installed``, + ``_compile_sketch``, ``_start_qemu``, ``_flash``) would first invoke + the resolver, which runs ``nix build .#dimos_arduino_tools``. The + unit tests are *unit* tests — they have no business touching the + Nix store — so we replace the resolver with a fixed fake ``bin/`` + directory. Tests that mock ``subprocess.run`` will then see calls + routed through absolute paths under this fake dir, which is fine + because the mocks don't care what the argv's first element is. + """ + fake_bin = tmp_path_factory.mktemp("fake_arduino_tools") / "bin" + fake_bin.mkdir() + with mock.patch( + "dimos.core.arduino_module._arduino_tools_bin_dir", + return_value=fake_bin, + ): + yield fake_bin + + +class _ExampleConfig(ArduinoModuleConfig): + """Minimal config for tests — no auto-detect, no flash, no virtual.""" + + sketch_path: str = "sketch/sketch.ino" + board_fqbn: str = "arduino:avr:uno" + baudrate: int = 115200 + auto_detect: bool = False + auto_flash: bool = False + virtual: bool = False + port: str | None = "/dev/ttyACM0" + # Custom config field that should end up in the generated header. + greeting: str = 'he said "hi"' + tick_rate_hz: int = 50 + + +class _ExampleModule(ArduinoModule): + config: _ExampleConfig + twist_in: In[Twist] + twist_echo_out: Out[Twist] + + +def _make_module() -> _ExampleModule: + """Build an _ExampleModule without triggering its __init__ machinery. + + ArduinoModule subclasses pydantic Module whose real `__init__` spins + up RPC / worker plumbing we don't need for unit tests. We use + `__new__` to bypass it, then install bare `In` / `Out` stubs (via + `__new__` again) into `self.__dict__` — the `Module.inputs` / + `Module.outputs` properties are read-only `@property`s that scan + `__dict__` for any attribute that is an `In`/`Out` instance, so + this is the minimum required to make those properties return the + expected port names. + """ + inst = _ExampleModule.__new__(_ExampleModule) + inst.config = _ExampleConfig() + inst.__dict__["twist_in"] = In.__new__(In) + inst.__dict__["twist_echo_out"] = Out.__new__(Out) + return inst + + +# _build_topic_enum + + +def test_build_topic_enum_assigns_1_based_alphabetical() -> None: + mod = _make_module() + enum = mod._build_topic_enum() + # Alphabetical order, topic 0 reserved for debug. + assert enum == {"twist_echo_out": 1, "twist_in": 2} + + +# _generate_header — config embedding & escaping + + +def _patch_sketch_and_build_dirs(mod: _ExampleModule, tmp_path: Path) -> Any: + """Redirect ``_resolve_sketch_dir`` and ``_build_dir`` into ``tmp_path``. + + ``_generate_header`` writes the header into the sketch dir (so + arduino-cli's sketch preprocessor can find it) and also wipes + + recreates the build dir. Tests need both paths diverted away from + the real repo. + """ + sketch_dir = tmp_path + build_dir = tmp_path / "build" + sketch_patch = mock.patch.object(mod, "_resolve_sketch_dir", return_value=sketch_dir) + build_patch = mock.patch.object(mod, "_build_dir", return_value=build_dir) + return sketch_patch, build_patch + + +def test_generate_header_escapes_quoted_strings(tmp_path: Path) -> None: + """A config string containing " or \\ must not produce invalid C.""" + mod = _make_module() + sketch_patch, build_patch = _patch_sketch_and_build_dirs(mod, tmp_path) + with sketch_patch, build_patch: + mod._generate_header() + text = (tmp_path / "dimos_arduino.h").read_text() + + # The greeting contains an embedded double-quote. If the header + # generator naively interpolated it, the resulting C file would have + # an unterminated string literal. `json.dumps` escapes it to \". + assert r'#define DIMOS_GREETING "he said \"hi\""' in text + assert "#define DIMOS_BAUDRATE 115200" in text + assert "#define DIMOS_TICK_RATE_HZ 50" in text + + +def test_generate_header_includes_topic_enum_and_message_header( + tmp_path: Path, +) -> None: + mod = _make_module() + sketch_patch, build_patch = _patch_sketch_and_build_dirs(mod, tmp_path) + with sketch_patch, build_patch: + mod._generate_header() + text = (tmp_path / "dimos_arduino.h").read_text() + + assert "enum dimos_topic {" in text + assert "DIMOS_TOPIC_DEBUG = 0" in text + assert "DIMOS_TOPIC__TWIST_ECHO_OUT = 1" in text + assert "DIMOS_TOPIC__TWIST_IN = 2" in text + # Twist → geometry_msgs/Twist.h + assert '#include "geometry_msgs/Twist.h"' in text + # LCM pubsub layer and serial adapter always pulled in. + assert '#include "dimos_lcm_pubsub.h"' in text + assert '#include "dimos_lcm_serial.h"' in text + + +def test_generate_header_rejects_non_finite_float(tmp_path: Path) -> None: + class _NaNConfig(_ExampleConfig): + nan_val: float = float("nan") + + mod = _make_module() + mod.config = _NaNConfig() + sketch_patch, build_patch = _patch_sketch_and_build_dirs(mod, tmp_path) + with sketch_patch, build_patch: + with pytest.raises(ValueError, match="non-finite"): + mod._generate_header() + + +def test_generate_header_rejects_unembeddable_type(tmp_path: Path) -> None: + class _ListConfig(_ExampleConfig): + the_list: list[int] = [1, 2, 3] + + mod = _make_module() + mod.config = _ListConfig() + sketch_patch, build_patch = _patch_sketch_and_build_dirs(mod, tmp_path) + with sketch_patch, build_patch: + with pytest.raises(TypeError, match="Cannot embed config field 'the_list'"): + mod._generate_header() + + +def test_generate_header_includes_arduino_defines(tmp_path: Path) -> None: + """arduino_defines config values appear as #define in the header.""" + + class _DefinesConfig(_ExampleConfig): + arduino_defines: dict[str, int | float | str | bool] = { + "MOTOR_PIN": 13, + "SENSOR_THRESHOLD": 0.5, + "ROBOT_NAME": "uno_bot", + "ENABLE_DEBUG": True, + } + + mod = _make_module() + mod.config = _DefinesConfig() + sketch_patch, build_patch = _patch_sketch_and_build_dirs(mod, tmp_path) + with sketch_patch, build_patch: + mod._generate_header() + text = (tmp_path / "dimos_arduino.h").read_text() + + assert "#define MOTOR_PIN 13" in text + assert "#define SENSOR_THRESHOLD 0.5f" in text + assert '#define ROBOT_NAME "uno_bot"' in text + assert "#define ENABLE_DEBUG 1" in text + assert "User-defined constants" in text + + +def test_generate_header_rejects_invalid_define_name(tmp_path: Path) -> None: + """arduino_defines keys must be valid C identifiers.""" + + class _BadDefConfig(_ExampleConfig): + arduino_defines: dict[str, int | float | str | bool] = {"123bad": 1} + + mod = _make_module() + mod.config = _BadDefConfig() + sketch_patch, build_patch = _patch_sketch_and_build_dirs(mod, tmp_path) + with sketch_patch, build_patch: + with pytest.raises(ValueError, match="not a valid C identifier"): + mod._generate_header() + + +def test_tuning_fields_default_to_none() -> None: + """Tuning knobs default to None (use header defaults).""" + cfg = _ExampleConfig() + assert cfg.max_subs is None + assert cfg.max_pending is None + assert cfg.max_msg_size is None + assert cfg.max_payload is None + + +# _detect_port — mocked arduino-cli + + +def _run_result(stdout: str, returncode: int = 0) -> subprocess.CompletedProcess[str]: + return subprocess.CompletedProcess( + args=["arduino-cli"], returncode=returncode, stdout=stdout, stderr="" + ) + + +def test_detect_port_matches_fqbn() -> None: + mod = _make_module() + payload: dict[str, Any] = { + "detected_ports": [ + { + "port": {"address": "/dev/ttyACM1"}, + "matching_boards": [{"fqbn": "arduino:avr:uno"}], + }, + { + "port": {"address": "/dev/ttyUSB0"}, + "matching_boards": [{"fqbn": "arduino:avr:mega"}], + }, + ] + } + with mock.patch( + "dimos.core.arduino_module.subprocess.run", + return_value=_run_result(json.dumps(payload)), + ): + assert mod._detect_port() == "/dev/ttyACM1" + + +def test_detect_port_raises_on_no_match() -> None: + mod = _make_module() + payload: dict[str, list[Any]] = {"detected_ports": []} + with mock.patch( + "dimos.core.arduino_module.subprocess.run", + return_value=_run_result(json.dumps(payload)), + ): + with pytest.raises(RuntimeError, match="No Arduino board found matching FQBN"): + mod._detect_port() + + +def test_detect_port_wraps_invalid_json() -> None: + mod = _make_module() + with mock.patch( + "dimos.core.arduino_module.subprocess.run", + return_value=_run_result("not-json-at-all"), + ): + with pytest.raises(RuntimeError, match="invalid JSON"): + mod._detect_port() + + +def test_arduino_tools_bin_dir_raises_on_missing_nix() -> None: + """The resolver surfaces a clean RuntimeError (not a bare + FileNotFoundError) when ``nix`` itself is missing from PATH. That is + the only failure mode of the toolchain resolver now that + ``arduino-cli`` / ``avrdude`` / ``qemu-system-avr`` are packaged as + a flake output and come from a ``nix build`` rather than from + ``$PATH``. + """ + # Clear the ``lru_cache`` so we actually re-enter the function body. + _real_arduino_tools_bin_dir.cache_clear() + with mock.patch( + "dimos.core.arduino_module.subprocess.run", + side_effect=FileNotFoundError, + ): + with pytest.raises(RuntimeError, match="nix"): + _real_arduino_tools_bin_dir() + # Leave the cache cleared so the next test (which may have its own + # fake_arduino_tools_bin_dir fixture) starts from a known state. + _real_arduino_tools_bin_dir.cache_clear() + + +def test_detect_port_wraps_non_zero_exit() -> None: + mod = _make_module() + with mock.patch( + "dimos.core.arduino_module.subprocess.run", + return_value=subprocess.CompletedProcess( + args=[], returncode=1, stdout="", stderr="permission denied" + ), + ): + with pytest.raises(RuntimeError, match="permission denied"): + mod._detect_port() + + +# _cleanup_qemu — idempotency + leak sealing + + +def test_cleanup_qemu_is_idempotent_on_unstarted_module() -> None: + mod = _make_module() + # Never started — all slots None. Must not raise. + mod._cleanup_qemu() + mod._cleanup_qemu() + assert mod._qemu_proc is None + assert mod._qemu_log_fd is None + assert mod._qemu_log_path is None + assert mod._virtual_pty is None + + +def test_cleanup_qemu_closes_log_fd_and_removes_log_file(tmp_path: Path) -> None: + mod = _make_module() + log_path = tmp_path / "qemu.log" + log_path.write_text("hi") + mod._qemu_log_path = str(log_path) + fd = open(log_path, "wb") + mod._qemu_log_fd = fd + mod._qemu_proc = None # no process — just the fd + file + + mod._cleanup_qemu() + + assert fd.closed + assert not log_path.exists() + assert mod._qemu_log_fd is None + assert mod._qemu_log_path is None + + +def test_cleanup_qemu_terminates_live_process() -> None: + mod = _make_module() + proc = mock.Mock(spec=subprocess.Popen) + # poll() returns None while alive, then 0 after wait. + proc.poll.side_effect = [None] + proc.wait.return_value = 0 + mod._qemu_proc = proc + + mod._cleanup_qemu() + + proc.terminate.assert_called_once() + proc.wait.assert_called_once_with(timeout=5) + assert mod._qemu_proc is None + + +def test_cleanup_qemu_kills_on_terminate_timeout() -> None: + mod = _make_module() + proc = mock.Mock(spec=subprocess.Popen) + proc.poll.side_effect = [None] + proc.wait.side_effect = [subprocess.TimeoutExpired(cmd="qemu", timeout=5), 0] + mod._qemu_proc = proc + + mod._cleanup_qemu() + + proc.terminate.assert_called_once() + proc.kill.assert_called_once() + assert proc.wait.call_count == 2 + assert mod._qemu_proc is None + + +# Registry sync — _KNOWN_TYPE_HEADERS vs arduino_msgs/ vs main.cpp + + +def _arduino_common_dir() -> Path: + """Resolve Arduino message headers from dimos-lcm via nix. + + These headers now live in dimos-lcm (not vendored in dimos4), so we + resolve them through `nix build` the same way ArduinoModule does at + runtime. Skips if nix is not available. + """ + import shutil + import subprocess + + if shutil.which("nix") is None: + pytest.skip("nix not available — cannot resolve Arduino message headers") + + result = subprocess.run( + ["nix", "build", ".#dimos_arduino_tools", "--print-out-paths", "--no-link"], + cwd=str(_ARDUINO_HW_DIR), + capture_output=True, + text=True, + timeout=120, + ) + if result.returncode != 0: + pytest.skip(f"nix build failed: {result.stderr[:200]}") + + out_paths = [line for line in result.stdout.splitlines() if line.strip()] + if not out_paths: + pytest.skip("nix build returned no paths") + + msgs_dir = Path(out_paths[-1]) / "share" / "arduino_msgs" + if not msgs_dir.is_dir(): + pytest.skip(f"Arduino message headers not found at {msgs_dir}") + + return msgs_dir + + +def _main_cpp_path() -> Path: + return _ARDUINO_HW_DIR / "cpp" / "main.cpp" + + +def test_registry_headers_exist_on_disk() -> None: + common = _arduino_common_dir() + missing = [ + (msg_name, header) + for msg_name, header in _KNOWN_TYPE_HEADERS.items() + if not (common / header).is_file() + ] + assert not missing, ( + f"Every entry in _KNOWN_TYPE_HEADERS must point to an existing " + f"arduino_msgs header, but these are missing: {missing}" + ) + + +def test_registry_matches_main_cpp_hash_registry() -> None: + """Every type in `_KNOWN_TYPE_HEADERS` must also appear in the C++ + bridge's `init_hash_registry()` and vice versa. Either half is a + silent wire-format bug waiting to happen.""" + main_cpp = _main_cpp_path().read_text() + + # The C++ side stores keys as "std_msgs.Time" etc. + cpp_entries = set(re.findall(r'hash_registry\["([^"]+)"\]', main_cpp)) + py_entries = set(_KNOWN_TYPE_HEADERS.keys()) + + only_in_py = py_entries - cpp_entries + only_in_cpp = cpp_entries - py_entries + + assert not only_in_py, ( + f"These message types are in _KNOWN_TYPE_HEADERS but NOT in " + f"main.cpp::init_hash_registry: {sorted(only_in_py)}. Add them to " + f"dimos/hardware/arduino/cpp/main.cpp or remove from the Python registry." + ) + assert not only_in_cpp, ( + f"These message types are in main.cpp::init_hash_registry but NOT " + f"in _KNOWN_TYPE_HEADERS: {sorted(only_in_cpp)}. Add them to " + f"dimos/core/arduino_module.py::_KNOWN_TYPE_HEADERS or remove from main.cpp." + ) + + +# _resolve_topics — validates LCM-typed channel strings + +# NOTE: these tests monkey-patch ``inputs``/``outputs`` on the ``_ExampleModule`` +# *instance* rather than relying on stream auto-discovery, because +# ``_make_module`` uses bypass constructors that don't set up real +# transports. The parent ``_collect_topics`` walks transports via +# ``getattr(self, name)._transport.topic``, so we stub the whole chain. + + +class _FakeTransport: + def __init__(self, topic: str) -> None: + self.topic = topic + + +class _FakeStream: + def __init__(self, topic: str) -> None: + self._transport = _FakeTransport(topic) + + +def _make_module_with_topics(topics: dict[str, str]) -> _ExampleModule: + """Build a module whose `super()._collect_topics()` returns `topics`.""" + mod = _make_module() + # Install fake streams so NativeModule._collect_topics walks them. + for name, topic in topics.items(): + mod.__dict__[name] = _FakeStream(topic) + # Force `inputs` / `outputs` to report the fake names (otherwise + # the module-level reflection sees only the two In/Out stubs from + # _make_module). Monkey-patch the properties on this instance. + inputs_list = list(topics) + mod.__class__.inputs = property(lambda self: inputs_list) # type: ignore[method-assign,assignment] + mod.__class__.outputs = property(lambda self: []) # type: ignore[method-assign,assignment] + return mod + + +def test_resolve_topics_accepts_typed_lcm_channels() -> None: + mod = _make_module_with_topics({"twist_in": "twist_command#geometry_msgs.Twist"}) + try: + resolved = mod._resolve_topics() + assert resolved == {"twist_in": "twist_command#geometry_msgs.Twist"} + finally: + # Unwind the monkey-patched properties so later tests see + # the real Module.inputs/outputs descriptors. + del mod.__class__.inputs + del mod.__class__.outputs + + +def test_resolve_topics_rejects_bare_channel_names() -> None: + mod = _make_module_with_topics({"twist_in": "twist_command"}) + try: + with pytest.raises(RuntimeError, match="'#msg_type' suffix"): + mod._resolve_topics() + finally: + del mod.__class__.inputs + del mod.__class__.outputs + + +# _validate_inbound_payload_sizes — AVR SRAM guard + + +# Module-scope classes for the payload-size tests. They must live at +# module level (not inside the test functions) because +# ``_get_stream_types`` uses ``get_type_hints`` which re-evaluates the +# string annotations via ``eval(..., globals=module.__dict__, ...)`` +# and can't see locals of a test function. +from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance + + +class _BigInboundModule(ArduinoModule): + config: _ExampleConfig + pose_in: In[PoseWithCovariance] + + +class _BigOutboundModule(ArduinoModule): + config: _ExampleConfig + pose_out: Out[PoseWithCovariance] + + +class _Esp32Config(_ExampleConfig): + board_fqbn: str = "esp32:esp32:esp32" + + +class _Esp32Module(ArduinoModule): + config: _Esp32Config + pose_in: In[PoseWithCovariance] + + +def test_validate_inbound_payload_sizes_passes_for_small_inbound() -> None: + """Twist is 56 bytes on the wire (8B fingerprint + 48B data) — well under the 256 AVR limit.""" + mod = _make_module() + # twist_in is declared as In[Twist] — 48 bytes, passes. + mod._validate_inbound_payload_sizes(mod._get_stream_types()) + + +def test_validate_inbound_payload_sizes_rejects_oversized_inbound() -> None: + """PoseWithCovariance is 352 bytes on the wire (8B fingerprint + 344B data) — exceeds the 256 AVR default.""" + mod = _BigInboundModule.__new__(_BigInboundModule) + mod.config = _ExampleConfig() + mod.__dict__["pose_in"] = In.__new__(In) + + with pytest.raises(ValueError, match="DSP_MAX_PAYLOAD"): + mod._validate_inbound_payload_sizes(mod._get_stream_types()) + + +def test_validate_inbound_payload_sizes_ignores_outbound() -> None: + """Even an oversized *outbound* stream is fine — the Arduino owns the encoder.""" + mod = _BigOutboundModule.__new__(_BigOutboundModule) + mod.config = _ExampleConfig() + mod.__dict__["pose_out"] = Out.__new__(Out) + + mod._validate_inbound_payload_sizes(mod._get_stream_types()) # must not raise + + +def test_validate_inbound_payload_sizes_skips_non_avr_board() -> None: + """A non-AVR FQBN skips the check entirely — non-AVR gets 1024.""" + mod = _Esp32Module.__new__(_Esp32Module) + mod.config = _Esp32Config() + mod.__dict__["pose_in"] = In.__new__(In) + + mod._validate_inbound_payload_sizes(mod._get_stream_types()) # must not raise + + +def test_registry_headers_cover_all_arduino_msgs_files() -> None: + """Every header referenced by _KNOWN_TYPE_HEADERS must exist on disk. + Extra generated headers (from dimos-lcm codegen) and infrastructure + headers (lcm_coretypes_arduino.h, dimos_lcm_pubsub.h) are allowed + without registry entries since they are auto-generated dependencies, + not user-facing message types.""" + common = _arduino_common_dir() + on_disk = {str(p.relative_to(common)) for p in common.rglob("*.h")} + referenced = set(_KNOWN_TYPE_HEADERS.values()) + missing = referenced - on_disk + assert not missing, ( + f"These headers are referenced by _KNOWN_TYPE_HEADERS but missing " + f"on disk: {sorted(missing)}" + ) diff --git a/dimos/hardware/arduino/.gitignore b/dimos/hardware/arduino/.gitignore new file mode 100644 index 0000000000..c8c39aeb54 --- /dev/null +++ b/dimos/hardware/arduino/.gitignore @@ -0,0 +1,16 @@ +# Nix build output symlinks +result +result-* + +# Build lock used by ArduinoModule._build_bridge() +.bridge_build.lock + +# Arduino sketch build artifacts +examples/*/sketch/build/ + +# ArduinoModule auto-generates this header next to each sketch.ino at +# build time (see arduino_module.py::_generate_header). It lives in the +# sketch dir because arduino-cli's sketch preprocessor doesn't honor -I +# flags from compiler.cpp.extra_flags, so headers outside the sketch +# dir aren't visible during the preprocessing pass. +examples/*/sketch/dimos_arduino.h diff --git a/dimos/hardware/arduino/common/dimos_lcm_serial.h b/dimos/hardware/arduino/common/dimos_lcm_serial.h new file mode 100644 index 0000000000..488da8ebe8 --- /dev/null +++ b/dimos/hardware/arduino/common/dimos_lcm_serial.h @@ -0,0 +1,204 @@ +/* + * dimos_lcm_serial.h — Serial transport adapter for the DimOS LCM pubsub layer. + * + * Bridges the transport-agnostic LCM pubsub engine (dimos_lcm_pubsub.h) with + * the DSP serial framing protocol (dsp_protocol.h), providing a user-friendly + * API that matches standard LCM patterns: + * + * dimos_init(baud) — initialise serial + pubsub + * dimos_subscribe(ch, type, handler, ud) — register a typed callback + * dimos_publish(ch, type, data, len) — send a message + * dimos_handle(timeout_ms) — poll serial, dispatch, send + * + * The generated dimos_arduino.h must define the following BEFORE including + * this header: + * + * DIMOS_NUM_TOPICS — number of topic mappings + * _dimos_topic_map[] — array of { topic_id, channel_name } structs + * + * Copyright 2025-2026 Dimensional Inc. Apache-2.0. + */ + +#ifndef DIMOS_LCM_SERIAL_H +#define DIMOS_LCM_SERIAL_H + +#include "dimos_lcm_pubsub.h" +#include "dsp_protocol.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* ====================================================================== + * Topic mapping (populated by generated dimos_arduino.h) + * ====================================================================== */ + +#ifndef DIMOS_TOPIC_MAPPING_DEFINED +#define DIMOS_TOPIC_MAPPING_DEFINED +typedef struct { + uint16_t topic_id; + const char *channel; +} dimos_topic_mapping_t; +#endif + +/* These must be defined by the generated header before including us: + * static const dimos_topic_mapping_t _dimos_topic_map[]; + * #define DIMOS_NUM_TOPICS + */ + +/* ====================================================================== + * Global state + * + * Uses the same inline-function-with-static-local trick as dsp_protocol.h + * to ensure a single instance across translation units. + * ====================================================================== */ + +inline dimos_lcm_t &_dimos_lcm_ref(void) +{ + static dimos_lcm_t s; + return s; +} + +/* ====================================================================== + * Channel ↔ topic ID lookup + * ====================================================================== */ + +static inline const char *_dimos_channel_for_topic(uint16_t topic_id) +{ + for (int i = 0; i < DIMOS_NUM_TOPICS; i++) { + if (_dimos_topic_map[i].topic_id == topic_id) + return _dimos_topic_map[i].channel; + } + return (const char *)0; +} + +static inline uint16_t _dimos_topic_for_channel(const char *channel) +{ + for (int i = 0; i < DIMOS_NUM_TOPICS; i++) { + if (strcmp(_dimos_topic_map[i].channel, channel) == 0) + return _dimos_topic_map[i].topic_id; + } + return 0xFFFF; /* not found */ +} + +/* ====================================================================== + * Public API + * ====================================================================== */ + +/** + * Initialise DimOS serial + LCM pubsub. + * Call this in setup() before any other dimos_* calls. + */ +static inline void dimos_init(uint32_t baud) +{ + _dsp_usart_init(baud); + dsp_parser_init(&_dsp_state_ref()); + dimos_lcm_init(&_dimos_lcm_ref()); +} + +/** + * Subscribe to messages on a channel with automatic decoding. + * + * The handler receives (channel_name, decoded_msg_ptr, user_data). + * The decoded_msg_ptr is valid only for the duration of the callback. + * + * @param channel Channel name (e.g. "/twist") + * @param type Type descriptor (e.g. &dimos_msg__Twist__type) + * @param handler Callback: void handler(const char *ch, const void *msg, void *ud) + * @param user_data Opaque pointer forwarded to handler + * @return Subscription index (>=0) on success, -1 on error + * + * Example: + * void on_twist(const char *ch, const void *msg, void *ud) { + * const dimos_msg__Twist *t = (const dimos_msg__Twist *)msg; + * // use t->linear.x ... + * } + * dimos_subscribe("/twist", &dimos_msg__Twist__type, on_twist, NULL); + */ +static inline int dimos_subscribe(const char *channel, + const dimos_lcm_type_t *type, + dimos_lcm_handler_t handler, + void *user_data) +{ + return dimos_lcm_subscribe(&_dimos_lcm_ref(), channel, type, handler, user_data); +} + +/** + * Publish a message on a channel. + * + * @param channel Channel name + * @param type Type descriptor + * @param encoded_data Pre-encoded message bytes (from _encode()) + * @param data_len Length of encoded data + * @return 0 on success, -1 on error + */ +static inline int dimos_publish(const char *channel, + const dimos_lcm_type_t *type, + const uint8_t *encoded_data, + uint16_t data_len) +{ + return dimos_lcm_publish(&_dimos_lcm_ref(), channel, type, encoded_data, data_len); +} + +/** + * Poll serial, dispatch inbound messages, and send outbound messages. + * + * This is the main event loop function, matching lcm.handleTimeout(ms). + * Call this in loop() to process messages. + * + * @param timeout_ms Not used on Arduino (non-blocking poll), but kept + * for API compatibility with C++ LCM. + * @return Number of messages dispatched. + */ +#ifndef DSP_CHECK_MAX_BYTES +#define DSP_CHECK_MAX_BYTES 256 +#endif + +static inline int dimos_handle(int timeout_ms) +{ + (void)timeout_ms; /* Arduino is always non-blocking poll */ + + dimos_lcm_t *lcm = &_dimos_lcm_ref(); + struct dsp_parser &parser = _dsp_state_ref(); + int dispatched = 0; + + /* --- Inbound: serial → DSP parser → LCM pubsub dispatch --- */ + uint16_t bytes_processed = 0; + while (_dsp_usart_available() && bytes_processed < DSP_CHECK_MAX_BYTES) { + uint8_t b = _dsp_usart_read(); + bytes_processed++; + + enum dsp_parse_event ev = dsp_feed_byte(&parser, b); + if (ev == DSP_PARSE_MESSAGE) { + /* Look up the channel name for this topic ID */ + const char *channel = _dimos_channel_for_topic(parser.rx_topic); + if (channel != (const char *)0) { + /* Payload contains [8B fingerprint][encoded data] */ + int n = dimos_lcm_dispatch(lcm, channel, parser.rx_buf, parser.rx_len); + if (n > 0) dispatched += n; + } + } + /* CRC_FAIL / OVERFLOW: parser resets itself, keep reading */ + } + + /* --- Outbound: LCM pubsub outbox → DSP frames → serial --- */ + while (dimos_lcm_has_outbound(lcm)) { + const char *channel = (const char *)0; + uint8_t wire_buf[DIMOS_LCM_FINGERPRINT_SIZE + DIMOS_LCM_MAX_MSG_SIZE]; + uint16_t wire_len = dimos_lcm_pop_outbound(lcm, &channel, wire_buf, sizeof(wire_buf)); + if (wire_len == 0 || channel == (const char *)0) break; + + uint16_t topic_id = _dimos_topic_for_channel(channel); + if (topic_id != 0xFFFF) { + dimos_send(topic_id, wire_buf, wire_len); + } + } + + return dispatched; +} + +#ifdef __cplusplus +} +#endif + +#endif /* DIMOS_LCM_SERIAL_H */ diff --git a/dimos/hardware/arduino/common/dsp_protocol.h b/dimos/hardware/arduino/common/dsp_protocol.h new file mode 100644 index 0000000000..1e579f95e0 --- /dev/null +++ b/dimos/hardware/arduino/common/dsp_protocol.h @@ -0,0 +1,572 @@ +/* + * dsp_protocol.h — DimOS Serial Protocol (DSP) + * + * Framed binary protocol for Arduino ↔ Host communication over USB serial. + * + * Frame format: + * [0xD1] [TOPIC 2B LE] [LENGTH 2B LE] [PAYLOAD 0-1024B] [CRC8 1B] + * + * Topic 0 is always DEBUG (UTF-8 text from Serial.print shim). + * Topics 1..N are data streams, assigned by the generated dimos_arduino.h. + * + * This file provides: + * - CRC-8/MAXIM table + computation + * - dimos_init(baud) + * - dimos_send(topic, data, len) + * - dimos_check_message() / dimos_message_topic() / dimos_message_data() / dimos_message_len() + * - DimosSerial class (Serial.print shim → debug frames) + * + * Copyright 2025-2026 Dimensional Inc. Apache-2.0. + */ + +#ifndef DIMOS_DSP_PROTOCOL_H +#define DIMOS_DSP_PROTOCOL_H + +#include +#include + +/* ====================================================================== + * Constants + * ====================================================================== */ + +#define DSP_START_BYTE 0xD1 +#define DSP_TOPIC_DEBUG 0 +#define DSP_HEADER_SIZE 5 /* START + TOPIC(2) + LENGTH(2) */ +#define DSP_OVERHEAD 6 /* HEADER + CRC8 */ + +/* Maximum payload size. + * + * The host-side bridge keeps a generous 1024-byte limit so it can + * forward any fixed-size LCM message we support (the biggest is + * PoseWithCovariance at 344 bytes, with room to spare for future + * types). + * + * On AVR, `_dsp_rx_buf` is a static buffer sized at this value — so a + * 1024-byte default on an Arduino Uno would eat 50% of the chip's 2KB + * SRAM before the user's sketch has a chance to allocate anything. + * Default to 256 on AVR (enough for any geometry_msg we ship that's + * <=256 bytes, which covers every type except PoseWithCovariance and + * its friends). Users who need a larger buffer on a chip with more + * SRAM (Mega 2560 has 8KB) can override via + * `-DDSP_MAX_PAYLOAD=` in their sketch's compile flags. */ +#ifndef DSP_MAX_PAYLOAD +# ifdef __AVR__ +# define DSP_MAX_PAYLOAD 256 +# else +# define DSP_MAX_PAYLOAD 1024 +# endif +#endif + +/* Maximum number of topic handlers */ +#ifndef DSP_MAX_TOPICS +#define DSP_MAX_TOPICS 16 +#endif + +/* Debug buffer size (Serial.print lines) */ +#ifndef DSP_DEBUG_BUF_SIZE +#define DSP_DEBUG_BUF_SIZE 128 +#endif + +/* ====================================================================== + * CRC-8/MAXIM (polynomial 0x31, init 0x00) + * + * Lookup table — 256 bytes of flash on AVR. + * ====================================================================== */ + +#ifdef __AVR__ +#include +#define DSP_CRC_TABLE_ATTR PROGMEM +#define DSP_CRC_READ(addr) pgm_read_byte(addr) +#else +#define DSP_CRC_TABLE_ATTR +#define DSP_CRC_READ(addr) (*(addr)) +#endif + +static const uint8_t _dsp_crc8_table[256] DSP_CRC_TABLE_ATTR = { + 0x00, 0x5E, 0xBC, 0xE2, 0x61, 0x3F, 0xDD, 0x83, + 0xC2, 0x9C, 0x7E, 0x20, 0xA3, 0xFD, 0x1F, 0x41, + 0x9D, 0xC3, 0x21, 0x7F, 0xFC, 0xA2, 0x40, 0x1E, + 0x5F, 0x01, 0xE3, 0xBD, 0x3E, 0x60, 0x82, 0xDC, + 0x23, 0x7D, 0x9F, 0xC1, 0x42, 0x1C, 0xFE, 0xA0, + 0xE1, 0xBF, 0x5D, 0x03, 0x80, 0xDE, 0x3C, 0x62, + 0xBE, 0xE0, 0x02, 0x5C, 0xDF, 0x81, 0x63, 0x3D, + 0x7C, 0x22, 0xC0, 0x9E, 0x1D, 0x43, 0xA1, 0xFF, + 0x46, 0x18, 0xFA, 0xA4, 0x27, 0x79, 0x9B, 0xC5, + 0x84, 0xDA, 0x38, 0x66, 0xE5, 0xBB, 0x59, 0x07, + 0xDB, 0x85, 0x67, 0x39, 0xBA, 0xE4, 0x06, 0x58, + 0x19, 0x47, 0xA5, 0xFB, 0x78, 0x26, 0xC4, 0x9A, + 0x65, 0x3B, 0xD9, 0x87, 0x04, 0x5A, 0xB8, 0xE6, + 0xA7, 0xF9, 0x1B, 0x45, 0xC6, 0x98, 0x7A, 0x24, + 0xF8, 0xA6, 0x44, 0x1A, 0x99, 0xC7, 0x25, 0x7B, + 0x3A, 0x64, 0x86, 0xD8, 0x5B, 0x05, 0xE7, 0xB9, + 0x8C, 0xD2, 0x30, 0x6E, 0xED, 0xB3, 0x51, 0x0F, + 0x4E, 0x10, 0xF2, 0xAC, 0x2F, 0x71, 0x93, 0xCD, + 0x11, 0x4F, 0xAD, 0xF3, 0x70, 0x2E, 0xCC, 0x92, + 0xD3, 0x8D, 0x6F, 0x31, 0xB2, 0xEC, 0x0E, 0x50, + 0xAF, 0xF1, 0x13, 0x4D, 0xCE, 0x90, 0x72, 0x2C, + 0x6D, 0x33, 0xD1, 0x8F, 0x0C, 0x52, 0xB0, 0xEE, + 0x32, 0x6C, 0x8E, 0xD0, 0x53, 0x0D, 0xEF, 0xB1, + 0xF0, 0xAE, 0x4C, 0x12, 0x91, 0xCF, 0x2D, 0x73, + 0xCA, 0x94, 0x76, 0x28, 0xAB, 0xF5, 0x17, 0x49, + 0x08, 0x56, 0xB4, 0xEA, 0x69, 0x37, 0xD5, 0x8B, + 0x57, 0x09, 0xEB, 0xB5, 0x36, 0x68, 0x8A, 0xD4, + 0x95, 0xCB, 0x29, 0x77, 0xF4, 0xAA, 0x48, 0x16, + 0xE9, 0xB7, 0x55, 0x0B, 0x88, 0xD6, 0x34, 0x6A, + 0x2B, 0x75, 0x97, 0xC9, 0x4A, 0x14, 0xF6, 0xA8, + 0x74, 0x2A, 0xC8, 0x96, 0x15, 0x4B, 0xA9, 0xF7, + 0xB6, 0xE8, 0x0A, 0x54, 0xD7, 0x89, 0x6B, 0x35, +}; + +static inline uint8_t dsp_crc8(const uint8_t *data, uint16_t len) +{ + uint8_t crc = 0x00; + uint16_t i; + for (i = 0; i < len; i++) { + crc = DSP_CRC_READ(&_dsp_crc8_table[crc ^ data[i]]); + } + return crc; +} + +/* ====================================================================== + * Shared parser state machine + * + * The wire-framing parser is identical on the Arduino and on the host + * bridge — both reconstruct a DSP frame from a byte stream — so the + * state struct and the single-byte step function live here, in the + * portable section of the header, and are #included by both sides. + * Historically these were duplicated in main.cpp and drifted, so + * protocol changes had to be made in two places with no CI guard on + * drift. One definition, one place to change. + * + * Public API: + * - ``struct dsp_parser`` — callers keep one per stream + * - ``dsp_parser_init(&p)`` — zero-initialize before first feed + * - ``dsp_feed_byte(&p, b)`` — step machine with one byte, returns + * a ``dsp_parse_event`` telling the + * caller what to do next + * + * Event semantics: + * DSP_PARSE_NONE — keep feeding + * DSP_PARSE_MESSAGE — complete frame; read ``p.rx_topic/rx_len/rx_buf`` + * before the next ``dsp_feed_byte`` call + * DSP_PARSE_CRC_FAIL — framing OK but CRC mismatch; parser is already + * reset and ready for more + * DSP_PARSE_OVERFLOW — declared length exceeds ``DSP_MAX_PAYLOAD``; + * parser is reset + * ====================================================================== */ + +enum dsp_parse_state { + DSP_WAIT_START, + DSP_READ_TOPIC_LO, + DSP_READ_TOPIC_HI, + DSP_READ_LEN_LO, + DSP_READ_LEN_HI, + DSP_READ_PAYLOAD, + DSP_READ_CRC +}; + +enum dsp_parse_event { + DSP_PARSE_NONE = 0, + DSP_PARSE_MESSAGE, + DSP_PARSE_CRC_FAIL, + DSP_PARSE_OVERFLOW +}; + +struct dsp_parser { + enum dsp_parse_state state; + uint16_t rx_topic; + uint16_t rx_len; + uint16_t rx_payload_pos; + uint8_t rx_buf[DSP_MAX_PAYLOAD]; +}; + +static inline void dsp_parser_init(struct dsp_parser *p) +{ + p->state = DSP_WAIT_START; + p->rx_topic = 0; + p->rx_len = 0; + p->rx_payload_pos = 0; +} + +static inline enum dsp_parse_event dsp_feed_byte(struct dsp_parser *p, uint8_t b) +{ + switch (p->state) { + case DSP_WAIT_START: + if (b == DSP_START_BYTE) { + p->state = DSP_READ_TOPIC_LO; + } + return DSP_PARSE_NONE; + + case DSP_READ_TOPIC_LO: + p->rx_topic = b; + p->state = DSP_READ_TOPIC_HI; + return DSP_PARSE_NONE; + + case DSP_READ_TOPIC_HI: + p->rx_topic |= ((uint16_t)b << 8); + p->state = DSP_READ_LEN_LO; + return DSP_PARSE_NONE; + + case DSP_READ_LEN_LO: + p->rx_len = b; + p->state = DSP_READ_LEN_HI; + return DSP_PARSE_NONE; + + case DSP_READ_LEN_HI: + p->rx_len |= ((uint16_t)b << 8); + if (p->rx_len > DSP_MAX_PAYLOAD) { + p->state = DSP_WAIT_START; + return DSP_PARSE_OVERFLOW; + } + p->rx_payload_pos = 0; + p->state = (p->rx_len == 0) ? DSP_READ_CRC : DSP_READ_PAYLOAD; + return DSP_PARSE_NONE; + + case DSP_READ_PAYLOAD: + p->rx_buf[p->rx_payload_pos++] = b; + if (p->rx_payload_pos >= p->rx_len) { + p->state = DSP_READ_CRC; + } + return DSP_PARSE_NONE; + + case DSP_READ_CRC: { + /* CRC-8/MAXIM over TOPIC_LO + TOPIC_HI + LEN_LO + LEN_HI + PAYLOAD, + * computed incrementally via the table. No temporary buffer needed. */ + uint8_t crc = 0x00; + crc = DSP_CRC_READ(&_dsp_crc8_table[crc ^ (uint8_t)(p->rx_topic & 0xFF)]); + crc = DSP_CRC_READ(&_dsp_crc8_table[crc ^ (uint8_t)((p->rx_topic >> 8) & 0xFF)]); + crc = DSP_CRC_READ(&_dsp_crc8_table[crc ^ (uint8_t)(p->rx_len & 0xFF)]); + crc = DSP_CRC_READ(&_dsp_crc8_table[crc ^ (uint8_t)((p->rx_len >> 8) & 0xFF)]); + for (uint16_t k = 0; k < p->rx_len; k++) { + crc = DSP_CRC_READ(&_dsp_crc8_table[crc ^ p->rx_buf[k]]); + } + + p->state = DSP_WAIT_START; + if (crc == b) { + return DSP_PARSE_MESSAGE; + } + return DSP_PARSE_CRC_FAIL; + } + } + /* Unreachable — every enum value is handled above. Reset defensively. */ + p->state = DSP_WAIT_START; + return DSP_PARSE_NONE; +} + +/* ====================================================================== + * Platform abstraction (Arduino vs host C++) + * + * On Arduino: uses HardwareSerial directly. + * On host (for testing): stubs can be provided. + * ====================================================================== */ + +#ifdef ARDUINO + +/* ====================================================================== + * Arduino Implementation + * + * Two back-ends: + * + * 1. **HardwareSerial** (default on real hardware) — uses Arduino's + * ``Serial`` object, which has a 64-byte interrupt-driven RX buffer. + * At 115200 baud bytes arrive every ~87µs; the 2-byte hardware FIFO + * overflows whenever the sketch does any non-trivial work (encoding a + * reply, printing debug text, even a 1ms delay). HardwareSerial + * handles this transparently via the RXCIE0 ISR. + * + * 2. **Direct USART register access** (QEMU / simulator builds) — used + * when ``DSP_DIRECT_USART`` is defined. QEMU's AVR USART model + * does not fire interrupts, so HardwareSerial's ISR never triggers + * and the RX buffer stays empty. Direct register access works + * because QEMU simulates AVR instructions far faster than real time, + * so the 2-byte FIFO never overflows in practice. + * + * To force the direct path on real hardware (e.g. bare-metal without + * the Arduino core), ``-DDSP_DIRECT_USART`` in compile flags. + * ====================================================================== */ + +#include + +#ifdef DSP_DIRECT_USART + +/* --- Direct USART0 (QEMU / bare-metal) --- */ + +#include + +#if !defined(__AVR_ATmega328P__) && !defined(__AVR_ATmega328PB__) && \ + !defined(__AVR_ATmega2560__) && !defined(__AVR_ATmega1280__) +#error "DSP_DIRECT_USART only supports ATmega328P / 328PB / 1280 / 2560 USART0." +#endif + +static inline void _dsp_usart_init(uint32_t baud) { + uint16_t ubrr = (uint16_t)((F_CPU / 8 / baud) - 1); + UBRR0H = (uint8_t)(ubrr >> 8); + UBRR0L = (uint8_t)(ubrr & 0xFF); + UCSR0A = (1 << U2X0); + UCSR0B = (1 << RXEN0) | (1 << TXEN0); + UCSR0C = (1 << UCSZ01) | (1 << UCSZ00); +} + +static inline void _dsp_usart_write(uint8_t b) { + while (!(UCSR0A & (1 << UDRE0))) {} + UDR0 = b; +} + +static inline bool _dsp_usart_available(void) { + return (UCSR0A & (1 << RXC0)) != 0; +} + +static inline uint8_t _dsp_usart_read(void) { + return UDR0; +} + +#else /* !DSP_DIRECT_USART — HardwareSerial (real hardware) */ + +static inline void _dsp_usart_init(uint32_t baud) { + Serial.begin(baud); +} + +static inline void _dsp_usart_write(uint8_t b) { + Serial.write(b); +} + +static inline bool _dsp_usart_available(void) { + return Serial.available() > 0; +} + +static inline uint8_t _dsp_usart_read(void) { + return (uint8_t)Serial.read(); +} + +#endif /* DSP_DIRECT_USART */ + +/* --- Internal state --- + * + * The parser state must be shared across all translation units that + * include this header, otherwise a sketch split across multiple .cpp / + * .ino files ends up with one independent state machine per TU — the + * second TU's `dimos_check_message()` would see an empty buffer. + * + * We expose the parser via a plain (non-``static``) ``inline`` function + * whose function-local static is guaranteed by the C++ standard to + * resolve to a single object across TUs. The parser struct and the + * step function themselves live above, in the portable section shared + * with the host bridge. */ + +inline struct dsp_parser &_dsp_state_ref(void) +{ + static struct dsp_parser s = { + /* state */ DSP_WAIT_START, + /* rx_topic */ 0, + /* rx_len */ 0, + /* rx_payload_pos */ 0, + /* rx_buf */ {0}, + }; + return s; +} + +/** + * Initialize DimOS serial protocol. + * Call this in setup() before any other dimos_* calls. + * + * When the LCM serial adapter is used (dimos_lcm_serial.h), that header + * provides its own dimos_init() that also initialises the pubsub layer. + */ +#ifndef DIMOS_LCM_SERIAL_H +static inline void dimos_init(uint32_t baud) +{ + _dsp_usart_init(baud); + dsp_parser_init(&_dsp_state_ref()); +} +#endif + +/** + * Send a DSP frame. + * + * @param topic Topic enum value (DIMOS_TOPIC_DEBUG, DIMOS_TOPIC__*, etc.) + * @param data Payload bytes (LCM-encoded for data topics, UTF-8 for debug) + * @param len Payload length in bytes + */ +static inline void dimos_send(uint16_t topic, const uint8_t *data, uint16_t len) +{ + if (len > DSP_MAX_PAYLOAD) return; + + /* Build header: START, TOPIC (2B LE), LENGTH (2B LE) */ + uint8_t header[DSP_HEADER_SIZE]; + header[0] = DSP_START_BYTE; + header[1] = (uint8_t)(topic & 0xFF); + header[2] = (uint8_t)((topic >> 8) & 0xFF); + header[3] = (uint8_t)(len & 0xFF); + header[4] = (uint8_t)((len >> 8) & 0xFF); + + /* CRC over TOPIC + LENGTH + PAYLOAD */ + uint8_t crc = 0x00; + uint16_t i; + for (i = 1; i < DSP_HEADER_SIZE; i++) { + crc = DSP_CRC_READ(&_dsp_crc8_table[crc ^ header[i]]); + } + for (i = 0; i < len; i++) { + crc = DSP_CRC_READ(&_dsp_crc8_table[crc ^ data[i]]); + } + + /* Write frame byte-by-byte via direct USART */ + uint16_t k; + for (k = 0; k < DSP_HEADER_SIZE; k++) _dsp_usart_write(header[k]); + for (k = 0; k < len; k++) _dsp_usart_write(data[k]); + _dsp_usart_write(crc); +} + +/** + * Check for the next incoming DSP message. + * + * Reads available serial bytes and attempts to parse a complete frame. + * Returns true if a valid message is ready. Use dimos_message_topic(), + * dimos_message_data(), and dimos_message_len() to access it. + * + * Typical usage in loop(): + * + * while (dimos_check_message()) { + * switch (dimos_message_topic()) { + * case DIMOS_TOPIC__MY_INPUT: + * MyType msg; + * MyType_decode(dimos_message_data(), 0, dimos_message_len(), &msg); + * // use msg... + * break; + * } + * } + */ +/* Maximum bytes `dimos_check_message` will process in one call. Prevents + * a flood of 1-byte frames from starving the user's loop(). Override by + * defining DSP_CHECK_MAX_BYTES before including this header. */ +#ifndef DSP_CHECK_MAX_BYTES +#define DSP_CHECK_MAX_BYTES 256 +#endif + +static inline bool dimos_check_message(void) +{ + struct dsp_parser &s = _dsp_state_ref(); + + uint16_t bytes_processed = 0; + while (_dsp_usart_available() && bytes_processed < DSP_CHECK_MAX_BYTES) { + uint8_t b = _dsp_usart_read(); + bytes_processed++; + + enum dsp_parse_event ev = dsp_feed_byte(&s, b); + if (ev == DSP_PARSE_MESSAGE) { + return true; + } + /* DSP_PARSE_CRC_FAIL / DSP_PARSE_OVERFLOW / DSP_PARSE_NONE: + * the parser has already reset itself; just keep reading. */ + } + + return false; /* no complete message available (yet) */ +} + +/** + * Get the topic of the last received message. + * Only valid after dimos_check_message() returned true. + */ +static inline enum dimos_topic dimos_message_topic(void) +{ + return (enum dimos_topic)_dsp_state_ref().rx_topic; +} + +/** + * Get a pointer to the payload of the last received message. + * Only valid after dimos_check_message() returned true. + */ +static inline const uint8_t *dimos_message_data(void) +{ + return _dsp_state_ref().rx_buf; +} + +/** + * Get the payload length of the last received message. + * Only valid after dimos_check_message() returned true. + */ +static inline uint16_t dimos_message_len(void) +{ + return _dsp_state_ref().rx_len; +} + +/* ====================================================================== + * Serial.print shim + * + * Intercepts Serial.print/println and sends output as DSP debug frames + * (topic 0). Flushes on newline or buffer full. + * ====================================================================== */ + +class DimosSerial_ : public Print { +public: + size_t write(uint8_t b) override { + _buf[_pos++] = b; + if (b == '\n' || _pos >= DSP_DEBUG_BUF_SIZE) { + _flush(); + } + return 1; + } + + size_t write(const uint8_t *buffer, size_t size) override { + size_t i; + for (i = 0; i < size; i++) { + write(buffer[i]); + } + return size; + } + + void flush() { + if (_pos > 0) _flush(); + } + +private: + uint8_t _buf[DSP_DEBUG_BUF_SIZE]; + uint8_t _pos = 0; + + void _flush() { + dimos_send((uint16_t)DSP_TOPIC_DEBUG, _buf, _pos); + _pos = 0; + } +}; + +/* Must be `inline` (not `static inline`) so the function-local static + * below collapses to one object across every TU that includes this + * header — the same trick used by `_dsp_state_ref` above. A plain + * `static DimosSerial_ DimosSerial;` here would give each TU its own + * buffer, and a library-backed `.cpp` writing through the name would + * see independent `_buf`/`_pos` state from the `.ino`. */ +inline DimosSerial_ &_dimos_serial_ref(void) +{ + static DimosSerial_ s; + return s; +} +#define DimosSerial (_dimos_serial_ref()) + +/* + * IMPORTANT: use `DimosSerial.print/println(...)` in your sketch, not + * `Serial.print/println(...)`. + * + * Earlier versions of this header installed `#define Serial DimosSerial` + * so that existing `Serial.print` calls would transparently route through + * the DSP debug channel. That was removed because macro-replacing + * `Serial` breaks any third-party library (Wire, SPI, motor drivers, + * etc.) that references `Serial` internally — those libraries would try + * to call `DimosSerial.available()` / `.read()` which don't exist, and + * fail to compile deep inside the library header. + * + * If you want a shim in your own sketch, add this AFTER all library + * includes: + * + * #define Serial DimosSerial + */ + +#endif /* ARDUINO */ + +/* ====================================================================== + * Host-side (C++) utilities + * + * The C++ bridge doesn't use dimos_init/dimos_send/dimos_poll (it has + * its own implementation with termios). But it shares the constants + * and CRC function. + * ====================================================================== */ + +#endif /* DIMOS_DSP_PROTOCOL_H */ diff --git a/dimos/hardware/arduino/cpp/CMakeLists.txt b/dimos/hardware/arduino/cpp/CMakeLists.txt new file mode 100644 index 0000000000..aa3450d29c --- /dev/null +++ b/dimos/hardware/arduino/cpp/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.16) +project(arduino_bridge CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# dimos-lcm C++ headers (for fingerprint hash computation) +set(DIMOS_LCM_DIR "" CACHE PATH "Path to dimos-lcm repo") +if(NOT DIMOS_LCM_DIR) + message(FATAL_ERROR "Set -DDIMOS_LCM_DIR to the dimos-lcm source directory") +endif() +set(LCM_CPP_MSGS "${DIMOS_LCM_DIR}/generated/cpp_lcm_msgs") + +# Arduino common headers (dsp_protocol.h, dimos_lcm_serial.h) +set(ARDUINO_COMMON "${CMAKE_CURRENT_SOURCE_DIR}/../common") + +# Arduino C message headers from dimos-lcm (lcm_coretypes_arduino.h, etc.) +set(ARDUINO_MSGS "${DIMOS_LCM_DIR}/generated/arduino_c_msgs") + +# LCM library +find_package(lcm REQUIRED) + +add_executable(arduino_bridge main.cpp) + +target_include_directories(arduino_bridge PRIVATE + ${LCM_CPP_MSGS} # LCM C++ generated headers + ${ARDUINO_COMMON} # dsp_protocol.h, dimos_lcm_serial.h + ${ARDUINO_MSGS} # lcm_coretypes_arduino.h, dimos_lcm_pubsub.h +) + +target_link_libraries(arduino_bridge PRIVATE lcm pthread) + +install(TARGETS arduino_bridge DESTINATION bin) diff --git a/dimos/hardware/arduino/cpp/main.cpp b/dimos/hardware/arduino/cpp/main.cpp new file mode 100644 index 0000000000..fc517b4000 --- /dev/null +++ b/dimos/hardware/arduino/cpp/main.cpp @@ -0,0 +1,637 @@ +/* + * arduino_bridge — Generic serial ↔ LCM relay for DimOS ArduinoModule. + * + * This binary is module-agnostic. It receives topic→LCM channel mappings + * via CLI args and forwards raw bytes between USB serial and LCM multicast, + * passing raw payload bytes through (fingerprint is part of the DSP payload). + * + * Usage: + * ./arduino_bridge \ + * --serial_port /dev/ttyACM0 \ + * --baudrate 115200 \ + * --reconnect true \ + * --reconnect_interval 2.0 \ + * --topic_out 1 "/imu#sensor_msgs.Imu" \ + * --topic_in 2 "/cmd#geometry_msgs.Twist" + * + * Copyright 2025-2026 Dimensional Inc. Apache-2.0. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Serial (POSIX) */ +#include +#include +#include +#include + +/* LCM */ +#include + +/* DSP protocol constants + CRC */ +#include "dsp_protocol.h" + +/* ====================================================================== + * Bridge state + * + * One Bridge per process. All state the threads and signal handler touch + * lives here so the signal handler only needs a single pointer and so the + * relationship between "user asked us to quit" (`running`) and "serial link + * is up" (`serial_connected`) is explicit. + * ====================================================================== */ + +/* Topic mapping — owned via unique_ptr so that raw pointers stored in the + * lookup maps (and in RawHandler::tm) are never invalidated by reallocation + * of the containing vector. */ +struct TopicMapping { + uint16_t topic_id; + std::string lcm_channel; /* full "name#msg_type" */ + bool is_output; /* true = Arduino→Host (publish), false = Host→Arduino (subscribe) */ +}; + +/* Forward decl — RawHandler body is below the Bridge so it can reference it. */ +class RawHandler; + +struct Bridge { + /* Shutdown: user hit ^C or sent SIGTERM. Process-lifetime. */ + std::atomic running{true}; + /* Serial link currently open and threads may use g_serial_fd. Cycles + * per reconnect. */ + std::atomic serial_connected{false}; + /* Set by `lcm_handler_thread` when `handleTimeout` returns an error + * before clearing `running`. The main loop reads this after its + * join()s so we can exit(1) — letting the coordinator restart the + * bridge — instead of reporting a clean shutdown. */ + std::atomic lcm_failed{false}; + + int serial_fd{-1}; + std::mutex serial_write_mutex; + + std::vector> topics; + + lcm::LCM *lcm{nullptr}; + std::map hash_registry; + + std::map topic_out_map; + std::map topic_in_map; + std::vector> raw_handlers; + + std::string serial_port; + int baudrate{115200}; + bool reconnect{true}; + float reconnect_interval{2.0f}; +}; + +/* Single process-global pointer the signal handler touches. All other + * code takes the Bridge by reference. */ +static Bridge *g_bridge = nullptr; + +/* ====================================================================== + * CLI Parsing + * ====================================================================== */ + +/* Returns the termios speed constant for `baud`, or B0 with *ok=false on + * unsupported rates. + * + * Darwin's only defines POSIX-standard constants up to + * B230400; B460800 / B500000 / B576000 / B921600 / B1000000 are + * Linux-specific extensions. We gate them behind `__linux__` so the + * bridge builds cleanly on macOS — useful for the virtual-Arduino + * path where the bridge talks to a QEMU PTY and any baud rate is + * effectively a no-op anyway. */ +static speed_t baud_to_speed(int baud, bool *ok) +{ + *ok = true; + switch (baud) { + case 9600: return B9600; + case 19200: return B19200; + case 38400: return B38400; + case 57600: return B57600; + case 115200: return B115200; + case 230400: return B230400; +#ifdef __linux__ + case 460800: return B460800; + case 500000: return B500000; + case 576000: return B576000; + case 921600: return B921600; + case 1000000: return B1000000; +#endif + default: + *ok = false; + return B0; + } +} + +/* `exit(1)` on unknown flags, missing required arg values, or unsupported + * baud rates. Silently falling back to a default is a footgun. */ +static void parse_args(Bridge &b, int argc, char **argv) +{ + for (int i = 1; i < argc; i++) { + std::string arg(argv[i]); + + if (arg == "--serial_port" && i + 1 < argc) { + b.serial_port = argv[++i]; + } else if (arg == "--baudrate" && i + 1 < argc) { + b.baudrate = std::atoi(argv[++i]); + bool ok; + (void)baud_to_speed(b.baudrate, &ok); + if (!ok) { + fprintf(stderr, "[bridge] Unsupported baud rate: %d\n", b.baudrate); + exit(1); + } + } else if (arg == "--reconnect" && i + 1 < argc) { + std::string val(argv[++i]); + b.reconnect = (val == "true" || val == "1"); + } else if (arg == "--reconnect_interval" && i + 1 < argc) { + b.reconnect_interval = std::atof(argv[++i]); + } else if ((arg == "--topic_out" || arg == "--topic_in") && i + 2 < argc) { + auto tm = std::make_unique(); + tm->topic_id = (uint16_t)std::atoi(argv[++i]); + tm->lcm_channel = argv[++i]; + tm->is_output = (arg == "--topic_out"); + b.topics.push_back(std::move(tm)); + } else { + fprintf(stderr, "[bridge] Unknown or malformed argument: %s\n", arg.c_str()); + exit(1); + } + } +} + +/* ====================================================================== + * LCM Fingerprint Hash Registry + * + * The 2-byte topic DSP protocol includes the 8-byte LCM fingerprint + * INSIDE the DSP payload, so the bridge is a pure passthrough — no + * prepending or stripping needed. We keep the hash registry for + * validation and logging purposes. + * ====================================================================== */ + +/* + * Include all LCM C++ message headers we support. + * The fingerprint hash is available via Type::getHash(). + */ +#include "std_msgs/Time.hpp" +#include "std_msgs/Bool.hpp" +#include "std_msgs/Int32.hpp" +#include "std_msgs/Float32.hpp" +#include "std_msgs/Float64.hpp" +#include "std_msgs/ColorRGBA.hpp" +#include "geometry_msgs/Vector3.hpp" +#include "geometry_msgs/Point.hpp" +#include "geometry_msgs/Point32.hpp" +#include "geometry_msgs/Quaternion.hpp" +#include "geometry_msgs/Pose.hpp" +#include "geometry_msgs/Pose2D.hpp" +#include "geometry_msgs/Twist.hpp" +#include "geometry_msgs/Accel.hpp" +#include "geometry_msgs/Transform.hpp" +#include "geometry_msgs/Wrench.hpp" +#include "geometry_msgs/Inertia.hpp" +#include "geometry_msgs/PoseWithCovariance.hpp" +#include "geometry_msgs/TwistWithCovariance.hpp" +#include "geometry_msgs/AccelWithCovariance.hpp" + +static void init_hash_registry(Bridge &b) +{ + /* Register all known types. + * + * NOTE: this list is kept in sync with three other places and there is + * a Python test (`test_arduino_msg_registry_sync`) that fails CI if any + * of them drift: + * - dimos/core/arduino_module.py :: _KNOWN_TYPE_HEADERS + * - dimos/hardware/arduino/common/arduino_msgs (Arduino-side .h) + * - this function (C++ bridge hash registry) + */ + b.hash_registry["std_msgs.Time"] = std_msgs::Time::getHash(); + b.hash_registry["std_msgs.Bool"] = std_msgs::Bool::getHash(); + b.hash_registry["std_msgs.Int32"] = std_msgs::Int32::getHash(); + b.hash_registry["std_msgs.Float32"] = std_msgs::Float32::getHash(); + b.hash_registry["std_msgs.Float64"] = std_msgs::Float64::getHash(); + b.hash_registry["std_msgs.ColorRGBA"] = std_msgs::ColorRGBA::getHash(); + + b.hash_registry["geometry_msgs.Vector3"] = geometry_msgs::Vector3::getHash(); + b.hash_registry["geometry_msgs.Point"] = geometry_msgs::Point::getHash(); + b.hash_registry["geometry_msgs.Point32"] = geometry_msgs::Point32::getHash(); + b.hash_registry["geometry_msgs.Quaternion"] = geometry_msgs::Quaternion::getHash(); + b.hash_registry["geometry_msgs.Pose"] = geometry_msgs::Pose::getHash(); + b.hash_registry["geometry_msgs.Pose2D"] = geometry_msgs::Pose2D::getHash(); + b.hash_registry["geometry_msgs.Twist"] = geometry_msgs::Twist::getHash(); + b.hash_registry["geometry_msgs.Accel"] = geometry_msgs::Accel::getHash(); + b.hash_registry["geometry_msgs.Transform"] = geometry_msgs::Transform::getHash(); + b.hash_registry["geometry_msgs.Wrench"] = geometry_msgs::Wrench::getHash(); + b.hash_registry["geometry_msgs.Inertia"] = geometry_msgs::Inertia::getHash(); + b.hash_registry["geometry_msgs.PoseWithCovariance"] = geometry_msgs::PoseWithCovariance::getHash(); + b.hash_registry["geometry_msgs.TwistWithCovariance"] = geometry_msgs::TwistWithCovariance::getHash(); + b.hash_registry["geometry_msgs.AccelWithCovariance"] = geometry_msgs::AccelWithCovariance::getHash(); +} + +/* Extract "msg_type" from "topic_name#msg_type" */ +static std::string extract_msg_type(const std::string &channel) +{ + auto pos = channel.find('#'); + if (pos == std::string::npos) return ""; + return channel.substr(pos + 1); +} + +/* Extract "topic_name" from "topic_name#msg_type" */ +static std::string extract_topic_name(const std::string &channel) +{ + auto pos = channel.find('#'); + if (pos == std::string::npos) return channel; + return channel.substr(0, pos); +} + +/* Validate that all topic message types are in the hash registry (for logging). */ +static bool validate_topic_types(Bridge &b) +{ + for (auto &tm : b.topics) { + std::string msg_type = extract_msg_type(tm->lcm_channel); + auto it = b.hash_registry.find(msg_type); + if (it == b.hash_registry.end()) { + fprintf(stderr, + "[bridge] Unknown message type: %s (topic_id=%u, channel=%s)\n", + msg_type.c_str(), tm->topic_id, tm->lcm_channel.c_str()); + return false; + } + } + return true; +} + +/* ====================================================================== + * Serial Port + * ====================================================================== */ + +static int serial_open(const std::string &port, int baud) +{ + int fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + if (fd < 0) { + fprintf(stderr, "[bridge] Cannot open %s: %s\n", port.c_str(), strerror(errno)); + return -1; + } + + /* Clear O_NONBLOCK after open (we want blocking reads in the reader thread) */ + int flags = fcntl(fd, F_GETFL, 0); + fcntl(fd, F_SETFL, flags & ~O_NONBLOCK); + + struct termios tio; + memset(&tio, 0, sizeof(tio)); + tcgetattr(fd, &tio); + + /* Raw mode: no echo, no canonical, no signals */ + cfmakeraw(&tio); + + /* 8N1 */ + tio.c_cflag &= ~(CSIZE | PARENB | CSTOPB); + tio.c_cflag |= CS8 | CLOCAL | CREAD; + + /* No flow control */ + tio.c_cflag &= ~CRTSCTS; + tio.c_iflag &= ~(IXON | IXOFF | IXANY); + + /* Set baud — parse_args already validated this, so `ok` should always be + * true here. Assert to be safe. */ + bool speed_ok; + speed_t speed = baud_to_speed(baud, &speed_ok); + if (!speed_ok) { + fprintf(stderr, "[bridge] BUG: serial_open called with unsupported baud %d\n", baud); + close(fd); + return -1; + } + cfsetispeed(&tio, speed); + cfsetospeed(&tio, speed); + + /* Read timeout: return after 100ms or 1 byte, whichever first */ + tio.c_cc[VMIN] = 0; + tio.c_cc[VTIME] = 1; /* 100ms in deciseconds */ + + tcsetattr(fd, TCSANOW, &tio); + tcflush(fd, TCIOFLUSH); + + return fd; +} + +static void serial_close(int fd) +{ + if (fd >= 0) close(fd); +} + +/* ====================================================================== + * Serial → LCM (reader thread) + * ====================================================================== */ + +static void serial_reader_thread(Bridge &b) +{ + /* Framing state lives entirely inside `dsp_parser` — shared with the + * AVR side via `dsp_protocol.h`. One implementation, one place to + * change when the wire format evolves. Reads bytes in small chunks + * to amortize the `read()` syscall (VMIN=0/VTIME=1 already gives us + * a bounded blocking read, so bulk reads are safe). */ + struct dsp_parser parser; + dsp_parser_init(&parser); + + uint8_t chunk[64]; + + /* Exit on either global shutdown or a serial disconnect flagged by the + * writer path. `serial_connected` being a separate atomic means that + * `signal_handler` flipping `running` to false and the writer flipping + * `serial_connected` to false don't race each other's meaning. */ + while (b.running.load() && b.serial_connected.load()) { + int n = read(b.serial_fd, chunk, sizeof(chunk)); + if (n < 0) { + if (errno == EINTR) continue; + fprintf(stderr, "[bridge] Serial read error: %s\n", strerror(errno)); + b.serial_connected.store(false); + break; + } + if (n == 0) continue; /* VTIME timeout, loop back */ + + for (int i = 0; i < n; i++) { + enum dsp_parse_event ev = dsp_feed_byte(&parser, chunk[i]); + + if (ev == DSP_PARSE_CRC_FAIL) { + fprintf(stderr, "[bridge] CRC mismatch on topic %u\n", parser.rx_topic); + continue; + } + if (ev == DSP_PARSE_OVERFLOW) { + fprintf(stderr, "[bridge] Frame length %u exceeds DSP_MAX_PAYLOAD=%d on topic %u\n", + parser.rx_len, DSP_MAX_PAYLOAD, parser.rx_topic); + continue; + } + if (ev != DSP_PARSE_MESSAGE) continue; + + /* Handle frame */ + if (parser.rx_topic == DSP_TOPIC_DEBUG) { + /* Debug: print to stdout */ + fwrite(parser.rx_buf, 1, parser.rx_len, stdout); + fflush(stdout); + } else { + /* Data: payload already contains [8B fingerprint][data], + * publish directly to LCM as a pure passthrough. */ + auto it = b.topic_out_map.find(parser.rx_topic); + if (it != b.topic_out_map.end()) { + TopicMapping *tm = it->second; + b.lcm->publish(tm->lcm_channel, parser.rx_buf, parser.rx_len); + } else { + fprintf(stderr, "[bridge] Unknown outbound topic: %u\n", parser.rx_topic); + } + } + } + } +} + +/* ====================================================================== + * LCM → Serial (subscription handler) + * ====================================================================== */ + +/* write_all — loop until `len` bytes are written or a hard error occurs. + * + * On EINTR we retry; on any other error we return false so the caller can + * flag the serial link down and let the reconnect loop run. A partial + * write on a dying USB device would otherwise corrupt the DSP frame. */ +static bool write_all(int fd, const void *buf, size_t len) +{ + const uint8_t *p = static_cast(buf); + size_t remaining = len; + while (remaining > 0) { + ssize_t n = ::write(fd, p, remaining); + if (n < 0) { + if (errno == EINTR) continue; + return false; + } + if (n == 0) return false; /* shouldn't happen on a blocking fd */ + p += (size_t)n; + remaining -= (size_t)n; + } + return true; +} + +/* Forward declaration */ +static void send_lcm_to_serial(Bridge &b, + const lcm::ReceiveBuffer *rbuf, + TopicMapping *tm); + +class RawHandler { +public: + Bridge *bridge; + TopicMapping *tm; + RawHandler(Bridge *br, TopicMapping *t) : bridge(br), tm(t) {} + void handle(const lcm::ReceiveBuffer *rbuf, const std::string & /*channel*/) { + send_lcm_to_serial(*bridge, rbuf, tm); + } +}; + +static void send_lcm_to_serial(Bridge &b, + const lcm::ReceiveBuffer *rbuf, + TopicMapping *tm) +{ + /* LCM message already contains [8B fingerprint][data]. Pass the + * FULL payload (with fingerprint) into the DSP frame — pure passthrough. */ + size_t data_size = (size_t)rbuf->data_size; + const uint8_t *payload = (const uint8_t *)rbuf->data; + size_t payload_len_raw = data_size; + + if (payload_len_raw > DSP_MAX_PAYLOAD) { + fprintf(stderr, + "[bridge] Dropping LCM message on %s: payload %zu > DSP_MAX_PAYLOAD %d\n", + tm->lcm_channel.c_str(), payload_len_raw, DSP_MAX_PAYLOAD); + return; + } + uint16_t payload_len = (uint16_t)payload_len_raw; + + /* Build DSP frame header: START + TOPIC(2B LE) + LENGTH(2B LE) */ + uint8_t header[DSP_HEADER_SIZE]; + header[0] = DSP_START_BYTE; + header[1] = (uint8_t)(tm->topic_id & 0xFF); + header[2] = (uint8_t)((tm->topic_id >> 8) & 0xFF); + header[3] = (uint8_t)(payload_len & 0xFF); + header[4] = (uint8_t)((payload_len >> 8) & 0xFF); + + /* CRC-8/MAXIM over TOPIC_LO + TOPIC_HI + LEN_LO + LEN_HI + PAYLOAD, incremental. */ + uint8_t crc = 0x00; + crc = _dsp_crc8_table[crc ^ header[1]]; + crc = _dsp_crc8_table[crc ^ header[2]]; + crc = _dsp_crc8_table[crc ^ header[3]]; + crc = _dsp_crc8_table[crc ^ header[4]]; + for (uint16_t k = 0; k < payload_len; k++) { + crc = _dsp_crc8_table[crc ^ payload[k]]; + } + + /* Write to serial (thread-safe w.r.t. other writers). + * + * If any write fails (USB disconnect, short write on a dead fd, etc.) + * we flag `serial_connected` false so the reader thread bails and the + * reconnect loop takes over. Dropping a partial frame is strictly + * better than continuing to corrupt the outbound stream. */ + std::lock_guard lock(b.serial_write_mutex); + if (!b.serial_connected.load()) return; + bool ok = write_all(b.serial_fd, header, DSP_HEADER_SIZE); + if (ok && payload_len > 0) { + ok = write_all(b.serial_fd, payload, payload_len); + } + if (ok) { + ok = write_all(b.serial_fd, &crc, 1); + } + if (!ok) { + fprintf(stderr, + "[bridge] Serial write failed on topic %u (%s): %s — flagging disconnect\n", + tm->topic_id, tm->lcm_channel.c_str(), strerror(errno)); + b.serial_connected.store(false); + } +} + +static void lcm_handler_thread(Bridge &b) +{ + while (b.running.load() && b.serial_connected.load()) { + int ret = b.lcm->handleTimeout(100); /* 100ms timeout */ + if (ret < 0) { + /* LCM is sick (multicast group went away, kernel socket + * broke, etc.) — cycling the serial port would not help and + * would unnecessarily discard in-flight data on a link that + * is still perfectly healthy. Instead, clear `running` so + * the main loop exits with a non-zero status and the + * coordinator can restart the whole bridge subprocess on a + * fresh LCM instance. */ + fprintf(stderr, "[bridge] LCM handle error — exiting so coordinator can restart\n"); + b.lcm_failed.store(true); + b.running.store(false); + break; + } + } +} + +/* ====================================================================== + * Signal handling + * ====================================================================== */ + +static void signal_handler(int /*sig*/) +{ + if (g_bridge) g_bridge->running.store(false); +} + +/* Sleep for at most `seconds`, waking early if `running` is cleared. */ +static void interruptible_sleep(Bridge &b, float seconds) +{ + const int step_ms = 50; + const int total_ms = (int)(seconds * 1000.0f); + int elapsed = 0; + while (elapsed < total_ms && b.running.load()) { + std::this_thread::sleep_for(std::chrono::milliseconds(step_ms)); + elapsed += step_ms; + } +} + +/* ====================================================================== + * Main + * ====================================================================== */ + +int main(int argc, char **argv) +{ + Bridge bridge; + g_bridge = &bridge; + + parse_args(bridge, argc, argv); + + if (bridge.serial_port.empty()) { + fprintf(stderr, "Usage: arduino_bridge --serial_port --baudrate " + "[--topic_out ] [--topic_in ] ...\n"); + return 1; + } + + /* Initialize hash registry and validate topic message types */ + init_hash_registry(bridge); + if (!validate_topic_types(bridge)) { + return 1; + } + + /* Build lookup maps — use the unique_ptr-owned storage so raw pointers + * into the vector remain valid. */ + for (auto &tm : bridge.topics) { + if (tm->is_output) { + bridge.topic_out_map[tm->topic_id] = tm.get(); + } else { + bridge.topic_in_map[tm->lcm_channel] = tm.get(); + } + } + + /* Signal handlers */ + signal(SIGTERM, signal_handler); + signal(SIGINT, signal_handler); + + /* Init LCM */ + lcm::LCM lcm; + if (!lcm.good()) { + fprintf(stderr, "[bridge] LCM init failed\n"); + return 1; + } + bridge.lcm = &lcm; + + /* Subscribe to inbound LCM topics */ + for (auto &tm : bridge.topics) { + if (!tm->is_output) { + auto handler = std::make_unique(&bridge, tm.get()); + lcm.subscribe(tm->lcm_channel, &RawHandler::handle, handler.get()); + bridge.raw_handlers.push_back(std::move(handler)); + printf("[bridge] Subscribed LCM→Serial: topic %u ← %s\n", + tm->topic_id, tm->lcm_channel.c_str()); + } else { + printf("[bridge] Serial→LCM: topic %u → %s\n", + tm->topic_id, tm->lcm_channel.c_str()); + } + } + + /* Open serial port */ + printf("[bridge] Opening %s at %d baud\n", bridge.serial_port.c_str(), bridge.baudrate); + + while (bridge.running.load()) { + bridge.serial_fd = serial_open(bridge.serial_port, bridge.baudrate); + if (bridge.serial_fd < 0) { + if (!bridge.reconnect) return 1; + fprintf(stderr, "[bridge] Retrying in %.1fs...\n", bridge.reconnect_interval); + interruptible_sleep(bridge, bridge.reconnect_interval); + continue; + } + + printf("[bridge] Serial port opened (fd=%d)\n", bridge.serial_fd); + bridge.serial_connected.store(true); + + /* Start threads */ + std::thread reader([&bridge] { serial_reader_thread(bridge); }); + std::thread lcm_thread([&bridge] { lcm_handler_thread(bridge); }); + + /* Wait for reader to exit (serial disconnect or shutdown) */ + reader.join(); + + /* Reader bailed — ensure connectivity flag is false and join LCM. */ + bridge.serial_connected.store(false); + lcm_thread.join(); + + serial_close(bridge.serial_fd); + bridge.serial_fd = -1; + + if (!bridge.reconnect || !bridge.running.load()) break; + + /* Reconnect. DO NOT touch `running` here — only the signal handler + * clears it, and we don't want to overwrite a ^C that arrives during + * the backoff sleep. */ + printf("[bridge] Disconnected, reconnecting in %.1fs...\n", bridge.reconnect_interval); + interruptible_sleep(bridge, bridge.reconnect_interval); + } + + printf("[bridge] Shutting down\n"); + /* Distinguish graceful shutdown (SIGTERM/SIGINT) from an LCM failure + * that forced us out of the main loop. Non-zero exit tells the + * coordinator to restart us with a fresh LCM subscriber. */ + return bridge.lcm_failed.load() ? 1 : 0; +} diff --git a/dimos/hardware/arduino/examples/arduino_multi_echo/module.py b/dimos/hardware/arduino/examples/arduino_multi_echo/module.py new file mode 100644 index 0000000000..a6717cea9b --- /dev/null +++ b/dimos/hardware/arduino/examples/arduino_multi_echo/module.py @@ -0,0 +1,52 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Multi-type echo ArduinoModule for hardware round-trip testing. + +Echoes Bool, Int32, Vector3, and Quaternion messages to validate +serialization correctness and float64→float32 precision on AVR. +""" + +from __future__ import annotations + +from dimos.core.arduino_module import ArduinoModule, ArduinoModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.std_msgs.Bool import Bool +from dimos.msgs.std_msgs.Int32 import Int32 + + +class MultiEchoConfig(ArduinoModuleConfig): + sketch_path: str = "sketch/sketch.ino" + board_fqbn: str = "arduino:avr:uno" + baudrate: int = 115200 + + +class MultiEcho(ArduinoModule): + """Arduino that echoes multiple message types back.""" + + config: MultiEchoConfig + + bool_in: In[Bool] + bool_out: Out[Bool] + + int32_in: In[Int32] + int32_out: Out[Int32] + + vec3_in: In[Vector3] + vec3_out: Out[Vector3] + + quat_in: In[Quaternion] + quat_out: Out[Quaternion] diff --git a/dimos/hardware/arduino/examples/arduino_multi_echo/sketch/sketch.ino b/dimos/hardware/arduino/examples/arduino_multi_echo/sketch/sketch.ino new file mode 100644 index 0000000000..d46eb23f7f --- /dev/null +++ b/dimos/hardware/arduino/examples/arduino_multi_echo/sketch/sketch.ino @@ -0,0 +1,60 @@ +/* + * Multi-type Echo — DimOS Arduino hardware test sketch. + * + * Echoes Bool, Int32, Vector3, and Quaternion messages back to the host. + * Used to validate round-trip serialization correctness across multiple + * message types, including float64→float32 precision on AVR. + */ + +#include "dimos_arduino.h" +#include + +/* --- Bool echo --- */ +void on_bool(const char *ch, const void *msg, void *ctx) { + (void)ch; (void)ctx; + const dimos_msg__Bool *m = (const dimos_msg__Bool *)msg; + uint8_t buf[1]; + int n = dimos_msg__Bool__encode(buf, 0, sizeof(buf), m); + if (n > 0) dimos_publish(DIMOS_CHANNEL__BOOL_OUT, &dimos_msg__Bool__type, buf, n); +} + +/* --- Int32 echo --- */ +void on_int32(const char *ch, const void *msg, void *ctx) { + (void)ch; (void)ctx; + const dimos_msg__Int32 *m = (const dimos_msg__Int32 *)msg; + uint8_t buf[4]; + int n = dimos_msg__Int32__encode(buf, 0, sizeof(buf), m); + if (n > 0) dimos_publish(DIMOS_CHANNEL__INT32_OUT, &dimos_msg__Int32__type, buf, n); +} + +/* --- Vector3 echo --- */ +void on_vec3(const char *ch, const void *msg, void *ctx) { + (void)ch; (void)ctx; + const dimos_msg__Vector3 *m = (const dimos_msg__Vector3 *)msg; + uint8_t buf[24]; + int n = dimos_msg__Vector3__encode(buf, 0, sizeof(buf), m); + if (n > 0) dimos_publish(DIMOS_CHANNEL__VEC3_OUT, &dimos_msg__Vector3__type, buf, n); +} + +/* --- Quaternion echo --- */ +void on_quat(const char *ch, const void *msg, void *ctx) { + (void)ch; (void)ctx; + const dimos_msg__Quaternion *m = (const dimos_msg__Quaternion *)msg; + uint8_t buf[32]; + int n = dimos_msg__Quaternion__encode(buf, 0, sizeof(buf), m); + if (n > 0) dimos_publish(DIMOS_CHANNEL__QUAT_OUT, &dimos_msg__Quaternion__type, buf, n); +} + +void setup() { + dimos_init(DIMOS_BAUDRATE); + dimos_subscribe(DIMOS_CHANNEL__BOOL_IN, &dimos_msg__Bool__type, on_bool, NULL); + dimos_subscribe(DIMOS_CHANNEL__INT32_IN, &dimos_msg__Int32__type, on_int32, NULL); + dimos_subscribe(DIMOS_CHANNEL__VEC3_IN, &dimos_msg__Vector3__type, on_vec3, NULL); + dimos_subscribe(DIMOS_CHANNEL__QUAT_IN, &dimos_msg__Quaternion__type, on_quat, NULL); + DimosSerial.println("MultiEcho ready"); +} + +void loop() { + dimos_handle(10); + _delay_ms(1); +} diff --git a/dimos/hardware/arduino/examples/arduino_multi_echo/test_multi_echo_hardware.py b/dimos/hardware/arduino/examples/arduino_multi_echo/test_multi_echo_hardware.py new file mode 100644 index 0000000000..d3b83de201 --- /dev/null +++ b/dimos/hardware/arduino/examples/arduino_multi_echo/test_multi_echo_hardware.py @@ -0,0 +1,346 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Hardware round-trip test for multiple Arduino message types. + +Sends Bool, Int32, Vector3, and Quaternion messages to a physical Arduino +and verifies the echoed values. Includes float64->float32 precision checks +for types with double fields (Vector3, Quaternion). + +Requires: + - Arduino Uno connected via USB + - nix on PATH + +Run: + uv run python dimos/hardware/arduino/examples/arduino_multi_echo/test_multi_echo_hardware.py +""" + +from __future__ import annotations + +import struct +import sys +import threading +import time +from typing import Any + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.coordination.module_coordinator import ModuleCoordinator +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.hardware.arduino.examples.arduino_multi_echo.module import MultiEcho +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.std_msgs.Bool import Bool +from dimos.msgs.std_msgs.Int32 import Int32 +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# Float64 values chosen to exercise precision loss on AVR (double = float32). +# These have more than 7 significant digits, so truncation to float32 is +# guaranteed but should remain within float32 representable range. +F64_TEST_VALUES = [ + 3.141592653589793, # pi + 2.718281828459045, # e + -0.123456789012345, # negative, many decimals + 1.0000001192092896, # just above 1.0 + float32 epsilon + 100000.015625, # large integer part + fractional + 1e-7, # small magnitude +] + +BOOL_TESTS = [True, False, True] +INT32_TESTS = [0, 1, -1, 42, 2147483647, -2147483648] + + +class TestHarnessConfig(ModuleConfig): + pass + + +class TestHarness(Module): + """Sends test messages and collects echoes via internal thread.""" + + config: TestHarnessConfig + + bool_out: Out[Bool] + bool_in: In[Bool] + int32_out: Out[Int32] + int32_in: In[Int32] + vec3_out: Out[Vector3] + vec3_in: In[Vector3] + quat_out: Out[Quaternion] + quat_in: In[Quaternion] + + _bool_echoes: list[bool] + _int32_echoes: list[int] + _vec3_echoes: list[tuple[float, float, float]] + _quat_echoes: list[tuple[float, float, float, float]] + _lock: Any # threading.Lock + _done: bool + + @rpc + def start(self) -> None: + super().start() + self._bool_echoes = [] + self._int32_echoes = [] + self._vec3_echoes = [] + self._quat_echoes = [] + self._lock = threading.Lock() + self._done = False + + self.bool_in.subscribe(self._on_bool) + self.int32_in.subscribe(self._on_int32) + self.vec3_in.subscribe(self._on_vec3) + self.quat_in.subscribe(self._on_quat) + + # Start sending in a background thread + threading.Thread(target=self._send_all, daemon=True).start() + + def _on_bool(self, msg: Bool) -> None: + with self._lock: + self._bool_echoes.append(msg.data) + + def _on_int32(self, msg: Int32) -> None: + with self._lock: + self._int32_echoes.append(msg.data) + + def _on_vec3(self, msg: Vector3) -> None: + with self._lock: + self._vec3_echoes.append((msg.x, msg.y, msg.z)) + + def _on_quat(self, msg: Quaternion) -> None: + with self._lock: + self._quat_echoes.append((msg.x, msg.y, msg.z, msg.w)) + + def _send_all(self) -> None: + """Send all test messages with small gaps.""" + # Wait for bridge to be ready + time.sleep(2) + + for val in BOOL_TESTS: + self.bool_out.publish(Bool(data=val)) + time.sleep(0.2) + + for val in INT32_TESTS: + self.int32_out.publish(Int32(data=val)) + time.sleep(0.2) + + vec3_tests = [ + Vector3(F64_TEST_VALUES[0], F64_TEST_VALUES[1], F64_TEST_VALUES[2]), + Vector3(F64_TEST_VALUES[3], F64_TEST_VALUES[4], F64_TEST_VALUES[5]), + Vector3(0.0, 0.0, 0.0), + ] + for vec in vec3_tests: + self.vec3_out.publish(vec) + time.sleep(0.2) + + quat_tests = [ + Quaternion( + F64_TEST_VALUES[0], + F64_TEST_VALUES[1], + F64_TEST_VALUES[2], + F64_TEST_VALUES[3], + ), + Quaternion(0.0, 0.0, 0.0, 1.0), + Quaternion( + F64_TEST_VALUES[4], + F64_TEST_VALUES[5], + -F64_TEST_VALUES[0], + F64_TEST_VALUES[1], + ), + ] + for q in quat_tests: + self.quat_out.publish(q) + time.sleep(0.2) + + time.sleep(2) + with self._lock: + self._done = True + logger.info("All test messages sent") + + @rpc + def get_results(self) -> dict[str, Any]: + """Return collected echo data for validation.""" + with self._lock: + return { + "done": self._done, + "bool_echoes": list(self._bool_echoes), + "int32_echoes": list(self._int32_echoes), + "vec3_echoes": list(self._vec3_echoes), + "quat_echoes": list(self._quat_echoes), + } + + +def float64_to_float32(val: float) -> float: + """Round-trip a float64 through float32 representation.""" + return struct.unpack("f", struct.pack("f", val))[0] + + +def validate_results(results: dict[str, Any]) -> bool: + passed = True + + # Bool checks + print(f"\n{'=' * 60}") + print("BOOL ECHO TEST") + print(f"{'=' * 60}") + bool_echoes = results["bool_echoes"] + if len(bool_echoes) >= len(BOOL_TESTS): + for sent, got in zip(BOOL_TESTS, bool_echoes, strict=False): + status = "OK" if sent == got else "FAIL" + print(f" [{status}] sent={sent} got={got}") + if sent != got: + passed = False + else: + print(f" [FAIL] Expected {len(BOOL_TESTS)} echoes, got {len(bool_echoes)}") + passed = False + + # Int32 checks + print(f"\n{'=' * 60}") + print("INT32 ECHO TEST") + print(f"{'=' * 60}") + int32_echoes = results["int32_echoes"] + if len(int32_echoes) >= len(INT32_TESTS): + for sent, got in zip(INT32_TESTS, int32_echoes, strict=False): + status = "OK" if sent == got else "FAIL" + print(f" [{status}] sent={sent} got={got}") + if sent != got: + passed = False + else: + print(f" [FAIL] Expected {len(INT32_TESTS)} echoes, got {len(int32_echoes)}") + passed = False + + # Vector3 checks + print(f"\n{'=' * 60}") + print("VECTOR3 ECHO TEST (float64 -> float32 precision)") + print(f"{'=' * 60}") + vec3_tests = [ + (F64_TEST_VALUES[0], F64_TEST_VALUES[1], F64_TEST_VALUES[2]), + (F64_TEST_VALUES[3], F64_TEST_VALUES[4], F64_TEST_VALUES[5]), + (0.0, 0.0, 0.0), + ] + vec3_echoes = results["vec3_echoes"] + if len(vec3_echoes) >= len(vec3_tests): + for i, (sent, got) in enumerate(zip(vec3_tests, vec3_echoes, strict=False)): + for axis, (s, g) in zip("xyz", zip(sent, got, strict=False), strict=False): + expected_f32 = float64_to_float32(s) + abs_err = abs(g - expected_f32) + tol = max(abs(expected_f32) * 1.2e-7, 1e-45) + status = "OK" if abs_err <= tol else "FAIL" + precision_lost = abs(s - g) + print( + f" [{status}] vec[{i}].{axis}: " + f"sent_f64={s:.15g} expected_f32={expected_f32:.8g} " + f"got={g:.8g} err_vs_f32={abs_err:.2e} " + f"total_precision_loss={precision_lost:.2e}" + ) + if abs_err > tol: + passed = False + else: + print(f" [FAIL] Expected {len(vec3_tests)} echoes, got {len(vec3_echoes)}") + passed = False + + # Quaternion checks + print(f"\n{'=' * 60}") + print("QUATERNION ECHO TEST (float64 -> float32 precision)") + print(f"{'=' * 60}") + quat_tests = [ + (F64_TEST_VALUES[0], F64_TEST_VALUES[1], F64_TEST_VALUES[2], F64_TEST_VALUES[3]), + (0.0, 0.0, 0.0, 1.0), + (F64_TEST_VALUES[4], F64_TEST_VALUES[5], -F64_TEST_VALUES[0], F64_TEST_VALUES[1]), + ] + quat_echoes = results["quat_echoes"] + if len(quat_echoes) >= len(quat_tests): + for i, (sent, got) in enumerate(zip(quat_tests, quat_echoes, strict=False)): + for axis, (s, g) in zip("xyzw", zip(sent, got, strict=False), strict=False): + expected_f32 = float64_to_float32(s) + abs_err = abs(g - expected_f32) + tol = max(abs(expected_f32) * 1.2e-7, 1e-45) + status = "OK" if abs_err <= tol else "FAIL" + precision_lost = abs(s - g) + print( + f" [{status}] quat[{i}].{axis}: " + f"sent_f64={s:.15g} expected_f32={expected_f32:.8g} " + f"got={g:.8g} err_vs_f32={abs_err:.2e} " + f"total_precision_loss={precision_lost:.2e}" + ) + if abs_err > tol: + passed = False + else: + print(f" [FAIL] Expected {len(quat_tests)} echoes, got {len(quat_echoes)}") + passed = False + + print(f"\n{'=' * 60}") + if passed: + print("ALL TESTS PASSED") + else: + print("SOME TESTS FAILED") + print(f"{'=' * 60}") + return passed + + +def main() -> None: + bp = ( + autoconnect( + TestHarness.blueprint(), + MultiEcho.blueprint(virtual=False), + ) + .remappings( + [ + (TestHarness, "bool_out", "bool_cmd"), + (MultiEcho, "bool_in", "bool_cmd"), + (MultiEcho, "bool_out", "bool_echo"), + (TestHarness, "bool_in", "bool_echo"), + (TestHarness, "int32_out", "int32_cmd"), + (MultiEcho, "int32_in", "int32_cmd"), + (MultiEcho, "int32_out", "int32_echo"), + (TestHarness, "int32_in", "int32_echo"), + (TestHarness, "vec3_out", "vec3_cmd"), + (MultiEcho, "vec3_in", "vec3_cmd"), + (MultiEcho, "vec3_out", "vec3_echo"), + (TestHarness, "vec3_in", "vec3_echo"), + (TestHarness, "quat_out", "quat_cmd"), + (MultiEcho, "quat_in", "quat_cmd"), + (MultiEcho, "quat_out", "quat_echo"), + (TestHarness, "quat_in", "quat_echo"), + ] + ) + .global_config(n_workers=2) + ) + + coord = ModuleCoordinator.build(bp) + + harness = coord.get_instance(TestHarness) + assert harness is not None, "TestHarness not found in coordinator" + + # Poll until the harness says it's done + deadline = time.time() + 60 + results: dict[str, Any] = {} + while time.time() < deadline: + results = harness.get_results() + if results.get("done"): + break + time.sleep(1) + + coord.stop() + + if not results.get("done"): + print("FAIL: Test harness did not finish within 30 seconds") + sys.exit(1) + + ok = validate_results(results) + sys.exit(0 if ok else 1) + + +if __name__ == "__main__": + main() diff --git a/dimos/hardware/arduino/examples/arduino_twist_echo/blueprint.py b/dimos/hardware/arduino/examples/arduino_twist_echo/blueprint.py new file mode 100644 index 0000000000..70b83e383f --- /dev/null +++ b/dimos/hardware/arduino/examples/arduino_twist_echo/blueprint.py @@ -0,0 +1,54 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Blueprint: virtual Arduino TwistEcho + a test publisher. + +Run with: + dimos run arduino-twist-echo-virtual + +Or, since this lives outside the auto-discovery path, run by importing +this module's variable from a script. +""" + +from __future__ import annotations + +from dimos.core.coordination.blueprints import autoconnect +from dimos.hardware.arduino.examples.arduino_twist_echo.module import TwistEcho +from dimos.hardware.arduino.examples.arduino_twist_echo.test_publisher import ( + TestPublisher, +) + +# Two modules wired by autoconnect via stream name+type matching: +# TestPublisher.cmd_out (Out[Twist]) ──┐ +# TwistEcho.twist_in (In[Twist]) ◀──┘ via remapping +# +# TwistEcho.twist_echo_out (Out[Twist]) ──┐ +# TestPublisher.echo_in (In[Twist]) ◀─┘ via remapping +arduino_twist_echo_virtual = ( + autoconnect( + TestPublisher.blueprint(publish_period_s=0.5), + TwistEcho.blueprint(virtual=True), + ) + .remappings( + [ + # TestPublisher.cmd_out → TwistEcho.twist_in + (TestPublisher, "cmd_out", "twist_command"), + (TwistEcho, "twist_in", "twist_command"), + # TwistEcho.twist_echo_out → TestPublisher.echo_in + (TwistEcho, "twist_echo_out", "twist_echo"), + (TestPublisher, "echo_in", "twist_echo"), + ] + ) + .global_config(n_workers=2) +) diff --git a/dimos/hardware/arduino/examples/arduino_twist_echo/module.py b/dimos/hardware/arduino/examples/arduino_twist_echo/module.py new file mode 100644 index 0000000000..57a9681462 --- /dev/null +++ b/dimos/hardware/arduino/examples/arduino_twist_echo/module.py @@ -0,0 +1,47 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Example ArduinoModule: receives Twist commands, echoes them back. + +Demonstrates bidirectional communication between DimOS and an Arduino. +The Arduino receives Twist commands on ``twist_in`` and echoes them +back on ``twist_echo_out``. +""" + +from __future__ import annotations + +from dimos.core.arduino_module import ArduinoModule, ArduinoModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.Twist import Twist + + +class TwistEchoConfig(ArduinoModuleConfig): + sketch_path: str = "sketch/sketch.ino" + board_fqbn: str = "arduino:avr:uno" + baudrate: int = 115200 + + # Custom config value — embedded as #define DIMOS_ECHO_DELAY_MS 50 + echo_delay_ms: int = 50 + + +class TwistEcho(ArduinoModule): + """Arduino that echoes received Twist commands back.""" + + config: TwistEchoConfig + + # DimOS sends Twist commands to the Arduino + twist_in: In[Twist] + + # Arduino echoes them back + twist_echo_out: Out[Twist] diff --git a/dimos/hardware/arduino/examples/arduino_twist_echo/sketch/sketch.ino b/dimos/hardware/arduino/examples/arduino_twist_echo/sketch/sketch.ino new file mode 100644 index 0000000000..a1f1796dd5 --- /dev/null +++ b/dimos/hardware/arduino/examples/arduino_twist_echo/sketch/sketch.ino @@ -0,0 +1,57 @@ +/* + * Twist Echo — Example DimOS Arduino sketch. + * + * Receives Twist commands from the host, echoes them back. + * Demonstrates: + * - dimos_init() / dimos_subscribe() / dimos_handle() + * - Typed callbacks matching C++ LCM style + * - Using generated type descriptors for subscribe/publish + * - DimosSerial.println() going through the DSP debug channel + * - Config values available as #defines + * + * NOTE: We use _delay_ms() from instead of Arduino's delay() + * because delay() relies on timer 0 interrupts which don't fire in QEMU's + * AVR model. _delay_ms is a pure busy loop and works in any simulator. + */ + +#include "dimos_arduino.h" +#include + +/* Shared state — accessible from callback */ +dimos_msg__Twist last_twist; +uint32_t msg_count = 0; + +void on_twist(const char *channel, const void *msg, void *ctx) { + (void)channel; + (void)ctx; + const dimos_msg__Twist *twist = (const dimos_msg__Twist *)msg; + + /* Copy to shared state */ + last_twist = *twist; + msg_count++; + + DimosSerial.print("Got twist #"); + DimosSerial.print(msg_count); + DimosSerial.print(": linear.x="); + DimosSerial.println(twist->linear.x); + + /* Echo it back */ + uint8_t buf[48]; + int encoded = dimos_msg__Twist__encode(buf, 0, sizeof(buf), twist); + if (encoded > 0) { + dimos_publish(DIMOS_CHANNEL__TWIST_ECHO_OUT, + &dimos_msg__Twist__type, buf, encoded); + } +} + +void setup() { + dimos_init(DIMOS_BAUDRATE); + dimos_subscribe(DIMOS_CHANNEL__TWIST_IN, + &dimos_msg__Twist__type, on_twist, NULL); + DimosSerial.println("TwistEcho ready"); +} + +void loop() { + dimos_handle(10); + _delay_ms(1); +} diff --git a/dimos/hardware/arduino/examples/arduino_twist_echo/test_e2e_virtual.py b/dimos/hardware/arduino/examples/arduino_twist_echo/test_e2e_virtual.py new file mode 100644 index 0000000000..9d241cd72e --- /dev/null +++ b/dimos/hardware/arduino/examples/arduino_twist_echo/test_e2e_virtual.py @@ -0,0 +1,137 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""End-to-end test for the virtual-Arduino path. + +Boots the ``arduino_twist_echo_virtual`` blueprint — which spins up a +``TestPublisher`` module, compiles the sketch via ``arduino-cli``, +launches ``qemu-system-avr`` with the ELF, and runs the ``arduino_bridge`` +subprocess — then waits for several Twist messages to round-trip +``TestPublisher.cmd_out → twist_command → QEMU → twist_echo → TestPublisher.echo_in``. + +This is the only test that actually exercises QEMU + the bridge binary + +a compiled AVR sketch as one unit, so it's the primary regression guard +for the ``virtual=True`` runtime feature. + +Requirements: + - ``nix`` must be on PATH. ``ArduinoModule`` resolves ``arduino-cli``, + ``avrdude``, and ``qemu-system-avr`` through the ``dimos_arduino_tools`` + flake output, so only ``nix`` itself needs to be installed — the + Arduino toolchain is materialized on demand via ``nix build``. + +Run with:: + + uv run pytest dimos/hardware/arduino/examples/arduino_twist_echo/test_e2e_virtual.py -v -m "slow and tool" +""" + +from __future__ import annotations + +import shutil +import time +from types import MappingProxyType + +import pytest + +# Heavy imports are deferred past the skip check so a machine without the +# Arduino toolchain can still collect this file cheaply. +pytestmark = [pytest.mark.slow, pytest.mark.tool] + + +def _missing_binaries() -> list[str]: + # Only `nix` is required on PATH; the rest of the toolchain + # (arduino-cli, avrdude, qemu-system-avr) is resolved through the + # arduino flake's `dimos_arduino_tools` package inside + # ``ArduinoModule`` — no user-facing `nix develop` required. + return ["nix"] if shutil.which("nix") is None else [] + + +# Budget for the full pipeline: first-run `nix build` can be minutes on a +# cold store, sketch compile ~30s, QEMU boot ~1s. We poll for echoes up to +# this total wall-clock time before giving up. +_BUILD_AND_RUN_TIMEOUT_S = 300.0 +# After the pipeline is running, how many echoes we require before calling +# the test a pass. Three gives us confidence that it's not a fluke of the +# first publish happening to race the bridge startup. +_REQUIRED_ECHOES = 3 + + +def test_virtual_arduino_round_trip() -> None: + """Full coordinator + QEMU + bridge round-trip. + + Passes iff ``TestPublisher.echo_count()`` reaches ``_REQUIRED_ECHOES`` + within the timeout, which can only happen if all of these worked: + + 1. ``ArduinoModule.build()`` generated ``dimos_arduino.h`` correctly + 2. ``arduino-cli`` compiled the sketch against the Arduino LCM headers + 3. ``nix build .#arduino_bridge`` produced the bridge binary + 4. ``qemu-system-avr`` booted the ELF and announced a PTY + 5. The bridge opened the PTY, resolved fingerprints, and subscribed to LCM + 6. ``TestPublisher`` published a Twist on ``twist_command`` + 7. The bridge serialized it into a DSP frame and wrote it to the PTY + 8. The AVR sketch decoded it with the Arduino-side C helpers + 9. The sketch re-encoded the Twist and sent it back on the echo topic + 10. The bridge parsed the inbound frame, prepended the LCM fingerprint, + and published it on ``twist_echo`` + 11. ``TestPublisher.echo_in`` received and counted it + """ + if _missing_binaries(): + pytest.skip( + "Virtual Arduino e2e test requires `nix` on PATH — the rest of " + "the Arduino toolchain (arduino-cli, avrdude, qemu-system-avr) " + "is fetched via `nix build .#dimos_arduino_tools` inside " + "ArduinoModule. Install Nix and re-run." + ) + + # Deferred imports — pulling in ModuleCoordinator spins up a lot of + # machinery, no reason to pay for it on a skip. + from dimos.core.coordination.module_coordinator import ModuleCoordinator + from dimos.hardware.arduino.examples.arduino_twist_echo.blueprint import ( + arduino_twist_echo_virtual, + ) + from dimos.hardware.arduino.examples.arduino_twist_echo.test_publisher import ( + TestPublisher, + ) + + # Disable Rerun so tests don't try to launch a viewer. + build_overrides = MappingProxyType({"g": {"viewer": "none"}}) + + coordinator = ModuleCoordinator.build(arduino_twist_echo_virtual, build_overrides.copy()) + try: + publisher = coordinator.get_instance(TestPublisher) + assert publisher is not None, ( + "TestPublisher not found in coordinator — blueprint wiring regressed" + ) + + deadline = time.monotonic() + _BUILD_AND_RUN_TIMEOUT_S + last_count = 0 + while time.monotonic() < deadline: + count = publisher.echo_count() + if count != last_count: + # Useful signal when running -v: shows echoes trickling in + # during the wait, so a human watching can tell whether it's + # the build or the runtime that's slow. + print(f"[e2e] echoes received: {count}") + last_count = count + if count >= _REQUIRED_ECHOES: + return + time.sleep(0.25) + + pytest.fail( + f"Virtual Arduino round-trip did not reach {_REQUIRED_ECHOES} echoes " + f"within {_BUILD_AND_RUN_TIMEOUT_S:.0f}s (got {last_count}). " + f"Check that the bridge built, the sketch compiled, and QEMU booted — " + f"inspect the coordinator logs for details." + ) + finally: + coordinator.stop() diff --git a/dimos/hardware/arduino/examples/arduino_twist_echo/test_publisher.py b/dimos/hardware/arduino/examples/arduino_twist_echo/test_publisher.py new file mode 100644 index 0000000000..4db71fe2ce --- /dev/null +++ b/dimos/hardware/arduino/examples/arduino_twist_echo/test_publisher.py @@ -0,0 +1,123 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""TestPublisher: emits a Twist every 500ms, listens for echoes.""" + +from __future__ import annotations + +import threading +from typing import Any + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class TestPublisherConfig(ModuleConfig): + publish_period_s: float = 0.5 + + +class TestPublisher(Module): + """Publishes a Twist on ``cmd_out`` every publish_period_s, prints any echo + received on ``echo_in``.""" + + config: TestPublisherConfig + + cmd_out: Out[Twist] + echo_in: In[Twist] + + _thread: threading.Thread | None = None + _stop_event: threading.Event | None = None + _counter: int = 0 + _echo_count: int = 0 + # `threading.Lock` is a factory function, not a class, so + # `threading.Lock | None` fails at runtime when pydantic evaluates + # type hints via `get_type_hints`. Use `Any` so the annotation is + # accepted but still carries the type intent in the comment. + _echo_lock: Any = None # actually threading.Lock | None + + @rpc + def start(self) -> None: + super().start() + self._stop_event = threading.Event() + self._echo_lock = threading.Lock() + self.echo_in.subscribe(self._on_echo) + self._thread = threading.Thread(target=self._publish_loop, daemon=True) + self._thread.start() + logger.info("TestPublisher started") + + @rpc + def echo_count(self) -> int: + """Return the number of echoes received since start(). + + Exposed as an RPC so e2e tests running in the host process can + observe progress of the QEMU-backed round-trip without having to + subscribe to LCM directly. + """ + if self._echo_lock is None: + return 0 + with self._echo_lock: + return self._echo_count + + @rpc + def stop(self) -> None: + # Signal the loop via Event so it wakes from the period sleep + # immediately instead of waiting up to `publish_period_s`. + if self._stop_event is not None: + self._stop_event.set() + if self._thread is not None: + # Give the loop one full period plus a small grace window to + # exit cleanly. + join_timeout = max(2.0, self.config.publish_period_s + 0.5) + self._thread.join(timeout=join_timeout) + if self._thread.is_alive(): + logger.warning("TestPublisher thread did not exit in time") + super().stop() + + def _publish_loop(self) -> None: + assert self._stop_event is not None + while not self._stop_event.is_set(): + self._counter += 1 + twist = Twist( + linear=Vector3(self._counter * 0.1, 0.0, 0.0), + angular=Vector3(0.0, 0.0, self._counter * 0.05), + ) + self.cmd_out.publish(twist) + logger.info( + "Published twist", + n=self._counter, + linear_x=twist.linear.x, + angular_z=twist.angular.z, + ) + # `Event.wait(timeout=...)` returns True when the event is set, + # so stop() wakes the loop immediately instead of waiting for + # the full period. + self._stop_event.wait(timeout=self.config.publish_period_s) + + def _on_echo(self, msg: Twist) -> None: + if self._echo_lock is not None: + with self._echo_lock: + self._echo_count += 1 + logger.info( + "Received echo", + linear_x=msg.linear.x, + linear_y=msg.linear.y, + linear_z=msg.linear.z, + angular_z=msg.angular.z, + ) diff --git a/dimos/hardware/arduino/flake.lock b/dimos/hardware/arduino/flake.lock new file mode 100644 index 0000000000..64596fa6dc --- /dev/null +++ b/dimos/hardware/arduino/flake.lock @@ -0,0 +1,148 @@ +{ + "nodes": { + "dimos-lcm": { + "flake": false, + "locked": { + "lastModified": 1776561835, + "narHash": "sha256-aNYGV0l/gXP94/ppzEeYjPo3Yzl2NbhehT5JU5/C9go=", + "owner": "dimensionalOS", + "repo": "dimos-lcm", + "rev": "9300bae7f7b18e531c5d3ccc25b082a6107161da", + "type": "github" + }, + "original": { + "owner": "dimensionalOS", + "ref": "jeff/feat/arduino", + "repo": "dimos-lcm", + "type": "github" + } + }, + "flake-utils": { + "inputs": { + "systems": "systems" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "flake-utils_2": { + "inputs": { + "systems": "systems_2" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "lcm-extended": { + "inputs": { + "flake-utils": "flake-utils_2", + "nixpkgs": "nixpkgs" + }, + "locked": { + "lastModified": 1774902379, + "narHash": "sha256-gRFvEkbXCEoG4jEmsT+i0bMZ5kDHOtAaPsrbStXjdu4=", + "owner": "jeff-hykin", + "repo": "lcm_extended", + "rev": "7d12ad8546d3daae30528a6c28f2c9ff5b10baf7", + "type": "github" + }, + "original": { + "owner": "jeff-hykin", + "repo": "lcm_extended", + "type": "github" + } + }, + "nixpkgs": { + "locked": { + "lastModified": 1774709303, + "narHash": "sha256-D3Q07BbIA2KnTcSXIqqu9P586uWxN74zNoCH3h2ESHg=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "8110df5ad7abf5d4c0f6fb0f8f978390e77f9685", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixos-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "nixpkgs_2": { + "locked": { + "lastModified": 1775710090, + "narHash": "sha256-ar3rofg+awPB8QXDaFJhJ2jJhu+KqN/PRCXeyuXR76E=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "4c1018dae018162ec878d42fec712642d214fdfa", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixos-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "dimos-lcm": "dimos-lcm", + "flake-utils": "flake-utils", + "lcm-extended": "lcm-extended", + "nixpkgs": "nixpkgs_2" + } + }, + "systems": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + }, + "systems_2": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/dimos/hardware/arduino/flake.nix b/dimos/hardware/arduino/flake.nix new file mode 100644 index 0000000000..e13a1fd8ee --- /dev/null +++ b/dimos/hardware/arduino/flake.nix @@ -0,0 +1,108 @@ +{ + description = "DimOS Arduino support — bridge binary + Arduino toolchain"; + + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; + flake-utils.url = "github:numtide/flake-utils"; + dimos-lcm = { + # Pin to jeff/feat/arduino until that branch merges to main. + url = "github:dimensionalOS/dimos-lcm/jeff/feat/arduino"; + flake = false; + }; + # Patched LCM that builds cleanly on macOS (pkg-config + fdatasync + # fixes). On Linux this is identical to upstream pkgs.lcm. + lcm-extended.url = "github:jeff-hykin/lcm_extended"; + }; + + outputs = { self, nixpkgs, flake-utils, dimos-lcm, lcm-extended }: + flake-utils.lib.eachDefaultSystem (system: + let + pkgs = nixpkgs.legacyPackages.${system}; + + # Single-output LCM built on top of lcm_extended. + # We still collapse outputs to a single `out` so downstream + # CMakeLists.txt doesn't have to juggle `lcm` vs `lcm-dev` paths. + lcmFull = (lcm-extended.packages.${system}.lcm).overrideAttrs (old: { + outputs = [ "out" ]; + postInstall = ""; + }); + + # Bundle of external tools ArduinoModule shells out to at + # runtime: the unwrapped arduino-cli Go binary (not the bwrap- + # wrapped `pkgs.arduino-cli`), avrdude for physical uploads, + # and qemu for virtual-Arduino mode (`pkgs.qemu` ships all + # system targets including qemu-system-avr). Exposed as a + # single flake output so `ArduinoModule` can resolve all three + # via one ``nix build .#dimos_arduino_tools`` — the alternative + # is requiring the user to enter ``nix develop`` before + # running their blueprint, which defeats the point of dimos + # being a normal Python library you can import and run. + # On Linux, arduino-cli ships a bwrap-wrapped FHS environment + # that fails on sandboxed hosts. The unwrapped Go binary + # (pureGoPkg) works everywhere. On macOS, pureGoPkg doesn't + # exist — the plain package is already unwrapped. + arduino-cli-unwrapped = + pkgs.arduino-cli.pureGoPkg or pkgs.arduino-cli; + + # Generated Arduino C message headers from dimos-lcm. Packaged + # into dimos_arduino_tools so ArduinoModule can resolve them from + # the same nix store path it already uses for arduino-cli. + arduino_c_msgs = pkgs.runCommand "arduino-c-msgs" {} '' + mkdir -p $out/share/arduino_msgs + cp -r ${dimos-lcm}/generated/arduino_c_msgs/* $out/share/arduino_msgs/ + ''; + + dimos_arduino_tools = pkgs.symlinkJoin { + name = "dimos-arduino-tools"; + paths = [ + arduino-cli-unwrapped + pkgs.avrdude + pkgs.qemu + arduino_c_msgs + ]; + }; + + # The generic serial↔LCM bridge + arduino_bridge = pkgs.stdenv.mkDerivation { + pname = "arduino_bridge"; + version = "0.1.0"; + src = ./.; + + nativeBuildInputs = [ pkgs.cmake pkgs.pkg-config ]; + buildInputs = [ lcmFull pkgs.glib ]; + + cmakeFlags = [ + "-DDIMOS_LCM_DIR=${dimos-lcm}" + ]; + + # CMakeLists.txt is in cpp/ subdirectory + cmakeDir = "../cpp"; + + installPhase = '' + mkdir -p $out/bin + cp arduino_bridge $out/bin/ + ''; + }; + + in { + packages = { + inherit arduino_bridge dimos_arduino_tools; + default = arduino_bridge; + }; + + devShells.default = pkgs.mkShell { + packages = [ + arduino_bridge + # Reuse the same unwrapped binary defined above. + arduino-cli-unwrapped + pkgs.avrdude + pkgs.picocom + # qemu-system-avr for virtual-Arduino mode. `pkgs.qemu` builds + # all system targets including avr and works on darwin + + # linux; it's ~400MB but cached via the public binary cache + # on common platforms so first-time install is the only cost. + pkgs.qemu + ]; + }; + }); +} diff --git a/dimos/hardware/arduino/test/CMakeLists.txt b/dimos/hardware/arduino/test/CMakeLists.txt new file mode 100644 index 0000000000..63ada5583c --- /dev/null +++ b/dimos/hardware/arduino/test/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.16) +project(arduino_wire_compat_test CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# dimos-lcm C++ headers (LCM-generated reference types) +set(DIMOS_LCM_DIR "" CACHE PATH "Path to dimos-lcm repo (contains generated/cpp_lcm_msgs)") +if(NOT DIMOS_LCM_DIR) + message(FATAL_ERROR "Set -DDIMOS_LCM_DIR to the dimos-lcm source directory") +endif() +set(LCM_CPP_MSGS "${DIMOS_LCM_DIR}/generated/cpp_lcm_msgs") + +# Arduino C message headers from dimos-lcm +set(ARDUINO_MSGS "${DIMOS_LCM_DIR}/generated/arduino_c_msgs") +set(ARDUINO_COMMON "${CMAKE_CURRENT_SOURCE_DIR}/../common") + +# LCM library (for lcm_coretypes.h used by the C++ reference types) +find_package(lcm REQUIRED) + +add_executable(test_wire_compat test_wire_compat.cpp) + +target_include_directories(test_wire_compat PRIVATE + ${ARDUINO_MSGS} # C headers from dimos-lcm: geometry_msgs/Vector3.h etc. + ${ARDUINO_COMMON} # dsp_protocol.h, dimos_lcm_serial.h + ${LCM_CPP_MSGS} # LCM C++ reference: geometry_msgs/Vector3.hpp etc. +) + +target_link_libraries(test_wire_compat PRIVATE lcm) + +install(TARGETS test_wire_compat DESTINATION bin) diff --git a/dimos/hardware/arduino/test/flake.lock b/dimos/hardware/arduino/test/flake.lock new file mode 100644 index 0000000000..c40297113a --- /dev/null +++ b/dimos/hardware/arduino/test/flake.lock @@ -0,0 +1,134 @@ +{ + "nodes": { + "dimos-lcm": { + "flake": false, + "locked": { + "lastModified": 1776561835, + "narHash": "sha256-aNYGV0l/gXP94/ppzEeYjPo3Yzl2NbhehT5JU5/C9go=", + "owner": "dimensionalOS", + "repo": "dimos-lcm", + "rev": "9300bae7f7b18e531c5d3ccc25b082a6107161da", + "type": "github" + }, + "original": { + "owner": "dimensionalOS", + "ref": "jeff/feat/arduino", + "repo": "dimos-lcm", + "type": "github" + } + }, + "flake-utils": { + "inputs": { + "systems": "systems" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "flake-utils_2": { + "inputs": { + "systems": "systems_2" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "lcm-extended": { + "inputs": { + "flake-utils": "flake-utils_2", + "nixpkgs": [ + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1774902379, + "narHash": "sha256-gRFvEkbXCEoG4jEmsT+i0bMZ5kDHOtAaPsrbStXjdu4=", + "owner": "jeff-hykin", + "repo": "lcm_extended", + "rev": "7d12ad8546d3daae30528a6c28f2c9ff5b10baf7", + "type": "github" + }, + "original": { + "owner": "jeff-hykin", + "repo": "lcm_extended", + "type": "github" + } + }, + "nixpkgs": { + "locked": { + "lastModified": 1776169885, + "narHash": "sha256-l/iNYDZ4bGOAFQY2q8y5OAfBBtrDAaPuRQqWaFHVRXM=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "4bd9165a9165d7b5e33ae57f3eecbcb28fb231c9", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixos-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "dimos-lcm": "dimos-lcm", + "flake-utils": "flake-utils", + "lcm-extended": "lcm-extended", + "nixpkgs": "nixpkgs" + } + }, + "systems": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + }, + "systems_2": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/dimos/hardware/arduino/test/flake.nix b/dimos/hardware/arduino/test/flake.nix new file mode 100644 index 0000000000..d0fea46a30 --- /dev/null +++ b/dimos/hardware/arduino/test/flake.nix @@ -0,0 +1,53 @@ +{ + description = "Arduino LCM wire format compatibility test"; + + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; + flake-utils.url = "github:numtide/flake-utils"; + dimos-lcm = { + # Pin to jeff/feat/arduino until that branch merges to main. + url = "github:dimensionalOS/dimos-lcm/jeff/feat/arduino"; + flake = false; + }; + lcm-extended = { + url = "github:jeff-hykin/lcm_extended"; + inputs.nixpkgs.follows = "nixpkgs"; + }; + }; + + outputs = { self, nixpkgs, flake-utils, dimos-lcm, lcm-extended }: + flake-utils.lib.eachDefaultSystem (system: + let + pkgs = nixpkgs.legacyPackages.${system}; + lcmFull = (lcm-extended.packages.${system}.default).overrideAttrs (old: { + outputs = [ "out" ]; + postInstall = ""; + }); + in { + packages.default = pkgs.stdenv.mkDerivation { + pname = "test_wire_compat"; + version = "0.1.0"; + # Source is the parent arduino/ directory so we can reach common/ + src = ./..; + + nativeBuildInputs = [ pkgs.cmake pkgs.pkg-config ]; + buildInputs = [ lcmFull pkgs.glib ]; + + cmakeFlags = [ + "-DDIMOS_LCM_DIR=${dimos-lcm}" + ]; + + # CMakeLists.txt is in test/ subdirectory + cmakeDir = "../test"; + + installPhase = '' + mkdir -p $out/bin + cp test_wire_compat $out/bin/ + ''; + }; + + devShells.default = pkgs.mkShell { + inputsFrom = [ self.packages.${system}.default ]; + }; + }); +} diff --git a/dimos/hardware/arduino/test/test_wire_compat.cpp b/dimos/hardware/arduino/test/test_wire_compat.cpp new file mode 100644 index 0000000000..ea8e653970 --- /dev/null +++ b/dimos/hardware/arduino/test/test_wire_compat.cpp @@ -0,0 +1,614 @@ +/* + * test_wire_compat.cpp + * + * Verifies that the Arduino C encode/decode functions produce byte-for-byte + * identical output to the LCM C++ encode (minus the 8-byte fingerprint hash). + * + * Build & run: + * cd dimos/hardware/arduino/test + * nix build && ./result/bin/test_wire_compat + */ + +#include +#include +#include +#include +#include + +/* ---- Arduino C headers (our code under test) ---- */ +#include "std_msgs/Time.h" +#include "std_msgs/Bool.h" +#include "std_msgs/Int32.h" +#include "std_msgs/Float32.h" +#include "std_msgs/Float64.h" +#include "std_msgs/ColorRGBA.h" +#include "geometry_msgs/Vector3.h" +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Point32.h" +#include "geometry_msgs/Quaternion.h" +#include "geometry_msgs/Pose.h" +#include "geometry_msgs/Pose2D.h" +#include "geometry_msgs/Twist.h" +#include "geometry_msgs/Accel.h" +#include "geometry_msgs/Transform.h" +#include "geometry_msgs/Wrench.h" +#include "geometry_msgs/Inertia.h" +#include "geometry_msgs/PoseWithCovariance.h" +#include "geometry_msgs/TwistWithCovariance.h" +#include "geometry_msgs/AccelWithCovariance.h" + +/* + * LCM C++ headers (reference implementation). + * These live in namespace geometry_msgs:: / std_msgs:: already. + * Our C types use geometry_msgs_* prefix so no collision. + * + * The .hpp includes use the system lcm/lcm_coretypes.h (with malloc etc.) + * while our .h includes use lcm_coretypes_arduino.h — both are in scope + * but don't conflict because the upstream header has its own include guard. + */ +#include "geometry_msgs/Vector3.hpp" +#include "geometry_msgs/Point.hpp" +#include "geometry_msgs/Point32.hpp" +#include "geometry_msgs/Quaternion.hpp" +#include "geometry_msgs/Pose.hpp" +#include "geometry_msgs/Pose2D.hpp" +#include "geometry_msgs/Twist.hpp" +#include "geometry_msgs/Accel.hpp" +#include "geometry_msgs/Transform.hpp" +#include "geometry_msgs/Wrench.hpp" +#include "geometry_msgs/Inertia.hpp" +#include "geometry_msgs/PoseWithCovariance.hpp" +#include "geometry_msgs/TwistWithCovariance.hpp" +#include "geometry_msgs/AccelWithCovariance.hpp" +#include "std_msgs/Time.hpp" +#include "std_msgs/Bool.hpp" +#include "std_msgs/Int32.hpp" +#include "std_msgs/Float32.hpp" +#include "std_msgs/Float64.hpp" +#include "std_msgs/ColorRGBA.hpp" + +static int tests_run = 0; +static int tests_passed = 0; + +static void hex_dump(const char *label, const uint8_t *data, int len) { + printf(" %s (%d bytes): ", label, len); + for (int i = 0; i < len && i < 64; i++) + printf("%02x ", data[i]); + if (len > 64) printf("..."); + printf("\n"); +} + +/* + * Compare C-encoded bytes against the LCM C++ _encode_one output. + * + * LCM lcm_encode() prepends an 8-byte fingerprint hash, then calls + * _encode_one() for the fields. We compare against _encode_one() output + * (i.e., the bytes AFTER the hash), since the Arduino side skips the hash. + */ +static bool compare_bytes(const char *name, + const uint8_t *c_buf, int c_len, + const uint8_t *cpp_buf, int cpp_len) +{ + tests_run++; + /* cpp_buf starts with 8-byte hash — skip it */ + const int HASH_SIZE = 8; + int field_len = cpp_len - HASH_SIZE; + const uint8_t *cpp_fields = cpp_buf + HASH_SIZE; + + if (c_len != field_len) { + printf("FAIL %s: size mismatch (C=%d, C++=%d after hash)\n", + name, c_len, field_len); + hex_dump("C ", c_buf, c_len); + hex_dump("C++", cpp_fields, field_len); + return false; + } + if (memcmp(c_buf, cpp_fields, c_len) != 0) { + printf("FAIL %s: content mismatch\n", name); + hex_dump("C ", c_buf, c_len); + hex_dump("C++", cpp_fields, field_len); + return false; + } + printf("PASS %s (%d bytes)\n", name, c_len); + tests_passed++; + return true; +} + +/* Helper: encode an LCM C++ message via lcm_encode() and return the bytes */ +template +static int lcm_encode_to_buf(const T &msg, uint8_t *buf, int maxlen) +{ + int sz = msg.getEncodedSize(); + if (sz > maxlen) return -1; + msg.encode(buf, 0, sz); + return sz; +} + +/* ============================================================ + * Test functions for each type + * ============================================================ */ + +static void test_time() { + dimos_msg__Time c = {1234567890, 123456789}; + + ::std_msgs::Time cpp; + cpp.sec = 1234567890; + cpp.nsec = 123456789; + + uint8_t c_buf[64], cpp_buf[64]; + int c_len = dimos_msg__Time__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("std_msgs::Time", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_bool() { + dimos_msg__Bool c = {1}; + + ::std_msgs::Bool cpp; + cpp.data = 1; + + uint8_t c_buf[64], cpp_buf[64]; + int c_len = dimos_msg__Bool__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("std_msgs::Bool", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_int32() { + dimos_msg__Int32 c = {-42}; + + ::std_msgs::Int32 cpp; + cpp.data = -42; + + uint8_t c_buf[64], cpp_buf[64]; + int c_len = dimos_msg__Int32__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("std_msgs::Int32", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_float32() { + dimos_msg__Float32 c = {3.14159f}; + + ::std_msgs::Float32 cpp; + cpp.data = 3.14159f; + + uint8_t c_buf[64], cpp_buf[64]; + int c_len = dimos_msg__Float32__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("std_msgs::Float32", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_float64() { + dimos_msg__Float64 c = {2.718281828459045}; + + ::std_msgs::Float64 cpp; + cpp.data = 2.718281828459045; + + uint8_t c_buf[64], cpp_buf[64]; + int c_len = dimos_msg__Float64__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("std_msgs::Float64", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_colorrgba() { + dimos_msg__ColorRGBA c = {0.1f, 0.2f, 0.3f, 1.0f}; + + ::std_msgs::ColorRGBA cpp; + cpp.r = 0.1f; cpp.g = 0.2f; cpp.b = 0.3f; cpp.a = 1.0f; + + uint8_t c_buf[64], cpp_buf[64]; + int c_len = dimos_msg__ColorRGBA__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("std_msgs::ColorRGBA", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_vector3() { + dimos_msg__Vector3 c = {1.1, 2.2, 3.3}; + + ::geometry_msgs::Vector3 cpp; + cpp.x = 1.1; cpp.y = 2.2; cpp.z = 3.3; + + uint8_t c_buf[64], cpp_buf[64]; + int c_len = dimos_msg__Vector3__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::Vector3", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_point() { + dimos_msg__Point c = {4.0, 5.0, 6.0}; + + ::geometry_msgs::Point cpp; + cpp.x = 4.0; cpp.y = 5.0; cpp.z = 6.0; + + uint8_t c_buf[64], cpp_buf[64]; + int c_len = dimos_msg__Point__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::Point", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_point32() { + dimos_msg__Point32 c = {1.5f, 2.5f, 3.5f}; + + ::geometry_msgs::Point32 cpp; + cpp.x = 1.5f; cpp.y = 2.5f; cpp.z = 3.5f; + + uint8_t c_buf[64], cpp_buf[64]; + int c_len = dimos_msg__Point32__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::Point32", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_quaternion() { + dimos_msg__Quaternion c = {0.0, 0.0, 0.7071, 0.7071}; + + ::geometry_msgs::Quaternion cpp; + cpp.x = 0.0; cpp.y = 0.0; cpp.z = 0.7071; cpp.w = 0.7071; + + uint8_t c_buf[64], cpp_buf[64]; + int c_len = dimos_msg__Quaternion__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::Quaternion", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_pose() { + dimos_msg__Pose c; + c.position = {1.0, 2.0, 3.0}; + c.orientation = {0.0, 0.0, 0.7071, 0.7071}; + + ::geometry_msgs::Pose cpp; + cpp.position.x = 1.0; cpp.position.y = 2.0; cpp.position.z = 3.0; + cpp.orientation.x = 0.0; cpp.orientation.y = 0.0; + cpp.orientation.z = 0.7071; cpp.orientation.w = 0.7071; + + uint8_t c_buf[128], cpp_buf[128]; + int c_len = dimos_msg__Pose__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::Pose", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_pose2d() { + dimos_msg__Pose2D c = {10.0, 20.0, 1.5708}; + + ::geometry_msgs::Pose2D cpp; + cpp.x = 10.0; cpp.y = 20.0; cpp.theta = 1.5708; + + uint8_t c_buf[64], cpp_buf[64]; + int c_len = dimos_msg__Pose2D__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::Pose2D", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_twist() { + dimos_msg__Twist c; + c.linear = {1.0, 0.0, 0.0}; + c.angular = {0.0, 0.0, 0.5}; + + ::geometry_msgs::Twist cpp; + cpp.linear.x = 1.0; cpp.linear.y = 0.0; cpp.linear.z = 0.0; + cpp.angular.x = 0.0; cpp.angular.y = 0.0; cpp.angular.z = 0.5; + + uint8_t c_buf[128], cpp_buf[128]; + int c_len = dimos_msg__Twist__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::Twist", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_accel() { + dimos_msg__Accel c; + c.linear = {0.0, 0.0, 9.81}; + c.angular = {0.1, 0.2, 0.3}; + + ::geometry_msgs::Accel cpp; + cpp.linear.x = 0.0; cpp.linear.y = 0.0; cpp.linear.z = 9.81; + cpp.angular.x = 0.1; cpp.angular.y = 0.2; cpp.angular.z = 0.3; + + uint8_t c_buf[128], cpp_buf[128]; + int c_len = dimos_msg__Accel__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::Accel", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_transform() { + dimos_msg__Transform c; + c.translation = {1.0, 2.0, 3.0}; + c.rotation = {0.0, 0.0, 0.0, 1.0}; + + ::geometry_msgs::Transform cpp; + cpp.translation.x = 1.0; cpp.translation.y = 2.0; cpp.translation.z = 3.0; + cpp.rotation.x = 0.0; cpp.rotation.y = 0.0; + cpp.rotation.z = 0.0; cpp.rotation.w = 1.0; + + uint8_t c_buf[128], cpp_buf[128]; + int c_len = dimos_msg__Transform__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::Transform", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_wrench() { + dimos_msg__Wrench c; + c.force = {10.0, 20.0, 30.0}; + c.torque = {0.1, 0.2, 0.3}; + + ::geometry_msgs::Wrench cpp; + cpp.force.x = 10.0; cpp.force.y = 20.0; cpp.force.z = 30.0; + cpp.torque.x = 0.1; cpp.torque.y = 0.2; cpp.torque.z = 0.3; + + uint8_t c_buf[128], cpp_buf[128]; + int c_len = dimos_msg__Wrench__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::Wrench", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_inertia() { + dimos_msg__Inertia c; + c.m = 5.0; + c.com = {0.1, 0.2, 0.3}; + c.ixx = 1.0; c.ixy = 0.0; c.ixz = 0.0; + c.iyy = 2.0; c.iyz = 0.0; c.izz = 3.0; + + ::geometry_msgs::Inertia cpp; + cpp.m = 5.0; + cpp.com.x = 0.1; cpp.com.y = 0.2; cpp.com.z = 0.3; + cpp.ixx = 1.0; cpp.ixy = 0.0; cpp.ixz = 0.0; + cpp.iyy = 2.0; cpp.iyz = 0.0; cpp.izz = 3.0; + + uint8_t c_buf[128], cpp_buf[128]; + int c_len = dimos_msg__Inertia__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::Inertia", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_pose_with_covariance() { + dimos_msg__PoseWithCovariance c; + c.pose.position = {1.0, 2.0, 3.0}; + c.pose.orientation = {0.0, 0.0, 0.0, 1.0}; + for (int i = 0; i < 36; i++) c.covariance[i] = (i == 0 || i == 7 || i == 14 || + i == 21 || i == 28 || i == 35) + ? 0.01 : 0.0; + + ::geometry_msgs::PoseWithCovariance cpp; + cpp.pose.position.x = 1.0; cpp.pose.position.y = 2.0; cpp.pose.position.z = 3.0; + cpp.pose.orientation.x = 0.0; cpp.pose.orientation.y = 0.0; + cpp.pose.orientation.z = 0.0; cpp.pose.orientation.w = 1.0; + for (int i = 0; i < 36; i++) cpp.covariance[i] = c.covariance[i]; + + uint8_t c_buf[512], cpp_buf[512]; + int c_len = dimos_msg__PoseWithCovariance__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::PoseWithCovariance", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_twist_with_covariance() { + dimos_msg__TwistWithCovariance c; + c.twist.linear = {1.0, 0.0, 0.0}; + c.twist.angular = {0.0, 0.0, 0.5}; + for (int i = 0; i < 36; i++) c.covariance[i] = 0.0; + c.covariance[0] = 0.1; + + ::geometry_msgs::TwistWithCovariance cpp; + cpp.twist.linear.x = 1.0; cpp.twist.linear.y = 0.0; cpp.twist.linear.z = 0.0; + cpp.twist.angular.x = 0.0; cpp.twist.angular.y = 0.0; cpp.twist.angular.z = 0.5; + for (int i = 0; i < 36; i++) cpp.covariance[i] = c.covariance[i]; + + uint8_t c_buf[512], cpp_buf[512]; + int c_len = dimos_msg__TwistWithCovariance__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::TwistWithCovariance", c_buf, c_len, cpp_buf, cpp_len); +} + +static void test_accel_with_covariance() { + dimos_msg__AccelWithCovariance c; + c.accel.linear = {0.0, 0.0, 9.81}; + c.accel.angular = {0.0, 0.0, 0.0}; + for (int i = 0; i < 36; i++) c.covariance[i] = 0.0; + + ::geometry_msgs::AccelWithCovariance cpp; + cpp.accel.linear.x = 0.0; cpp.accel.linear.y = 0.0; cpp.accel.linear.z = 9.81; + cpp.accel.angular.x = 0.0; cpp.accel.angular.y = 0.0; cpp.accel.angular.z = 0.0; + for (int i = 0; i < 36; i++) cpp.covariance[i] = 0.0; + + uint8_t c_buf[512], cpp_buf[512]; + int c_len = dimos_msg__AccelWithCovariance__encode(c_buf, 0, sizeof(c_buf), &c); + int cpp_len = lcm_encode_to_buf(cpp, cpp_buf, sizeof(cpp_buf)); + compare_bytes("geometry_msgs::AccelWithCovariance", c_buf, c_len, cpp_buf, cpp_len); +} + +/* ---- Roundtrip test: encode with C, decode with C, verify fields ---- */ +static void test_roundtrip_pose() { + tests_run++; + dimos_msg__Pose original; + original.position = {1.23456789, -2.34567890, 3.45678901}; + original.orientation = {0.1, 0.2, 0.3, 0.9327}; + + uint8_t buf[128]; + int len = dimos_msg__Pose__encode(buf, 0, sizeof(buf), &original); + + dimos_msg__Pose decoded; + int dec_len = dimos_msg__Pose__decode(buf, 0, len, &decoded); + + if (dec_len != len || + decoded.position.x != original.position.x || + decoded.position.y != original.position.y || + decoded.position.z != original.position.z || + decoded.orientation.x != original.orientation.x || + decoded.orientation.y != original.orientation.y || + decoded.orientation.z != original.orientation.z || + decoded.orientation.w != original.orientation.w) { + printf("FAIL roundtrip Pose: decoded values don't match\n"); + return; + } + printf("PASS roundtrip Pose\n"); + tests_passed++; +} + +/* ====================================================================== + * AVR double-promotion path + * + * On AVR `sizeof(double)==4` so lcm_coretypes_arduino.h promotes the + * 4-byte IEEE 754 single to an 8-byte double bit-pattern on encode and + * truncates back on decode. The `#if defined(__AVR__)` encode/decode + * functions are never exercised by the rest of this test file (which + * runs on x86_64), so Paul's worry was that they could silently + * produce the wrong bytes for months before anyone notices. + * + * `_dimos_f32_to_f64_bits` and `_dimos_f64_bits_to_f32` were moved out + * of the `#if defined(__AVR__)` block and into the unconditional + * portion of the header so we can call them here and compare against + * the platform's own (IEEE 754 compliant) `(double)f` conversion. + * ====================================================================== */ + +#include + +static void test_avr_double_promotion() +{ + /* Mix of values that exercise every branch: + * - zero (±) + * - smallest and largest normals + * - typical sensor-range values + * - ±infinity + * - NaN (checks sign + mantissa-nonzero propagation) + * + * Denorms are intentionally omitted — the algorithm documents they + * flush to zero and that would produce a mismatch with native + * promotion. One denorm test below asserts that flush-to-zero + * behaviour explicitly. */ + const float normals[] = { + 0.0f, + -0.0f, + 1.0f, + -1.0f, + 0.5f, + -0.5f, + M_PI, + -M_PI, + 9.81f, + -273.15f, + 1.17549435e-38f, /* smallest positive normal float */ + 3.40282347e38f, /* FLT_MAX */ + -3.40282347e38f, + }; + + for (size_t i = 0; i < sizeof(normals) / sizeof(normals[0]); i++) { + float f = normals[i]; + tests_run++; + + uint64_t got_bits = _dimos_f32_to_f64_bits(f); + double native = (double)f; + uint64_t native_bits; + memcpy(&native_bits, &native, sizeof(native_bits)); + + if (got_bits != native_bits) { + printf("FAIL avr_double_promote: f=%.8g got=0x%016llx native=0x%016llx\n", + f, (unsigned long long)got_bits, (unsigned long long)native_bits); + continue; + } + + /* Round-trip back and check f2f32 inverts exactly on the normals. */ + float roundtrip = _dimos_f64_bits_to_f32(got_bits); + if (memcmp(&roundtrip, &f, sizeof(f)) != 0) { + printf("FAIL avr_double_roundtrip: f=%.8g rt=%.8g\n", f, roundtrip); + continue; + } + + tests_passed++; + } + printf("PASS avr_double_promotion (%zu normals)\n", + sizeof(normals) / sizeof(normals[0])); + + /* ±infinity */ + { + tests_run++; + uint64_t got = _dimos_f32_to_f64_bits(INFINITY); + double native_inf = (double)INFINITY; + uint64_t native_bits; + memcpy(&native_bits, &native_inf, sizeof(native_bits)); + if (got == native_bits) { + tests_passed++; + printf("PASS avr_double_promotion (+inf)\n"); + } else { + printf("FAIL avr_double_promotion (+inf): got=0x%016llx native=0x%016llx\n", + (unsigned long long)got, (unsigned long long)native_bits); + } + } + { + tests_run++; + uint64_t got = _dimos_f32_to_f64_bits(-INFINITY); + double native_inf = (double)-INFINITY; + uint64_t native_bits; + memcpy(&native_bits, &native_inf, sizeof(native_bits)); + if (got == native_bits) { + tests_passed++; + printf("PASS avr_double_promotion (-inf)\n"); + } else { + printf("FAIL avr_double_promotion (-inf): got=0x%016llx native=0x%016llx\n", + (unsigned long long)got, (unsigned long long)native_bits); + } + } + + /* NaN — sign bit preserved, exponent all-ones, mantissa nonzero. */ + { + tests_run++; + uint64_t got = _dimos_f32_to_f64_bits(NAN); + uint64_t sign_mask = 0x8000000000000000ULL; + uint64_t exp_mask = 0x7ff0000000000000ULL; + uint64_t mant_mask = 0x000fffffffffffffULL; + bool ok = ((got & exp_mask) == exp_mask) && ((got & mant_mask) != 0); + (void)sign_mask; + if (ok) { + tests_passed++; + printf("PASS avr_double_promotion (NaN)\n"); + } else { + printf("FAIL avr_double_promotion (NaN): got=0x%016llx\n", + (unsigned long long)got); + } + } + + /* Smallest positive denorm — algorithm flushes to +0. */ + { + tests_run++; + float denorm = 1.0e-40f; /* below FLT_MIN, a denorm */ + uint64_t got = _dimos_f32_to_f64_bits(denorm); + if (got == 0ULL) { + tests_passed++; + printf("PASS avr_double_promotion (denorm flush-to-zero)\n"); + } else { + printf("FAIL avr_double_promotion (denorm flush): got=0x%016llx\n", + (unsigned long long)got); + } + } +} + +int main() { + printf("=== Arduino LCM Wire Format Compatibility Tests ===\n\n"); + + /* std_msgs */ + test_time(); + test_bool(); + test_int32(); + test_float32(); + test_float64(); + test_colorrgba(); + + /* geometry_msgs */ + test_vector3(); + test_point(); + test_point32(); + test_quaternion(); + test_pose(); + test_pose2d(); + test_twist(); + test_accel(); + test_transform(); + test_wrench(); + test_inertia(); + test_pose_with_covariance(); + test_twist_with_covariance(); + test_accel_with_covariance(); + + /* roundtrip */ + test_roundtrip_pose(); + + /* AVR double-promotion path (normally compiled out on x86_64) */ + test_avr_double_promotion(); + + printf("\n=== Results: %d/%d passed ===\n", tests_passed, tests_run); + return (tests_passed == tests_run) ? 0 : 1; +} diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 96ce1c6784..cbd300332f 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -16,6 +16,7 @@ # Run `pytest dimos/robot/test_all_blueprints_generation.py` to regenerate. all_blueprints = { + "arduino-twist-echo-virtual": "dimos.hardware.arduino.examples.arduino_twist_echo.blueprint:arduino_twist_echo_virtual", "coordinator-basic": "dimos.control.blueprints.basic:coordinator_basic", "coordinator-cartesian-ik-mock": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_mock", "coordinator-cartesian-ik-piper": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_piper", @@ -146,6 +147,7 @@ "module-a": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleA", "module-b": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleB", "mujoco-sim-module": "dimos.simulation.engines.mujoco_sim_module.MujocoSimModule", + "multi-echo": "dimos.hardware.arduino.examples.arduino_multi_echo.module.MultiEcho", "navigation-module": "dimos.robot.unitree.rosnav.NavigationModule", "navigation-skill-container": "dimos.agents.skills.navigation.NavigationSkillContainer", "object-db-module": "dimos.perception.detection.moduleDB.ObjectDBModule", @@ -174,6 +176,7 @@ "spatial-memory": "dimos.perception.spatial_perception.SpatialMemory", "speak-skill": "dimos.agents.skills.speak_skill.SpeakSkill", "temporal-memory": "dimos.perception.experimental.temporal_memory.temporal_memory.TemporalMemory", + "twist-echo": "dimos.hardware.arduino.examples.arduino_twist_echo.module.TwistEcho", "twist-teleop-module": "dimos.teleop.quest.quest_extensions.TwistTeleopModule", "unitree-g1-skill-container": "dimos.robot.unitree.g1.skill_container.UnitreeG1SkillContainer", "unitree-skill-container": "dimos.robot.unitree.unitree_skill_container.UnitreeSkillContainer", diff --git a/dimos/test_no_sections.py b/dimos/test_no_sections.py index 902288b2e6..577861abcd 100644 --- a/dimos/test_no_sections.py +++ b/dimos/test_no_sections.py @@ -52,6 +52,7 @@ ".tox", # third-party vendored code "gtsam", + ".ignore.enhance", } # Lines that match section patterns but are actually programmatic / intentional. diff --git a/examples/arduino_led_echo/demo_led_blink.py b/examples/arduino_led_echo/demo_led_blink.py new file mode 100644 index 0000000000..f590db3666 --- /dev/null +++ b/examples/arduino_led_echo/demo_led_blink.py @@ -0,0 +1,89 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Demo: blink an Arduino's built-in LED from Python. + + uv run python examples/arduino_led_echo/demo_led_blink.py + +Requires an Arduino Uno plugged in and ``nix`` on PATH. +""" + +from __future__ import annotations + +import time + +from dimos.core.arduino_module import ArduinoModule, ArduinoModuleConfig +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.coordination.module_coordinator import ModuleCoordinator +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.std_msgs.Bool import Bool +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# Arduino module + + +class LedEchoConfig(ArduinoModuleConfig): + # basically all of the arduino module lives inside sketch.ino + sketch_path: str = "sketch/sketch.ino" + board_fqbn: str = "arduino:avr:uno" + baudrate: int = 115200 + + +class LedEcho(ArduinoModule): + config: LedEchoConfig + led_cmd: In[Bool] + led_state: Out[Bool] + + +# Host-side blinker + + +class Blinker(Module): + config: ModuleConfig + + led_cmd: Out[Bool] + led_state: In[Bool] + + @rpc + def start(self) -> None: + super().start() + + # output from the arduino + self.led_state.subscribe( + lambda msg: logger.info(f"LED STATUS: {'ON' if msg.data else 'OFF'}") + ) + + # input to the arduino + led_is_on = True + count = 20 + while count > 0: + count -= 1 + self.led_cmd.publish(Bool(data=led_is_on)) + led_is_on = not led_is_on + time.sleep(0.5) + + +# Blueprint & run + +blueprint = autoconnect( + Blinker.blueprint(), + LedEcho.blueprint(), +).global_config(n_workers=2) + +if __name__ == "__main__": + ModuleCoordinator.build(blueprint).loop() diff --git a/examples/arduino_led_echo/sketch/.gitignore b/examples/arduino_led_echo/sketch/.gitignore new file mode 100644 index 0000000000..c7c0db0413 --- /dev/null +++ b/examples/arduino_led_echo/sketch/.gitignore @@ -0,0 +1 @@ +dimos_arduino.h diff --git a/examples/arduino_led_echo/sketch/sketch.ino b/examples/arduino_led_echo/sketch/sketch.ino new file mode 100644 index 0000000000..e6043d3a44 --- /dev/null +++ b/examples/arduino_led_echo/sketch/sketch.ino @@ -0,0 +1,50 @@ +/* + * LED Echo — Example DimOS Arduino sketch. + * + * Receives Bool commands from the host to control the built-in LED. + * Echoes the LED state back so the host can confirm it changed. + * + * Demonstrates the simplest possible ArduinoModule: + * - One input stream (Bool → LED on/off) + * - One output stream (Bool → confirm LED state) + * - DimosSerial debug prints + * - Subscribe/callback API matching C++ LCM + */ + +#include "dimos_arduino.h" +#include + +#define LED_PIN 13 + +void on_led_cmd(const char *channel, const void *msg, void *ctx) { + (void)channel; + (void)ctx; + const dimos_msg__Bool *cmd = (const dimos_msg__Bool *)msg; + + /* Set the LED */ + digitalWrite(LED_PIN, cmd->data ? HIGH : LOW); + DimosSerial.print("LED "); + DimosSerial.println(cmd->data ? "ON" : "OFF"); + + /* Echo back the state */ + uint8_t buf[1]; + int encoded = dimos_msg__Bool__encode(buf, 0, sizeof(buf), cmd); + if (encoded > 0) { + dimos_publish(DIMOS_CHANNEL__LED_STATE, + &dimos_msg__Bool__type, buf, encoded); + } +} + +void setup() { + dimos_init(DIMOS_BAUDRATE); + pinMode(LED_PIN, OUTPUT); + digitalWrite(LED_PIN, LOW); + dimos_subscribe(DIMOS_CHANNEL__LED_CMD, + &dimos_msg__Bool__type, on_led_cmd, NULL); + DimosSerial.println("LED Echo ready"); +} + +void loop() { + dimos_handle(10); + _delay_ms(1); +} diff --git a/pr_responses.yaml b/pr_responses.yaml new file mode 100644 index 0000000000..0fc5cfccf7 --- /dev/null +++ b/pr_responses.yaml @@ -0,0 +1,50 @@ +- pr: 1879 + comment_id: 3106141416 + author: jeff-hykin + file: dimos/protocol/rpc/pubsubrpc.py + line: null + problem: "this shouldnt be here" + solution: > + Jeff's self-note. The file is identical to origin/dev on this branch — the + diff visible in the PR is an artifact of the merge-base being behind dev. + No commits on jeff/feat/arduino touch this file. File was included + unintentionally; requires Jeff's confirmation on what specifically to remove. + commit: null + +- pr: 1879 + comment_id: 3106141794 + author: jeff-hykin + file: dimos/robot/all_blueprints.py + line: 172 + problem: "should be named different" + solution: > + Jeff's self-note about naming. Requires Jeff's input on preferred name. + Previous autofix renamed twist-echo to arduino-twist-echo in the + all_modules registry to distinguish it from non-Arduino modules. + commit: null + +- pr: 1879 + comment_id: 3106142023 + author: jeff-hykin + file: dimos/simulation/engines/mujoco_shm.py + line: null + problem: "shouldnt be here" + solution: > + Jeff's self-note. The file is identical to origin/dev on this branch — the + diff visible in the PR is an artifact of the merge-base being behind dev. + No commits on jeff/feat/arduino touch this file. Needs Jeff's confirmation + before removal. + commit: null + +- pr: 1879 + comment_id: 4272491345 + author: leshy + file: null + line: null + problem: > + Discord comment about using LCM proto for serial comms instead of custom + DSP framing — LCM proto gives pubsub, topic encoding, packets for serial. + solution: > + Design discussion about architecture direction. Requires in-person + discussion between Jeff and leshy. No code change made. + commit: null