From f17370a0f87a5682c3726a5912f44b3eb17d9384 Mon Sep 17 00:00:00 2001 From: Richeek Das Date: Mon, 11 May 2026 15:29:34 -0400 Subject: [PATCH 01/10] PR1 --- ros2_bridge/PLAN.md | 556 ++++++++++++++++++ ros2_bridge/cortex_ros2_bridge/CMakeLists.txt | 64 ++ ros2_bridge/cortex_ros2_bridge/README.md | 19 + .../config/example_full.yaml | 67 +++ .../config/example_minimal.yaml | 10 + .../include/cortex_ros2_bridge/config.hpp | 99 ++++ ros2_bridge/cortex_ros2_bridge/package.xml | 24 + ros2_bridge/cortex_ros2_bridge/src/config.cpp | 312 ++++++++++ .../cortex_ros2_bridge/test/test_config.cpp | 324 ++++++++++ 9 files changed, 1475 insertions(+) create mode 100644 ros2_bridge/PLAN.md create mode 100644 ros2_bridge/cortex_ros2_bridge/CMakeLists.txt create mode 100644 ros2_bridge/cortex_ros2_bridge/README.md create mode 100644 ros2_bridge/cortex_ros2_bridge/config/example_full.yaml create mode 100644 ros2_bridge/cortex_ros2_bridge/config/example_minimal.yaml create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/package.xml create mode 100644 ros2_bridge/cortex_ros2_bridge/src/config.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_config.cpp diff --git a/ros2_bridge/PLAN.md b/ros2_bridge/PLAN.md new file mode 100644 index 0000000..3241d97 --- /dev/null +++ b/ros2_bridge/PLAN.md @@ -0,0 +1,556 @@ +# Plan: Cortex ↔ ROS 2 Bridge + +A pair of native ROS 2 nodes (`cortex_to_ros2`, `ros2_to_cortex`) that bridge Cortex's typed pub/sub topics to ROS 2 topics on the same host. Configured through a single clean YAML schema, deployed via launch files using **composable nodes** (intra-process / loaned messages) so that bridged data can move between the bridge and downstream ROS 2 consumers without an additional copy. + +Reference for inspiration only — not the target API or quality bar: . That implementation: + +- hard-codes 5 message kinds in C++ (`enable_imu`, `enable_color`, ...), +- spins one std::thread per topic with raw `nlohmann::json` decoding, +- ships a single non-composable `Node`, no zero-copy considered, +- never uses Cortex's actual wire format (24-byte header + msgpack metadata + OOB frames) — it reinvents a separate JSON / "numpy parts" wire. + +We will do better: drive the bridge from Cortex's own message registry, decode through the canonical `Message.from_frames` path, expose the bridge as composable components, and treat zero-copy as a first-class deployment mode. + +--- + +## 1. Goals & non-goals + +### Goals + +1. **Bidirectional** bridging of Cortex's standard message types (everything in [src/cortex/messages/standard.py](../src/cortex/messages/standard.py)) to/from canonical ROS 2 types (`sensor_msgs`, `geometry_msgs`, `std_msgs`, `tf2_msgs`). +2. A **single declarative YAML** describing every bridged topic, with sane defaults and per-topic QoS overrides. No C++ enable-flag explosion. +3. **Composable nodes** so the bridge can be loaded into a component container alongside downstream ROS 2 nodes — enabling intra-process (`use_intra_process_comms=True`) and, where the ROS 2 middleware supports it, **loaned-message** zero-copy. +4. **Zero-copy on the Cortex side** — reuse the OOB frame memoryviews already exposed by `Message.from_frames`; never `.tobytes()` array payloads. +5. **Minimal new dependencies on the Cortex side.** The bridge lives in its own ROS 2 package; building Cortex itself does not require a ROS 2 install. +6. **Clean shutdown, clean restarts**, no zombie zmq contexts, no leaked discovery registrations. +7. **One executable per direction**, both implemented as composable components, both runnable as plain `ros2 run` executables for development. + +### Non-goals (for v1) + +- Bridging *custom* user-defined Cortex messages (no `RobotState`-style classes) — only the standard catalogue. Custom types can be added by extending the registry; the design must not block this, but auto-derivation is out of scope. +- Bridging ROS 2 services or actions. Topics only. +- TCP / cross-host transport. The bridge assumes Cortex IPC and a co-located ROS 2 graph. (Multi-machine is on Cortex's broader roadmap — see [docs/TODO](../docs/TODO).) +- DDS-level tuning (RMW QoS profiles beyond what YAML exposes), security profiles, lifecycle nodes. + +--- + +## 2. What "zero-copy" actually buys us, and where + +ROS 2 zero-copy has two distinct mechanisms; the bridge needs both to give a single end-to-end story: + +| Layer | Mechanism | Win | +| ------------------------------ | ------------------------------------------------------------------------------------------------------ | ------------------------------------------------------------------ | +| Cortex IPC → bridge process | ZMQ multipart with OOB frames; `np.frombuffer(frame.buffer, ...)` aliases ZMQ memory (no copy) | Avoids 1 copy per array on ingest | +| Bridge → ROS 2 in-process consumer | Composable node container + `use_intra_process_comms=True` | Avoids serialize/deserialize hop and shared_ptr message is passed by reference | +| Bridge → ROS 2 cross-process consumer (advanced) | `rmw_zero_copy_cyclonedds` / Iceoryx loaned messages via `publisher->borrow_loaned_message()` | Avoids DDS network stack copy; needs shared-mem RMW | + +The bridge implementation must: + +1. Never copy the OOB buffer when constructing the ROS 2 message (`sensor_msgs::Image::data` should be filled with a `std::move` of a `std::vector` backed by the ZMQ frame, or via `borrow_loaned_message()` when available). +2. Publish using `std::unique_ptr` (the API path that `rclcpp` requires for intra-process zero-copy with multiple subscribers). +3. Detect at runtime whether `RMW_IMPLEMENTATION` supports loaned messages (`can_loan_messages()`), and fall back gracefully if not. + +The launch file must support loading the bridge **inside** a `ComponentContainer` together with downstream ROS 2 nodes — this is where intra-process composition actually pays off. A standalone `ros2 run` executable is provided for development/debug, but production deployments compose. + +--- + +## 3. Language choice: C++ + +**Decision: C++ from v1.** Python is not viable. + +Reasoning: + +- `rclpy` does not participate in `rclcpp_components` containers in any way that delivers zero-copy. Intra-process comms with serialization elision is an `rclcpp`-only path. A Python bridge would always serialize on egress, defeating the goal in §2. +- Composability is non-negotiable for this design — the point of the bridge is to let downstream ROS 2 consumers receive Cortex data without an extra copy, which requires co-loading into a `ComponentContainer` that only accepts `rclcpp` plugins. +- Loaned-message paths (`borrow_loaned_message()` on Iceoryx-class RMWs) are also C++-first; `rclpy` support is partial-to-nonexistent depending on distro. + +The cost is duplicating Cortex's wire format on the C++ side. This is tractable: the wire is small, stable, and **the bridge only needs decoders for the standard catalogue** (no introspection of user-defined types — see §3.1). The duplication is bounded. + +### 3.1 What needs reimplementing in C++ + +A self-contained `cortex_wire/` C++ library inside the package, no dependency on the Python cortex install: + +- **`MessageHeader`** — 24 bytes, big-endian `(fingerprint u64, timestamp_ns u64, sequence u64)`. Trivial. Lives in `cortex_wire/header.hpp`. +- **Fingerprint table** — hand-maintained `constexpr` map from fingerprint → message-kind enum, generated once by a Python script that imports the standard catalogue and prints the table. Committed in-tree, regenerated when the standard catalogue changes (CI checks staleness). +- **Metadata frame decoder** — msgpack-c (header-only, ament-packageable). The metadata frame is a small msgpack array of ordered field values; arrays/tensors appear as `{"__cortex_oob__": "numpy", "buffer": i, "dtype": "...", "shape": [...]}` dicts. Decoder walks the array per declared schema for each message kind. +- **OOB buffer handoff** — `zmq::message_t` already owns the buffer; we hand it to a `std::shared_ptr` and let the ROS 2 message's `data` field hold a non-owning view into it (via a custom `Allocator` that releases the shared_ptr on destruction). This is the key to zero-copy: the ZMQ frame's memory backs the published `sensor_msgs/Image::data` for its entire ROS-side lifetime. + +### 3.2 Custom user message types (later) + +The standard catalogue covers v1. For users to bridge their own Cortex messages, two paths are possible later: + +1. Hand-write a C++ adapter mirroring the Python dataclass (verbose but explicit; same approach as ROS 2 `.msg` files). +2. Code-gen the C++ struct + decoder from the Python dataclass via a small build-time tool. Defer to v2. + +Neither blocks v1; the architecture must just not paint itself into a corner. The adapter registry uses runtime lookup keyed by fingerprint, so adding new types later is a new file plus a registry entry. + +--- + +## 4. Package layout + +``` +ros2_bridge/ +├── PLAN.md ← this document +├── cortex_ros2_bridge/ ← ament_cmake ROS 2 package +│ ├── package.xml +│ ├── CMakeLists.txt +│ ├── include/cortex_ros2_bridge/ +│ │ ├── cortex_wire/ +│ │ │ ├── header.hpp ← 24-byte MessageHeader +│ │ │ ├── fingerprint_table.hpp ← auto-generated fingerprint → kind enum +│ │ │ ├── metadata.hpp ← msgpack metadata frame decoder +│ │ │ └── oob_buffer.hpp ← shared zmq::message_t ownership helpers +│ │ ├── config.hpp ← YAML schema dataclass-equivalent + loader +│ │ ├── qos.hpp ← YAML → rclcpp::QoS mapping +│ │ ├── registry.hpp ← adapter registry (kind+ros_type → factory) +│ │ ├── adapter.hpp ← AdapterBase template +│ │ └── adapters/ +│ │ ├── primitives.hpp +│ │ ├── arrays.hpp +│ │ ├── image.hpp +│ │ ├── pointcloud.hpp +│ │ ├── pose.hpp +│ │ ├── transform.hpp +│ │ ├── tensor.hpp +│ │ ├── header.hpp +│ │ └── multi.hpp +│ ├── src/ +│ │ ├── cortex_to_ros2_node.cpp ← rclcpp_components plugin +│ │ ├── ros2_to_cortex_node.cpp ← rclcpp_components plugin +│ │ ├── config.cpp +│ │ ├── registry.cpp +│ │ ├── cortex_wire/metadata.cpp +│ │ └── adapters/*.cpp ← one TU per adapter +│ ├── scripts/ +│ │ └── gen_fingerprint_table.py ← imports cortex.messages, prints C++ header +│ ├── config/ +│ │ ├── example_minimal.yaml +│ │ ├── example_full.yaml +│ │ └── schema.json ← optional JSON schema for editors +│ ├── launch/ +│ │ ├── cortex_to_ros2.launch.py +│ │ ├── ros2_to_cortex.launch.py +│ │ ├── bidirectional.launch.py +│ │ └── composable_container.launch.py ← loads both bridge components into one container +│ └── test/ +│ ├── test_config.cpp +│ ├── test_wire_decode.cpp +│ ├── test_adapters_roundtrip.cpp +│ └── test_launch_smoke.py ← launch_testing +└── README.md +``` + +`cortex_ros2_bridge` is an `ament_cmake` package. The two components are registered via `rclcpp_components_register_nodes(...)` so they can be loaded into any standard `component_container_mt`. Plain executables (`cortex_to_ros2`, `ros2_to_cortex`) are emitted via `rclcpp_components_register_node(... EXECUTABLE ...)` for development/debug runs. + +### 4.1 Dependencies + +- `rclcpp`, `rclcpp_components` +- `sensor_msgs`, `geometry_msgs`, `std_msgs`, `builtin_interfaces`, `tf2_msgs`, `tf2_ros` +- `cppzmq` (header-only wrapper over libzmq) +- `msgpack-cxx` (header-only msgpack-c) +- `yaml-cpp` +- Build-time: Python 3 with `cortex` installed, for `scripts/gen_fingerprint_table.py` to run at configure time (or pre-checked-in output if Python cortex is unavailable on the build host). + +--- + +## 5. Configuration: a single clean YAML + +The neurosim approach (flat `enable_X` parameters, per-message hard-coded fields) does not scale to Cortex's standard catalogue and is impossible to extend cleanly. We replace it with a structured schema. + +### 5.1 Schema (v1) + +```yaml +# Cortex ↔ ROS 2 bridge configuration +version: 1 + +cortex: + # Discovery daemon address; defaults to ipc:///tmp/cortex_discovery + discovery_address: "ipc:///tmp/cortex_discovery" + node_name_prefix: "cortex_bridge" + +defaults: + # Applied to every direction unless overridden per-topic + qos: + reliability: reliable # reliable | best_effort + durability: volatile # volatile | transient_local + history: keep_last # keep_last | keep_all + depth: 10 + loaned_messages: auto # auto | force | never (use rmw loaned msgs when available) + +# Cortex → ROS 2 bridges +cortex_to_ros2: + - name: lidar_points + cortex: + topic: "/sensor/lidar/points" + type: PointCloudMessage # name from cortex.messages.standard + ros2: + topic: "/lidar/points" + type: "sensor_msgs/msg/PointCloud2" # optional; default chosen by adapter + frame_id: "lidar" # static override; else use msg.frame_id + qos: + reliability: best_effort + depth: 5 + + - name: camera_image + cortex: + topic: "/camera/rgb" + type: ImageMessage + ros2: + topic: "/camera/image_raw" + # type inferred from adapter (sensor_msgs/Image) + + - name: robot_pose + cortex: + topic: "/state/pose" + type: PoseMessage + ros2: + topic: "/robot/pose" + broadcast_tf: true # adapter option: also publish to /tf + tf_parent: "map" + +# ROS 2 → Cortex bridges +ros2_to_cortex: + - name: cmd_vel + ros2: + topic: "/cmd_vel" + type: "geometry_msgs/msg/Twist" + cortex: + topic: "/control/cmd" + type: PoseMessage # adapter handles the lossy mapping + qos: + reliability: reliable + depth: 1 + durability: volatile + + - name: clock + ros2: + topic: "/clock" + type: "rosgraph_msgs/msg/Clock" + cortex: + topic: "/sim/time" + type: TimestampMessage +``` + +### 5.2 What the loader does + +1. Parses YAML and validates against a Pydantic / `dataclass`-backed schema in `config.py`. Invalid entries fail loudly *at launch*, never silently at runtime. +2. Resolves Cortex `type` strings to actual classes through `cortex.messages.MessageType.get_all()` (which already keys by class — we add a name→class lookup) so users cannot reference messages that are not registered. +3. Resolves ROS 2 `type` strings via `rclpy.utilities.get_message` or by importing the module; if `type` is omitted, the adapter's default ROS type is used. +4. Confirms that an adapter is registered for each (cortex_type → ros2_type) pair; otherwise refuses to start. No silent passthrough. +5. Stamps each bridge entry with a unique `name` used in logs, parameter overrides, and metrics labels. + +### 5.3 Why this schema (vs. neurosim's flat `enable_X`) + +- **Open set of types.** Neurosim's flat schema cannot describe a second IMU without editing C++. Ours describes *N* bridges, each a (cortex_topic, ros2_topic, type, qos) tuple. +- **QoS is per-bridge.** Image streams need `best_effort`; control commands need `reliable, depth=1`. Mixing them in one process requires per-entry QoS. +- **Adapters declare their own types.** The `ImageMessage` adapter knows its ROS 2 counterpart is `sensor_msgs/Image`. Users only specify it when overriding. +- **No silent failures.** Validation happens at config load, not at first message arrival. + +--- + +## 6. Adapter registry + +Each (Cortex type ↔ ROS 2 type) pair is one **Adapter**. C++ template: + +```cpp +// adapter.hpp +template +struct Adapter { + static constexpr CortexMessageKind kind = CortexKind::kind; + using ros2_type = Ros2Msg; + + // Cortex → ROS 2. `frames` are the zmq::message_t parts after the topic frame + // (header, metadata, oob0..). The returned unique_ptr is what rclcpp wants + // for the intra-process zero-copy publish path. + static std::unique_ptr to_ros2( + const MessageHeader & header, + const DecodedMetadata & meta, + std::vector> & oob, + const BridgeEntry & cfg); + + // ROS 2 → Cortex. Builds the multipart frames for transport. + static std::vector to_cortex( + const Ros2Msg & msg, + uint64_t sequence, + const BridgeEntry & cfg); +}; +``` + +Adapters register themselves into a runtime table keyed by `(CortexMessageKind, ros2_type_name)`: + +```cpp +REGISTER_ADAPTER(ImageAdapter, CortexMessageKind::Image, "sensor_msgs/msg/Image"); +``` + +### 6.1 Zero-copy in the Image adapter + +The critical adapter. Sketch: + +```cpp +std::unique_ptr ImageAdapter::to_ros2( + const MessageHeader & header, + const DecodedMetadata & meta, + std::vector> & oob, + const BridgeEntry & cfg) +{ + auto out = std::make_unique(); + out->header.stamp = ns_to_time(header.timestamp_ns); + out->header.frame_id = cfg.frame_id.value_or(""); + out->height = meta.fields["height"].as(); + out->width = meta.fields["width"].as(); + out->encoding = meta.fields["encoding"].as(); + out->is_bigendian = 0; + out->step = meta.fields["data"].oob_stride0(); + + // The raw image bytes live in oob[0] — a zmq::message_t we already own + // via shared_ptr. Hand its memory to the ROS message via a custom + // allocator that keeps the shared_ptr alive for the message's lifetime. + auto buf = oob[0]; + out->data = ZmqBackedVector( + static_cast(buf->data()), buf->size(), buf); + return out; +} +``` + +`ZmqBackedVector` is a thin wrapper around `std::vector` that uses a stateful allocator carrying the `shared_ptr`. The vector never copies — its `data()` pointer is the ZMQ frame's buffer; deallocation releases the `shared_ptr`. The published ROS message owns one reference to the ZMQ frame, and any intra-process subscriber receiving the `unique_ptr` (or a `const &` to a shared one) sees the same memory. + +For loaned-message RMWs, the adapter has an alternate path: `borrow_loaned_message()`, `memcpy` from the ZMQ frame into the loaned buffer, and `publish(std::move(loaned))`. This costs one copy but enables shared-memory cross-process delivery. + +The adapter registry is the *only* place that knows about specific ROS 2 message types — adding `CompressedImage` is one new adapter source file plus one schema entry. + +### 6.1 Standard catalogue → ROS 2 mapping + +| Cortex | ROS 2 (default) | Notes | +| ----------------------- | ---------------------------------------- | ---------------------------------------------------------------- | +| `StringMessage` | `std_msgs/String` | Trivial | +| `IntMessage` | `std_msgs/Int64` | | +| `FloatMessage` | `std_msgs/Float64` | | +| `BytesMessage` | `std_msgs/ByteMultiArray` | | +| `DictMessage` | `std_msgs/String` (JSON-encoded) | Opt-in only; warn loudly. Better: ask user to define a custom msg. | +| `ListMessage` | `std_msgs/String` (JSON-encoded) | Same caveat | +| `ArrayMessage` | `std_msgs/Float32MultiArray` (and friends, per dtype) | Adapter dispatches on `arr.dtype` | +| `MultiArrayMessage` | (none — must be split per key) | YAML must specify the per-key target topic | +| `TensorMessage` | `std_msgs/Float32MultiArray` | Tensors are CPU'd on Cortex side already | +| `MultiTensorMessage` | (none — same as MultiArray) | | +| `ImageMessage` | `sensor_msgs/Image` | Zero-copy data buffer | +| `PointCloudMessage` | `sensor_msgs/PointCloud2` | Pack fields per attribute presence (xyz, rgb, intensity, normals); zero-copy where possible | +| `PoseMessage` | `geometry_msgs/PoseStamped` | `frame_id` from msg or YAML | +| `TransformMessage` | `geometry_msgs/TransformStamped` (+ optional `/tf` broadcast) | adapter decomposes 4×4 → translation + quaternion | +| `TimestampMessage` | `builtin_interfaces/Time` | | +| `HeaderMessage` | `std_msgs/Header` | | + +For each row, we also implement the reverse direction except where lossy (the JSON-encoded dict path is one-way by default, opt-in only). + +--- + +## 7. Bridge components + +Both are `rclcpp::Node` subclasses registered with `RCLCPP_COMPONENTS_REGISTER_NODE(...)`. Loaded into a `component_container_mt` they participate fully in intra-process comms (`use_intra_process_comms: true` on the container). + +### 7.1 `CortexToRos2Bridge` (component) + +Responsibilities: + +1. Construct a single `zmq::context_t` and a discovery client. The discovery client talks REQ/REP to the daemon to look up each Cortex topic's IPC endpoint, exactly as the Python `Subscriber` does (the discovery protocol is a small msgpack REQ/REP — port to C++ alongside the wire library). +2. For each `cortex_to_ros2` YAML entry: + - Create a `zmq::socket_t` SUB connected to the discovered endpoint, subscribed to the topic-name frame. + - Verify fingerprint by reading the first message header and asserting it matches the expected `CortexMessageKind`; refuse to start the bridge entry on mismatch (always strict). + - Create an `rclcpp::Publisher` using the YAML-derived `rclcpp::QoS`. + - Look up the adapter in the registry by `(kind, ros2_type_name)`. +3. Run a dedicated `std::jthread` per SUB socket (one OS thread per Cortex topic). Each thread loops on `recv_multipart` and calls the adapter, then `publisher_->publish(std::move(unique_ptr_msg))`. Per-topic threads are cheap, keep slow topics from blocking fast ones, and map naturally onto Cortex's per-topic socket model. +4. Use a single shared `zmq::poller_t` instead of N threads when the number of bridge entries is large (>16). The threshold is an internal knob, not exposed; both code paths exist behind the same `RecvLoop` interface. +5. On destruction: signal stop, join threads, close sockets, term context. + +Publishing from a non-rclcpp thread is supported (`rclcpp::Publisher::publish` is thread-safe). Intra-process delivery still works: rclcpp's intra-process manager dispatches the `unique_ptr` onto subscribers' executors regardless of which thread called `publish`. + +### 7.2 `Ros2ToCortexBridge` (component) + +Mirror direction. + +1. One `zmq::context_t`, one PUB socket per Cortex topic, register each with the discovery daemon. +2. For each `ros2_to_cortex` entry, create an `rclcpp::Subscription` with the YAML QoS, callback bound to the adapter's `to_cortex(...)`. +3. The callback runs on the rclcpp executor thread, builds the multipart frames via the adapter, and `socket.send_multipart(...)`. PUB sockets aren't thread-safe, so each topic's PUB socket is only ever touched from the rclcpp executor — the `MultiThreadedExecutor` mutex-guards the callback group per subscription. +4. Sequence numbers are per-publisher monotonic, matching Cortex's Python publisher semantics. + +### 7.3 Composable node container + +`composable_container.launch.py` loads both components into a single `component_container_mt` with `use_intra_process_comms=True`. Downstream ROS 2 nodes the user wants colocated can be added to the same container (their launch file extends ours, or copies the `ComposableNode` descriptor) and will receive bridge-published messages **without** serialization. + +Development run (no container, no intra-process): + +```bash +ros2 run cortex_ros2_bridge cortex_to_ros2 --ros-args -p config_path:=/tmp/bridge.yaml +``` + +Production run: + +```bash +ros2 launch cortex_ros2_bridge composable_container.launch.py config:=/tmp/bridge.yaml +``` + +--- + +## 8. Launch files + +### 8.1 `composable_container.launch.py` (the main one) + +```python +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + config_arg = DeclareLaunchArgument( + "config", description="Path to bridge YAML config") + container_name_arg = DeclareLaunchArgument( + "container_name", default_value="cortex_bridge_container") + + config = LaunchConfiguration("config") + + container = ComposableNodeContainer( + name=LaunchConfiguration("container_name"), + namespace="", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="cortex_ros2_bridge", + plugin="cortex_ros2_bridge::CortexToRos2Bridge", + name="cortex_to_ros2", + parameters=[{"config_path": config}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="cortex_ros2_bridge", + plugin="cortex_ros2_bridge::Ros2ToCortexBridge", + name="ros2_to_cortex", + parameters=[{"config_path": config}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + ) + + return LaunchDescription([config_arg, container_name_arg, container]) +``` + +### 8.2 Single-direction launch files + +`cortex_to_ros2.launch.py` and `ros2_to_cortex.launch.py` load only one component into the container. Useful when the user only needs one direction or wants to colocate the bridge with their own composable node graph in their own launch file (they can copy the `ComposableNode` descriptor). + +### 8.3 Standalone (debug) executables + +`cortex_to_ros2` and `ros2_to_cortex` are also entry points in `setup.py`, so they run without a container. No intra-process zero-copy in this mode — useful only for debugging. + +--- + +## 9. Implementation phases (PR-sized) + +Each phase ends with passing tests and a working YAML example. + +### PR1 — Package skeleton & config loader + +- Create the `cortex_ros2_bridge` ament_cmake package, `package.xml`, `CMakeLists.txt`, `colcon build` green. +- Implement `config.hpp`/`.cpp` — yaml-cpp parser, schema validation, structured errors. +- gtest cases for valid/invalid YAML. +- No ZMQ or adapters yet. + +### PR2 — Cortex wire library (C++) + +- Port `MessageHeader` (24 bytes, big-endian) and `DecodedMetadata` (msgpack-cxx). +- Port the discovery REQ/REP protocol just enough to look up a topic's endpoint. +- `scripts/gen_fingerprint_table.py` introspects the Python standard catalogue and emits `fingerprint_table.hpp`. CMake invokes it at configure time; result is also checked in for builds without a Python cortex install. +- gtest decode against fixture frames captured from a Python publisher (golden test). +- `ZmqBackedVector` allocator + lifetime tests. + +### PR3 — Adapter base + primitives + +- `Adapter` template + `REGISTER_ADAPTER` macro + runtime registry. +- Implement `StringMessage`, `IntMessage`, `FloatMessage`, `BytesMessage`, `TimestampMessage`, `HeaderMessage`. +- Pure-conversion round-trip tests (no ZMQ, no rclcpp executor). + +### PR4 — `CortexToRos2Bridge` MVP + +- Component class, per-topic thread, recv loop, adapter dispatch. +- `RCLCPP_COMPONENTS_REGISTER_NODE` registration; plain executable target via `rclcpp_components_register_node(... EXECUTABLE ...)`. +- `cortex_to_ros2.launch.py` with this component. +- launch_testing smoke: Python cortex publisher in a subprocess → bridge → assert ROS topic payload. + +### PR5 — `Ros2ToCortexBridge` MVP + +- Symmetric component, rclcpp subscription → adapter → zmq PUB. +- launch_testing smoke in reverse: Python cortex subscriber receives data published from a ROS `topic pub`. + +### PR6 — Array / Image / PointCloud adapters + +- Implement `ArrayMessage`, `MultiArrayMessage`, `ImageMessage`, `PointCloudMessage` (both directions). +- Verify zero-copy on Cortex→ROS 2: assert `sensor_msgs::Image::data.data()` == `zmq::message_t::data()` (pointer equality) in a test fixture. +- `bench_bridge_throughput.cpp` measuring 1920×1080 RGB at 60 Hz: end-to-end latency, CPU, and `perf stat` cache misses with vs. without zero-copy. + +### PR7 — Tensor / Pose / Transform / Multi* + +- Remaining standard catalogue. +- Optional `/tf` broadcast on `TransformMessage` and `PoseMessage` (shared `tf2_ros::TransformBroadcaster` per component). + +### PR8 — Intra-process composability test + +- `composable_container.launch.py` loads both components into one container with `use_intra_process_comms=True`. +- Integration test: a third `ComposableNode` (a simple test consumer in C++) subscribes to a bridge-published `sensor_msgs::Image` and asserts the pointer received in the callback equals the pointer published by the bridge — proving the intra-process zero-copy path. + +### PR9 — Loaned-message path (advanced) + +- Detect `can_loan_messages()` at runtime per publisher. +- For `Image`, `PointCloud2`, and large `Float32MultiArray`, when loaning is available, copy from the ZMQ frame into the loaned buffer and `publish(std::move(loaned))`. +- Document the requirement: `RMW_IMPLEMENTATION=rmw_cyclonedds_cpp` + Iceoryx, or `rmw_iceoryx_cpp`. + +### PR10 — Docs & examples + +- Update [docs/TODO](../docs/TODO). +- Add `docs/guides/ros2-bridge.md` walking through the YAML, the launch file, and a working demo. +- Add `examples/ros2_bridge/` with a `publisher.py` (cortex side) and a `ros2 topic echo` recipe. + +--- + +## 10. Threading & lifecycle + +- `CortexToRos2Bridge`: one `std::jthread` per Cortex SUB socket (small N) or one shared `zmq::poller_t` thread (large N). Threads exit when a `std::stop_token` is requested in the destructor. +- `Ros2ToCortexBridge`: subscription callbacks run on the rclcpp executor. Each Cortex PUB socket is owned by a single subscription's callback group so it is only ever published from one thread (PUB sockets are not thread-safe). +- The container is `component_container_mt`. Its `MultiThreadedExecutor` dispatches subscription callbacks across a small thread pool, but each callback group is serialized — the per-socket invariant holds. +- ZMQ context is shared across all sockets in one component. Context termination on destruction waits on `LINGER` (set to 0 to avoid teardown hangs). +- The discovery daemon is **not** managed by the bridge. The bridge logs a clear error and refuses to start any bridge entry whose topic cannot be resolved within a configurable timeout. + +--- + +## 11. Testing & CI + +- gtest on the config loader (PR1), wire decoder (PR2), and each adapter (PR3+). +- `launch_testing` integration tests that spin up `cortex.discovery.daemon` (Python) + a Python publisher/subscriber as helper processes, launch the bridge container, and assert end-to-end delivery. Fixtures live in `test/fixtures/`. +- Intra-process delivery test (PR8) using a small C++ consumer component. +- A staleness check in CI: re-run `gen_fingerprint_table.py` against the installed cortex package and `diff` against the checked-in header. Mismatch fails CI. +- Throughput benchmark for images and point clouds (PR6). +- CI matrix: ROS 2 Humble + Jazzy + Rolling on Ubuntu 22.04 + 24.04. + +--- + +## 12. Open questions + +1. **`DictMessage` / `ListMessage` interop.** JSON-encoded `std_msgs/String` is gross but unblocks users. Better long-term: encourage users to define a proper ROS message and a custom Cortex `Message` subclass, then write an adapter. Decide whether the JSON fallback ships at all in v1, or whether we refuse and fail loudly. +2. **Custom-type extension API.** v1 only covers the standard catalogue. The cleanest extension path is a separate ament_cmake package that depends on `cortex_ros2_bridge`, defines its own adapter, and calls `REGISTER_ADAPTER` in a plugin loaded via `pluginlib`. Decide whether to wire pluginlib in v1 or defer. +3. **Configurable adapter overrides in YAML.** e.g. `adapter: "my_pkg::BayerImageAdapter"` for non-default encodings. Trivial once pluginlib is in. +4. **TF integration scope.** `broadcast_tf: true` on `TransformMessage`/`PoseMessage`. Decide: one shared `tf2_ros::TransformBroadcaster` per component (cheaper, simpler) vs. one per bridge entry (more isolated). Default to shared. +5. **Sync vs. async Cortex mode.** Since the bridge is pure C++, the Cortex async-vs-sync distinction goes away — we always use raw `zmq::socket_t` with our own poller. This is closer to Cortex's sync subscriber path (see [docs/plans/sync-subscriber-latency.md](../docs/plans/sync-subscriber-latency.md)) and should hit sub-100 µs add-on latency on top of the underlying transport. The YAML `cortex_mode` field is therefore likely unnecessary — remove it before v1 freezes. +6. **Discovery client port.** The discovery REQ/REP protocol must be reimplemented in C++. It's small but it's a maintenance surface — confirm the protocol is stable (see [src/cortex/discovery/protocol.py](../src/cortex/discovery/protocol.py)) before committing. + +--- + +## 13. Done definition + +- `colcon build` builds the package on a clean ROS 2 Humble install. +- `ros2 launch cortex_ros2_bridge composable_container.launch.py config:=config/example_full.yaml` brings up the bridge, both directions, and `ros2 topic list` shows all configured topics. +- The example YAML round-trips every standard message type from a Python publisher → ROS 2 topic → another Python subscriber via the reverse bridge, with the original Cortex header preserved or reconstructed faithfully. +- For `ImageMessage`, profiling confirms no array copy on the Cortex→ROS 2 path (verified by memory address equality on the OOB buffer through the adapter). +- Integration test demonstrates that a co-loaded composable consumer of `sensor_msgs/Image` receives the message via intra-process delivery (no DDS roundtrip). +- Docs page exists at `docs/guides/ros2-bridge.md`. diff --git a/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt b/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt new file mode 100644 index 0000000..a8428c2 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.16) +project(cortex_ros2_bridge VERSION 0.1.0 LANGUAGES CXX) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(yaml-cpp REQUIRED) + +# ---- libraries ------------------------------------------------------------ + +add_library(cortex_ros2_bridge_config SHARED + src/config.cpp +) +target_include_directories(cortex_ros2_bridge_config PUBLIC + $ + $ +) +target_link_libraries(cortex_ros2_bridge_config PUBLIC yaml-cpp) + +# ---- install -------------------------------------------------------------- + +install(DIRECTORY include/ DESTINATION include) + +install(TARGETS cortex_ros2_bridge_config + EXPORT export_cortex_ros2_bridge + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +ament_export_include_directories(include) +ament_export_targets(export_cortex_ros2_bridge HAS_LIBRARY_TARGET) +ament_export_dependencies(yaml-cpp) + +# ---- tests ---------------------------------------------------------------- + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + + # Skip the opinionated style linters; keep the substantive ones (cppcheck, + # lint_cmake, xmllint). Style is enforced by clang-format separately if/when + # we add one. + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_config test/test_config.cpp) + target_link_libraries(test_config cortex_ros2_bridge_config) +endif() + +ament_package() diff --git a/ros2_bridge/cortex_ros2_bridge/README.md b/ros2_bridge/cortex_ros2_bridge/README.md new file mode 100644 index 0000000..79c2e3d --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/README.md @@ -0,0 +1,19 @@ +# cortex_ros2_bridge + +ROS 2 package that bridges Cortex pub/sub topics to ROS 2 topics, in both directions. + +This is **PR1 of [the implementation plan](../PLAN.md)** — package skeleton and YAML config loader only. No bridging is wired up yet; subsequent PRs add the Cortex wire decoder, adapter registry, and the two composable nodes. + +## Build + +```bash +# from a colcon workspace whose src/ contains this package +colcon build --packages-select cortex_ros2_bridge +colcon test --packages-select cortex_ros2_bridge +``` + +## Configuration + +A YAML file describes every bridged topic. See [config/example_minimal.yaml](config/example_minimal.yaml) and [config/example_full.yaml](config/example_full.yaml). The schema is documented in [../PLAN.md](../PLAN.md) §5. + +The C++ loader lives in [include/cortex_ros2_bridge/config.hpp](include/cortex_ros2_bridge/config.hpp); use `cortex_ros2_bridge::load_config(path)` from any node. diff --git a/ros2_bridge/cortex_ros2_bridge/config/example_full.yaml b/ros2_bridge/cortex_ros2_bridge/config/example_full.yaml new file mode 100644 index 0000000..1bcca4a --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/config/example_full.yaml @@ -0,0 +1,67 @@ +# Full bridge configuration exercising every schema feature. +version: 1 + +cortex: + discovery_address: "ipc:///tmp/cortex_discovery" + node_name_prefix: "cortex_bridge" + +defaults: + qos: + reliability: reliable # reliable | best_effort + durability: volatile # volatile | transient_local + history: keep_last # keep_last | keep_all + depth: 10 + loaned_messages: auto # auto | force | never + +cortex_to_ros2: + - name: lidar_points + cortex: + topic: "/sensor/lidar/points" + type: PointCloudMessage + ros2: + topic: "/lidar/points" + type: "sensor_msgs/msg/PointCloud2" + frame_id: "lidar" + qos: + reliability: best_effort + depth: 5 + + - name: camera_image + cortex: + topic: "/sensor/camera/rgb" + type: ImageMessage + ros2: + topic: "/camera/image_raw" + qos: + reliability: best_effort + + - name: robot_pose + cortex: + topic: "/state/pose" + type: PoseMessage + ros2: + topic: "/robot/pose" + broadcast_tf: true + tf_parent: "map" + +ros2_to_cortex: + - name: cmd_vel + ros2: + topic: "/cmd_vel" + type: "geometry_msgs/msg/Twist" + cortex: + topic: "/control/cmd" + type: PoseMessage + qos: + reliability: reliable + depth: 1 + + - name: sim_clock + ros2: + topic: "/clock" + type: "rosgraph_msgs/msg/Clock" + cortex: + topic: "/sim/time" + type: TimestampMessage + qos: + durability: transient_local diff --git a/ros2_bridge/cortex_ros2_bridge/config/example_minimal.yaml b/ros2_bridge/cortex_ros2_bridge/config/example_minimal.yaml new file mode 100644 index 0000000..e993e44 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/config/example_minimal.yaml @@ -0,0 +1,10 @@ +# Minimal bridge configuration: a single camera stream Cortex -> ROS 2. +version: 1 + +cortex_to_ros2: + - name: camera_rgb + cortex: + topic: "/sensor/camera/rgb" + type: ImageMessage + ros2: + topic: "/camera/image_raw" diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp new file mode 100644 index 0000000..fd98b01 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__CONFIG_HPP_ +#define CORTEX_ROS2_BRIDGE__CONFIG_HPP_ + +#include +#include +#include +#include +#include + +namespace cortex_ros2_bridge +{ + +enum class Reliability { Reliable, BestEffort }; +enum class Durability { Volatile, TransientLocal }; +enum class History { KeepLast, KeepAll }; +enum class LoanedMessagesMode { Auto, Force, Never }; +enum class Direction { CortexToRos2, Ros2ToCortex }; + +struct QosSettings +{ + Reliability reliability = Reliability::Reliable; + Durability durability = Durability::Volatile; + History history = History::KeepLast; + std::uint32_t depth = 10; +}; + +struct CortexSettings +{ + std::string discovery_address = "ipc:///tmp/cortex_discovery"; + std::string node_name_prefix = "cortex_bridge"; +}; + +struct DefaultsSettings +{ + QosSettings qos; + LoanedMessagesMode loaned_messages = LoanedMessagesMode::Auto; +}; + +struct CortexEndpoint +{ + std::string topic; + std::string type; // Cortex message class name, e.g. "ImageMessage" +}; + +struct Ros2Endpoint +{ + std::string topic; + // Empty / nullopt → adapter chooses the default ROS 2 type for the Cortex type. + std::optional type; + // Static override; if unset, adapter uses the message's own frame_id field + // (when present) or the empty string. + std::optional frame_id; + // For PoseMessage / TransformMessage: also publish to /tf using tf2. + bool broadcast_tf = false; + std::optional tf_parent; +}; + +struct BridgeEntry +{ + std::string name; + Direction direction; + CortexEndpoint cortex; + Ros2Endpoint ros2; + QosSettings qos; // defaults already merged +}; + +struct BridgeConfig +{ + int version = 1; + CortexSettings cortex; + DefaultsSettings defaults; + std::vector entries; +}; + +class ConfigError : public std::runtime_error +{ +public: + using std::runtime_error::runtime_error; +}; + +// Parse YAML text into a validated BridgeConfig. Throws ConfigError on any +// schema violation or YAML syntax error. +BridgeConfig parse_config(const std::string & yaml_text); + +// Load YAML from a file. Throws ConfigError if the file cannot be opened, or +// for any error parse_config would raise. +BridgeConfig load_config(const std::string & path); + +// Helpers for diagnostics / logging. +const char * to_string(Reliability v); +const char * to_string(Durability v); +const char * to_string(History v); +const char * to_string(LoanedMessagesMode v); +const char * to_string(Direction v); + +} // namespace cortex_ros2_bridge + +#endif // CORTEX_ROS2_BRIDGE__CONFIG_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/package.xml b/ros2_bridge/cortex_ros2_bridge/package.xml new file mode 100644 index 0000000..accf2b7 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/package.xml @@ -0,0 +1,24 @@ + + + + cortex_ros2_bridge + 0.1.0 + + Bidirectional bridge between Cortex pub/sub (ZeroMQ IPC) and ROS 2 topics. + Configurable via YAML; composable nodes for intra-process zero-copy. + + Richeek Das + Apache-2.0 + + ament_cmake + + yaml-cpp + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_bridge/cortex_ros2_bridge/src/config.cpp b/ros2_bridge/cortex_ros2_bridge/src/config.cpp new file mode 100644 index 0000000..ecfc653 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/config.cpp @@ -0,0 +1,312 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/config.hpp" + +#include + +#include +#include +#include +#include + +namespace cortex_ros2_bridge +{ + +namespace +{ + +std::string join_path(const std::string & parent, const std::string & key) +{ + return parent.empty() ? key : parent + "." + key; +} + +template +T required_scalar(const YAML::Node & node, const std::string & key, const std::string & parent) +{ + const std::string field = join_path(parent, key); + if (!node[key]) { + throw ConfigError(field + ": required field is missing"); + } + try { + return node[key].as(); + } catch (const YAML::Exception &) { + throw ConfigError(field + ": invalid value type"); + } +} + +template +std::optional optional_scalar( + const YAML::Node & node, const std::string & key, const std::string & parent) +{ + if (!node[key]) { + return std::nullopt; + } + const std::string field = join_path(parent, key); + try { + return node[key].as(); + } catch (const YAML::Exception &) { + throw ConfigError(field + ": invalid value type"); + } +} + +Reliability parse_reliability(const std::string & s, const std::string & path) +{ + if (s == "reliable") return Reliability::Reliable; + if (s == "best_effort") return Reliability::BestEffort; + throw ConfigError( + path + ": invalid reliability '" + s + "' (expected 'reliable' or 'best_effort')"); +} + +Durability parse_durability(const std::string & s, const std::string & path) +{ + if (s == "volatile") return Durability::Volatile; + if (s == "transient_local") return Durability::TransientLocal; + throw ConfigError( + path + ": invalid durability '" + s + "' (expected 'volatile' or 'transient_local')"); +} + +History parse_history(const std::string & s, const std::string & path) +{ + if (s == "keep_last") return History::KeepLast; + if (s == "keep_all") return History::KeepAll; + throw ConfigError(path + ": invalid history '" + s + "' (expected 'keep_last' or 'keep_all')"); +} + +LoanedMessagesMode parse_loaned(const std::string & s, const std::string & path) +{ + if (s == "auto") return LoanedMessagesMode::Auto; + if (s == "force") return LoanedMessagesMode::Force; + if (s == "never") return LoanedMessagesMode::Never; + throw ConfigError( + path + ": invalid loaned_messages '" + s + "' (expected 'auto', 'force', or 'never')"); +} + +void require_non_empty(const std::string & value, const std::string & path) +{ + if (value.empty()) { + throw ConfigError(path + ": must be a non-empty string"); + } +} + +void apply_qos_overrides(QosSettings & out, const YAML::Node & node, const std::string & path) +{ + if (!node) { + return; + } + if (!node.IsMap()) { + throw ConfigError(path + ": expected mapping"); + } + if (auto v = optional_scalar(node, "reliability", path)) { + out.reliability = parse_reliability(*v, join_path(path, "reliability")); + } + if (auto v = optional_scalar(node, "durability", path)) { + out.durability = parse_durability(*v, join_path(path, "durability")); + } + if (auto v = optional_scalar(node, "history", path)) { + out.history = parse_history(*v, join_path(path, "history")); + } + if (auto v = optional_scalar(node, "depth", path)) { + out.depth = *v; + } +} + +BridgeEntry parse_entry( + const YAML::Node & node, Direction direction, const QosSettings & defaults_qos, + const std::string & path) +{ + if (!node.IsMap()) { + throw ConfigError(path + ": expected mapping"); + } + + BridgeEntry e; + e.direction = direction; + e.name = required_scalar(node, "name", path); + require_non_empty(e.name, join_path(path, "name")); + + const auto cortex_node = node["cortex"]; + if (!cortex_node || !cortex_node.IsMap()) { + throw ConfigError(join_path(path, "cortex") + ": required mapping"); + } + const std::string cortex_path = join_path(path, "cortex"); + e.cortex.topic = required_scalar(cortex_node, "topic", cortex_path); + e.cortex.type = required_scalar(cortex_node, "type", cortex_path); + require_non_empty(e.cortex.topic, join_path(cortex_path, "topic")); + require_non_empty(e.cortex.type, join_path(cortex_path, "type")); + + const auto ros2_node = node["ros2"]; + if (!ros2_node || !ros2_node.IsMap()) { + throw ConfigError(join_path(path, "ros2") + ": required mapping"); + } + const std::string ros2_path = join_path(path, "ros2"); + e.ros2.topic = required_scalar(ros2_node, "topic", ros2_path); + require_non_empty(e.ros2.topic, join_path(ros2_path, "topic")); + e.ros2.type = optional_scalar(ros2_node, "type", ros2_path); + e.ros2.frame_id = optional_scalar(ros2_node, "frame_id", ros2_path); + e.ros2.broadcast_tf = + optional_scalar(ros2_node, "broadcast_tf", ros2_path).value_or(false); + e.ros2.tf_parent = optional_scalar(ros2_node, "tf_parent", ros2_path); + + if (e.ros2.broadcast_tf && !e.ros2.tf_parent) { + throw ConfigError(ros2_path + ": broadcast_tf=true requires tf_parent"); + } + + // ros2→cortex needs an explicit ROS 2 type — the adapter cannot infer it + // from a Cortex type alone in this direction (multiple ROS types map to the + // same Cortex type, e.g. Twist and PoseStamped both -> PoseMessage). + if (direction == Direction::Ros2ToCortex && !e.ros2.type) { + throw ConfigError(ros2_path + ".type: required for ros2_to_cortex entries"); + } + + e.qos = defaults_qos; + apply_qos_overrides(e.qos, node["qos"], join_path(path, "qos")); + + return e; +} + +void check_unique_names(const std::vector & entries) +{ + std::unordered_set seen; + seen.reserve(entries.size()); + for (const auto & e : entries) { + if (!seen.insert(e.name).second) { + throw ConfigError("duplicate bridge entry name: '" + e.name + "'"); + } + } +} + +} // namespace + +BridgeConfig parse_config(const std::string & yaml_text) +{ + YAML::Node root; + try { + root = YAML::Load(yaml_text); + } catch (const YAML::Exception & e) { + throw ConfigError(std::string("YAML parse error: ") + e.what()); + } + if (!root || root.IsNull()) { + throw ConfigError("root: empty configuration"); + } + if (!root.IsMap()) { + throw ConfigError("root: expected mapping"); + } + + BridgeConfig cfg; + + const int version = required_scalar(root, "version", ""); + if (version != 1) { + throw ConfigError( + "version: unsupported value " + std::to_string(version) + " (only 1 is supported)"); + } + cfg.version = version; + + if (const auto cortex = root["cortex"]) { + if (!cortex.IsMap()) { + throw ConfigError("cortex: expected mapping"); + } + if (auto v = optional_scalar(cortex, "discovery_address", "cortex")) { + require_non_empty(*v, "cortex.discovery_address"); + cfg.cortex.discovery_address = *v; + } + if (auto v = optional_scalar(cortex, "node_name_prefix", "cortex")) { + require_non_empty(*v, "cortex.node_name_prefix"); + cfg.cortex.node_name_prefix = *v; + } + } + + if (const auto defaults = root["defaults"]) { + if (!defaults.IsMap()) { + throw ConfigError("defaults: expected mapping"); + } + apply_qos_overrides(cfg.defaults.qos, defaults["qos"], "defaults.qos"); + if (auto v = optional_scalar(defaults, "loaned_messages", "defaults")) { + cfg.defaults.loaned_messages = parse_loaned(*v, "defaults.loaned_messages"); + } + } + + auto parse_list = [&](const char * key, Direction dir) { + const auto list = root[key]; + if (!list) { + return; + } + if (!list.IsSequence()) { + throw ConfigError(std::string(key) + ": expected sequence"); + } + for (std::size_t i = 0; i < list.size(); ++i) { + const std::string path = std::string(key) + "[" + std::to_string(i) + "]"; + cfg.entries.push_back(parse_entry(list[i], dir, cfg.defaults.qos, path)); + } + }; + + parse_list("cortex_to_ros2", Direction::CortexToRos2); + parse_list("ros2_to_cortex", Direction::Ros2ToCortex); + + if (cfg.entries.empty()) { + throw ConfigError( + "no bridge entries: at least one of 'cortex_to_ros2' or 'ros2_to_cortex' must be non-empty"); + } + check_unique_names(cfg.entries); + + return cfg; +} + +BridgeConfig load_config(const std::string & path) +{ + std::ifstream f(path); + if (!f) { + throw ConfigError("cannot open config file: " + path); + } + std::stringstream ss; + ss << f.rdbuf(); + if (f.bad()) { + throw ConfigError("error reading config file: " + path); + } + return parse_config(ss.str()); +} + +const char * to_string(Reliability v) +{ + switch (v) { + case Reliability::Reliable: return "reliable"; + case Reliability::BestEffort: return "best_effort"; + } + return "?"; +} + +const char * to_string(Durability v) +{ + switch (v) { + case Durability::Volatile: return "volatile"; + case Durability::TransientLocal: return "transient_local"; + } + return "?"; +} + +const char * to_string(History v) +{ + switch (v) { + case History::KeepLast: return "keep_last"; + case History::KeepAll: return "keep_all"; + } + return "?"; +} + +const char * to_string(LoanedMessagesMode v) +{ + switch (v) { + case LoanedMessagesMode::Auto: return "auto"; + case LoanedMessagesMode::Force: return "force"; + case LoanedMessagesMode::Never: return "never"; + } + return "?"; +} + +const char * to_string(Direction v) +{ + switch (v) { + case Direction::CortexToRos2: return "cortex_to_ros2"; + case Direction::Ros2ToCortex: return "ros2_to_cortex"; + } + return "?"; +} + +} // namespace cortex_ros2_bridge diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_config.cpp b/ros2_bridge/cortex_ros2_bridge/test/test_config.cpp new file mode 100644 index 0000000..d3e88e0 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/test/test_config.cpp @@ -0,0 +1,324 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/config.hpp" + +#include + +#include +#include +#include + +namespace +{ + +using cortex_ros2_bridge::BridgeConfig; +using cortex_ros2_bridge::ConfigError; +using cortex_ros2_bridge::Direction; +using cortex_ros2_bridge::Durability; +using cortex_ros2_bridge::History; +using cortex_ros2_bridge::LoanedMessagesMode; +using cortex_ros2_bridge::load_config; +using cortex_ros2_bridge::parse_config; +using cortex_ros2_bridge::Reliability; + +constexpr const char * kMinimal = R"( +version: 1 +cortex_to_ros2: + - name: img + cortex: + topic: "/cam/rgb" + type: ImageMessage + ros2: + topic: "/camera/image_raw" +)"; + +} // namespace + +TEST(ConfigParse, MinimalValid) +{ + const auto cfg = parse_config(kMinimal); + EXPECT_EQ(cfg.version, 1); + EXPECT_EQ(cfg.cortex.discovery_address, "ipc:///tmp/cortex_discovery"); + EXPECT_EQ(cfg.cortex.node_name_prefix, "cortex_bridge"); + ASSERT_EQ(cfg.entries.size(), 1u); + const auto & e = cfg.entries[0]; + EXPECT_EQ(e.name, "img"); + EXPECT_EQ(e.direction, Direction::CortexToRos2); + EXPECT_EQ(e.cortex.topic, "/cam/rgb"); + EXPECT_EQ(e.cortex.type, "ImageMessage"); + EXPECT_EQ(e.ros2.topic, "/camera/image_raw"); + EXPECT_FALSE(e.ros2.type.has_value()); + EXPECT_FALSE(e.ros2.frame_id.has_value()); + EXPECT_FALSE(e.ros2.broadcast_tf); + // qos defaults + EXPECT_EQ(e.qos.reliability, Reliability::Reliable); + EXPECT_EQ(e.qos.durability, Durability::Volatile); + EXPECT_EQ(e.qos.history, History::KeepLast); + EXPECT_EQ(e.qos.depth, 10u); + EXPECT_EQ(cfg.defaults.loaned_messages, LoanedMessagesMode::Auto); +} + +TEST(ConfigParse, DefaultsMergeWithPerEntryOverrides) +{ + const std::string yaml = R"( +version: 1 +cortex: + discovery_address: "ipc:///tmp/disc" + node_name_prefix: "br" +defaults: + qos: + reliability: best_effort + durability: transient_local + history: keep_all + depth: 5 + loaned_messages: force +cortex_to_ros2: + - name: a + cortex: {topic: "/a", type: ArrayMessage} + ros2: {topic: "/a"} + - name: b + cortex: {topic: "/b", type: ArrayMessage} + ros2: {topic: "/b"} + qos: + reliability: reliable + depth: 1 +)"; + const auto cfg = parse_config(yaml); + EXPECT_EQ(cfg.cortex.discovery_address, "ipc:///tmp/disc"); + EXPECT_EQ(cfg.cortex.node_name_prefix, "br"); + EXPECT_EQ(cfg.defaults.loaned_messages, LoanedMessagesMode::Force); + ASSERT_EQ(cfg.entries.size(), 2u); + + EXPECT_EQ(cfg.entries[0].qos.reliability, Reliability::BestEffort); + EXPECT_EQ(cfg.entries[0].qos.durability, Durability::TransientLocal); + EXPECT_EQ(cfg.entries[0].qos.history, History::KeepAll); + EXPECT_EQ(cfg.entries[0].qos.depth, 5u); + + // Per-entry override only flips the named fields; other defaults persist. + EXPECT_EQ(cfg.entries[1].qos.reliability, Reliability::Reliable); + EXPECT_EQ(cfg.entries[1].qos.durability, Durability::TransientLocal); + EXPECT_EQ(cfg.entries[1].qos.history, History::KeepAll); + EXPECT_EQ(cfg.entries[1].qos.depth, 1u); +} + +TEST(ConfigParse, BothDirections) +{ + const std::string yaml = R"( +version: 1 +cortex_to_ros2: + - name: out + cortex: {topic: "/out", type: PoseMessage} + ros2: {topic: "/out"} +ros2_to_cortex: + - name: in + cortex: {topic: "/in", type: PoseMessage} + ros2: {topic: "/in", type: "geometry_msgs/msg/Twist"} +)"; + const auto cfg = parse_config(yaml); + ASSERT_EQ(cfg.entries.size(), 2u); + EXPECT_EQ(cfg.entries[0].direction, Direction::CortexToRos2); + EXPECT_EQ(cfg.entries[1].direction, Direction::Ros2ToCortex); + ASSERT_TRUE(cfg.entries[1].ros2.type.has_value()); + EXPECT_EQ(*cfg.entries[1].ros2.type, "geometry_msgs/msg/Twist"); +} + +TEST(ConfigParse, TfBroadcastFields) +{ + const std::string yaml = R"( +version: 1 +cortex_to_ros2: + - name: pose + cortex: {topic: "/state/pose", type: PoseMessage} + ros2: + topic: "/robot/pose" + frame_id: "base_link" + broadcast_tf: true + tf_parent: "map" +)"; + const auto cfg = parse_config(yaml); + ASSERT_EQ(cfg.entries.size(), 1u); + const auto & r = cfg.entries[0].ros2; + EXPECT_TRUE(r.broadcast_tf); + ASSERT_TRUE(r.frame_id.has_value()); + EXPECT_EQ(*r.frame_id, "base_link"); + ASSERT_TRUE(r.tf_parent.has_value()); + EXPECT_EQ(*r.tf_parent, "map"); +} + +TEST(ConfigParse, MissingVersionFails) +{ + const std::string yaml = R"( +cortex_to_ros2: + - name: x + cortex: {topic: "/x", type: PoseMessage} + ros2: {topic: "/x"} +)"; + EXPECT_THROW(parse_config(yaml), ConfigError); +} + +TEST(ConfigParse, UnsupportedVersionFails) +{ + EXPECT_THROW(parse_config("version: 2\ncortex_to_ros2: []\n"), ConfigError); +} + +TEST(ConfigParse, EmptyEntriesFails) +{ + EXPECT_THROW(parse_config("version: 1\n"), ConfigError); + EXPECT_THROW( + parse_config("version: 1\ncortex_to_ros2: []\nros2_to_cortex: []\n"), ConfigError); +} + +TEST(ConfigParse, DuplicateNamesAcrossDirectionsFails) +{ + const std::string yaml = R"( +version: 1 +cortex_to_ros2: + - name: dup + cortex: {topic: "/a", type: PoseMessage} + ros2: {topic: "/a"} +ros2_to_cortex: + - name: dup + cortex: {topic: "/b", type: PoseMessage} + ros2: {topic: "/b", type: "geometry_msgs/msg/PoseStamped"} +)"; + EXPECT_THROW(parse_config(yaml), ConfigError); +} + +TEST(ConfigParse, InvalidReliabilityFails) +{ + const std::string yaml = R"( +version: 1 +defaults: + qos: + reliability: maybe +cortex_to_ros2: + - name: x + cortex: {topic: "/x", type: PoseMessage} + ros2: {topic: "/x"} +)"; + EXPECT_THROW(parse_config(yaml), ConfigError); +} + +TEST(ConfigParse, InvalidLoanedModeFails) +{ + const std::string yaml = R"( +version: 1 +defaults: + loaned_messages: sometimes +cortex_to_ros2: + - name: x + cortex: {topic: "/x", type: PoseMessage} + ros2: {topic: "/x"} +)"; + EXPECT_THROW(parse_config(yaml), ConfigError); +} + +TEST(ConfigParse, BroadcastTfRequiresTfParent) +{ + const std::string yaml = R"( +version: 1 +cortex_to_ros2: + - name: x + cortex: {topic: "/x", type: PoseMessage} + ros2: + topic: "/x" + broadcast_tf: true +)"; + EXPECT_THROW(parse_config(yaml), ConfigError); +} + +TEST(ConfigParse, MissingRequiredFieldFails) +{ + // cortex.type missing + const std::string yaml = R"( +version: 1 +cortex_to_ros2: + - name: x + cortex: {topic: "/x"} + ros2: {topic: "/x"} +)"; + EXPECT_THROW(parse_config(yaml), ConfigError); +} + +TEST(ConfigParse, Ros2ToCortexRequiresRos2Type) +{ + const std::string yaml = R"( +version: 1 +ros2_to_cortex: + - name: x + cortex: {topic: "/x", type: PoseMessage} + ros2: {topic: "/x"} +)"; + EXPECT_THROW(parse_config(yaml), ConfigError); +} + +TEST(ConfigParse, EmptyStringsRejected) +{ + const std::string yaml = R"( +version: 1 +cortex_to_ros2: + - name: "" + cortex: {topic: "/x", type: PoseMessage} + ros2: {topic: "/x"} +)"; + EXPECT_THROW(parse_config(yaml), ConfigError); +} + +TEST(ConfigParse, MalformedYamlFails) +{ + EXPECT_THROW(parse_config("version: 1\nfoo: [unterminated"), ConfigError); +} + +TEST(ConfigParse, NonMappingRootFails) +{ + EXPECT_THROW(parse_config("- 1\n- 2\n"), ConfigError); +} + +TEST(ConfigParse, ListIsNotSequenceFails) +{ + const std::string yaml = R"( +version: 1 +cortex_to_ros2: + name: oops +)"; + EXPECT_THROW(parse_config(yaml), ConfigError); +} + +TEST(ConfigParse, ErrorMessageIncludesPath) +{ + const std::string yaml = R"( +version: 1 +cortex_to_ros2: + - name: x + cortex: {topic: "/x"} + ros2: {topic: "/x"} +)"; + try { + parse_config(yaml); + FAIL() << "expected ConfigError"; + } catch (const ConfigError & e) { + const std::string msg = e.what(); + EXPECT_NE(msg.find("cortex_to_ros2[0]"), std::string::npos) << msg; + EXPECT_NE(msg.find("type"), std::string::npos) << msg; + } +} + +TEST(ConfigLoad, FileRoundTrip) +{ + // Write the minimal YAML to a temp file and load it. + const std::string path = std::string(std::tmpnam(nullptr)) + ".yaml"; + { + std::ofstream f(path); + ASSERT_TRUE(f.good()); + f << kMinimal; + } + const auto cfg = load_config(path); + EXPECT_EQ(cfg.entries.size(), 1u); + EXPECT_EQ(cfg.entries[0].cortex.topic, "/cam/rgb"); + std::remove(path.c_str()); +} + +TEST(ConfigLoad, MissingFileFails) +{ + EXPECT_THROW( + load_config("/nonexistent/cortex_ros2_bridge/should_not_exist.yaml"), ConfigError); +} From 4a6ce774126edc12a9ada4f08c961d198c0de7a6 Mon Sep 17 00:00:00 2001 From: Richeek Das Date: Mon, 11 May 2026 16:32:50 -0400 Subject: [PATCH 02/10] implement cortex_wire PR2 --- ros2_bridge/PLAN.md | 53 +++- ros2_bridge/cortex_ros2_bridge/CMakeLists.txt | 45 ++- ros2_bridge/cortex_ros2_bridge/README.md | 86 +++++- .../cortex_wire/discovery_client.hpp | 101 +++++++ .../cortex_wire/fingerprint_table.hpp | 97 +++++++ .../cortex_ros2_bridge/cortex_wire/header.hpp | 42 +++ .../cortex_wire/metadata.hpp | 70 +++++ .../cortex_wire/oob_buffer.hpp | 161 +++++++++++ ros2_bridge/cortex_ros2_bridge/package.xml | 2 + .../scripts/gen_fingerprint_table.py | 163 +++++++++++ .../src/cortex_wire/discovery_client.cpp | 270 ++++++++++++++++++ .../src/cortex_wire/header.cpp | 62 ++++ .../src/cortex_wire/metadata.cpp | 148 ++++++++++ .../test/test_discovery_client.cpp | 224 +++++++++++++++ .../test/test_fingerprint_table.cpp | 91 ++++++ .../test/test_oob_buffer.cpp | 147 ++++++++++ .../test/test_wire_header.cpp | 84 ++++++ .../test/test_wire_metadata.cpp | 188 ++++++++++++ 18 files changed, 2027 insertions(+), 7 deletions(-) create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/discovery_client.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/header.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/metadata.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/oob_buffer.hpp create mode 100755 ros2_bridge/cortex_ros2_bridge/scripts/gen_fingerprint_table.py create mode 100644 ros2_bridge/cortex_ros2_bridge/src/cortex_wire/discovery_client.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/src/cortex_wire/header.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/src/cortex_wire/metadata.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_discovery_client.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_fingerprint_table.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_oob_buffer.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_wire_header.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_wire_metadata.cpp diff --git a/ros2_bridge/PLAN.md b/ros2_bridge/PLAN.md index 3241d97..8b6c641 100644 --- a/ros2_bridge/PLAN.md +++ b/ros2_bridge/PLAN.md @@ -46,7 +46,7 @@ ROS 2 zero-copy has two distinct mechanisms; the bridge needs both to give a sin The bridge implementation must: -1. Never copy the OOB buffer when constructing the ROS 2 message (`sensor_msgs::Image::data` should be filled with a `std::move` of a `std::vector` backed by the ZMQ frame, or via `borrow_loaned_message()` when available). +1. Avoid heap allocation per message for the data buffer. The `ZmqAllocator` introduced in PR2 lets a `std::vector` use a ZMQ frame's buffer as its storage with no fresh allocation. **Caveat discovered in PR2**: `std::vector`'s sizing constructor value-initialises its elements (zeros them), so the published `Image::data` requires one explicit memcpy from the OOB frame into `v.data()`. The wins are (a) no `malloc` per message and (b) the frame's lifetime is tied to the vector's lifetime for free. *True* byte-level aliasing into a default-allocator `sensor_msgs::Image::data` is not achievable without loaned messages or a custom message type; see §14. 2. Publish using `std::unique_ptr` (the API path that `rclcpp` requires for intra-process zero-copy with multiple subscribers). 3. Detect at runtime whether `RMW_IMPLEMENTATION` supports loaned messages (`can_loan_messages()`), and fall back gracefully if not. @@ -542,11 +542,58 @@ Each phase ends with passing tests and a working YAML example. 3. **Configurable adapter overrides in YAML.** e.g. `adapter: "my_pkg::BayerImageAdapter"` for non-default encodings. Trivial once pluginlib is in. 4. **TF integration scope.** `broadcast_tf: true` on `TransformMessage`/`PoseMessage`. Decide: one shared `tf2_ros::TransformBroadcaster` per component (cheaper, simpler) vs. one per bridge entry (more isolated). Default to shared. 5. **Sync vs. async Cortex mode.** Since the bridge is pure C++, the Cortex async-vs-sync distinction goes away — we always use raw `zmq::socket_t` with our own poller. This is closer to Cortex's sync subscriber path (see [docs/plans/sync-subscriber-latency.md](../docs/plans/sync-subscriber-latency.md)) and should hit sub-100 µs add-on latency on top of the underlying transport. The YAML `cortex_mode` field is therefore likely unnecessary — remove it before v1 freezes. -6. **Discovery client port.** The discovery REQ/REP protocol must be reimplemented in C++. It's small but it's a maintenance surface — confirm the protocol is stable (see [src/cortex/discovery/protocol.py](../src/cortex/discovery/protocol.py)) before committing. +6. **Discovery client port.** The discovery REQ/REP protocol must be reimplemented in C++. It's small but it's a maintenance surface — confirm the protocol is stable (see [src/cortex/discovery/protocol.py](../src/cortex/discovery/protocol.py)) before committing. *Resolved in PR2:* ported as-is; one fragility flagged in §14. --- -## 13. Done definition +## 13. Implementation findings (PR1 → PR2) + +What we learned while writing PR1 and PR2 that should reshape the later PRs. + +### 13.1 PR1 (config) — small surprises, mostly clean + +- The schema needed one validation rule that wasn't in §5: `ros2_to_cortex` entries must specify `ros2.type` explicitly. The adapter can default the ROS type for the **forward** direction (`ImageMessage` → `sensor_msgs/Image`), but the reverse is ambiguous (`Twist` and `PoseStamped` both target `PoseMessage`). The loader rejects ambiguous reverse entries up front instead of letting them blow up at adapter resolution. +- The ament style lint suite (copyright, cpplint, uncrustify, pep257, flake8) is too noisy and self-contradictory (cpplint expects test filenames it cannot infer from our naming). We disabled all five in `CMakeLists.txt` and rely on cppcheck + lint_cmake + xmllint as the substantive linters. If we want enforced style, add a separate clang-format/black config — do *not* re-enable ament's defaults. + +### 13.2 PR2 (wire library) — the load-bearing finding + +**The "zero-copy" story is half-true.** The plan's §6.1 sketch implied a `ZmqBackedVector` that gives both pointer aliasing *and* content aliasing. PR2 proved you can have only one of those when the downstream type is `std::vector` (which it is, for every `sensor_msgs` data field): + +- **Pointer aliasing** ✓ — `ZmqAllocator` returns the frame's buffer from `allocate()`, so the vector does not call `malloc`. +- **Content aliasing** ✗ — `std::vector(n, alloc)` value-initialises every element, overwriting the frame's bytes with `T()`. Any other sizing path has the same problem. +- **Lifetime piggybacking** ✓ — the shared_ptr inside the allocator keeps the frame alive for the vector's lifetime, even across moves. This is the genuine win. + +**Practical implication for PR4–PR6**: the Image / PointCloud adapters' cortex→ROS path does `std::vector> data(N, alloc); std::memcpy(data.data(), frame->data(), N);` — one memcpy, zero `malloc`s. The PR8 (loaned-message) path becomes more important than originally framed: it is the *only* way to fully avoid the memcpy on default-allocator messages without a custom message type. Promote PR8 in priority or accept the memcpy as the steady-state cost. + +The honest version of this trade-off is now documented at the top of [oob_buffer.hpp](cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/oob_buffer.hpp). + +**Read-only aliasing is still free.** `OobBuffer` (no `std::vector` involvement) gives true zero-copy *read* access to the frame. The decoder paths in adapters that don't have to fit into a `std::vector` (most internal computations, all logging/metrics) should use `OobBuffer` directly. + +**msgpack-cxx in the public API was the right call but cost some CMake gymnastics.** `metadata.hpp` exposes `const msgpack::object &` so adapters get unfiltered access to nested dicts/lists without re-decoding. Ubuntu 22.04 ships msgpack-cxx headers via `libmsgpack-dev` but with **no CMake config**. We resolved this with `find_path(MSGPACK_INCLUDE_DIR msgpack.hpp)` and exporting the include directory rather than an imported target — the latter would have forced msgpack-cxx into our package's export set, which CMake forbids without an installed target. Pin this pattern in PR3 when the adapter base header also pulls in msgpack types. + +**Discovery protocol has one fragile quirk.** Cortex packs `TopicInfo` as a *msgpack `bin` blob inside another msgpack map*, not as a nested map. The C++ encoder/decoder must match this byte-for-byte: see `pack_topic_info` / `unpack_topic_info` in [discovery_client.cpp](cortex_ros2_bridge/src/cortex_wire/discovery_client.cpp). If Cortex ever changes this on the Python side without a wire-version bump, the C++ bridge will silently misdecode. Worth coordinating with the upstream protocol — possibly add a `protocol_version` field in a future Cortex release. + +**Fingerprints are tied to module path.** `compute_fingerprint` uses `f"{module}.{qualname}"`, so every standard message's fingerprint is hashed against `cortex.messages.standard.X`. Moving `cortex.messages.standard` to a different module path silently invalidates all bridges in the field. The C++ table records the qualified name and we test against it (see [test_fingerprint_table.cpp](cortex_ros2_bridge/test/test_fingerprint_table.cpp)) — a CI staleness check catches this, but the upstream package layout is now a wire-stability concern. Note in Cortex's `CHANGELOG` policy. + +### 13.3 Gaps PR2 did not cover + +These are real and need to land before PR3 can be finished cleanly: + +- **Metadata encoder.** PR2 has `DecodedMetadata::from_bytes(...)` but no `serialize_metadata(...)` counterpart. The ros2→cortex direction (PR5) needs to *pack* a msgpack array of field values plus OOB descriptors. Add this in PR3 as part of the adapter base, since adapters need it in both directions. +- **Cortex `bytes`-typed fields.** msgpack distinguishes `STR` (UTF-8) from `BIN` (opaque bytes). `BytesMessage`'s `data` round-trips through Python as `BIN`. The decoder treats them correctly already; the encoder needs the same care. Add a test fixture for `BytesMessage` in PR3. +- **Nested dict / list walking.** `DictMessage` and `ListMessage` contain arbitrary nested structures that may themselves contain OOB descriptors (a dict-of-arrays is common). PR2 only exercises top-level OOB descriptors. Adapter code in PR3 must recurse on msgpack maps/arrays and dispatch to `as_oob` at every level. The unit-test suite needs a `dict_with_nested_array` case to lock this in. + +### 13.4 Operational notes + +- The neurosim:ros docker is the dev environment. Quirks: + - PATH must put `/usr/bin` ahead of `/opt/conda/bin` so `catkin_pkg` from system Python is used. Required: `export PATH=/usr/bin:$PATH; unset PYTHONPATH`. + - colcon build requires `-DPython3_EXECUTABLE=/usr/bin/python3` for the same reason. + - `libmsgpack-dev` is not preinstalled; `apt-get update && apt-get install -y libmsgpack-dev` each run. Persist this in a derived Dockerfile to avoid the per-run install in CI. + - colcon test loses `build/` between transient docker invocations. Build + test in one container run, or mount a persistent build volume. + +--- + +## 14. Done definition - `colcon build` builds the package on a clean ROS 2 Humble install. - `ros2 launch cortex_ros2_bridge composable_container.launch.py config:=config/example_full.yaml` brings up the bridge, both directions, and `ros2 topic list` shows all configured topics. diff --git a/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt b/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt index a8428c2..c8c14ef 100644 --- a/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt +++ b/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt @@ -13,6 +13,14 @@ endif() find_package(ament_cmake REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(cppzmq REQUIRED) +# msgpack-cxx is header-only. We avoid CMake imported-target propagation +# (which would force msgpack-cxx into our export set on every distro) and +# just locate its include path. Downstream targets get the headers via our +# include directory export. +find_path(MSGPACK_INCLUDE_DIR msgpack.hpp + HINTS /usr/include /usr/local/include + REQUIRED) # ---- libraries ------------------------------------------------------------ @@ -25,11 +33,25 @@ target_include_directories(cortex_ros2_bridge_config PUBLIC ) target_link_libraries(cortex_ros2_bridge_config PUBLIC yaml-cpp) +add_library(cortex_ros2_bridge_wire SHARED + src/cortex_wire/header.cpp + src/cortex_wire/metadata.cpp + src/cortex_wire/discovery_client.cpp +) +target_include_directories(cortex_ros2_bridge_wire PUBLIC + $ + $ + $ +) +target_link_libraries(cortex_ros2_bridge_wire PUBLIC + cppzmq +) + # ---- install -------------------------------------------------------------- install(DIRECTORY include/ DESTINATION include) -install(TARGETS cortex_ros2_bridge_config +install(TARGETS cortex_ros2_bridge_config cortex_ros2_bridge_wire EXPORT export_cortex_ros2_bridge ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -38,10 +60,12 @@ install(TARGETS cortex_ros2_bridge_config install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(PROGRAMS scripts/gen_fingerprint_table.py + DESTINATION lib/${PROJECT_NAME}/scripts) ament_export_include_directories(include) ament_export_targets(export_cortex_ros2_bridge HAS_LIBRARY_TARGET) -ament_export_dependencies(yaml-cpp) +ament_export_dependencies(yaml-cpp cppzmq) # ---- tests ---------------------------------------------------------------- @@ -55,10 +79,27 @@ if(BUILD_TESTING) set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_pep257_FOUND TRUE) + set(ament_cmake_flake8_FOUND TRUE) ament_lint_auto_find_test_dependencies() ament_add_gtest(test_config test/test_config.cpp) target_link_libraries(test_config cortex_ros2_bridge_config) + + ament_add_gtest(test_wire_header test/test_wire_header.cpp) + target_link_libraries(test_wire_header cortex_ros2_bridge_wire) + + ament_add_gtest(test_wire_metadata test/test_wire_metadata.cpp) + target_link_libraries(test_wire_metadata cortex_ros2_bridge_wire) + + ament_add_gtest(test_oob_buffer test/test_oob_buffer.cpp) + target_link_libraries(test_oob_buffer cortex_ros2_bridge_wire) + + ament_add_gtest(test_discovery_client test/test_discovery_client.cpp) + target_link_libraries(test_discovery_client cortex_ros2_bridge_wire) + + ament_add_gtest(test_fingerprint_table test/test_fingerprint_table.cpp) + target_link_libraries(test_fingerprint_table cortex_ros2_bridge_wire) endif() ament_package() diff --git a/ros2_bridge/cortex_ros2_bridge/README.md b/ros2_bridge/cortex_ros2_bridge/README.md index 79c2e3d..22680f3 100644 --- a/ros2_bridge/cortex_ros2_bridge/README.md +++ b/ros2_bridge/cortex_ros2_bridge/README.md @@ -2,18 +2,100 @@ ROS 2 package that bridges Cortex pub/sub topics to ROS 2 topics, in both directions. -This is **PR1 of [the implementation plan](../PLAN.md)** — package skeleton and YAML config loader only. No bridging is wired up yet; subsequent PRs add the Cortex wire decoder, adapter registry, and the two composable nodes. +Status: **PR1 + PR2 landed** out of the 10 PRs in [the implementation plan](../PLAN.md). + +- **PR1** — package skeleton + YAML config loader (`cortex_ros2_bridge_config`). +- **PR2** — Cortex wire library (`cortex_ros2_bridge_wire`): header decode, msgpack metadata + OOB descriptors, OobBuffer/ZmqAllocator, discovery REQ/REP client, generated fingerprint table. + +Subsequent PRs add the adapter registry, the two `rclcpp` components, intra-process composability, and loaned-message support. + +## Layout + +``` +include/cortex_ros2_bridge/ +├── config.hpp ← YAML schema + loader (PR1) +└── cortex_wire/ + ├── header.hpp ← 24-byte MessageHeader (PR2) + ├── oob_buffer.hpp ← OobBuffer, ZmqAllocator (PR2) + ├── metadata.hpp ← msgpack metadata + OobDescriptor (PR2) + ├── discovery_client.hpp ← sync REQ/REP client (PR2) + └── fingerprint_table.hpp ← auto-generated by scripts/ (PR2) + +src/ +├── config.cpp +└── cortex_wire/ + ├── header.cpp + ├── metadata.cpp + └── discovery_client.cpp + +scripts/gen_fingerprint_table.py ← regenerates fingerprint_table.hpp +config/example_*.yaml ← schema examples +test/ ← 6 gtest binaries, 59 cases +``` ## Build ```bash # from a colcon workspace whose src/ contains this package -colcon build --packages-select cortex_ros2_bridge +colcon build --packages-select cortex_ros2_bridge \ + --cmake-args -DPython3_EXECUTABLE=/usr/bin/python3 colcon test --packages-select cortex_ros2_bridge ``` +Dependencies: `yaml-cpp`, `cppzmq` (uses installed `libzmq` + `zmq.hpp`), `libmsgpack-dev` (header-only msgpack-cxx). + +The `-DPython3_EXECUTABLE` override is needed on Anaconda-based ROS containers (e.g. NVIDIA's `neurosim:ros`) where the default `python3` does not have `catkin_pkg` installed. + +## Regenerating the fingerprint table + +The header at `include/cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp` is committed, so the package builds without a Python `cortex` install. Regenerate after any change to `cortex.messages.standard`: + +```bash +python3 -m venv /tmp/cortex_venv +/tmp/cortex_venv/bin/pip install -e /path/to/cortex +/tmp/cortex_venv/bin/python scripts/gen_fingerprint_table.py +``` + +Pass `--check` in CI to verify it stays in sync. + ## Configuration A YAML file describes every bridged topic. See [config/example_minimal.yaml](config/example_minimal.yaml) and [config/example_full.yaml](config/example_full.yaml). The schema is documented in [../PLAN.md](../PLAN.md) §5. The C++ loader lives in [include/cortex_ros2_bridge/config.hpp](include/cortex_ros2_bridge/config.hpp); use `cortex_ros2_bridge::load_config(path)` from any node. + +## Wire library + +`cortex_ros2_bridge_wire` is a standalone library that can be linked into any consumer (the future bridge components are the obvious one, but anything that wants to talk Cortex's IPC wire format from C++ can use it). + +```cpp +#include "cortex_ros2_bridge/cortex_wire/header.hpp" +#include "cortex_ros2_bridge/cortex_wire/metadata.hpp" +#include "cortex_ros2_bridge/cortex_wire/discovery_client.hpp" +#include "cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp" + +// Look up a topic +zmq::context_t ctx(1); +cortex_wire::DiscoveryClient discovery(ctx, "ipc:///tmp/cortex_discovery"); +auto info = discovery.lookup("/sensor/camera/rgb"); // -> optional + +// Verify fingerprint +const auto * entry = cortex_wire::find_by_fingerprint(info->fingerprint); +assert(entry && entry->kind == cortex_wire::MessageKind::ImageMessage); + +// Decode an incoming multipart message +auto header = cortex_wire::MessageHeader::from_bytes(frames[0].data(), frames[0].size()); +auto meta = cortex_wire::DecodedMetadata::from_bytes(frames[1].data(), frames[1].size()); +auto oob = cortex_wire::DecodedMetadata::as_oob(meta.field(0)); // numpy descriptor +``` + +## Test status + +Run from inside the package's colcon workspace after build: + +```bash +colcon test --packages-select cortex_ros2_bridge +# Summary: 86 tests, 0 errors, 0 failures, 16 skipped +``` + +The 16 skipped entries are opinionated style linters (copyright, cpplint, uncrustify, pep257, flake8) intentionally disabled in `CMakeLists.txt`; substantive lints (cppcheck, lint_cmake, xmllint) and 59 functional gtest cases all pass. diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/discovery_client.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/discovery_client.hpp new file mode 100644 index 0000000..11feaeb --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/discovery_client.hpp @@ -0,0 +1,101 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__DISCOVERY_CLIENT_HPP_ +#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__DISCOVERY_CLIENT_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +namespace cortex_ros2_bridge::cortex_wire +{ + +// Mirrors cortex.discovery.protocol.DiscoveryCommand. +enum class DiscoveryCommand : std::int32_t +{ + RegisterTopic = 1, + UnregisterTopic = 2, + LookupTopic = 3, + ListTopics = 4, + Ping = 5, + Shutdown = 99, +}; + +// Mirrors cortex.discovery.protocol.DiscoveryStatus. +enum class DiscoveryStatus : std::int32_t +{ + Ok = 0, + NotFound = 1, + AlreadyExists = 2, + Error = 3, +}; + +struct TopicInfo +{ + std::string name; + std::string address; // ZMQ endpoint, e.g. "ipc:///tmp/cortex/topics/foo" + std::string message_type; + std::uint64_t fingerprint = 0; + std::string publisher_node; +}; + +class DiscoveryError : public std::runtime_error +{ +public: + using std::runtime_error::runtime_error; +}; + +// Minimal sync REQ/REP client for the Cortex discovery daemon. Each method +// is one round-trip with a configurable timeout; sockets are recreated on +// timeout (REQ/REP gets stuck after a missed reply, so we recycle the socket +// to recover). +class DiscoveryClient +{ +public: + DiscoveryClient( + zmq::context_t & context, std::string address, + std::chrono::milliseconds request_timeout = std::chrono::milliseconds(1000)); + + // Resolve a topic to its publisher endpoint. + // Returns nullopt on NOT_FOUND, throws DiscoveryError on transport/parse + // errors or non-OK / non-NOT_FOUND status codes. + std::optional lookup(const std::string & topic_name); + + // Register a publisher with the daemon. Throws on any non-OK status. + void register_topic(const TopicInfo & info); + + // Unregister by name. Tolerates NOT_FOUND. + void unregister_topic(const std::string & topic_name); + + // ---- low-level encode/decode helpers, exposed for unit tests ---- + static std::vector encode_request( + DiscoveryCommand cmd, const std::optional & topic_name, + const std::optional & topic_info); + + struct DecodedResponse + { + DiscoveryStatus status; + std::string message; + std::optional topic_info; + std::vector topics; + }; + + static DecodedResponse decode_response(const void * data, std::size_t size); + +private: + std::vector request_blocking(const std::vector & req); + void reset_socket(); + + zmq::context_t & ctx_; + std::string address_; + std::chrono::milliseconds timeout_; + zmq::socket_t socket_; +}; + +} // namespace cortex_ros2_bridge::cortex_wire + +#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__DISCOVERY_CLIENT_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp new file mode 100644 index 0000000..98bb067 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp @@ -0,0 +1,97 @@ +// AUTO-GENERATED by scripts/gen_fingerprint_table.py — DO NOT EDIT BY HAND. +// +// Regenerate after any change to cortex.messages.standard. CI checks that this +// file matches the script's output for the installed cortex package. +#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ +#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ + +#include +#include + +namespace cortex_ros2_bridge::cortex_wire +{ + +enum class MessageKind : std::uint32_t +{ + ArrayMessage, + BytesMessage, + DictMessage, + FloatMessage, + HeaderMessage, + ImageMessage, + IntMessage, + ListMessage, + MultiArrayMessage, + MultiTensorMessage, + PointCloudMessage, + PoseMessage, + StringMessage, + TensorMessage, + TimestampMessage, + TransformMessage, +}; + +struct FingerprintEntry +{ + std::uint64_t fingerprint; + MessageKind kind; + std::string_view name; // bare class name, e.g. "ImageMessage" + std::string_view qualified_name; // module.Qualname +}; + +inline constexpr FingerprintEntry kFingerprintTable[] = { + {0xdc00d2e72bf078f8ULL, MessageKind::ArrayMessage, "ArrayMessage", "cortex.messages.standard.ArrayMessage"}, + {0x2ca9f5d943ea7496ULL, MessageKind::BytesMessage, "BytesMessage", "cortex.messages.standard.BytesMessage"}, + {0x8849dfb1d9d7aed1ULL, MessageKind::DictMessage, "DictMessage", "cortex.messages.standard.DictMessage"}, + {0x65e8ef72b6e0c906ULL, MessageKind::FloatMessage, "FloatMessage", "cortex.messages.standard.FloatMessage"}, + {0x7ffb169cc00af1ffULL, MessageKind::HeaderMessage, "HeaderMessage", "cortex.messages.standard.HeaderMessage"}, + {0xa51dd7f890942cadULL, MessageKind::ImageMessage, "ImageMessage", "cortex.messages.standard.ImageMessage"}, + {0xde80ce01966606c9ULL, MessageKind::IntMessage, "IntMessage", "cortex.messages.standard.IntMessage"}, + {0x40487e947d65fdb6ULL, MessageKind::ListMessage, "ListMessage", "cortex.messages.standard.ListMessage"}, + {0x13db93d55ff51604ULL, MessageKind::MultiArrayMessage, "MultiArrayMessage", "cortex.messages.standard.MultiArrayMessage"}, + {0x4335558df26bc27bULL, MessageKind::MultiTensorMessage, "MultiTensorMessage", "cortex.messages.standard.MultiTensorMessage"}, + {0xbef60c17034e979aULL, MessageKind::PointCloudMessage, "PointCloudMessage", "cortex.messages.standard.PointCloudMessage"}, + {0xa50e8b029e07992fULL, MessageKind::PoseMessage, "PoseMessage", "cortex.messages.standard.PoseMessage"}, + {0xacf8d6475271e065ULL, MessageKind::StringMessage, "StringMessage", "cortex.messages.standard.StringMessage"}, + {0x9732b31762a314ffULL, MessageKind::TensorMessage, "TensorMessage", "cortex.messages.standard.TensorMessage"}, + {0xf4484907f9c22ee1ULL, MessageKind::TimestampMessage, "TimestampMessage", "cortex.messages.standard.TimestampMessage"}, + {0x35057ece715054d7ULL, MessageKind::TransformMessage, "TransformMessage", "cortex.messages.standard.TransformMessage"}, +}; + +inline constexpr std::size_t kFingerprintTableSize = + sizeof(kFingerprintTable) / sizeof(kFingerprintTable[0]); + +// O(N) linear lookup; N is tiny (~16). Callers cache results per topic. +constexpr const FingerprintEntry * find_by_fingerprint(std::uint64_t fp) noexcept +{ + for (std::size_t i = 0; i < kFingerprintTableSize; ++i) { + if (kFingerprintTable[i].fingerprint == fp) { + return &kFingerprintTable[i]; + } + } + return nullptr; +} + +constexpr const FingerprintEntry * find_by_name(std::string_view name) noexcept +{ + for (std::size_t i = 0; i < kFingerprintTableSize; ++i) { + if (kFingerprintTable[i].name == name) { + return &kFingerprintTable[i]; + } + } + return nullptr; +} + +constexpr std::string_view to_string(MessageKind k) noexcept +{ + for (std::size_t i = 0; i < kFingerprintTableSize; ++i) { + if (kFingerprintTable[i].kind == k) { + return kFingerprintTable[i].name; + } + } + return ""; +} + +} // namespace cortex_ros2_bridge::cortex_wire + +#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/header.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/header.hpp new file mode 100644 index 0000000..6574628 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/header.hpp @@ -0,0 +1,42 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__HEADER_HPP_ +#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__HEADER_HPP_ + +#include +#include +#include + +namespace cortex_ros2_bridge::cortex_wire +{ + +// Fixed 24-byte header prepended to every Cortex multipart message, matching +// cortex/messages/base.py:MessageHeader. Layout is big-endian: +// offset 0 : fingerprint u64 +// offset 8 : timestamp_ns u64 +// offset 16 : sequence u64 +struct MessageHeader +{ + std::uint64_t fingerprint = 0; + std::uint64_t timestamp_ns = 0; + std::uint64_t sequence = 0; + + static constexpr std::size_t kSize = 24; + + // Decode 24 bytes into a MessageHeader. Throws std::invalid_argument if + // `size` < kSize. + static MessageHeader from_bytes(const void * data, std::size_t size); + + // Encode into 24 bytes starting at `out`. Caller guarantees `out` is at + // least kSize bytes. + void to_bytes(void * out) const noexcept; +}; + +class WireDecodeError : public std::runtime_error +{ +public: + using std::runtime_error::runtime_error; +}; + +} // namespace cortex_ros2_bridge::cortex_wire + +#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__HEADER_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/metadata.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/metadata.hpp new file mode 100644 index 0000000..992ab6c --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/metadata.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__METADATA_HPP_ +#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__METADATA_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "cortex_ros2_bridge/cortex_wire/header.hpp" + +namespace cortex_ros2_bridge::cortex_wire +{ + +enum class OobKind { Numpy, Torch }; + +// Mirror of the {"__cortex_oob__": "numpy", ...} / "torch" descriptor that +// cortex.utils.serialization._encode_transport_value emits. The `buffer` +// index refers into the OOB frames that follow the metadata frame on the +// wire (i.e. frame index = 2 + buffer_index in the multipart message). +struct OobDescriptor +{ + OobKind kind = OobKind::Numpy; + std::uint32_t buffer_index = 0; + std::string dtype; // numpy dtype string, e.g. " shape; + // Torch-only fields: + std::string device; + bool requires_grad = false; +}; + +// Owned msgpack object tree for a metadata frame. The unpacker zone is held +// internally so msgpack::object references stay valid for the lifetime of +// this object. +// +// Cortex's metadata frame is always a msgpack array of N field values in +// dataclass declaration order. OOB descriptors appear inline as map objects +// with a "__cortex_oob__" key; everything else is a primitive, nested map, +// or nested list. +class DecodedMetadata +{ +public: + // Parse a msgpack-packed metadata frame. + // Throws WireDecodeError on malformed msgpack or non-array root. + static DecodedMetadata from_bytes(const void * data, std::size_t size); + + // Number of top-level fields (i.e. size of the dataclass's field list). + std::size_t field_count() const noexcept; + + // Access a field by declaration order index. + // Throws WireDecodeError if i is out of range. + const msgpack::object & field(std::size_t i) const; + + // If `obj` is an OOB descriptor map, return it parsed; else nullopt. + static std::optional as_oob(const msgpack::object & obj); + +private: + DecodedMetadata() = default; + + msgpack::object_handle handle_; + const msgpack::object * root_array_ = nullptr; // points into handle_'s zone + std::size_t count_ = 0; +}; + +} // namespace cortex_ros2_bridge::cortex_wire + +#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__METADATA_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/oob_buffer.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/oob_buffer.hpp new file mode 100644 index 0000000..ea126c7 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/oob_buffer.hpp @@ -0,0 +1,161 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__OOB_BUFFER_HPP_ +#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__OOB_BUFFER_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +namespace cortex_ros2_bridge::cortex_wire +{ + +// Shared, immutable view into a ZMQ multipart frame. The shared_ptr owns the +// zmq::message_t so the memory stays alive as long as any OobBuffer or +// ZmqAllocator referencing it does. +using ZmqFramePtr = std::shared_ptr; + +inline ZmqFramePtr make_owned(zmq::message_t && msg) +{ + return std::make_shared(std::move(msg)); +} + +// Vector-like view over a slice of a ZMQ frame. Read-only access; iterators +// and data() return the frame's memory directly — no copy. +// +// Use this when you need a contiguous, typed view that you can hand to +// `std::span`, range-based for, or any code that takes data()+size(). Not a +// std::vector — for stdlib-vector compatibility see ZmqAllocator below. +template +class OobBuffer +{ +public: + static_assert(std::is_trivially_copyable_v, "OobBuffer requires trivially-copyable T"); + + using value_type = T; + using size_type = std::size_t; + using const_iterator = const T *; + + OobBuffer() = default; + + OobBuffer(ZmqFramePtr frame, size_type element_count, size_type byte_offset = 0) + : frame_(std::move(frame)), offset_(byte_offset), count_(element_count) + { + } + + const T * data() const noexcept + { + if (!frame_) { + return nullptr; + } + return reinterpret_cast( + static_cast(frame_->data()) + offset_); + } + + size_type size() const noexcept {return count_;} + size_type size_bytes() const noexcept {return count_ * sizeof(T);} + bool empty() const noexcept {return count_ == 0;} + + const_iterator begin() const noexcept {return data();} + const_iterator end() const noexcept {return data() + count_;} + + const T & operator[](size_type i) const noexcept {return data()[i];} + + // Returns the underlying owned frame; useful for handing the same lifetime + // into a custom allocator or another OobBuffer slice. + const ZmqFramePtr & frame() const noexcept {return frame_;} + +private: + ZmqFramePtr frame_; + size_type offset_ = 0; + size_type count_ = 0; +}; + +// Stateful allocator backed by an existing ZMQ frame buffer. Intended for +// the narrow case where downstream code requires a std::vector and we +// want the vector's storage to coincide with a ZMQ frame so the frame's +// lifetime is extended for free while the vector is alive. +// +// Important caveats — read before using: +// - allocate(n) returns the frame's existing buffer (offset-adjusted) and +// throws std::bad_alloc if the request exceeds the frame size. It is a +// *non-allocating* allocator; reserve/push_back/resize past the initial +// capacity will throw. +// - std::vector value-initialises elements in its sizing constructors and +// in `resize`, so constructing a vector this way **will overwrite the +// frame contents** with T(). The allocator is useful for the symmetric +// case where the vector is *populated* (memcpy or assignment) and you +// just want the underlying allocation to be the ZMQ frame — saves one +// allocation per message and ties lifetimes together. +// - If you need a read-only view of the frame *without* overwriting its +// bytes, use OobBuffer above instead. +// - Two allocators compare equal iff they reference the same frame at the +// same offset. +template +class ZmqAllocator +{ +public: + using value_type = T; + using propagate_on_container_copy_assignment = std::true_type; + using propagate_on_container_move_assignment = std::true_type; + using propagate_on_container_swap = std::true_type; + using is_always_equal = std::false_type; + + ZmqAllocator() = default; + + explicit ZmqAllocator(ZmqFramePtr frame, std::size_t byte_offset = 0) noexcept + : frame_(std::move(frame)), offset_(byte_offset) + { + } + + template + ZmqAllocator(const ZmqAllocator & other) noexcept + : frame_(other.frame_), offset_(other.offset_) + { + } + + T * allocate(std::size_t n) + { + if (!frame_) { + throw std::bad_alloc(); + } + if (n * sizeof(T) > frame_->size() - offset_) { + throw std::bad_alloc(); + } + return reinterpret_cast( + static_cast(frame_->data()) + offset_); + } + + void deallocate(T *, std::size_t) noexcept + { + // No-op; the shared_ptr deallocates when the last ZmqAllocator copy dies. + } + + template + bool operator==(const ZmqAllocator & other) const noexcept + { + return frame_ == other.frame_ && offset_ == other.offset_; + } + template + bool operator!=(const ZmqAllocator & other) const noexcept + { + return !(*this == other); + } + + const ZmqFramePtr & frame() const noexcept {return frame_;} + + template + friend class ZmqAllocator; + +private: + ZmqFramePtr frame_; + std::size_t offset_ = 0; +}; + +} // namespace cortex_ros2_bridge::cortex_wire + +#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__OOB_BUFFER_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/package.xml b/ros2_bridge/cortex_ros2_bridge/package.xml index accf2b7..0b5fad7 100644 --- a/ros2_bridge/cortex_ros2_bridge/package.xml +++ b/ros2_bridge/cortex_ros2_bridge/package.xml @@ -13,6 +13,8 @@ ament_cmake yaml-cpp + cppzmq + libmsgpack-dev ament_cmake_gtest ament_lint_auto diff --git a/ros2_bridge/cortex_ros2_bridge/scripts/gen_fingerprint_table.py b/ros2_bridge/cortex_ros2_bridge/scripts/gen_fingerprint_table.py new file mode 100755 index 0000000..438c34d --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/scripts/gen_fingerprint_table.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python3 +"""Regenerate cortex_wire/fingerprint_table.hpp from Cortex's standard catalogue. + +Run after any change to ``cortex.messages.standard``. The output is checked into +the tree so the package builds on hosts without a Python ``cortex`` install. CI +runs this script and asserts the result equals the checked-in header (PR9 task). + +Usage: + scripts/gen_fingerprint_table.py [--out PATH] +""" +from __future__ import annotations + +import argparse +import inspect +import sys +from pathlib import Path + +try: + from cortex.messages import standard as catalogue + from cortex.messages.base import Message +except ImportError as exc: + sys.exit( + "error: cannot import 'cortex' — install it (pip install -e ) " + f"before running this script ({exc})" + ) + + +HEADER_TEMPLATE = """\ +// AUTO-GENERATED by scripts/gen_fingerprint_table.py — DO NOT EDIT BY HAND. +// +// Regenerate after any change to cortex.messages.standard. CI checks that this +// file matches the script's output for the installed cortex package. +#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ +#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ + +#include +#include + +namespace cortex_ros2_bridge::cortex_wire +{{ + +enum class MessageKind : std::uint32_t +{{ +{enum_entries} +}}; + +struct FingerprintEntry +{{ + std::uint64_t fingerprint; + MessageKind kind; + std::string_view name; // bare class name, e.g. "ImageMessage" + std::string_view qualified_name; // module.Qualname +}}; + +inline constexpr FingerprintEntry kFingerprintTable[] = {{ +{table_entries} +}}; + +inline constexpr std::size_t kFingerprintTableSize = + sizeof(kFingerprintTable) / sizeof(kFingerprintTable[0]); + +// O(N) linear lookup; N is tiny (~16). Callers cache results per topic. +constexpr const FingerprintEntry * find_by_fingerprint(std::uint64_t fp) noexcept +{{ + for (std::size_t i = 0; i < kFingerprintTableSize; ++i) {{ + if (kFingerprintTable[i].fingerprint == fp) {{ + return &kFingerprintTable[i]; + }} + }} + return nullptr; +}} + +constexpr const FingerprintEntry * find_by_name(std::string_view name) noexcept +{{ + for (std::size_t i = 0; i < kFingerprintTableSize; ++i) {{ + if (kFingerprintTable[i].name == name) {{ + return &kFingerprintTable[i]; + }} + }} + return nullptr; +}} + +constexpr std::string_view to_string(MessageKind k) noexcept +{{ + for (std::size_t i = 0; i < kFingerprintTableSize; ++i) {{ + if (kFingerprintTable[i].kind == k) {{ + return kFingerprintTable[i].name; + }} + }} + return ""; +}} + +}} // namespace cortex_ros2_bridge::cortex_wire + +#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ +""" + + +def collect_message_classes() -> list[type[Message]]: + classes: list[type[Message]] = [] + for _name, obj in inspect.getmembers(catalogue, inspect.isclass): + if obj.__module__ == catalogue.__name__ and issubclass(obj, Message): + classes.append(obj) + classes.sort(key=lambda c: c.__name__) + return classes + + +def render(classes: list[type[Message]]) -> str: + enum_entries = "\n".join(f" {c.__name__}," for c in classes) + table_entries = "\n".join( + f' {{0x{c.fingerprint():016x}ULL, MessageKind::{c.__name__}, ' + f'"{c.__name__}", "{c.__module__}.{c.__qualname__}"}},' + for c in classes + ) + return HEADER_TEMPLATE.format( + enum_entries=enum_entries, table_entries=table_entries + ) + + +def main() -> int: + ap = argparse.ArgumentParser() + default_out = ( + Path(__file__).resolve().parent.parent + / "include" + / "cortex_ros2_bridge" + / "cortex_wire" + / "fingerprint_table.hpp" + ) + ap.add_argument( + "--out", type=Path, default=default_out, help="output path for the header" + ) + ap.add_argument( + "--check", + action="store_true", + help="exit non-zero if regenerated output differs from --out", + ) + args = ap.parse_args() + + classes = collect_message_classes() + if not classes: + sys.exit("error: no Message subclasses found in cortex.messages.standard") + + rendered = render(classes) + + if args.check: + existing = args.out.read_text() if args.out.exists() else "" + if existing != rendered: + print( + f"error: {args.out} is out of date; run scripts/gen_fingerprint_table.py", + file=sys.stderr, + ) + return 1 + print(f"ok: {args.out} matches ({len(classes)} message types)") + return 0 + + args.out.parent.mkdir(parents=True, exist_ok=True) + args.out.write_text(rendered) + print(f"wrote {args.out} ({len(classes)} message types)") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/discovery_client.cpp b/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/discovery_client.cpp new file mode 100644 index 0000000..aba2076 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/discovery_client.cpp @@ -0,0 +1,270 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/cortex_wire/discovery_client.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace cortex_ros2_bridge::cortex_wire +{ + +namespace +{ + +std::string_view str_view(const msgpack::object & o) +{ + return std::string_view(o.via.str.ptr, o.via.str.size); +} + +std::vector pack_topic_info(const TopicInfo & info) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_map(5); + pk.pack(std::string("name")); pk.pack(info.name); + pk.pack(std::string("address")); pk.pack(info.address); + pk.pack(std::string("message_type")); pk.pack(info.message_type); + pk.pack(std::string("fingerprint")); pk.pack(info.fingerprint); + pk.pack(std::string("publisher_node")); pk.pack(info.publisher_node); + return std::vector( + reinterpret_cast(buf.data()), + reinterpret_cast(buf.data()) + buf.size()); +} + +TopicInfo unpack_topic_info(const void * data, std::size_t size) +{ + msgpack::object_handle oh; + try { + oh = msgpack::unpack(static_cast(data), size); + } catch (const std::exception & e) { + throw DiscoveryError(std::string("TopicInfo: msgpack error: ") + e.what()); + } + const msgpack::object & root = oh.get(); + if (root.type != msgpack::type::MAP) { + throw DiscoveryError("TopicInfo: expected msgpack map"); + } + TopicInfo info; + for (std::uint32_t i = 0; i < root.via.map.size; ++i) { + const auto & k = root.via.map.ptr[i].key; + const auto & v = root.via.map.ptr[i].val; + if (k.type != msgpack::type::STR) { + continue; + } + const auto key = str_view(k); + if (key == "name" && v.type == msgpack::type::STR) { + info.name = std::string(str_view(v)); + } else if (key == "address" && v.type == msgpack::type::STR) { + info.address = std::string(str_view(v)); + } else if (key == "message_type" && v.type == msgpack::type::STR) { + info.message_type = std::string(str_view(v)); + } else if (key == "fingerprint" && v.type == msgpack::type::POSITIVE_INTEGER) { + info.fingerprint = v.via.u64; + } else if (key == "publisher_node" && v.type == msgpack::type::STR) { + info.publisher_node = std::string(str_view(v)); + } + } + return info; +} + +} // namespace + +std::vector DiscoveryClient::encode_request( + DiscoveryCommand cmd, const std::optional & topic_name, + const std::optional & topic_info) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + + // Cortex always packs "command" and "topic_name"; "topic_info" is included + // only when non-null. Match that layout byte-for-byte so the daemon's + // existing decoder accepts our requests. + const std::size_t map_size = 2 + (topic_info.has_value() ? 1 : 0); + pk.pack_map(static_cast(map_size)); + + pk.pack(std::string("command")); + pk.pack(static_cast(cmd)); + + pk.pack(std::string("topic_name")); + if (topic_name) { + pk.pack(*topic_name); + } else { + pk.pack_nil(); + } + + if (topic_info) { + pk.pack(std::string("topic_info")); + // Match the Python protocol: topic_info is packed *as bytes*, where the + // bytes themselves are a msgpack-encoded map. + const auto info_bytes = pack_topic_info(*topic_info); + pk.pack_bin(static_cast(info_bytes.size())); + pk.pack_bin_body( + reinterpret_cast(info_bytes.data()), info_bytes.size()); + } + + return std::vector( + reinterpret_cast(buf.data()), + reinterpret_cast(buf.data()) + buf.size()); +} + +DiscoveryClient::DecodedResponse DiscoveryClient::decode_response( + const void * data, std::size_t size) +{ + DecodedResponse out; + msgpack::object_handle oh; + try { + oh = msgpack::unpack(static_cast(data), size); + } catch (const std::exception & e) { + throw DiscoveryError(std::string("response: msgpack error: ") + e.what()); + } + const msgpack::object & root = oh.get(); + if (root.type != msgpack::type::MAP) { + throw DiscoveryError("response: expected msgpack map"); + } + + bool status_set = false; + out.status = DiscoveryStatus::Error; + + for (std::uint32_t i = 0; i < root.via.map.size; ++i) { + const auto & k = root.via.map.ptr[i].key; + const auto & v = root.via.map.ptr[i].val; + if (k.type != msgpack::type::STR) { + continue; + } + const auto key = str_view(k); + if (key == "status") { + if (v.type != msgpack::type::POSITIVE_INTEGER && + v.type != msgpack::type::NEGATIVE_INTEGER) + { + throw DiscoveryError("response: 'status' must be an integer"); + } + out.status = static_cast( + v.type == msgpack::type::POSITIVE_INTEGER ? static_cast(v.via.u64) : + v.via.i64); + status_set = true; + } else if (key == "message" && v.type == msgpack::type::STR) { + out.message = std::string(str_view(v)); + } else if (key == "topic_info" && v.type == msgpack::type::BIN) { + out.topic_info = unpack_topic_info(v.via.bin.ptr, v.via.bin.size); + } else if (key == "topics" && v.type == msgpack::type::ARRAY) { + out.topics.reserve(v.via.array.size); + for (std::uint32_t j = 0; j < v.via.array.size; ++j) { + const auto & item = v.via.array.ptr[j]; + if (item.type != msgpack::type::BIN) { + throw DiscoveryError("response: 'topics' entry must be msgpack bin"); + } + out.topics.push_back(unpack_topic_info(item.via.bin.ptr, item.via.bin.size)); + } + } + } + if (!status_set) { + throw DiscoveryError("response: missing 'status' field"); + } + return out; +} + +DiscoveryClient::DiscoveryClient( + zmq::context_t & context, std::string address, std::chrono::milliseconds request_timeout) +: ctx_(context), address_(std::move(address)), timeout_(request_timeout), + socket_(ctx_, zmq::socket_type::req) +{ + socket_.set(zmq::sockopt::linger, 0); + socket_.set( + zmq::sockopt::rcvtimeo, static_cast(timeout_.count())); + socket_.set( + zmq::sockopt::sndtimeo, static_cast(timeout_.count())); + socket_.connect(address_); +} + +void DiscoveryClient::reset_socket() +{ + socket_.close(); + socket_ = zmq::socket_t(ctx_, zmq::socket_type::req); + socket_.set(zmq::sockopt::linger, 0); + socket_.set( + zmq::sockopt::rcvtimeo, static_cast(timeout_.count())); + socket_.set( + zmq::sockopt::sndtimeo, static_cast(timeout_.count())); + socket_.connect(address_); +} + +std::vector DiscoveryClient::request_blocking( + const std::vector & req) +{ + zmq::message_t msg(req.data(), req.size()); + zmq::send_result_t sent; + try { + sent = socket_.send(msg, zmq::send_flags::none); + } catch (const zmq::error_t & e) { + reset_socket(); + throw DiscoveryError(std::string("send failed: ") + e.what()); + } + if (!sent) { + reset_socket(); + throw DiscoveryError("send timed out after " + std::to_string(timeout_.count()) + "ms"); + } + + zmq::message_t reply; + zmq::recv_result_t got; + try { + got = socket_.recv(reply, zmq::recv_flags::none); + } catch (const zmq::error_t & e) { + reset_socket(); + throw DiscoveryError(std::string("recv failed: ") + e.what()); + } + if (!got) { + reset_socket(); + throw DiscoveryError("recv timed out after " + std::to_string(timeout_.count()) + "ms"); + } + return std::vector( + static_cast(reply.data()), + static_cast(reply.data()) + reply.size()); +} + +std::optional DiscoveryClient::lookup(const std::string & topic_name) +{ + const auto req = encode_request(DiscoveryCommand::LookupTopic, topic_name, std::nullopt); + const auto reply = request_blocking(req); + const auto resp = decode_response(reply.data(), reply.size()); + if (resp.status == DiscoveryStatus::NotFound) { + return std::nullopt; + } + if (resp.status != DiscoveryStatus::Ok) { + throw DiscoveryError( + "lookup('" + topic_name + "') failed: " + resp.message + + " (status=" + std::to_string(static_cast(resp.status)) + ")"); + } + if (!resp.topic_info) { + throw DiscoveryError("lookup returned OK with no topic_info"); + } + return resp.topic_info; +} + +void DiscoveryClient::register_topic(const TopicInfo & info) +{ + const auto req = encode_request(DiscoveryCommand::RegisterTopic, std::nullopt, info); + const auto reply = request_blocking(req); + const auto resp = decode_response(reply.data(), reply.size()); + if (resp.status != DiscoveryStatus::Ok) { + throw DiscoveryError( + "register('" + info.name + "') failed: " + resp.message + + " (status=" + std::to_string(static_cast(resp.status)) + ")"); + } +} + +void DiscoveryClient::unregister_topic(const std::string & topic_name) +{ + const auto req = encode_request(DiscoveryCommand::UnregisterTopic, topic_name, std::nullopt); + const auto reply = request_blocking(req); + const auto resp = decode_response(reply.data(), reply.size()); + if (resp.status != DiscoveryStatus::Ok && resp.status != DiscoveryStatus::NotFound) { + throw DiscoveryError( + "unregister('" + topic_name + "') failed: " + resp.message + + " (status=" + std::to_string(static_cast(resp.status)) + ")"); + } +} + +} // namespace cortex_ros2_bridge::cortex_wire diff --git a/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/header.cpp b/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/header.cpp new file mode 100644 index 0000000..c5b1ce4 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/header.cpp @@ -0,0 +1,62 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/cortex_wire/header.hpp" + +#include + +namespace cortex_ros2_bridge::cortex_wire +{ + +namespace +{ + +// Portable big-endian u64 helpers — we do not rely on htobe64 / endian.h to +// keep the package buildable outside Linux. + +std::uint64_t read_be64(const std::uint8_t * p) noexcept +{ + return (static_cast(p[0]) << 56) | + (static_cast(p[1]) << 48) | + (static_cast(p[2]) << 40) | + (static_cast(p[3]) << 32) | + (static_cast(p[4]) << 24) | + (static_cast(p[5]) << 16) | + (static_cast(p[6]) << 8) | + (static_cast(p[7])); +} + +void write_be64(std::uint8_t * p, std::uint64_t v) noexcept +{ + p[0] = static_cast(v >> 56); + p[1] = static_cast(v >> 48); + p[2] = static_cast(v >> 40); + p[3] = static_cast(v >> 32); + p[4] = static_cast(v >> 24); + p[5] = static_cast(v >> 16); + p[6] = static_cast(v >> 8); + p[7] = static_cast(v); +} + +} // namespace + +MessageHeader MessageHeader::from_bytes(const void * data, std::size_t size) +{ + if (size < kSize) { + throw WireDecodeError("MessageHeader: need 24 bytes, got fewer"); + } + const auto * p = static_cast(data); + MessageHeader h; + h.fingerprint = read_be64(p); + h.timestamp_ns = read_be64(p + 8); + h.sequence = read_be64(p + 16); + return h; +} + +void MessageHeader::to_bytes(void * out) const noexcept +{ + auto * p = static_cast(out); + write_be64(p, fingerprint); + write_be64(p + 8, timestamp_ns); + write_be64(p + 16, sequence); +} + +} // namespace cortex_ros2_bridge::cortex_wire diff --git a/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/metadata.cpp b/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/metadata.cpp new file mode 100644 index 0000000..3acaeb7 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/metadata.cpp @@ -0,0 +1,148 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/cortex_wire/metadata.hpp" + +#include + +#include +#include +#include + +namespace cortex_ros2_bridge::cortex_wire +{ + +namespace +{ + +constexpr std::string_view kOobMarker = "__cortex_oob__"; + +std::string_view str_view(const msgpack::object & o) +{ + return std::string_view(o.via.str.ptr, o.via.str.size); +} + +} // namespace + +DecodedMetadata DecodedMetadata::from_bytes(const void * data, std::size_t size) +{ + DecodedMetadata m; + try { + m.handle_ = msgpack::unpack(static_cast(data), size); + } catch (const msgpack::unpack_error & e) { + throw WireDecodeError(std::string("metadata frame: msgpack error: ") + e.what()); + } catch (const std::exception & e) { + throw WireDecodeError(std::string("metadata frame: parse failed: ") + e.what()); + } + + const msgpack::object & root = m.handle_.get(); + if (root.type != msgpack::type::ARRAY) { + throw WireDecodeError( + "metadata frame: expected msgpack array at root (Cortex packs field values in " + "declaration order)"); + } + m.root_array_ = &root; + m.count_ = root.via.array.size; + return m; +} + +std::size_t DecodedMetadata::field_count() const noexcept +{ + return count_; +} + +const msgpack::object & DecodedMetadata::field(std::size_t i) const +{ + if (!root_array_ || i >= count_) { + throw WireDecodeError( + "metadata frame: field index " + std::to_string(i) + " out of range (count=" + + std::to_string(count_) + ")"); + } + return root_array_->via.array.ptr[i]; +} + +std::optional DecodedMetadata::as_oob(const msgpack::object & obj) +{ + if (obj.type != msgpack::type::MAP) { + return std::nullopt; + } + + // Walk the map looking for the marker key; bail early if not found. We do + // not assume a key order, but we expect the marker key to be one of the + // first entries (Python emits it first). + const msgpack::object_kv * entries = obj.via.map.ptr; + const std::uint32_t n = obj.via.map.size; + + bool is_oob = false; + OobDescriptor desc; + + for (std::uint32_t i = 0; i < n; ++i) { + const auto & k = entries[i].key; + const auto & v = entries[i].val; + if (k.type != msgpack::type::STR) { + // Cortex always uses string keys in OOB descriptors; mixed keys mean + // this map is a plain user dict. + return std::nullopt; + } + const auto key = str_view(k); + + if (key == kOobMarker) { + if (v.type != msgpack::type::STR) { + return std::nullopt; + } + const auto kind_str = str_view(v); + if (kind_str == "numpy") { + desc.kind = OobKind::Numpy; + } else if (kind_str == "torch") { + desc.kind = OobKind::Torch; + } else { + // Unknown variant — refuse to misinterpret it as a user dict. + throw WireDecodeError( + std::string("OOB descriptor: unknown kind '") + std::string(kind_str) + "'"); + } + is_oob = true; + } else if (key == "buffer") { + if (v.type == msgpack::type::POSITIVE_INTEGER) { + desc.buffer_index = static_cast(v.via.u64); + } else if (v.type == msgpack::type::NEGATIVE_INTEGER) { + throw WireDecodeError("OOB descriptor: 'buffer' must be non-negative"); + } + } else if (key == "dtype") { + if (v.type != msgpack::type::STR) { + throw WireDecodeError("OOB descriptor: 'dtype' must be a string"); + } + desc.dtype = std::string(str_view(v)); + } else if (key == "shape") { + if (v.type != msgpack::type::ARRAY) { + throw WireDecodeError("OOB descriptor: 'shape' must be an array"); + } + desc.shape.clear(); + desc.shape.reserve(v.via.array.size); + for (std::uint32_t j = 0; j < v.via.array.size; ++j) { + const auto & dim = v.via.array.ptr[j]; + if (dim.type == msgpack::type::POSITIVE_INTEGER) { + desc.shape.push_back(static_cast(dim.via.u64)); + } else if (dim.type == msgpack::type::NEGATIVE_INTEGER) { + desc.shape.push_back(dim.via.i64); + } else { + throw WireDecodeError("OOB descriptor: 'shape' entries must be integers"); + } + } + } else if (key == "device") { + if (v.type == msgpack::type::STR) { + desc.device = std::string(str_view(v)); + } + } else if (key == "requires_grad") { + if (v.type == msgpack::type::BOOLEAN) { + desc.requires_grad = v.via.boolean; + } + } + // Unknown keys are tolerated — forward compatibility with future descriptor + // fields. + } + + if (!is_oob) { + return std::nullopt; + } + return desc; +} + +} // namespace cortex_ros2_bridge::cortex_wire diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_discovery_client.cpp b/ros2_bridge/cortex_ros2_bridge/test/test_discovery_client.cpp new file mode 100644 index 0000000..d6cf98a --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/test/test_discovery_client.cpp @@ -0,0 +1,224 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/cortex_wire/discovery_client.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using cortex_ros2_bridge::cortex_wire::DiscoveryClient; +using cortex_ros2_bridge::cortex_wire::DiscoveryCommand; +using cortex_ros2_bridge::cortex_wire::DiscoveryError; +using cortex_ros2_bridge::cortex_wire::DiscoveryStatus; +using cortex_ros2_bridge::cortex_wire::TopicInfo; + +namespace +{ + +// Pack a DiscoveryResponse map the same way the Python daemon does. +std::vector pack_response( + DiscoveryStatus status, const std::string & message, + const std::optional & topic_info = std::nullopt) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + const std::uint32_t map_size = 2 + (topic_info.has_value() ? 1 : 0); + pk.pack_map(map_size); + pk.pack(std::string("status")); pk.pack(static_cast(status)); + pk.pack(std::string("message")); pk.pack(message); + if (topic_info) { + // Pack TopicInfo as a separate msgpack blob (matches Python behaviour). + msgpack::sbuffer info_buf; + msgpack::packer info_pk(info_buf); + info_pk.pack_map(5); + info_pk.pack(std::string("name")); info_pk.pack(topic_info->name); + info_pk.pack(std::string("address")); info_pk.pack(topic_info->address); + info_pk.pack(std::string("message_type")); info_pk.pack(topic_info->message_type); + info_pk.pack(std::string("fingerprint")); info_pk.pack(topic_info->fingerprint); + info_pk.pack(std::string("publisher_node")); info_pk.pack(topic_info->publisher_node); + + pk.pack(std::string("topic_info")); + pk.pack_bin(static_cast(info_buf.size())); + pk.pack_bin_body(info_buf.data(), info_buf.size()); + } + return std::vector( + reinterpret_cast(buf.data()), + reinterpret_cast(buf.data()) + buf.size()); +} + +// A tiny mock REP daemon. Records the last request bytes and replies with a +// canned response. +class MockDaemon +{ +public: + MockDaemon(zmq::context_t & ctx, const std::string & address) + : socket_(ctx, zmq::socket_type::rep) + { + socket_.set(zmq::sockopt::linger, 0); + socket_.set(zmq::sockopt::rcvtimeo, 2000); + socket_.set(zmq::sockopt::sndtimeo, 2000); + socket_.bind(address); + } + + void serve_once(const std::vector & reply) + { + thread_ = std::thread( + [this, reply]() { + zmq::message_t req; + const auto got = socket_.recv(req, zmq::recv_flags::none); + if (!got) { + return; + } + last_request_.assign( + static_cast(req.data()), + static_cast(req.data()) + req.size()); + zmq::message_t rep(reply.data(), reply.size()); + (void)socket_.send(rep, zmq::send_flags::none); + }); + } + + ~MockDaemon() + { + if (thread_.joinable()) { + thread_.join(); + } + } + + std::vector last_request_; + +private: + zmq::socket_t socket_; + std::thread thread_; +}; + +std::string unique_endpoint() +{ + // inproc:// for unit tests avoids real filesystem state and is faster. + static std::atomic counter{0}; + return "inproc://discovery_test_" + std::to_string(counter.fetch_add(1)); +} + +} // namespace + +TEST(DiscoveryEncode, RequestShapeMatchesPython) +{ + // Verify the encoded request decodes back to the expected map shape (a + // crude golden test against the protocol; the daemon-side test is in the + // Python repo). + const auto bytes = DiscoveryClient::encode_request( + DiscoveryCommand::LookupTopic, std::string("/foo"), std::nullopt); + auto oh = msgpack::unpack(reinterpret_cast(bytes.data()), bytes.size()); + const auto & root = oh.get(); + ASSERT_EQ(root.type, msgpack::type::MAP); + // Expected: {command, topic_name} + EXPECT_EQ(root.via.map.size, 2u); +} + +TEST(DiscoveryEncode, RegisterIncludesTopicInfoBlob) +{ + TopicInfo info{"/cam", "ipc:///tmp/x", "ImageMessage", 0xa51dd7f890942cadULL, "cam_node"}; + const auto bytes = DiscoveryClient::encode_request( + DiscoveryCommand::RegisterTopic, std::nullopt, info); + auto oh = msgpack::unpack(reinterpret_cast(bytes.data()), bytes.size()); + const auto & root = oh.get(); + ASSERT_EQ(root.type, msgpack::type::MAP); + EXPECT_EQ(root.via.map.size, 3u); + + // Find the topic_info bin and re-decode the embedded TopicInfo. + bool found = false; + for (std::uint32_t i = 0; i < root.via.map.size; ++i) { + const auto & k = root.via.map.ptr[i].key; + const auto & v = root.via.map.ptr[i].val; + if (k.type == msgpack::type::STR && + std::string_view(k.via.str.ptr, k.via.str.size) == "topic_info") + { + ASSERT_EQ(v.type, msgpack::type::BIN); + auto info_oh = msgpack::unpack(v.via.bin.ptr, v.via.bin.size); + ASSERT_EQ(info_oh.get().type, msgpack::type::MAP); + found = true; + break; + } + } + EXPECT_TRUE(found); +} + +TEST(DiscoveryDecode, OkResponseWithTopicInfo) +{ + TopicInfo info{"/cam", "ipc:///tmp/cam_pub", "ImageMessage", 0xdeadbeef, "publisher"}; + const auto bytes = pack_response(DiscoveryStatus::Ok, "ok", info); + const auto resp = DiscoveryClient::decode_response(bytes.data(), bytes.size()); + EXPECT_EQ(resp.status, DiscoveryStatus::Ok); + ASSERT_TRUE(resp.topic_info.has_value()); + EXPECT_EQ(resp.topic_info->name, "/cam"); + EXPECT_EQ(resp.topic_info->address, "ipc:///tmp/cam_pub"); + EXPECT_EQ(resp.topic_info->message_type, "ImageMessage"); + EXPECT_EQ(resp.topic_info->fingerprint, 0xdeadbeefULL); + EXPECT_EQ(resp.topic_info->publisher_node, "publisher"); +} + +TEST(DiscoveryDecode, NotFoundStatusParsed) +{ + const auto bytes = pack_response(DiscoveryStatus::NotFound, "topic not registered"); + const auto resp = DiscoveryClient::decode_response(bytes.data(), bytes.size()); + EXPECT_EQ(resp.status, DiscoveryStatus::NotFound); + EXPECT_FALSE(resp.topic_info.has_value()); +} + +TEST(DiscoveryDecode, MissingStatusRejected) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_map(1); + pk.pack(std::string("message")); + pk.pack(std::string("oops")); + EXPECT_THROW( + DiscoveryClient::decode_response(buf.data(), buf.size()), DiscoveryError); +} + +TEST(DiscoveryClient, LookupReturnsTopicInfo) +{ + zmq::context_t ctx(1); + const auto endpoint = unique_endpoint(); + MockDaemon daemon(ctx, endpoint); + + TopicInfo info{"/lidar", "ipc:///tmp/lidar_pub", "PointCloudMessage", + 0xbef60c17034e979aULL, "lidar_node"}; + daemon.serve_once(pack_response(DiscoveryStatus::Ok, "ok", info)); + + DiscoveryClient client(ctx, endpoint, std::chrono::milliseconds(2000)); + const auto got = client.lookup("/lidar"); + ASSERT_TRUE(got.has_value()); + EXPECT_EQ(got->address, "ipc:///tmp/lidar_pub"); + EXPECT_EQ(got->fingerprint, 0xbef60c17034e979aULL); +} + +TEST(DiscoveryClient, LookupReturnsNullOptOnNotFound) +{ + zmq::context_t ctx(1); + const auto endpoint = unique_endpoint(); + MockDaemon daemon(ctx, endpoint); + daemon.serve_once(pack_response(DiscoveryStatus::NotFound, "missing")); + + DiscoveryClient client(ctx, endpoint, std::chrono::milliseconds(2000)); + EXPECT_FALSE(client.lookup("/nope").has_value()); +} + +TEST(DiscoveryClient, LookupTimeoutThrows) +{ + zmq::context_t ctx(1); + const auto endpoint = unique_endpoint(); + + // Bind a REP that never replies, so recv hits its timeout. + zmq::socket_t silent(ctx, zmq::socket_type::rep); + silent.set(zmq::sockopt::linger, 0); + silent.bind(endpoint); + + DiscoveryClient client(ctx, endpoint, std::chrono::milliseconds(150)); + EXPECT_THROW(client.lookup("/anything"), DiscoveryError); +} diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_fingerprint_table.cpp b/ros2_bridge/cortex_ros2_bridge/test/test_fingerprint_table.cpp new file mode 100644 index 0000000..4057865 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/test/test_fingerprint_table.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp" + +#include + +#include +#include + +using cortex_ros2_bridge::cortex_wire::find_by_fingerprint; +using cortex_ros2_bridge::cortex_wire::find_by_name; +using cortex_ros2_bridge::cortex_wire::kFingerprintTable; +using cortex_ros2_bridge::cortex_wire::kFingerprintTableSize; +using cortex_ros2_bridge::cortex_wire::MessageKind; +using cortex_ros2_bridge::cortex_wire::to_string; + +TEST(FingerprintTable, HasExpectedSize) +{ + // 16 standard catalogue messages as of 2026-05. If this drifts, regenerate + // the header with scripts/gen_fingerprint_table.py. + EXPECT_EQ(kFingerprintTableSize, 16u); +} + +TEST(FingerprintTable, FingerprintsAreUnique) +{ + std::set seen; + for (std::size_t i = 0; i < kFingerprintTableSize; ++i) { + EXPECT_TRUE(seen.insert(kFingerprintTable[i].fingerprint).second) + << "duplicate fingerprint at index " << i; + } +} + +TEST(FingerprintTable, NamesAreUnique) +{ + std::set seen; + for (std::size_t i = 0; i < kFingerprintTableSize; ++i) { + EXPECT_TRUE(seen.insert(kFingerprintTable[i].name).second) + << "duplicate name at index " << i; + } +} + +TEST(FingerprintTable, KnownEntriesLookup) +{ + // These fingerprints come directly from running the Python cortex tree; + // they form the canonical proof that the generator produces the same + // 64-bit hashes the daemon and publishers emit. Updating any of these + // means the wire is incompatible — bump a version, don't just change + // the constant. + const auto * image = find_by_fingerprint(0xa51dd7f890942cadULL); + ASSERT_NE(image, nullptr); + EXPECT_EQ(image->kind, MessageKind::ImageMessage); + EXPECT_EQ(image->name, "ImageMessage"); + + const auto * pose = find_by_fingerprint(0xa50e8b029e07992fULL); + ASSERT_NE(pose, nullptr); + EXPECT_EQ(pose->kind, MessageKind::PoseMessage); + + const auto * pc = find_by_fingerprint(0xbef60c17034e979aULL); + ASSERT_NE(pc, nullptr); + EXPECT_EQ(pc->kind, MessageKind::PointCloudMessage); +} + +TEST(FingerprintTable, FindByName) +{ + const auto * entry = find_by_name("TimestampMessage"); + ASSERT_NE(entry, nullptr); + EXPECT_EQ(entry->fingerprint, 0xf4484907f9c22ee1ULL); +} + +TEST(FingerprintTable, UnknownLookupsReturnNull) +{ + EXPECT_EQ(find_by_fingerprint(0xdeadbeefdeadbeefULL), nullptr); + EXPECT_EQ(find_by_name("NotAMessageType"), nullptr); +} + +TEST(FingerprintTable, ToStringWorksForEveryKind) +{ + for (std::size_t i = 0; i < kFingerprintTableSize; ++i) { + const auto s = to_string(kFingerprintTable[i].kind); + EXPECT_EQ(s, kFingerprintTable[i].name); + } +} + +TEST(FingerprintTable, QualifiedNamesMatchPython) +{ + // The qualified name encodes module + qualname exactly as Python sees it. + // This is what the fingerprint hash depends on, so a regression here is a + // clear sign the generator broke. + const auto * image = find_by_name("ImageMessage"); + ASSERT_NE(image, nullptr); + EXPECT_EQ(image->qualified_name, "cortex.messages.standard.ImageMessage"); +} diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_oob_buffer.cpp b/ros2_bridge/cortex_ros2_bridge/test/test_oob_buffer.cpp new file mode 100644 index 0000000..8221e50 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/test/test_oob_buffer.cpp @@ -0,0 +1,147 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/cortex_wire/oob_buffer.hpp" + +#include +#include + +#include +#include +#include +#include + +using cortex_ros2_bridge::cortex_wire::make_owned; +using cortex_ros2_bridge::cortex_wire::OobBuffer; +using cortex_ros2_bridge::cortex_wire::ZmqAllocator; +using cortex_ros2_bridge::cortex_wire::ZmqFramePtr; + +namespace +{ + +ZmqFramePtr make_frame_with(const std::vector & bytes) +{ + zmq::message_t msg(bytes.size()); + std::memcpy(msg.data(), bytes.data(), bytes.size()); + return make_owned(std::move(msg)); +} + +} // namespace + +TEST(OobBuffer, ReadsZmqFrameWithoutCopy) +{ + auto frame = make_frame_with({0, 1, 2, 3, 4, 5, 6, 7}); + const void * raw_data = frame->data(); + + OobBuffer view(frame, 8); + EXPECT_EQ(view.size(), 8u); + EXPECT_EQ(view.size_bytes(), 8u); + EXPECT_FALSE(view.empty()); + + // Pointer equality proves no copy occurred. + EXPECT_EQ(reinterpret_cast(view.data()), raw_data); + + // Element-wise content. + for (std::size_t i = 0; i < 8; ++i) { + EXPECT_EQ(view[i], static_cast(i)); + } +} + +TEST(OobBuffer, OffsetSlicing) +{ + auto frame = make_frame_with({10, 11, 12, 13, 14, 15}); + OobBuffer view(frame, 3, /*byte_offset=*/2); + ASSERT_EQ(view.size(), 3u); + EXPECT_EQ(view[0], 12); + EXPECT_EQ(view[1], 13); + EXPECT_EQ(view[2], 14); +} + +TEST(OobBuffer, OutlivesOriginalSharedPtr) +{ + // The buffer should still be readable after the only external shared_ptr + // is dropped — OobBuffer owns its own reference. + OobBuffer view; + { + auto frame = make_frame_with({0xaa, 0xbb, 0xcc, 0xdd}); + view = OobBuffer(frame, 4); + EXPECT_EQ(frame.use_count(), 2); // local + view + } + ASSERT_EQ(view.size(), 4u); + EXPECT_EQ(view[0], 0xaa); + EXPECT_EQ(view[3], 0xdd); +} + +TEST(OobBuffer, DefaultConstructedIsEmpty) +{ + OobBuffer empty; + EXPECT_TRUE(empty.empty()); + EXPECT_EQ(empty.size(), 0u); + EXPECT_EQ(empty.data(), nullptr); + EXPECT_EQ(empty.begin(), empty.end()); +} + +TEST(OobBuffer, RangeIteration) +{ + auto frame = make_frame_with({1, 2, 3, 4, 5}); + OobBuffer view(frame, 5); + int sum = 0; + for (auto v : view) { + sum += v; + } + EXPECT_EQ(sum, 15); +} + +TEST(ZmqAllocator, VectorStorageAliasesFrameBuffer) +{ + // The vector's storage pointer must equal the frame's data pointer — that + // is the contract that lets us tie a std::vector's lifetime to a zmq + // frame without allocating fresh memory. NOTE: std::vector's sizing ctor + // value-initialises elements, so the frame's *content* is zeroed by this + // constructor. The pointer equality is the zero-allocation proof; data + // must be written into v.data() separately (e.g. via memcpy). + auto frame = make_frame_with({0xde, 0xad, 0xbe, 0xef}); + void * frame_ptr = frame->data(); + + ZmqAllocator alloc(frame); + std::vector> v(4, alloc); + + EXPECT_EQ(reinterpret_cast(v.data()), frame_ptr); + EXPECT_EQ(v.size(), 4u); +} + +TEST(ZmqAllocator, OverflowRequestThrows) +{ + auto frame = make_frame_with({1, 2}); + ZmqAllocator alloc(frame); + EXPECT_THROW(alloc.allocate(64), std::bad_alloc); +} + +TEST(ZmqAllocator, EqualityComparesFrames) +{ + auto frame_a = make_frame_with({1, 2}); + auto frame_b = make_frame_with({3, 4}); + ZmqAllocator a1(frame_a); + ZmqAllocator a2(frame_a); + ZmqAllocator b(frame_b); + EXPECT_TRUE(a1 == a2); + EXPECT_FALSE(a1 == b); +} + +TEST(ZmqAllocator, KeepsFrameAliveAcrossVectorMove) +{ + auto frame = make_frame_with({9, 8, 7, 6}); + void * raw = frame->data(); + std::weak_ptr weak = frame; + + std::vector> v1( + 4, ZmqAllocator(frame)); + frame.reset(); // drop the local reference; only the allocator holds it now. + + auto v2 = std::move(v1); + EXPECT_FALSE(weak.expired()) << "vector move must propagate the allocator"; + EXPECT_EQ(reinterpret_cast(v2.data()), raw); + + // Confirm the writable storage actually points into the frame: writing + // through v2 should be observable at the original buffer address. + v2[0] = 0xa5; + EXPECT_EQ(static_cast(raw)[0], 0xa5); +} diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_wire_header.cpp b/ros2_bridge/cortex_ros2_bridge/test/test_wire_header.cpp new file mode 100644 index 0000000..87006cd --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/test/test_wire_header.cpp @@ -0,0 +1,84 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/cortex_wire/header.hpp" + +#include + +#include +#include +#include + +using cortex_ros2_bridge::cortex_wire::MessageHeader; +using cortex_ros2_bridge::cortex_wire::WireDecodeError; + +TEST(MessageHeader, SizeIs24Bytes) +{ + EXPECT_EQ(MessageHeader::kSize, 24u); +} + +TEST(MessageHeader, EncodeDecodeRoundTrip) +{ + MessageHeader h; + h.fingerprint = 0xa51dd7f890942cadULL; // ImageMessage from the catalogue + h.timestamp_ns = 0x0123'4567'89ab'cdefULL; + h.sequence = 42; + + std::array buf{}; + h.to_bytes(buf.data()); + + const auto decoded = MessageHeader::from_bytes(buf.data(), buf.size()); + EXPECT_EQ(decoded.fingerprint, h.fingerprint); + EXPECT_EQ(decoded.timestamp_ns, h.timestamp_ns); + EXPECT_EQ(decoded.sequence, h.sequence); +} + +TEST(MessageHeader, BigEndianByteLayout) +{ + // Spot-check that we agree with Python's struct.pack(">QQQ", ...). The + // first byte of `fingerprint` must be its MSB. + MessageHeader h; + h.fingerprint = 0x0102030405060708ULL; + h.timestamp_ns = 0x1112131415161718ULL; + h.sequence = 0x2122232425262728ULL; + + std::array buf{}; + h.to_bytes(buf.data()); + + const std::array expected = { + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, + 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, + 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, + }; + EXPECT_EQ(buf, expected); +} + +TEST(MessageHeader, ShortBufferThrows) +{ + std::array buf{}; + EXPECT_THROW(MessageHeader::from_bytes(buf.data(), buf.size()), WireDecodeError); +} + +TEST(MessageHeader, ZeroIsValid) +{ + std::array buf{}; + const auto h = MessageHeader::from_bytes(buf.data(), buf.size()); + EXPECT_EQ(h.fingerprint, 0u); + EXPECT_EQ(h.timestamp_ns, 0u); + EXPECT_EQ(h.sequence, 0u); +} + +TEST(MessageHeader, ExtraBytesIgnored) +{ + // from_bytes only reads the first 24 bytes. + std::array buf{}; + buf.fill(0xff); + MessageHeader h; + h.fingerprint = 7; + h.timestamp_ns = 8; + h.sequence = 9; + h.to_bytes(buf.data()); + + const auto decoded = MessageHeader::from_bytes(buf.data(), buf.size()); + EXPECT_EQ(decoded.fingerprint, 7u); + EXPECT_EQ(decoded.timestamp_ns, 8u); + EXPECT_EQ(decoded.sequence, 9u); +} diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_wire_metadata.cpp b/ros2_bridge/cortex_ros2_bridge/test/test_wire_metadata.cpp new file mode 100644 index 0000000..cafb117 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/test/test_wire_metadata.cpp @@ -0,0 +1,188 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/cortex_wire/metadata.hpp" + +#include +#include + +#include +#include +#include + +using cortex_ros2_bridge::cortex_wire::DecodedMetadata; +using cortex_ros2_bridge::cortex_wire::OobDescriptor; +using cortex_ros2_bridge::cortex_wire::OobKind; +using cortex_ros2_bridge::cortex_wire::WireDecodeError; + +namespace +{ + +// Build a metadata frame that mirrors what Cortex's +// serialize_message_frames() emits for an ImageMessage(data=ndarray, +// encoding="rgb8", width=640, height=480). Field order matches the dataclass +// declaration in cortex.messages.standard.ImageMessage: +// data, encoding, width, height +std::vector build_image_metadata() +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(4); + + // Field 0: OOB numpy descriptor (data field, buffer index 0) + pk.pack_map(4); + pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("numpy")); + pk.pack(std::string("buffer")); pk.pack(static_cast(0)); + pk.pack(std::string("dtype")); pk.pack(std::string("(480)); + pk.pack(static_cast(640)); + pk.pack(static_cast(3)); + + // Field 1: encoding (str) + pk.pack(std::string("rgb8")); + // Field 2: width (u32) + pk.pack(static_cast(640)); + // Field 3: height (u32) + pk.pack(static_cast(480)); + + return std::vector( + reinterpret_cast(buf.data()), + reinterpret_cast(buf.data()) + buf.size()); +} + +} // namespace + +TEST(DecodedMetadata, ParsesImageLikeMetadata) +{ + const auto bytes = build_image_metadata(); + const auto meta = DecodedMetadata::from_bytes(bytes.data(), bytes.size()); + + ASSERT_EQ(meta.field_count(), 4u); + + // Field 0: OOB descriptor + const auto oob_opt = DecodedMetadata::as_oob(meta.field(0)); + ASSERT_TRUE(oob_opt.has_value()); + EXPECT_EQ(oob_opt->kind, OobKind::Numpy); + EXPECT_EQ(oob_opt->buffer_index, 0u); + EXPECT_EQ(oob_opt->dtype, "shape.size(), 3u); + EXPECT_EQ(oob_opt->shape[0], 480); + EXPECT_EQ(oob_opt->shape[1], 640); + EXPECT_EQ(oob_opt->shape[2], 3); + + // Field 1: encoding + ASSERT_EQ(meta.field(1).type, msgpack::type::STR); + EXPECT_EQ( + std::string(meta.field(1).via.str.ptr, meta.field(1).via.str.size), "rgb8"); + + // Field 2,3: width/height + ASSERT_EQ(meta.field(2).type, msgpack::type::POSITIVE_INTEGER); + EXPECT_EQ(meta.field(2).via.u64, 640u); + ASSERT_EQ(meta.field(3).type, msgpack::type::POSITIVE_INTEGER); + EXPECT_EQ(meta.field(3).via.u64, 480u); +} + +TEST(DecodedMetadata, NonOobMapsAreNotOob) +{ + // A regular user dict {"foo": 1} must not be misread as an OOB descriptor. + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(1); + pk.pack_map(1); + pk.pack(std::string("foo")); + pk.pack(static_cast(1)); + + const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); + ASSERT_EQ(meta.field_count(), 1u); + EXPECT_FALSE(DecodedMetadata::as_oob(meta.field(0)).has_value()); +} + +TEST(DecodedMetadata, TorchOobDescriptor) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(1); + pk.pack_map(6); + pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("torch")); + pk.pack(std::string("buffer")); pk.pack(static_cast(2)); + pk.pack(std::string("dtype")); pk.pack(std::string("(64)); + pk.pack(static_cast(64)); + pk.pack(std::string("device")); pk.pack(std::string("cuda:0")); + pk.pack(std::string("requires_grad")); pk.pack(true); + + const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); + const auto oob = DecodedMetadata::as_oob(meta.field(0)); + ASSERT_TRUE(oob.has_value()); + EXPECT_EQ(oob->kind, OobKind::Torch); + EXPECT_EQ(oob->buffer_index, 2u); + EXPECT_EQ(oob->dtype, "shape, (std::vector{64, 64})); + EXPECT_EQ(oob->device, "cuda:0"); + EXPECT_TRUE(oob->requires_grad); +} + +TEST(DecodedMetadata, NonArrayRootRejected) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_map(1); + pk.pack(std::string("a")); + pk.pack(1); + EXPECT_THROW(DecodedMetadata::from_bytes(buf.data(), buf.size()), WireDecodeError); +} + +TEST(DecodedMetadata, OutOfRangeAccessThrows) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(1); + pk.pack(42); + const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); + EXPECT_NO_THROW((void)meta.field(0)); + EXPECT_THROW((void)meta.field(1), WireDecodeError); +} + +TEST(DecodedMetadata, MalformedBytesThrow) +{ + const std::uint8_t junk[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + EXPECT_THROW(DecodedMetadata::from_bytes(junk, sizeof(junk)), WireDecodeError); +} + +TEST(DecodedMetadata, UnknownOobKindThrows) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(1); + pk.pack_map(2); + pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("alien")); + pk.pack(std::string("buffer")); pk.pack(static_cast(0)); + + const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); + EXPECT_THROW((void)DecodedMetadata::as_oob(meta.field(0)), WireDecodeError); +} + +TEST(DecodedMetadata, ForwardCompatibleUnknownKeys) +{ + // A descriptor with an unknown extra key must still parse cleanly — we + // want to roll out new fields on the Python side without breaking older + // bridge builds. + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(1); + pk.pack_map(5); + pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("numpy")); + pk.pack(std::string("buffer")); pk.pack(static_cast(0)); + pk.pack(std::string("dtype")); pk.pack(std::string("(10)); + pk.pack(std::string("future_field")); pk.pack(123); + + const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); + const auto oob = DecodedMetadata::as_oob(meta.field(0)); + ASSERT_TRUE(oob.has_value()); + EXPECT_EQ(oob->dtype, " Date: Mon, 11 May 2026 16:53:33 -0400 Subject: [PATCH 03/10] separate cortex wire cpp independent of ROS2 --- cortex_wire_cpp/CMakeLists.txt | 146 +++++++ cortex_wire_cpp/README.md | 80 ++++ .../cmake/cortex_wire_cppConfig.cmake.in | 13 + .../include}/cortex_wire/discovery_client.hpp | 10 +- .../cortex_wire/fingerprint_table.hpp | 10 +- .../include}/cortex_wire/header.hpp | 10 +- .../include/cortex_wire/metadata.hpp | 162 ++++++++ .../include}/cortex_wire/oob_buffer.hpp | 10 +- .../scripts/gen_fingerprint_table.py | 11 +- .../src}/discovery_client.cpp | 6 +- .../src}/header.cpp | 6 +- .../src}/metadata.cpp | 113 +++++- .../test/test_discovery_client.cpp | 12 +- .../test/test_fingerprint_table.cpp | 14 +- .../test/test_header.cpp | 6 +- cortex_wire_cpp/test/test_metadata.cpp | 373 ++++++++++++++++++ .../test/test_oob_buffer.cpp | 10 +- .../cortex_wire/metadata.hpp | 70 ---- .../test/test_wire_metadata.cpp | 188 --------- 19 files changed, 936 insertions(+), 314 deletions(-) create mode 100644 cortex_wire_cpp/CMakeLists.txt create mode 100644 cortex_wire_cpp/README.md create mode 100644 cortex_wire_cpp/cmake/cortex_wire_cppConfig.cmake.in rename {ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge => cortex_wire_cpp/include}/cortex_wire/discovery_client.hpp (89%) rename {ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge => cortex_wire_cpp/include}/cortex_wire/fingerprint_table.hpp (92%) rename {ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge => cortex_wire_cpp/include}/cortex_wire/header.hpp (79%) create mode 100644 cortex_wire_cpp/include/cortex_wire/metadata.hpp rename {ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge => cortex_wire_cpp/include}/cortex_wire/oob_buffer.hpp (94%) rename {ros2_bridge/cortex_ros2_bridge => cortex_wire_cpp}/scripts/gen_fingerprint_table.py (93%) rename {ros2_bridge/cortex_ros2_bridge/src/cortex_wire => cortex_wire_cpp/src}/discovery_client.cpp (98%) rename {ros2_bridge/cortex_ros2_bridge/src/cortex_wire => cortex_wire_cpp/src}/header.cpp (92%) rename {ros2_bridge/cortex_ros2_bridge/src/cortex_wire => cortex_wire_cpp/src}/metadata.cpp (57%) rename {ros2_bridge/cortex_ros2_bridge => cortex_wire_cpp}/test/test_discovery_client.cpp (95%) rename {ros2_bridge/cortex_ros2_bridge => cortex_wire_cpp}/test/test_fingerprint_table.cpp (87%) rename ros2_bridge/cortex_ros2_bridge/test/test_wire_header.cpp => cortex_wire_cpp/test/test_header.cpp (93%) create mode 100644 cortex_wire_cpp/test/test_metadata.cpp rename {ros2_bridge/cortex_ros2_bridge => cortex_wire_cpp}/test/test_oob_buffer.cpp (93%) delete mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/metadata.hpp delete mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_wire_metadata.cpp diff --git a/cortex_wire_cpp/CMakeLists.txt b/cortex_wire_cpp/CMakeLists.txt new file mode 100644 index 0000000..367059e --- /dev/null +++ b/cortex_wire_cpp/CMakeLists.txt @@ -0,0 +1,146 @@ +cmake_minimum_required(VERSION 3.16) +project(cortex_wire_cpp VERSION 0.1.0 LANGUAGES CXX + DESCRIPTION "C++ port of Cortex's IPC wire format (header, msgpack metadata, OOB frames, discovery)" + HOMEPAGE_URL "https://github.com/sudoRicheek/cortex" +) + +# ---- options -------------------------------------------------------------- + +option(CORTEX_WIRE_BUILD_TESTS "Build cortex_wire_cpp unit tests" ON) +option(CORTEX_WIRE_INSTALL "Generate install rules for cortex_wire_cpp" ON) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include(GNUInstallDirs) +include(CMakePackageConfigHelpers) + +# ---- dependencies --------------------------------------------------------- + +find_package(cppzmq REQUIRED) + +# msgpack-cxx ships either as a CMake config (msgpack-cxxConfig.cmake on newer +# distros) or as a plain header set under libmsgpack-dev (Ubuntu 22.04). Try +# config first; fall back to a path lookup. +find_package(msgpack-cxx QUIET) +if(NOT msgpack-cxx_FOUND) + find_path(MSGPACK_INCLUDE_DIR msgpack.hpp + HINTS /usr/include /usr/local/include + REQUIRED) +endif() + +# ---- library -------------------------------------------------------------- + +add_library(cortex_wire + src/header.cpp + src/metadata.cpp + src/discovery_client.cpp +) +add_library(cortex_wire::cortex_wire ALIAS cortex_wire) + +target_include_directories(cortex_wire PUBLIC + $ + $ +) +if(msgpack-cxx_FOUND) + target_link_libraries(cortex_wire PUBLIC msgpack-cxx) +else() + # Header-only path: expose the include dir transitively, but do not enter + # the install/export set (we'd need an imported target for that). + target_include_directories(cortex_wire PUBLIC + $ + ) +endif() +target_link_libraries(cortex_wire PUBLIC cppzmq) + +set_target_properties(cortex_wire PROPERTIES + VERSION ${PROJECT_VERSION} + SOVERSION ${PROJECT_VERSION_MAJOR} + EXPORT_NAME cortex_wire + # Allow linkage into downstream SHARED libraries (e.g. the ROS 2 bridge + # adapter lib) even when we are built as a static archive. + POSITION_INDEPENDENT_CODE ON +) + +# ---- install -------------------------------------------------------------- + +if(CORTEX_WIRE_INSTALL) + install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + + install(TARGETS cortex_wire + EXPORT cortex_wire_cppTargets + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ) + + install(PROGRAMS scripts/gen_fingerprint_table.py + DESTINATION ${CMAKE_INSTALL_LIBEXECDIR}/cortex_wire_cpp/scripts + ) + + set(cortex_wire_cpp_cmake_dir ${CMAKE_INSTALL_LIBDIR}/cmake/cortex_wire_cpp) + + install(EXPORT cortex_wire_cppTargets + FILE cortex_wire_cppTargets.cmake + NAMESPACE cortex_wire:: + DESTINATION ${cortex_wire_cpp_cmake_dir} + ) + + configure_package_config_file( + cmake/cortex_wire_cppConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/cortex_wire_cppConfig.cmake + INSTALL_DESTINATION ${cortex_wire_cpp_cmake_dir} + ) + write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/cortex_wire_cppConfigVersion.cmake + VERSION ${PROJECT_VERSION} + COMPATIBILITY SameMajorVersion + ) + install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/cortex_wire_cppConfig.cmake + ${CMAKE_CURRENT_BINARY_DIR}/cortex_wire_cppConfigVersion.cmake + DESTINATION ${cortex_wire_cpp_cmake_dir} + ) +endif() + +# ---- tests ---------------------------------------------------------------- + +if(CORTEX_WIRE_BUILD_TESTS) + enable_testing() + # Prefer a system-installed GTest; fall back to FetchContent for environments + # without one (CI runners, hermetic builds). + find_package(GTest QUIET) + if(NOT GTest_FOUND) + include(FetchContent) + FetchContent_Declare(googletest + GIT_REPOSITORY https://github.com/google/googletest.git + GIT_TAG v1.14.0 + ) + set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) + FetchContent_MakeAvailable(googletest) + endif() + + set(_cortex_wire_tests + test_header + test_metadata + test_oob_buffer + test_discovery_client + test_fingerprint_table + ) + foreach(t ${_cortex_wire_tests}) + add_executable(${t} test/${t}.cpp) + target_link_libraries(${t} PRIVATE + cortex_wire + GTest::gtest + GTest::gtest_main + ) + add_test(NAME ${t} COMMAND ${t}) + endforeach() +endif() diff --git a/cortex_wire_cpp/README.md b/cortex_wire_cpp/README.md new file mode 100644 index 0000000..fe00db9 --- /dev/null +++ b/cortex_wire_cpp/README.md @@ -0,0 +1,80 @@ +# cortex_wire_cpp + +C++ port of Cortex's IPC wire format. Standalone CMake library, no ROS 2 dependency. + +Used by the ROS 2 bridge ([ros2_bridge/cortex_ros2_bridge](../ros2_bridge/cortex_ros2_bridge)) but designed for any C++ consumer that wants to read or write Cortex's pub/sub IPC format: debug tools (LCM-spy style), other language bridges, or sub-100µs control-loop consumers that don't want to go through Python. + +## What it provides + +- `cortex_wire::MessageHeader` — 24-byte big-endian fingerprint/timestamp/sequence header. +- `cortex_wire::DecodedMetadata` — msgpack-cxx decoder for the metadata frame, with `OobDescriptor` parsing for numpy/torch out-of-band buffers. +- `cortex_wire::OobBuffer` — zero-copy read-only view into a ZMQ frame. +- `cortex_wire::ZmqAllocator` — std-allocator that backs a `std::vector` with a ZMQ frame's buffer (zero allocation; not zero-copy for content — see header for caveats). +- `cortex_wire::DiscoveryClient` — sync REQ/REP client for the discovery daemon. +- `cortex_wire::fingerprint_table` — generated map from Cortex's standard catalogue. + +Layout: + +``` +cortex_wire_cpp/ +├── CMakeLists.txt +├── include/cortex_wire/ +│ ├── header.hpp +│ ├── metadata.hpp +│ ├── oob_buffer.hpp +│ ├── discovery_client.hpp +│ └── fingerprint_table.hpp ← auto-generated +├── src/ +│ ├── header.cpp +│ ├── metadata.cpp +│ └── discovery_client.cpp +├── scripts/ +│ └── gen_fingerprint_table.py ← regen tool +├── cmake/ +│ └── cortex_wire_cppConfig.cmake.in +└── test/ ← 5 gtest binaries +``` + +## Dependencies + +- `cppzmq` — header-only C++ wrapper over `libzmq` (system). +- `msgpack-cxx` — header-only msgpack C++ library. On Ubuntu 22.04 install `libmsgpack-dev`. Newer distros provide a CMake config under `msgpack-cxx` which is picked up automatically. +- C++17. + +## Build & install + +Pure CMake, no colcon: + +```bash +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release +cmake --build build -j +ctest --test-dir build --output-on-failure +sudo cmake --install build +``` + +## Consuming from another CMake project + +```cmake +find_package(cortex_wire_cpp REQUIRED) + +add_executable(my_tool my_tool.cpp) +target_link_libraries(my_tool PRIVATE cortex_wire::cortex_wire) +``` + +For monorepo consumers (e.g. `ros2_bridge`) the same target is available via `add_subdirectory(../../cortex_wire_cpp ...)` — no install step needed for dev iteration. + +## Regenerating the fingerprint table + +`include/cortex_wire/fingerprint_table.hpp` is committed so the library builds without a Python `cortex` install. Regenerate after any change to `cortex.messages.standard`: + +```bash +python3 -m venv /tmp/cortex_venv +/tmp/cortex_venv/bin/pip install -e /path/to/cortex +/tmp/cortex_venv/bin/python scripts/gen_fingerprint_table.py +``` + +`--check` mode for CI: + +```bash +scripts/gen_fingerprint_table.py --check +``` diff --git a/cortex_wire_cpp/cmake/cortex_wire_cppConfig.cmake.in b/cortex_wire_cpp/cmake/cortex_wire_cppConfig.cmake.in new file mode 100644 index 0000000..ffee709 --- /dev/null +++ b/cortex_wire_cpp/cmake/cortex_wire_cppConfig.cmake.in @@ -0,0 +1,13 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) + +find_dependency(cppzmq) +find_dependency(msgpack-cxx QUIET) +# msgpack-cxx may be header-only with no CMake config (Ubuntu 22.04). In that +# case the target's public include directory has already been baked into the +# install interface so consumers do not need to re-find it. + +include("${CMAKE_CURRENT_LIST_DIR}/cortex_wire_cppTargets.cmake") + +check_required_components(cortex_wire_cpp) diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/discovery_client.hpp b/cortex_wire_cpp/include/cortex_wire/discovery_client.hpp similarity index 89% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/discovery_client.hpp rename to cortex_wire_cpp/include/cortex_wire/discovery_client.hpp index 11feaeb..50145e0 100644 --- a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/discovery_client.hpp +++ b/cortex_wire_cpp/include/cortex_wire/discovery_client.hpp @@ -1,6 +1,6 @@ // Copyright (c) 2026, Cortex contributors. Apache-2.0. -#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__DISCOVERY_CLIENT_HPP_ -#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__DISCOVERY_CLIENT_HPP_ +#ifndef CORTEX_WIRE__DISCOVERY_CLIENT_HPP_ +#define CORTEX_WIRE__DISCOVERY_CLIENT_HPP_ #include @@ -11,7 +11,7 @@ #include #include -namespace cortex_ros2_bridge::cortex_wire +namespace cortex_wire { // Mirrors cortex.discovery.protocol.DiscoveryCommand. @@ -96,6 +96,6 @@ class DiscoveryClient zmq::socket_t socket_; }; -} // namespace cortex_ros2_bridge::cortex_wire +} // namespace cortex_wire -#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__DISCOVERY_CLIENT_HPP_ +#endif // CORTEX_WIRE__DISCOVERY_CLIENT_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp b/cortex_wire_cpp/include/cortex_wire/fingerprint_table.hpp similarity index 92% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp rename to cortex_wire_cpp/include/cortex_wire/fingerprint_table.hpp index 98bb067..627d3b1 100644 --- a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp +++ b/cortex_wire_cpp/include/cortex_wire/fingerprint_table.hpp @@ -2,13 +2,13 @@ // // Regenerate after any change to cortex.messages.standard. CI checks that this // file matches the script's output for the installed cortex package. -#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ -#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ +#ifndef CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ +#define CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ #include #include -namespace cortex_ros2_bridge::cortex_wire +namespace cortex_wire { enum class MessageKind : std::uint32_t @@ -92,6 +92,6 @@ constexpr std::string_view to_string(MessageKind k) noexcept return ""; } -} // namespace cortex_ros2_bridge::cortex_wire +} // namespace cortex_wire -#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ +#endif // CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/header.hpp b/cortex_wire_cpp/include/cortex_wire/header.hpp similarity index 79% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/header.hpp rename to cortex_wire_cpp/include/cortex_wire/header.hpp index 6574628..8131b11 100644 --- a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/header.hpp +++ b/cortex_wire_cpp/include/cortex_wire/header.hpp @@ -1,12 +1,12 @@ // Copyright (c) 2026, Cortex contributors. Apache-2.0. -#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__HEADER_HPP_ -#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__HEADER_HPP_ +#ifndef CORTEX_WIRE__HEADER_HPP_ +#define CORTEX_WIRE__HEADER_HPP_ #include #include #include -namespace cortex_ros2_bridge::cortex_wire +namespace cortex_wire { // Fixed 24-byte header prepended to every Cortex multipart message, matching @@ -37,6 +37,6 @@ class WireDecodeError : public std::runtime_error using std::runtime_error::runtime_error; }; -} // namespace cortex_ros2_bridge::cortex_wire +} // namespace cortex_wire -#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__HEADER_HPP_ +#endif // CORTEX_WIRE__HEADER_HPP_ diff --git a/cortex_wire_cpp/include/cortex_wire/metadata.hpp b/cortex_wire_cpp/include/cortex_wire/metadata.hpp new file mode 100644 index 0000000..ced3c46 --- /dev/null +++ b/cortex_wire_cpp/include/cortex_wire/metadata.hpp @@ -0,0 +1,162 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_WIRE__METADATA_HPP_ +#define CORTEX_WIRE__METADATA_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "cortex_wire/header.hpp" + +namespace cortex_wire +{ + +enum class OobKind { Numpy, Torch }; + +// Mirror of the {"__cortex_oob__": "numpy", ...} / "torch" descriptor that +// cortex.utils.serialization._encode_transport_value emits. The `buffer` +// index refers into the OOB frames that follow the metadata frame on the +// wire (i.e. frame index = 2 + buffer_index in the multipart message). +struct OobDescriptor +{ + OobKind kind = OobKind::Numpy; + std::uint32_t buffer_index = 0; + std::string dtype; // numpy dtype string, e.g. " shape; + // Torch-only fields: + std::string device; + bool requires_grad = false; +}; + +// Owned msgpack object tree for a metadata frame. The unpacker zone is held +// internally so msgpack::object references stay valid for the lifetime of +// this object. +// +// Cortex's metadata frame is always a msgpack array of N field values in +// dataclass declaration order. OOB descriptors appear inline as map objects +// with a "__cortex_oob__" key; everything else is a primitive, nested map, +// or nested list. +class DecodedMetadata +{ +public: + // Parse a msgpack-packed metadata frame. + // Throws WireDecodeError on malformed msgpack or non-array root. + static DecodedMetadata from_bytes(const void * data, std::size_t size); + + // Number of top-level fields (i.e. size of the dataclass's field list). + std::size_t field_count() const noexcept; + + // Access a field by declaration order index. + // Throws WireDecodeError if i is out of range. + const msgpack::object & field(std::size_t i) const; + + // If `obj` is an OOB descriptor map, return it parsed; else nullopt. + static std::optional as_oob(const msgpack::object & obj); + + // Recursively walks `obj` and invokes `visitor(descriptor)` for every OOB + // descriptor found, including those nested inside dict / list values. + // Useful for DictMessage / ListMessage adapters that need to collect every + // OOB buffer reachable from a top-level field. + template + static void walk_oob(const msgpack::object & obj, Fn && visitor); + +private: + DecodedMetadata() = default; + + msgpack::object_handle handle_; + const msgpack::object * root_array_ = nullptr; // points into handle_'s zone + std::size_t count_ = 0; +}; + +// Encoder for the metadata frame plus the variable-length list of OOB buffer +// frames. Mirrors cortex.utils.serialization.serialize_message_frames: +// 1. The caller packs `field_count` top-level values via pack_*(). +// 2. Arrays are emitted via pack_numpy_oob() / pack_torch_oob() — the +// builder writes a descriptor map inline and queues the raw buffer for +// emission as a separate frame. +// 3. finish() returns the metadata bytes and the ordered OOB buffer list. +// +// For nested structures (Dict / List adapters), reach for packer() to write +// msgpack maps/arrays directly and use add_oob_buffer() to record buffers +// while emitting your own descriptor maps via packer(). +class MetadataBuilder +{ +public: + // `field_count` is the number of top-level dataclass fields the caller + // will pack. Packs the outer msgpack array header immediately. + explicit MetadataBuilder(std::uint32_t field_count); + + // ---- top-level primitive packers ---- + void pack_nil(); + void pack_bool(bool v); + void pack_int(std::int64_t v); + void pack_uint(std::uint64_t v); + void pack_double(double v); + void pack_str(std::string_view s); + void pack_bin(const void * data, std::size_t size); // msgpack BIN, for `bytes` + + // ---- OOB array packers ---- + // Writes the descriptor map inline and records the raw buffer for emission + // as the next OOB frame. Buffer bytes are copied into builder-owned storage. + void pack_numpy_oob( + std::string_view dtype, + const std::vector & shape, + const void * buffer_data, std::size_t buffer_size); + + void pack_torch_oob( + std::string_view dtype, + const std::vector & shape, + std::string_view device, + bool requires_grad, + const void * buffer_data, std::size_t buffer_size); + + // ---- low-level access ---- + msgpack::packer & packer() noexcept {return packer_;} + + // Records a buffer and returns its 0-based index. Caller is responsible + // for emitting the corresponding descriptor map via packer(). + std::uint32_t add_oob_buffer(const void * data, std::size_t size); + + // Finalize. Returns metadata bytes + OOB frames in emission order. The + // builder is left in a moved-from state. + struct Frames + { + std::vector metadata; + std::vector> oob_buffers; + }; + Frames finish() &&; + +private: + msgpack::sbuffer sbuf_; + msgpack::packer packer_; + std::vector> oob_buffers_; +}; + +// ---- inline template definitions ---- + +template +void DecodedMetadata::walk_oob(const msgpack::object & obj, Fn && visitor) +{ + if (auto desc = as_oob(obj)) { + visitor(*desc); + return; + } + if (obj.type == msgpack::type::MAP) { + for (std::uint32_t i = 0; i < obj.via.map.size; ++i) { + walk_oob(obj.via.map.ptr[i].val, std::forward(visitor)); + } + } else if (obj.type == msgpack::type::ARRAY) { + for (std::uint32_t i = 0; i < obj.via.array.size; ++i) { + walk_oob(obj.via.array.ptr[i], std::forward(visitor)); + } + } +} + +} // namespace cortex_wire + +#endif // CORTEX_WIRE__METADATA_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/oob_buffer.hpp b/cortex_wire_cpp/include/cortex_wire/oob_buffer.hpp similarity index 94% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/oob_buffer.hpp rename to cortex_wire_cpp/include/cortex_wire/oob_buffer.hpp index ea126c7..64ffb09 100644 --- a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/oob_buffer.hpp +++ b/cortex_wire_cpp/include/cortex_wire/oob_buffer.hpp @@ -1,6 +1,6 @@ // Copyright (c) 2026, Cortex contributors. Apache-2.0. -#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__OOB_BUFFER_HPP_ -#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__OOB_BUFFER_HPP_ +#ifndef CORTEX_WIRE__OOB_BUFFER_HPP_ +#define CORTEX_WIRE__OOB_BUFFER_HPP_ #include @@ -11,7 +11,7 @@ #include #include -namespace cortex_ros2_bridge::cortex_wire +namespace cortex_wire { // Shared, immutable view into a ZMQ multipart frame. The shared_ptr owns the @@ -156,6 +156,6 @@ class ZmqAllocator std::size_t offset_ = 0; }; -} // namespace cortex_ros2_bridge::cortex_wire +} // namespace cortex_wire -#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__OOB_BUFFER_HPP_ +#endif // CORTEX_WIRE__OOB_BUFFER_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/scripts/gen_fingerprint_table.py b/cortex_wire_cpp/scripts/gen_fingerprint_table.py similarity index 93% rename from ros2_bridge/cortex_ros2_bridge/scripts/gen_fingerprint_table.py rename to cortex_wire_cpp/scripts/gen_fingerprint_table.py index 438c34d..2f8a475 100755 --- a/ros2_bridge/cortex_ros2_bridge/scripts/gen_fingerprint_table.py +++ b/cortex_wire_cpp/scripts/gen_fingerprint_table.py @@ -30,13 +30,13 @@ // // Regenerate after any change to cortex.messages.standard. CI checks that this // file matches the script's output for the installed cortex package. -#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ -#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ +#ifndef CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ +#define CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ #include #include -namespace cortex_ros2_bridge::cortex_wire +namespace cortex_wire {{ enum class MessageKind : std::uint32_t @@ -90,9 +90,9 @@ return ""; }} -}} // namespace cortex_ros2_bridge::cortex_wire +}} // namespace cortex_wire -#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ +#endif // CORTEX_WIRE__FINGERPRINT_TABLE_HPP_ """ @@ -122,7 +122,6 @@ def main() -> int: default_out = ( Path(__file__).resolve().parent.parent / "include" - / "cortex_ros2_bridge" / "cortex_wire" / "fingerprint_table.hpp" ) diff --git a/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/discovery_client.cpp b/cortex_wire_cpp/src/discovery_client.cpp similarity index 98% rename from ros2_bridge/cortex_ros2_bridge/src/cortex_wire/discovery_client.cpp rename to cortex_wire_cpp/src/discovery_client.cpp index aba2076..6ca1c8e 100644 --- a/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/discovery_client.cpp +++ b/cortex_wire_cpp/src/discovery_client.cpp @@ -1,5 +1,5 @@ // Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/cortex_wire/discovery_client.hpp" +#include "cortex_wire/discovery_client.hpp" #include @@ -9,7 +9,7 @@ #include #include -namespace cortex_ros2_bridge::cortex_wire +namespace cortex_wire { namespace @@ -267,4 +267,4 @@ void DiscoveryClient::unregister_topic(const std::string & topic_name) } } -} // namespace cortex_ros2_bridge::cortex_wire +} // namespace cortex_wire diff --git a/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/header.cpp b/cortex_wire_cpp/src/header.cpp similarity index 92% rename from ros2_bridge/cortex_ros2_bridge/src/cortex_wire/header.cpp rename to cortex_wire_cpp/src/header.cpp index c5b1ce4..b0cdcaf 100644 --- a/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/header.cpp +++ b/cortex_wire_cpp/src/header.cpp @@ -1,9 +1,9 @@ // Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/cortex_wire/header.hpp" +#include "cortex_wire/header.hpp" #include -namespace cortex_ros2_bridge::cortex_wire +namespace cortex_wire { namespace @@ -59,4 +59,4 @@ void MessageHeader::to_bytes(void * out) const noexcept write_be64(p + 16, sequence); } -} // namespace cortex_ros2_bridge::cortex_wire +} // namespace cortex_wire diff --git a/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/metadata.cpp b/cortex_wire_cpp/src/metadata.cpp similarity index 57% rename from ros2_bridge/cortex_ros2_bridge/src/cortex_wire/metadata.cpp rename to cortex_wire_cpp/src/metadata.cpp index 3acaeb7..2f213d9 100644 --- a/ros2_bridge/cortex_ros2_bridge/src/cortex_wire/metadata.cpp +++ b/cortex_wire_cpp/src/metadata.cpp @@ -1,5 +1,5 @@ // Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/cortex_wire/metadata.hpp" +#include "cortex_wire/metadata.hpp" #include @@ -7,7 +7,7 @@ #include #include -namespace cortex_ros2_bridge::cortex_wire +namespace cortex_wire { namespace @@ -145,4 +145,111 @@ std::optional DecodedMetadata::as_oob(const msgpack::object & obj return desc; } -} // namespace cortex_ros2_bridge::cortex_wire +// ---- MetadataBuilder ------------------------------------------------------ + +MetadataBuilder::MetadataBuilder(std::uint32_t field_count) +: sbuf_(), packer_(sbuf_) +{ + packer_.pack_array(field_count); +} + +void MetadataBuilder::pack_nil() {packer_.pack_nil();} +void MetadataBuilder::pack_bool(bool v) {packer_.pack(v);} +void MetadataBuilder::pack_int(std::int64_t v) {packer_.pack(v);} +void MetadataBuilder::pack_uint(std::uint64_t v) {packer_.pack(v);} +void MetadataBuilder::pack_double(double v) {packer_.pack(v);} + +void MetadataBuilder::pack_str(std::string_view s) +{ + packer_.pack_str(static_cast(s.size())); + packer_.pack_str_body(s.data(), static_cast(s.size())); +} + +void MetadataBuilder::pack_bin(const void * data, std::size_t size) +{ + packer_.pack_bin(static_cast(size)); + packer_.pack_bin_body(static_cast(data), static_cast(size)); +} + +std::uint32_t MetadataBuilder::add_oob_buffer(const void * data, std::size_t size) +{ + const auto idx = static_cast(oob_buffers_.size()); + const auto * p = static_cast(data); + oob_buffers_.emplace_back(p, p + size); + return idx; +} + +namespace +{ + +void pack_shape(msgpack::packer & pk, const std::vector & shape) +{ + pk.pack_array(static_cast(shape.size())); + for (auto d : shape) { + if (d < 0) { + pk.pack(d); + } else { + pk.pack(static_cast(d)); + } + } +} + +void pack_str_key(msgpack::packer & pk, std::string_view key) +{ + pk.pack_str(static_cast(key.size())); + pk.pack_str_body(key.data(), static_cast(key.size())); +} + +void pack_str_pair( + msgpack::packer & pk, std::string_view key, std::string_view value) +{ + pack_str_key(pk, key); + pack_str_key(pk, value); +} + +} // namespace + +void MetadataBuilder::pack_numpy_oob( + std::string_view dtype, const std::vector & shape, + const void * buffer_data, std::size_t buffer_size) +{ + const auto idx = add_oob_buffer(buffer_data, buffer_size); + packer_.pack_map(4); + pack_str_pair(packer_, "__cortex_oob__", "numpy"); + pack_str_key(packer_, "buffer"); + packer_.pack(idx); + pack_str_pair(packer_, "dtype", dtype); + pack_str_key(packer_, "shape"); + pack_shape(packer_, shape); +} + +void MetadataBuilder::pack_torch_oob( + std::string_view dtype, const std::vector & shape, + std::string_view device, bool requires_grad, + const void * buffer_data, std::size_t buffer_size) +{ + const auto idx = add_oob_buffer(buffer_data, buffer_size); + packer_.pack_map(6); + pack_str_pair(packer_, "__cortex_oob__", "torch"); + pack_str_key(packer_, "buffer"); + packer_.pack(idx); + pack_str_pair(packer_, "dtype", dtype); + pack_str_key(packer_, "shape"); + pack_shape(packer_, shape); + pack_str_pair(packer_, "device", device); + pack_str_key(packer_, "requires_grad"); + packer_.pack(requires_grad); +} + +MetadataBuilder::Frames MetadataBuilder::finish() && +{ + Frames out; + out.metadata.assign( + reinterpret_cast(sbuf_.data()), + reinterpret_cast(sbuf_.data()) + sbuf_.size()); + out.oob_buffers = std::move(oob_buffers_); + sbuf_.clear(); + return out; +} + +} // namespace cortex_wire diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_discovery_client.cpp b/cortex_wire_cpp/test/test_discovery_client.cpp similarity index 95% rename from ros2_bridge/cortex_ros2_bridge/test/test_discovery_client.cpp rename to cortex_wire_cpp/test/test_discovery_client.cpp index d6cf98a..c084465 100644 --- a/ros2_bridge/cortex_ros2_bridge/test/test_discovery_client.cpp +++ b/cortex_wire_cpp/test/test_discovery_client.cpp @@ -1,5 +1,5 @@ // Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/cortex_wire/discovery_client.hpp" +#include "cortex_wire/discovery_client.hpp" #include #include @@ -12,11 +12,11 @@ #include #include -using cortex_ros2_bridge::cortex_wire::DiscoveryClient; -using cortex_ros2_bridge::cortex_wire::DiscoveryCommand; -using cortex_ros2_bridge::cortex_wire::DiscoveryError; -using cortex_ros2_bridge::cortex_wire::DiscoveryStatus; -using cortex_ros2_bridge::cortex_wire::TopicInfo; +using cortex_wire::DiscoveryClient; +using cortex_wire::DiscoveryCommand; +using cortex_wire::DiscoveryError; +using cortex_wire::DiscoveryStatus; +using cortex_wire::TopicInfo; namespace { diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_fingerprint_table.cpp b/cortex_wire_cpp/test/test_fingerprint_table.cpp similarity index 87% rename from ros2_bridge/cortex_ros2_bridge/test/test_fingerprint_table.cpp rename to cortex_wire_cpp/test/test_fingerprint_table.cpp index 4057865..abc47d4 100644 --- a/ros2_bridge/cortex_ros2_bridge/test/test_fingerprint_table.cpp +++ b/cortex_wire_cpp/test/test_fingerprint_table.cpp @@ -1,17 +1,17 @@ // Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp" +#include "cortex_wire/fingerprint_table.hpp" #include #include #include -using cortex_ros2_bridge::cortex_wire::find_by_fingerprint; -using cortex_ros2_bridge::cortex_wire::find_by_name; -using cortex_ros2_bridge::cortex_wire::kFingerprintTable; -using cortex_ros2_bridge::cortex_wire::kFingerprintTableSize; -using cortex_ros2_bridge::cortex_wire::MessageKind; -using cortex_ros2_bridge::cortex_wire::to_string; +using cortex_wire::find_by_fingerprint; +using cortex_wire::find_by_name; +using cortex_wire::kFingerprintTable; +using cortex_wire::kFingerprintTableSize; +using cortex_wire::MessageKind; +using cortex_wire::to_string; TEST(FingerprintTable, HasExpectedSize) { diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_wire_header.cpp b/cortex_wire_cpp/test/test_header.cpp similarity index 93% rename from ros2_bridge/cortex_ros2_bridge/test/test_wire_header.cpp rename to cortex_wire_cpp/test/test_header.cpp index 87006cd..71d8e29 100644 --- a/ros2_bridge/cortex_ros2_bridge/test/test_wire_header.cpp +++ b/cortex_wire_cpp/test/test_header.cpp @@ -1,5 +1,5 @@ // Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/cortex_wire/header.hpp" +#include "cortex_wire/header.hpp" #include @@ -7,8 +7,8 @@ #include #include -using cortex_ros2_bridge::cortex_wire::MessageHeader; -using cortex_ros2_bridge::cortex_wire::WireDecodeError; +using cortex_wire::MessageHeader; +using cortex_wire::WireDecodeError; TEST(MessageHeader, SizeIs24Bytes) { diff --git a/cortex_wire_cpp/test/test_metadata.cpp b/cortex_wire_cpp/test/test_metadata.cpp new file mode 100644 index 0000000..624a374 --- /dev/null +++ b/cortex_wire_cpp/test/test_metadata.cpp @@ -0,0 +1,373 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_wire/metadata.hpp" + +#include +#include + +#include +#include +#include + +using cortex_wire::DecodedMetadata; +using cortex_wire::MetadataBuilder; +using cortex_wire::OobDescriptor; +using cortex_wire::OobKind; +using cortex_wire::WireDecodeError; + +namespace +{ + +// Build a metadata frame that mirrors what Cortex's +// serialize_message_frames() emits for an ImageMessage(data=ndarray, +// encoding="rgb8", width=640, height=480). Field order matches the dataclass +// declaration in cortex.messages.standard.ImageMessage: +// data, encoding, width, height +std::vector build_image_metadata() +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(4); + + // Field 0: OOB numpy descriptor (data field, buffer index 0) + pk.pack_map(4); + pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("numpy")); + pk.pack(std::string("buffer")); pk.pack(static_cast(0)); + pk.pack(std::string("dtype")); pk.pack(std::string("(480)); + pk.pack(static_cast(640)); + pk.pack(static_cast(3)); + + // Field 1: encoding (str) + pk.pack(std::string("rgb8")); + // Field 2: width (u32) + pk.pack(static_cast(640)); + // Field 3: height (u32) + pk.pack(static_cast(480)); + + return std::vector( + reinterpret_cast(buf.data()), + reinterpret_cast(buf.data()) + buf.size()); +} + +} // namespace + +TEST(DecodedMetadata, ParsesImageLikeMetadata) +{ + const auto bytes = build_image_metadata(); + const auto meta = DecodedMetadata::from_bytes(bytes.data(), bytes.size()); + + ASSERT_EQ(meta.field_count(), 4u); + + // Field 0: OOB descriptor + const auto oob_opt = DecodedMetadata::as_oob(meta.field(0)); + ASSERT_TRUE(oob_opt.has_value()); + EXPECT_EQ(oob_opt->kind, OobKind::Numpy); + EXPECT_EQ(oob_opt->buffer_index, 0u); + EXPECT_EQ(oob_opt->dtype, "shape.size(), 3u); + EXPECT_EQ(oob_opt->shape[0], 480); + EXPECT_EQ(oob_opt->shape[1], 640); + EXPECT_EQ(oob_opt->shape[2], 3); + + // Field 1: encoding + ASSERT_EQ(meta.field(1).type, msgpack::type::STR); + EXPECT_EQ( + std::string(meta.field(1).via.str.ptr, meta.field(1).via.str.size), "rgb8"); + + // Field 2,3: width/height + ASSERT_EQ(meta.field(2).type, msgpack::type::POSITIVE_INTEGER); + EXPECT_EQ(meta.field(2).via.u64, 640u); + ASSERT_EQ(meta.field(3).type, msgpack::type::POSITIVE_INTEGER); + EXPECT_EQ(meta.field(3).via.u64, 480u); +} + +TEST(DecodedMetadata, NonOobMapsAreNotOob) +{ + // A regular user dict {"foo": 1} must not be misread as an OOB descriptor. + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(1); + pk.pack_map(1); + pk.pack(std::string("foo")); + pk.pack(static_cast(1)); + + const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); + ASSERT_EQ(meta.field_count(), 1u); + EXPECT_FALSE(DecodedMetadata::as_oob(meta.field(0)).has_value()); +} + +TEST(DecodedMetadata, TorchOobDescriptor) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(1); + pk.pack_map(6); + pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("torch")); + pk.pack(std::string("buffer")); pk.pack(static_cast(2)); + pk.pack(std::string("dtype")); pk.pack(std::string("(64)); + pk.pack(static_cast(64)); + pk.pack(std::string("device")); pk.pack(std::string("cuda:0")); + pk.pack(std::string("requires_grad")); pk.pack(true); + + const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); + const auto oob = DecodedMetadata::as_oob(meta.field(0)); + ASSERT_TRUE(oob.has_value()); + EXPECT_EQ(oob->kind, OobKind::Torch); + EXPECT_EQ(oob->buffer_index, 2u); + EXPECT_EQ(oob->dtype, "shape, (std::vector{64, 64})); + EXPECT_EQ(oob->device, "cuda:0"); + EXPECT_TRUE(oob->requires_grad); +} + +TEST(DecodedMetadata, NonArrayRootRejected) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_map(1); + pk.pack(std::string("a")); + pk.pack(1); + EXPECT_THROW(DecodedMetadata::from_bytes(buf.data(), buf.size()), WireDecodeError); +} + +TEST(DecodedMetadata, OutOfRangeAccessThrows) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(1); + pk.pack(42); + const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); + EXPECT_NO_THROW((void)meta.field(0)); + EXPECT_THROW((void)meta.field(1), WireDecodeError); +} + +TEST(DecodedMetadata, MalformedBytesThrow) +{ + const std::uint8_t junk[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + EXPECT_THROW(DecodedMetadata::from_bytes(junk, sizeof(junk)), WireDecodeError); +} + +TEST(DecodedMetadata, UnknownOobKindThrows) +{ + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(1); + pk.pack_map(2); + pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("alien")); + pk.pack(std::string("buffer")); pk.pack(static_cast(0)); + + const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); + EXPECT_THROW((void)DecodedMetadata::as_oob(meta.field(0)), WireDecodeError); +} + +TEST(DecodedMetadata, ForwardCompatibleUnknownKeys) +{ + // A descriptor with an unknown extra key must still parse cleanly — we + // want to roll out new fields on the Python side without breaking older + // bridge builds. + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_array(1); + pk.pack_map(5); + pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("numpy")); + pk.pack(std::string("buffer")); pk.pack(static_cast(0)); + pk.pack(std::string("dtype")); pk.pack(std::string("(10)); + pk.pack(std::string("future_field")); pk.pack(123); + + const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); + const auto oob = DecodedMetadata::as_oob(meta.field(0)); + ASSERT_TRUE(oob.has_value()); + EXPECT_EQ(oob->dtype, " pixels(640 * 480 * 3, 0xa5); + MetadataBuilder b(1); + b.pack_numpy_oob("kind, OobKind::Numpy); + EXPECT_EQ(oob->buffer_index, 0u); + EXPECT_EQ(oob->dtype, "shape, (std::vector{480, 640, 3})); +} + +TEST(MetadataBuilder, TorchOobCarriesDeviceAndGrad) +{ + const std::vector weights(8, 0.5f); + MetadataBuilder b(1); + b.pack_torch_oob( + "kind, OobKind::Torch); + EXPECT_EQ(oob->device, "cuda:0"); + EXPECT_TRUE(oob->requires_grad); +} + +TEST(MetadataBuilder, LowLevelPackerForNestedStructures) +{ + // DictMessage adapter path: emit a nested {string: numpy_array} map by + // hand using the low-level packer + add_oob_buffer interface. + MetadataBuilder b(1); + auto & pk = b.packer(); + pk.pack_map(2); + // entry 1: "weights" -> numpy array (buffer 0) + pk.pack_str(7); pk.pack_str_body("weights", 7); + const std::vector w(4, 1.0f); + const auto buf_idx = b.add_oob_buffer(w.data(), w.size() * sizeof(float)); + pk.pack_map(4); + pk.pack_str(14); pk.pack_str_body("__cortex_oob__", 14); + pk.pack_str(5); pk.pack_str_body("numpy", 5); + pk.pack_str(6); pk.pack_str_body("buffer", 6); + pk.pack(buf_idx); + pk.pack_str(5); pk.pack_str_body("dtype", 5); + pk.pack_str(3); pk.pack_str_body("(4)); + // entry 2: "scale" -> 2.0 + pk.pack_str(5); pk.pack_str_body("scale", 5); + pk.pack(2.0); + + auto frames = std::move(b).finish(); + ASSERT_EQ(frames.oob_buffers.size(), 1u); + + // Now decode and use walk_oob to discover the nested descriptor. + const auto meta = DecodedMetadata::from_bytes( + frames.metadata.data(), frames.metadata.size()); + std::vector found; + DecodedMetadata::walk_oob(meta.field(0), [&](const OobDescriptor & d) { + found.push_back(d); + }); + ASSERT_EQ(found.size(), 1u); + EXPECT_EQ(found[0].kind, OobKind::Numpy); + EXPECT_EQ(found[0].buffer_index, 0u); + EXPECT_EQ(found[0].dtype, " chunk(8, static_cast(i)); + const auto idx = b.add_oob_buffer(chunk.data(), chunk.size()); + pk.pack_map(4); + pk.pack_str(14); pk.pack_str_body("__cortex_oob__", 14); + pk.pack_str(5); pk.pack_str_body("numpy", 5); + pk.pack_str(6); pk.pack_str_body("buffer", 6); + pk.pack(idx); + pk.pack_str(5); pk.pack_str_body("dtype", 5); + pk.pack_str(3); pk.pack_str_body("(8)); + } + auto frames = std::move(b).finish(); + ASSERT_EQ(frames.oob_buffers.size(), 2u); + + const auto meta = DecodedMetadata::from_bytes( + frames.metadata.data(), frames.metadata.size()); + std::vector indexes; + DecodedMetadata::walk_oob(meta.field(0), [&](const OobDescriptor & d) { + indexes.push_back(d.buffer_index); + }); + ASSERT_EQ(indexes.size(), 2u); + EXPECT_EQ(indexes[0], 0u); + EXPECT_EQ(indexes[1], 1u); +} + +TEST(MetadataBuilder, WalkOobIgnoresPlainMaps) +{ + // A regular {"foo": 1, "bar": "baz"} dict must not be reported as OOB. + MetadataBuilder b(1); + auto & pk = b.packer(); + pk.pack_map(2); + pk.pack_str(3); pk.pack_str_body("foo", 3); + pk.pack(static_cast(1)); + pk.pack_str(3); pk.pack_str_body("bar", 3); + pk.pack_str(3); pk.pack_str_body("baz", 3); + auto frames = std::move(b).finish(); + + const auto meta = DecodedMetadata::from_bytes( + frames.metadata.data(), frames.metadata.size()); + std::vector found; + DecodedMetadata::walk_oob(meta.field(0), [&](const OobDescriptor & d) { + found.push_back(d); + }); + EXPECT_TRUE(found.empty()); +} diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_oob_buffer.cpp b/cortex_wire_cpp/test/test_oob_buffer.cpp similarity index 93% rename from ros2_bridge/cortex_ros2_bridge/test/test_oob_buffer.cpp rename to cortex_wire_cpp/test/test_oob_buffer.cpp index 8221e50..c08f127 100644 --- a/ros2_bridge/cortex_ros2_bridge/test/test_oob_buffer.cpp +++ b/cortex_wire_cpp/test/test_oob_buffer.cpp @@ -1,5 +1,5 @@ // Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/cortex_wire/oob_buffer.hpp" +#include "cortex_wire/oob_buffer.hpp" #include #include @@ -9,10 +9,10 @@ #include #include -using cortex_ros2_bridge::cortex_wire::make_owned; -using cortex_ros2_bridge::cortex_wire::OobBuffer; -using cortex_ros2_bridge::cortex_wire::ZmqAllocator; -using cortex_ros2_bridge::cortex_wire::ZmqFramePtr; +using cortex_wire::make_owned; +using cortex_wire::OobBuffer; +using cortex_wire::ZmqAllocator; +using cortex_wire::ZmqFramePtr; namespace { diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/metadata.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/metadata.hpp deleted file mode 100644 index 992ab6c..0000000 --- a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_wire/metadata.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#ifndef CORTEX_ROS2_BRIDGE__CORTEX_WIRE__METADATA_HPP_ -#define CORTEX_ROS2_BRIDGE__CORTEX_WIRE__METADATA_HPP_ - -#include - -#include -#include -#include -#include -#include - -#include "cortex_ros2_bridge/cortex_wire/header.hpp" - -namespace cortex_ros2_bridge::cortex_wire -{ - -enum class OobKind { Numpy, Torch }; - -// Mirror of the {"__cortex_oob__": "numpy", ...} / "torch" descriptor that -// cortex.utils.serialization._encode_transport_value emits. The `buffer` -// index refers into the OOB frames that follow the metadata frame on the -// wire (i.e. frame index = 2 + buffer_index in the multipart message). -struct OobDescriptor -{ - OobKind kind = OobKind::Numpy; - std::uint32_t buffer_index = 0; - std::string dtype; // numpy dtype string, e.g. " shape; - // Torch-only fields: - std::string device; - bool requires_grad = false; -}; - -// Owned msgpack object tree for a metadata frame. The unpacker zone is held -// internally so msgpack::object references stay valid for the lifetime of -// this object. -// -// Cortex's metadata frame is always a msgpack array of N field values in -// dataclass declaration order. OOB descriptors appear inline as map objects -// with a "__cortex_oob__" key; everything else is a primitive, nested map, -// or nested list. -class DecodedMetadata -{ -public: - // Parse a msgpack-packed metadata frame. - // Throws WireDecodeError on malformed msgpack or non-array root. - static DecodedMetadata from_bytes(const void * data, std::size_t size); - - // Number of top-level fields (i.e. size of the dataclass's field list). - std::size_t field_count() const noexcept; - - // Access a field by declaration order index. - // Throws WireDecodeError if i is out of range. - const msgpack::object & field(std::size_t i) const; - - // If `obj` is an OOB descriptor map, return it parsed; else nullopt. - static std::optional as_oob(const msgpack::object & obj); - -private: - DecodedMetadata() = default; - - msgpack::object_handle handle_; - const msgpack::object * root_array_ = nullptr; // points into handle_'s zone - std::size_t count_ = 0; -}; - -} // namespace cortex_ros2_bridge::cortex_wire - -#endif // CORTEX_ROS2_BRIDGE__CORTEX_WIRE__METADATA_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_wire_metadata.cpp b/ros2_bridge/cortex_ros2_bridge/test/test_wire_metadata.cpp deleted file mode 100644 index cafb117..0000000 --- a/ros2_bridge/cortex_ros2_bridge/test/test_wire_metadata.cpp +++ /dev/null @@ -1,188 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/cortex_wire/metadata.hpp" - -#include -#include - -#include -#include -#include - -using cortex_ros2_bridge::cortex_wire::DecodedMetadata; -using cortex_ros2_bridge::cortex_wire::OobDescriptor; -using cortex_ros2_bridge::cortex_wire::OobKind; -using cortex_ros2_bridge::cortex_wire::WireDecodeError; - -namespace -{ - -// Build a metadata frame that mirrors what Cortex's -// serialize_message_frames() emits for an ImageMessage(data=ndarray, -// encoding="rgb8", width=640, height=480). Field order matches the dataclass -// declaration in cortex.messages.standard.ImageMessage: -// data, encoding, width, height -std::vector build_image_metadata() -{ - msgpack::sbuffer buf; - msgpack::packer pk(buf); - pk.pack_array(4); - - // Field 0: OOB numpy descriptor (data field, buffer index 0) - pk.pack_map(4); - pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("numpy")); - pk.pack(std::string("buffer")); pk.pack(static_cast(0)); - pk.pack(std::string("dtype")); pk.pack(std::string("(480)); - pk.pack(static_cast(640)); - pk.pack(static_cast(3)); - - // Field 1: encoding (str) - pk.pack(std::string("rgb8")); - // Field 2: width (u32) - pk.pack(static_cast(640)); - // Field 3: height (u32) - pk.pack(static_cast(480)); - - return std::vector( - reinterpret_cast(buf.data()), - reinterpret_cast(buf.data()) + buf.size()); -} - -} // namespace - -TEST(DecodedMetadata, ParsesImageLikeMetadata) -{ - const auto bytes = build_image_metadata(); - const auto meta = DecodedMetadata::from_bytes(bytes.data(), bytes.size()); - - ASSERT_EQ(meta.field_count(), 4u); - - // Field 0: OOB descriptor - const auto oob_opt = DecodedMetadata::as_oob(meta.field(0)); - ASSERT_TRUE(oob_opt.has_value()); - EXPECT_EQ(oob_opt->kind, OobKind::Numpy); - EXPECT_EQ(oob_opt->buffer_index, 0u); - EXPECT_EQ(oob_opt->dtype, "shape.size(), 3u); - EXPECT_EQ(oob_opt->shape[0], 480); - EXPECT_EQ(oob_opt->shape[1], 640); - EXPECT_EQ(oob_opt->shape[2], 3); - - // Field 1: encoding - ASSERT_EQ(meta.field(1).type, msgpack::type::STR); - EXPECT_EQ( - std::string(meta.field(1).via.str.ptr, meta.field(1).via.str.size), "rgb8"); - - // Field 2,3: width/height - ASSERT_EQ(meta.field(2).type, msgpack::type::POSITIVE_INTEGER); - EXPECT_EQ(meta.field(2).via.u64, 640u); - ASSERT_EQ(meta.field(3).type, msgpack::type::POSITIVE_INTEGER); - EXPECT_EQ(meta.field(3).via.u64, 480u); -} - -TEST(DecodedMetadata, NonOobMapsAreNotOob) -{ - // A regular user dict {"foo": 1} must not be misread as an OOB descriptor. - msgpack::sbuffer buf; - msgpack::packer pk(buf); - pk.pack_array(1); - pk.pack_map(1); - pk.pack(std::string("foo")); - pk.pack(static_cast(1)); - - const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); - ASSERT_EQ(meta.field_count(), 1u); - EXPECT_FALSE(DecodedMetadata::as_oob(meta.field(0)).has_value()); -} - -TEST(DecodedMetadata, TorchOobDescriptor) -{ - msgpack::sbuffer buf; - msgpack::packer pk(buf); - pk.pack_array(1); - pk.pack_map(6); - pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("torch")); - pk.pack(std::string("buffer")); pk.pack(static_cast(2)); - pk.pack(std::string("dtype")); pk.pack(std::string("(64)); - pk.pack(static_cast(64)); - pk.pack(std::string("device")); pk.pack(std::string("cuda:0")); - pk.pack(std::string("requires_grad")); pk.pack(true); - - const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); - const auto oob = DecodedMetadata::as_oob(meta.field(0)); - ASSERT_TRUE(oob.has_value()); - EXPECT_EQ(oob->kind, OobKind::Torch); - EXPECT_EQ(oob->buffer_index, 2u); - EXPECT_EQ(oob->dtype, "shape, (std::vector{64, 64})); - EXPECT_EQ(oob->device, "cuda:0"); - EXPECT_TRUE(oob->requires_grad); -} - -TEST(DecodedMetadata, NonArrayRootRejected) -{ - msgpack::sbuffer buf; - msgpack::packer pk(buf); - pk.pack_map(1); - pk.pack(std::string("a")); - pk.pack(1); - EXPECT_THROW(DecodedMetadata::from_bytes(buf.data(), buf.size()), WireDecodeError); -} - -TEST(DecodedMetadata, OutOfRangeAccessThrows) -{ - msgpack::sbuffer buf; - msgpack::packer pk(buf); - pk.pack_array(1); - pk.pack(42); - const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); - EXPECT_NO_THROW((void)meta.field(0)); - EXPECT_THROW((void)meta.field(1), WireDecodeError); -} - -TEST(DecodedMetadata, MalformedBytesThrow) -{ - const std::uint8_t junk[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; - EXPECT_THROW(DecodedMetadata::from_bytes(junk, sizeof(junk)), WireDecodeError); -} - -TEST(DecodedMetadata, UnknownOobKindThrows) -{ - msgpack::sbuffer buf; - msgpack::packer pk(buf); - pk.pack_array(1); - pk.pack_map(2); - pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("alien")); - pk.pack(std::string("buffer")); pk.pack(static_cast(0)); - - const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); - EXPECT_THROW((void)DecodedMetadata::as_oob(meta.field(0)), WireDecodeError); -} - -TEST(DecodedMetadata, ForwardCompatibleUnknownKeys) -{ - // A descriptor with an unknown extra key must still parse cleanly — we - // want to roll out new fields on the Python side without breaking older - // bridge builds. - msgpack::sbuffer buf; - msgpack::packer pk(buf); - pk.pack_array(1); - pk.pack_map(5); - pk.pack(std::string("__cortex_oob__")); pk.pack(std::string("numpy")); - pk.pack(std::string("buffer")); pk.pack(static_cast(0)); - pk.pack(std::string("dtype")); pk.pack(std::string("(10)); - pk.pack(std::string("future_field")); pk.pack(123); - - const auto meta = DecodedMetadata::from_bytes(buf.data(), buf.size()); - const auto oob = DecodedMetadata::as_oob(meta.field(0)); - ASSERT_TRUE(oob.has_value()); - EXPECT_EQ(oob->dtype, " Date: Mon, 11 May 2026 17:00:12 -0400 Subject: [PATCH 04/10] cortex to ros2 and ros2 to cortex adapters added --- ros2_bridge/cortex_ros2_bridge/CMakeLists.txt | 80 +++-- .../include/cortex_ros2_bridge/adapter.hpp | 95 ++++++ .../adapters/primitives.hpp | 91 ++++++ .../include/cortex_ros2_bridge/registry.hpp | 166 ++++++++++ ros2_bridge/cortex_ros2_bridge/package.xml | 5 + .../src/adapters/primitives.cpp | 298 ++++++++++++++++++ .../cortex_ros2_bridge/src/registry.cpp | 28 ++ .../test/test_primitives.cpp | 266 ++++++++++++++++ .../cortex_ros2_bridge/test/test_registry.cpp | 178 +++++++++++ 9 files changed, 1174 insertions(+), 33 deletions(-) create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/src/adapters/primitives.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/src/registry.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_primitives.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_registry.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt b/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt index c8c14ef..ca4fea7 100644 --- a/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt +++ b/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt @@ -13,14 +13,26 @@ endif() find_package(ament_cmake REQUIRED) find_package(yaml-cpp REQUIRED) -find_package(cppzmq REQUIRED) -# msgpack-cxx is header-only. We avoid CMake imported-target propagation -# (which would force msgpack-cxx into our export set on every distro) and -# just locate its include path. Downstream targets get the headers via our -# include directory export. -find_path(MSGPACK_INCLUDE_DIR msgpack.hpp - HINTS /usr/include /usr/local/include - REQUIRED) +find_package(std_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) + +# cortex_wire_cpp — pure-CMake library, prefer installed package; fall back to +# in-tree add_subdirectory for monorepo dev (when this package is built before +# cortex_wire_cpp has been installed). +find_package(cortex_wire_cpp QUIET) +if(NOT cortex_wire_cpp_FOUND) + set(_cwc_dir "${CMAKE_CURRENT_SOURCE_DIR}/../../cortex_wire_cpp") + if(EXISTS "${_cwc_dir}/CMakeLists.txt") + message(STATUS "cortex_wire_cpp: using in-tree source at ${_cwc_dir}") + set(CORTEX_WIRE_BUILD_TESTS OFF CACHE BOOL "" FORCE) + set(CORTEX_WIRE_INSTALL OFF CACHE BOOL "" FORCE) + add_subdirectory("${_cwc_dir}" "${CMAKE_BINARY_DIR}/cortex_wire_cpp_external") + else() + message(FATAL_ERROR + "cortex_wire_cpp not found via find_package and no in-tree copy at " + "${_cwc_dir}. Install cortex_wire_cpp or fix the source layout.") + endif() +endif() # ---- libraries ------------------------------------------------------------ @@ -33,39 +45,50 @@ target_include_directories(cortex_ros2_bridge_config PUBLIC ) target_link_libraries(cortex_ros2_bridge_config PUBLIC yaml-cpp) -add_library(cortex_ros2_bridge_wire SHARED - src/cortex_wire/header.cpp - src/cortex_wire/metadata.cpp - src/cortex_wire/discovery_client.cpp +add_library(cortex_ros2_bridge_adapters SHARED + src/registry.cpp + src/adapters/primitives.cpp ) -target_include_directories(cortex_ros2_bridge_wire PUBLIC +target_include_directories(cortex_ros2_bridge_adapters PUBLIC $ - $ $ ) -target_link_libraries(cortex_ros2_bridge_wire PUBLIC - cppzmq +target_link_libraries(cortex_ros2_bridge_adapters PUBLIC + cortex_ros2_bridge_config + cortex_wire::cortex_wire +) +ament_target_dependencies(cortex_ros2_bridge_adapters PUBLIC + std_msgs + builtin_interfaces ) # ---- install -------------------------------------------------------------- install(DIRECTORY include/ DESTINATION include) -install(TARGETS cortex_ros2_bridge_config cortex_ros2_bridge_wire +install(TARGETS cortex_ros2_bridge_config EXPORT export_cortex_ros2_bridge ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +# cortex_ros2_bridge_adapters is internal to this package: consumed by the +# bridge components in PR4 but not exported downstream (it transitively +# depends on cortex_wire which is brought in via add_subdirectory and is +# therefore not installable from here). Re-evaluate when a downstream +# package needs to link against the adapter library directly. +install(TARGETS cortex_ros2_bridge_adapters + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) -install(PROGRAMS scripts/gen_fingerprint_table.py - DESTINATION lib/${PROJECT_NAME}/scripts) ament_export_include_directories(include) ament_export_targets(export_cortex_ros2_bridge HAS_LIBRARY_TARGET) -ament_export_dependencies(yaml-cpp cppzmq) +ament_export_dependencies(yaml-cpp std_msgs builtin_interfaces) # ---- tests ---------------------------------------------------------------- @@ -86,20 +109,11 @@ if(BUILD_TESTING) ament_add_gtest(test_config test/test_config.cpp) target_link_libraries(test_config cortex_ros2_bridge_config) - ament_add_gtest(test_wire_header test/test_wire_header.cpp) - target_link_libraries(test_wire_header cortex_ros2_bridge_wire) - - ament_add_gtest(test_wire_metadata test/test_wire_metadata.cpp) - target_link_libraries(test_wire_metadata cortex_ros2_bridge_wire) - - ament_add_gtest(test_oob_buffer test/test_oob_buffer.cpp) - target_link_libraries(test_oob_buffer cortex_ros2_bridge_wire) - - ament_add_gtest(test_discovery_client test/test_discovery_client.cpp) - target_link_libraries(test_discovery_client cortex_ros2_bridge_wire) + ament_add_gtest(test_registry test/test_registry.cpp) + target_link_libraries(test_registry cortex_ros2_bridge_adapters) - ament_add_gtest(test_fingerprint_table test/test_fingerprint_table.cpp) - target_link_libraries(test_fingerprint_table cortex_ros2_bridge_wire) + ament_add_gtest(test_primitives test/test_primitives.cpp) + target_link_libraries(test_primitives cortex_ros2_bridge_adapters) endif() ament_package() diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp new file mode 100644 index 0000000..4df1062 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp @@ -0,0 +1,95 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__ADAPTER_HPP_ +#define CORTEX_ROS2_BRIDGE__ADAPTER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "cortex_ros2_bridge/config.hpp" + +namespace cortex_ros2_bridge +{ + +// Bundle of inputs handed to a Cortex → ROS 2 adapter call. +// +// Lifetime contract: `metadata` and `oob_frames` reference memory that the +// caller (the bridge component's recv loop) keeps alive at least until the +// adapter returns. Adapters that want to hold on to an OOB frame beyond +// the call must store a copy of the `ZmqFramePtr` (cheap — it's a +// shared_ptr); the bytes are never copied. +struct CortexInbound +{ + cortex_wire::MessageHeader header; + const cortex_wire::DecodedMetadata & metadata; + const std::vector & oob_frames; + const BridgeEntry & cfg; +}; + +// Output of a ROS 2 → Cortex adapter call: a metadata frame plus the ordered +// list of OOB buffer frames to emit on the wire after it. +struct CortexOutbound +{ + std::vector metadata; + std::vector> oob_buffers; +}; + +// Adapter ABCs are templated on the ROS 2 message type so the bridge +// components can hold concrete `rclcpp::Publisher` / +// `rclcpp::Subscription` handles without runtime type erasure. +// Per-direction split lets adapters opt into one direction only. + +template +class CortexToRos2Adapter +{ +public: + virtual ~CortexToRos2Adapter() = default; + + // The Cortex message kind this adapter consumes. Used for fingerprint + // verification at subscriber setup. + virtual cortex_wire::MessageKind cortex_kind() const = 0; + + // The ROS 2 type string, e.g. "std_msgs/msg/String". Used as the registry + // key alongside cortex_kind() so multiple adapters can coexist for the + // same Cortex kind, mapping to different ROS 2 types. + virtual std::string_view ros2_type_name() const = 0; + + // Build a ROS 2 message from a decoded Cortex inbound. The returned + // unique_ptr is what `rclcpp::Publisher::publish` requires on the + // intra-process zero-copy path. + virtual std::unique_ptr to_ros2(const CortexInbound & in) const = 0; +}; + +template +class Ros2ToCortexAdapter +{ +public: + virtual ~Ros2ToCortexAdapter() = default; + + virtual cortex_wire::MessageKind cortex_kind() const = 0; + virtual std::string_view ros2_type_name() const = 0; + + // Build the Cortex outbound frames from a ROS 2 message. `sequence` is the + // per-publisher monotonic counter the component injects into the wire + // header (the adapter does not own it). + virtual CortexOutbound to_cortex( + const Ros2Msg & msg, std::uint64_t sequence, const BridgeEntry & cfg) const = 0; +}; + +// A bidirectional adapter is just one that inherits both interfaces. +// Provided as a convenience for the common case (most primitive adapters). +template +class BidirectionalAdapter + : public CortexToRos2Adapter, public Ros2ToCortexAdapter +{ +}; + +} // namespace cortex_ros2_bridge + +#endif // CORTEX_ROS2_BRIDGE__ADAPTER_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp new file mode 100644 index 0000000..8efa066 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__ADAPTERS__PRIMITIVES_HPP_ +#define CORTEX_ROS2_BRIDGE__ADAPTERS__PRIMITIVES_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "cortex_ros2_bridge/adapter.hpp" +#include "cortex_ros2_bridge/registry.hpp" + +namespace cortex_ros2_bridge::adapters +{ + +class StringAdapter : public BidirectionalAdapter +{ +public: + cortex_wire::MessageKind cortex_kind() const override; + std::string_view ros2_type_name() const override; + std::unique_ptr to_ros2(const CortexInbound & in) const override; + CortexOutbound to_cortex( + const std_msgs::msg::String & msg, std::uint64_t sequence, + const BridgeEntry & cfg) const override; +}; + +class IntAdapter : public BidirectionalAdapter +{ +public: + cortex_wire::MessageKind cortex_kind() const override; + std::string_view ros2_type_name() const override; + std::unique_ptr to_ros2(const CortexInbound & in) const override; + CortexOutbound to_cortex( + const std_msgs::msg::Int64 & msg, std::uint64_t sequence, + const BridgeEntry & cfg) const override; +}; + +class FloatAdapter : public BidirectionalAdapter +{ +public: + cortex_wire::MessageKind cortex_kind() const override; + std::string_view ros2_type_name() const override; + std::unique_ptr to_ros2(const CortexInbound & in) const override; + CortexOutbound to_cortex( + const std_msgs::msg::Float64 & msg, std::uint64_t sequence, + const BridgeEntry & cfg) const override; +}; + +class BytesAdapter : public BidirectionalAdapter +{ +public: + cortex_wire::MessageKind cortex_kind() const override; + std::string_view ros2_type_name() const override; + std::unique_ptr to_ros2(const CortexInbound & in) const override; + CortexOutbound to_cortex( + const std_msgs::msg::ByteMultiArray & msg, std::uint64_t sequence, + const BridgeEntry & cfg) const override; +}; + +class TimestampAdapter : public BidirectionalAdapter +{ +public: + cortex_wire::MessageKind cortex_kind() const override; + std::string_view ros2_type_name() const override; + std::unique_ptr to_ros2(const CortexInbound & in) const override; + CortexOutbound to_cortex( + const builtin_interfaces::msg::Time & msg, std::uint64_t sequence, + const BridgeEntry & cfg) const override; +}; + +class HeaderAdapter : public BidirectionalAdapter +{ +public: + cortex_wire::MessageKind cortex_kind() const override; + std::string_view ros2_type_name() const override; + std::unique_ptr to_ros2(const CortexInbound & in) const override; + CortexOutbound to_cortex( + const std_msgs::msg::Header & msg, std::uint64_t sequence, + const BridgeEntry & cfg) const override; +}; + +// Register all primitive adapters into the given registry. Returns the +// number of newly-registered entries (each direction counts separately). +// Idempotent — re-registration returns 0. +std::size_t register_primitives(AdapterRegistry & registry); + +} // namespace cortex_ros2_bridge::adapters + +#endif // CORTEX_ROS2_BRIDGE__ADAPTERS__PRIMITIVES_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp new file mode 100644 index 0000000..31b9e3e --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp @@ -0,0 +1,166 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__REGISTRY_HPP_ +#define CORTEX_ROS2_BRIDGE__REGISTRY_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "cortex_ros2_bridge/adapter.hpp" + +namespace cortex_ros2_bridge +{ + +// Type-erased registry of adapters keyed by (MessageKind, ros2_type_name). +// +// Design constraints / non-constraints: +// - The registry stores `std::shared_ptr` to erase the Ros2Msg +// template parameter. Callers look up adapters with the concrete +// template type at compile time; the cast is checked via the stored +// `typeid` of the originally-registered Ros2Msg type. +// - Lookups are O(N) over the small table (~16 entries); not worth a +// hash map. The registry is read-mostly: writes happen once at +// component construction, reads happen per message on hot paths. +// - Adapters are owned via shared_ptr because the bridge components in +// PR4/PR5 will each take a reference; one adapter instance per +// process is fine for stateless primitives. +class AdapterRegistry +{ +public: + // Register a Cortex → ROS 2 adapter. Returns false if (kind, ros2_type) + // is already registered for that direction. + template + bool register_cortex_to_ros2(std::shared_ptr> adapter); + + // Register a ROS 2 → Cortex adapter. + template + bool register_ros2_to_cortex(std::shared_ptr> adapter); + + // Convenience: register both directions of a bidirectional adapter at once. + template + bool register_bidirectional(std::shared_ptr> adapter); + + // Look up a Cortex → ROS 2 adapter by (kind, ros2_type_name). Returns + // nullptr if no adapter is registered. The template parameter must + // match the type the adapter was registered with; a mismatch returns + // nullptr (we use typeid to detect this). + template + std::shared_ptr> find_cortex_to_ros2( + cortex_wire::MessageKind kind, std::string_view ros2_type_name) const; + + template + std::shared_ptr> find_ros2_to_cortex( + cortex_wire::MessageKind kind, std::string_view ros2_type_name) const; + + // Returns true if any adapter exists for (kind, ros2_type_name) in the + // given direction. Used by config validation to fail fast before the + // component starts wiring sockets. + bool has_cortex_to_ros2(cortex_wire::MessageKind kind, std::string_view ros2_type_name) const; + bool has_ros2_to_cortex(cortex_wire::MessageKind kind, std::string_view ros2_type_name) const; + + // Process-wide registry. Adapter modules register into this at static + // initialization via the REGISTER_* helpers below. Tests can use a + // fresh AdapterRegistry instead. + static AdapterRegistry & global(); + +private: + struct Entry + { + std::shared_ptr adapter; // CortexToRos2Adapter / Ros2ToCortexAdapter + std::type_index ros2_msg_type; + std::string ros2_type_name; + }; + using Key = std::pair; + struct KeyHash + { + std::size_t operator()(const Key & k) const noexcept + { + return std::hash{}(k.second) ^ + (static_cast(k.first) * 0x9e3779b97f4a7c15ULL); + } + }; + + mutable std::mutex mu_; + std::unordered_map c2r_; + std::unordered_map r2c_; +}; + +// ---- template definitions ------------------------------------------------ + +template +bool AdapterRegistry::register_cortex_to_ros2( + std::shared_ptr> adapter) +{ + if (!adapter) {return false;} + const cortex_wire::MessageKind kind = adapter->cortex_kind(); + const std::string type_name(adapter->ros2_type_name()); + std::lock_guard g(mu_); + Entry e{ + std::static_pointer_cast(adapter), + std::type_index(typeid(Ros2Msg)), + type_name, + }; + return c2r_.emplace(Key{kind, type_name}, std::move(e)).second; +} + +template +bool AdapterRegistry::register_ros2_to_cortex( + std::shared_ptr> adapter) +{ + if (!adapter) {return false;} + const cortex_wire::MessageKind kind = adapter->cortex_kind(); + const std::string type_name(adapter->ros2_type_name()); + std::lock_guard g(mu_); + Entry e{ + std::static_pointer_cast(adapter), + std::type_index(typeid(Ros2Msg)), + type_name, + }; + return r2c_.emplace(Key{kind, type_name}, std::move(e)).second; +} + +template +bool AdapterRegistry::register_bidirectional( + std::shared_ptr> adapter) +{ + std::shared_ptr> c2r = adapter; + std::shared_ptr> r2c = adapter; + const bool a = register_cortex_to_ros2(std::move(c2r)); + const bool b = register_ros2_to_cortex(std::move(r2c)); + return a && b; +} + +template +std::shared_ptr> AdapterRegistry::find_cortex_to_ros2( + cortex_wire::MessageKind kind, std::string_view ros2_type_name) const +{ + std::lock_guard g(mu_); + auto it = c2r_.find(Key{kind, std::string(ros2_type_name)}); + if (it == c2r_.end()) {return nullptr;} + if (it->second.ros2_msg_type != std::type_index(typeid(Ros2Msg))) { + return nullptr; + } + return std::static_pointer_cast>(it->second.adapter); +} + +template +std::shared_ptr> AdapterRegistry::find_ros2_to_cortex( + cortex_wire::MessageKind kind, std::string_view ros2_type_name) const +{ + std::lock_guard g(mu_); + auto it = r2c_.find(Key{kind, std::string(ros2_type_name)}); + if (it == r2c_.end()) {return nullptr;} + if (it->second.ros2_msg_type != std::type_index(typeid(Ros2Msg))) { + return nullptr; + } + return std::static_pointer_cast>(it->second.adapter); +} + +} // namespace cortex_ros2_bridge + +#endif // CORTEX_ROS2_BRIDGE__REGISTRY_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/package.xml b/ros2_bridge/cortex_ros2_bridge/package.xml index 0b5fad7..9279bda 100644 --- a/ros2_bridge/cortex_ros2_bridge/package.xml +++ b/ros2_bridge/cortex_ros2_bridge/package.xml @@ -13,6 +13,11 @@ ament_cmake yaml-cpp + std_msgs + builtin_interfaces + cppzmq libmsgpack-dev diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/primitives.cpp b/ros2_bridge/cortex_ros2_bridge/src/adapters/primitives.cpp new file mode 100644 index 0000000..97f3560 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/adapters/primitives.cpp @@ -0,0 +1,298 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/adapters/primitives.hpp" + +#include + +#include +#include +#include + +namespace cortex_ros2_bridge::adapters +{ + +namespace +{ + +// ---- decode helpers ------------------------------------------------------ + +[[noreturn]] void throw_field_type( + std::string_view adapter, std::size_t field_index, std::string_view expected, + const msgpack::object & got) +{ + throw cortex_wire::WireDecodeError( + std::string(adapter) + ": field[" + std::to_string(field_index) + + "] expected " + std::string(expected) + ", got type " + + std::to_string(static_cast(got.type))); +} + +std::string read_str( + const cortex_wire::DecodedMetadata & m, std::size_t i, std::string_view adapter) +{ + const auto & f = m.field(i); + if (f.type != msgpack::type::STR) { + throw_field_type(adapter, i, "str", f); + } + return std::string(f.via.str.ptr, f.via.str.size); +} + +std::int64_t read_int( + const cortex_wire::DecodedMetadata & m, std::size_t i, std::string_view adapter) +{ + const auto & f = m.field(i); + if (f.type == msgpack::type::POSITIVE_INTEGER) { + return static_cast(f.via.u64); + } + if (f.type == msgpack::type::NEGATIVE_INTEGER) { + return f.via.i64; + } + throw_field_type(adapter, i, "int", f); +} + +double read_double( + const cortex_wire::DecodedMetadata & m, std::size_t i, std::string_view adapter) +{ + const auto & f = m.field(i); + if (f.type == msgpack::type::FLOAT64 || f.type == msgpack::type::FLOAT32) { + return f.via.f64; + } + // Tolerate Python ints landing on a float field — Cortex emits whatever + // the user passed; coerce so we don't break on the (float=3) case. + if (f.type == msgpack::type::POSITIVE_INTEGER) { + return static_cast(f.via.u64); + } + if (f.type == msgpack::type::NEGATIVE_INTEGER) { + return static_cast(f.via.i64); + } + throw_field_type(adapter, i, "float", f); +} + +std::vector read_bin( + const cortex_wire::DecodedMetadata & m, std::size_t i, std::string_view adapter) +{ + const auto & f = m.field(i); + if (f.type == msgpack::type::BIN) { + return std::vector( + f.via.bin.ptr, f.via.bin.ptr + f.via.bin.size); + } + // BytesMessage may be packed as STR on some round-trips; tolerate it. + if (f.type == msgpack::type::STR) { + return std::vector( + f.via.str.ptr, f.via.str.ptr + f.via.str.size); + } + throw_field_type(adapter, i, "bin", f); +} + +void check_field_count( + const cortex_wire::DecodedMetadata & m, std::size_t expected, std::string_view adapter) +{ + if (m.field_count() != expected) { + throw cortex_wire::WireDecodeError( + std::string(adapter) + ": expected " + std::to_string(expected) + + " fields, got " + std::to_string(m.field_count())); + } +} + +} // namespace + +// ---- StringAdapter ---- StringMessage(data: str) + +cortex_wire::MessageKind StringAdapter::cortex_kind() const +{ + return cortex_wire::MessageKind::StringMessage; +} +std::string_view StringAdapter::ros2_type_name() const +{ + return "std_msgs/msg/String"; +} + +std::unique_ptr StringAdapter::to_ros2(const CortexInbound & in) const +{ + check_field_count(in.metadata, 1, "StringAdapter"); + auto out = std::make_unique(); + out->data = read_str(in.metadata, 0, "StringAdapter"); + return out; +} + +CortexOutbound StringAdapter::to_cortex( + const std_msgs::msg::String & msg, std::uint64_t /*sequence*/, + const BridgeEntry & /*cfg*/) const +{ + cortex_wire::MetadataBuilder b(1); + b.pack_str(msg.data); + auto frames = std::move(b).finish(); + return CortexOutbound{std::move(frames.metadata), std::move(frames.oob_buffers)}; +} + +// ---- IntAdapter ---- IntMessage(data: int) + +cortex_wire::MessageKind IntAdapter::cortex_kind() const +{ + return cortex_wire::MessageKind::IntMessage; +} +std::string_view IntAdapter::ros2_type_name() const {return "std_msgs/msg/Int64";} + +std::unique_ptr IntAdapter::to_ros2(const CortexInbound & in) const +{ + check_field_count(in.metadata, 1, "IntAdapter"); + auto out = std::make_unique(); + out->data = read_int(in.metadata, 0, "IntAdapter"); + return out; +} + +CortexOutbound IntAdapter::to_cortex( + const std_msgs::msg::Int64 & msg, std::uint64_t, const BridgeEntry &) const +{ + cortex_wire::MetadataBuilder b(1); + b.pack_int(msg.data); + auto frames = std::move(b).finish(); + return CortexOutbound{std::move(frames.metadata), std::move(frames.oob_buffers)}; +} + +// ---- FloatAdapter ---- FloatMessage(data: float) + +cortex_wire::MessageKind FloatAdapter::cortex_kind() const +{ + return cortex_wire::MessageKind::FloatMessage; +} +std::string_view FloatAdapter::ros2_type_name() const {return "std_msgs/msg/Float64";} + +std::unique_ptr FloatAdapter::to_ros2(const CortexInbound & in) const +{ + check_field_count(in.metadata, 1, "FloatAdapter"); + auto out = std::make_unique(); + out->data = read_double(in.metadata, 0, "FloatAdapter"); + return out; +} + +CortexOutbound FloatAdapter::to_cortex( + const std_msgs::msg::Float64 & msg, std::uint64_t, const BridgeEntry &) const +{ + cortex_wire::MetadataBuilder b(1); + b.pack_double(msg.data); + auto frames = std::move(b).finish(); + return CortexOutbound{std::move(frames.metadata), std::move(frames.oob_buffers)}; +} + +// ---- BytesAdapter ---- BytesMessage(data: bytes) <-> std_msgs/ByteMultiArray + +cortex_wire::MessageKind BytesAdapter::cortex_kind() const +{ + return cortex_wire::MessageKind::BytesMessage; +} +std::string_view BytesAdapter::ros2_type_name() const {return "std_msgs/msg/ByteMultiArray";} + +std::unique_ptr BytesAdapter::to_ros2( + const CortexInbound & in) const +{ + check_field_count(in.metadata, 1, "BytesAdapter"); + auto out = std::make_unique(); + // std_msgs/ByteMultiArray uses signed bytes (int8) for data. + auto bin = read_bin(in.metadata, 0, "BytesAdapter"); + out->data.reserve(bin.size()); + for (auto b : bin) { + out->data.push_back(static_cast(b)); + } + out->layout.data_offset = 0; + return out; +} + +CortexOutbound BytesAdapter::to_cortex( + const std_msgs::msg::ByteMultiArray & msg, std::uint64_t, const BridgeEntry &) const +{ + cortex_wire::MetadataBuilder b(1); + b.pack_bin(msg.data.data(), msg.data.size()); + auto frames = std::move(b).finish(); + return CortexOutbound{std::move(frames.metadata), std::move(frames.oob_buffers)}; +} + +// ---- TimestampAdapter ---- TimestampMessage(sec: int, nanosec: int) + +cortex_wire::MessageKind TimestampAdapter::cortex_kind() const +{ + return cortex_wire::MessageKind::TimestampMessage; +} +std::string_view TimestampAdapter::ros2_type_name() const +{ + return "builtin_interfaces/msg/Time"; +} + +std::unique_ptr TimestampAdapter::to_ros2( + const CortexInbound & in) const +{ + check_field_count(in.metadata, 2, "TimestampAdapter"); + auto out = std::make_unique(); + out->sec = static_cast(read_int(in.metadata, 0, "TimestampAdapter")); + out->nanosec = static_cast(read_int(in.metadata, 1, "TimestampAdapter")); + return out; +} + +CortexOutbound TimestampAdapter::to_cortex( + const builtin_interfaces::msg::Time & msg, std::uint64_t, const BridgeEntry &) const +{ + cortex_wire::MetadataBuilder b(2); + b.pack_int(msg.sec); + b.pack_uint(msg.nanosec); + auto frames = std::move(b).finish(); + return CortexOutbound{std::move(frames.metadata), std::move(frames.oob_buffers)}; +} + +// ---- HeaderAdapter ---- HeaderMessage(stamp_sec, stamp_nanosec, frame_id, sequence) + +cortex_wire::MessageKind HeaderAdapter::cortex_kind() const +{ + return cortex_wire::MessageKind::HeaderMessage; +} +std::string_view HeaderAdapter::ros2_type_name() const {return "std_msgs/msg/Header";} + +std::unique_ptr HeaderAdapter::to_ros2(const CortexInbound & in) const +{ + check_field_count(in.metadata, 4, "HeaderAdapter"); + auto out = std::make_unique(); + out->stamp.sec = static_cast(read_int(in.metadata, 0, "HeaderAdapter")); + out->stamp.nanosec = static_cast(read_int(in.metadata, 1, "HeaderAdapter")); + out->frame_id = read_str(in.metadata, 2, "HeaderAdapter"); + // std_msgs/Header has no sequence field (it was removed in ROS 2). The + // Cortex `sequence` field is dropped on the forward path; the YAML config + // can attach a separate sequence-tracking node if needed. The reverse + // direction always emits 0 to keep the schema balanced. + (void)read_int(in.metadata, 3, "HeaderAdapter"); + // Apply YAML frame_id override if provided. + if (in.cfg.ros2.frame_id) { + out->frame_id = *in.cfg.ros2.frame_id; + } + return out; +} + +CortexOutbound HeaderAdapter::to_cortex( + const std_msgs::msg::Header & msg, std::uint64_t sequence, const BridgeEntry &) const +{ + cortex_wire::MetadataBuilder b(4); + b.pack_int(msg.stamp.sec); + b.pack_uint(msg.stamp.nanosec); + b.pack_str(msg.frame_id); + b.pack_uint(sequence); + auto frames = std::move(b).finish(); + return CortexOutbound{std::move(frames.metadata), std::move(frames.oob_buffers)}; +} + +// ---- registration -------------------------------------------------------- + +std::size_t register_primitives(AdapterRegistry & registry) +{ + std::size_t added = 0; + added += registry.register_bidirectional( + std::make_shared()) ? 2 : 0; + added += registry.register_bidirectional( + std::make_shared()) ? 2 : 0; + added += registry.register_bidirectional( + std::make_shared()) ? 2 : 0; + added += registry.register_bidirectional( + std::make_shared()) ? 2 : 0; + added += registry.register_bidirectional( + std::make_shared()) ? 2 : 0; + added += registry.register_bidirectional( + std::make_shared()) ? 2 : 0; + return added; +} + +} // namespace cortex_ros2_bridge::adapters diff --git a/ros2_bridge/cortex_ros2_bridge/src/registry.cpp b/ros2_bridge/cortex_ros2_bridge/src/registry.cpp new file mode 100644 index 0000000..bef9353 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/registry.cpp @@ -0,0 +1,28 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/registry.hpp" + +namespace cortex_ros2_bridge +{ + +bool AdapterRegistry::has_cortex_to_ros2( + cortex_wire::MessageKind kind, std::string_view ros2_type_name) const +{ + std::lock_guard g(mu_); + return c2r_.find(Key{kind, std::string(ros2_type_name)}) != c2r_.end(); +} + +bool AdapterRegistry::has_ros2_to_cortex( + cortex_wire::MessageKind kind, std::string_view ros2_type_name) const +{ + std::lock_guard g(mu_); + return r2c_.find(Key{kind, std::string(ros2_type_name)}) != r2c_.end(); +} + +AdapterRegistry & AdapterRegistry::global() +{ + // Meyers singleton; safe across multiple translation-unit initializers. + static AdapterRegistry instance; + return instance; +} + +} // namespace cortex_ros2_bridge diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_primitives.cpp b/ros2_bridge/cortex_ros2_bridge/test/test_primitives.cpp new file mode 100644 index 0000000..baef4b2 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/test/test_primitives.cpp @@ -0,0 +1,266 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/adapters/primitives.hpp" +#include "cortex_ros2_bridge/registry.hpp" + +#include + +#include + +#include +#include + +using cortex_ros2_bridge::AdapterRegistry; +using cortex_ros2_bridge::BridgeEntry; +using cortex_ros2_bridge::CortexInbound; +using cortex_ros2_bridge::adapters::BytesAdapter; +using cortex_ros2_bridge::adapters::FloatAdapter; +using cortex_ros2_bridge::adapters::HeaderAdapter; +using cortex_ros2_bridge::adapters::IntAdapter; +using cortex_ros2_bridge::adapters::StringAdapter; +using cortex_ros2_bridge::adapters::TimestampAdapter; +using cortex_wire::DecodedMetadata; +using cortex_wire::MessageHeader; +using cortex_wire::MessageKind; + +namespace +{ + +// Build a CortexInbound from a metadata byte buffer for tests. The frame +// references the caller-owned bytes; keep them alive past the call. +struct InboundFixture +{ + std::vector metadata_bytes; + cortex_wire::DecodedMetadata metadata; + std::vector oob_frames; + cortex_wire::MessageHeader header{}; + BridgeEntry cfg; + + explicit InboundFixture(std::vector bytes) + : metadata_bytes(std::move(bytes)), + metadata(DecodedMetadata::from_bytes(metadata_bytes.data(), metadata_bytes.size())) + { + } + + CortexInbound view() const {return CortexInbound{header, metadata, oob_frames, cfg};} +}; + +} // namespace + +// ---- StringAdapter -------------------------------------------------------- + +TEST(StringAdapter, CortexToRos2) +{ + cortex_wire::MetadataBuilder b(1); + b.pack_str("hello cortex"); + auto frames = std::move(b).finish(); + InboundFixture fx(std::move(frames.metadata)); + + StringAdapter adapter; + auto msg = adapter.to_ros2(fx.view()); + ASSERT_NE(msg, nullptr); + EXPECT_EQ(msg->data, "hello cortex"); +} + +TEST(StringAdapter, Ros2ToCortexRoundTrip) +{ + std_msgs::msg::String src; + src.data = "round trip"; + + StringAdapter adapter; + BridgeEntry cfg; + auto out = adapter.to_cortex(src, /*sequence=*/123, cfg); + EXPECT_TRUE(out.oob_buffers.empty()); + + InboundFixture fx(std::move(out.metadata)); + auto round = adapter.to_ros2(fx.view()); + ASSERT_NE(round, nullptr); + EXPECT_EQ(round->data, "round trip"); +} + +TEST(StringAdapter, RejectsWrongFieldCount) +{ + cortex_wire::MetadataBuilder b(2); + b.pack_str("a"); + b.pack_str("b"); + auto frames = std::move(b).finish(); + InboundFixture fx(std::move(frames.metadata)); + + StringAdapter adapter; + EXPECT_THROW(adapter.to_ros2(fx.view()), cortex_wire::WireDecodeError); +} + +TEST(StringAdapter, KindAndTypeNames) +{ + StringAdapter a; + EXPECT_EQ(a.cortex_kind(), MessageKind::StringMessage); + EXPECT_EQ(a.ros2_type_name(), "std_msgs/msg/String"); +} + +// ---- IntAdapter ----------------------------------------------------------- + +TEST(IntAdapter, CortexToRos2WithNegativeInt) +{ + cortex_wire::MetadataBuilder b(1); + b.pack_int(-42); + auto frames = std::move(b).finish(); + InboundFixture fx(std::move(frames.metadata)); + + IntAdapter adapter; + auto msg = adapter.to_ros2(fx.view()); + EXPECT_EQ(msg->data, -42); +} + +TEST(IntAdapter, RoundTrip) +{ + std_msgs::msg::Int64 src; + src.data = 1'234'567'890LL; + IntAdapter adapter; + BridgeEntry cfg; + auto out = adapter.to_cortex(src, 0, cfg); + InboundFixture fx(std::move(out.metadata)); + EXPECT_EQ(adapter.to_ros2(fx.view())->data, 1'234'567'890LL); +} + +// ---- FloatAdapter --------------------------------------------------------- + +TEST(FloatAdapter, RoundTrip) +{ + std_msgs::msg::Float64 src; + src.data = 2.718281828; + FloatAdapter adapter; + BridgeEntry cfg; + auto out = adapter.to_cortex(src, 0, cfg); + InboundFixture fx(std::move(out.metadata)); + EXPECT_DOUBLE_EQ(adapter.to_ros2(fx.view())->data, 2.718281828); +} + +TEST(FloatAdapter, AcceptsIntegerInCortex) +{ + // Cortex may emit an integer where the dataclass declares float; tolerate. + cortex_wire::MetadataBuilder b(1); + b.pack_int(7); + auto frames = std::move(b).finish(); + InboundFixture fx(std::move(frames.metadata)); + FloatAdapter adapter; + EXPECT_DOUBLE_EQ(adapter.to_ros2(fx.view())->data, 7.0); +} + +// ---- BytesAdapter --------------------------------------------------------- + +TEST(BytesAdapter, RoundTripUsesMsgpackBin) +{ + std_msgs::msg::ByteMultiArray src; + // std_msgs/ByteMultiArray::data is std::vector (rosidl maps + // byte[] to unsigned char), so we cast through that. + src.data = std::vector{0x00, 0x01, 0xfe, 0xff}; + BytesAdapter adapter; + BridgeEntry cfg; + auto out = adapter.to_cortex(src, 0, cfg); + + // Verify the metadata field is encoded as msgpack BIN, not STR. + auto md = DecodedMetadata::from_bytes(out.metadata.data(), out.metadata.size()); + ASSERT_EQ(md.field_count(), 1u); + EXPECT_EQ(md.field(0).type, msgpack::type::BIN); + + InboundFixture fx(std::move(out.metadata)); + auto round = adapter.to_ros2(fx.view()); + ASSERT_EQ(round->data.size(), 4u); + EXPECT_EQ(static_cast(round->data[0]), 0x00); + EXPECT_EQ(static_cast(round->data[2]), 0xfe); + EXPECT_EQ(static_cast(round->data[3]), 0xff); +} + +// ---- TimestampAdapter ----------------------------------------------------- + +TEST(TimestampAdapter, RoundTrip) +{ + builtin_interfaces::msg::Time src; + src.sec = 1715000000; + src.nanosec = 123456789; + + TimestampAdapter adapter; + BridgeEntry cfg; + auto out = adapter.to_cortex(src, 0, cfg); + InboundFixture fx(std::move(out.metadata)); + auto round = adapter.to_ros2(fx.view()); + EXPECT_EQ(round->sec, 1715000000); + EXPECT_EQ(round->nanosec, 123456789u); +} + +TEST(TimestampAdapter, RejectsBadFieldCount) +{ + cortex_wire::MetadataBuilder b(1); + b.pack_int(0); + auto frames = std::move(b).finish(); + InboundFixture fx(std::move(frames.metadata)); + TimestampAdapter adapter; + EXPECT_THROW(adapter.to_ros2(fx.view()), cortex_wire::WireDecodeError); +} + +// ---- HeaderAdapter -------------------------------------------------------- + +TEST(HeaderAdapter, RoundTrip) +{ + std_msgs::msg::Header src; + src.stamp.sec = 100; + src.stamp.nanosec = 200u; + src.frame_id = "base_link"; + + HeaderAdapter adapter; + BridgeEntry cfg; + auto out = adapter.to_cortex(src, /*sequence=*/55, cfg); + + // Cortex side carries 4 fields: stamp_sec, stamp_nanosec, frame_id, sequence. + auto md = DecodedMetadata::from_bytes(out.metadata.data(), out.metadata.size()); + ASSERT_EQ(md.field_count(), 4u); + + InboundFixture fx(std::move(out.metadata)); + auto round = adapter.to_ros2(fx.view()); + EXPECT_EQ(round->stamp.sec, 100); + EXPECT_EQ(round->stamp.nanosec, 200u); + EXPECT_EQ(round->frame_id, "base_link"); +} + +TEST(HeaderAdapter, FrameIdYamlOverride) +{ + cortex_wire::MetadataBuilder b(4); + b.pack_int(0); + b.pack_uint(0); + b.pack_str("sensor_frame"); + b.pack_uint(0); + auto frames = std::move(b).finish(); + + InboundFixture fx(std::move(frames.metadata)); + fx.cfg.ros2.frame_id = "map"; // YAML override + + HeaderAdapter adapter; + auto out = adapter.to_ros2(fx.view()); + EXPECT_EQ(out->frame_id, "map") << "YAML frame_id must override the wire value"; +} + +// ---- registration helper -------------------------------------------------- + +TEST(RegisterPrimitives, RegistersAllSixBidirectional) +{ + AdapterRegistry reg; + const auto added = cortex_ros2_bridge::adapters::register_primitives(reg); + EXPECT_EQ(added, 12u); // 6 adapters × 2 directions + + EXPECT_TRUE(reg.has_cortex_to_ros2(MessageKind::StringMessage, "std_msgs/msg/String")); + EXPECT_TRUE(reg.has_cortex_to_ros2(MessageKind::IntMessage, "std_msgs/msg/Int64")); + EXPECT_TRUE(reg.has_cortex_to_ros2(MessageKind::FloatMessage, "std_msgs/msg/Float64")); + EXPECT_TRUE(reg.has_cortex_to_ros2(MessageKind::BytesMessage, "std_msgs/msg/ByteMultiArray")); + EXPECT_TRUE(reg.has_cortex_to_ros2(MessageKind::TimestampMessage, "builtin_interfaces/msg/Time")); + EXPECT_TRUE(reg.has_cortex_to_ros2(MessageKind::HeaderMessage, "std_msgs/msg/Header")); + + EXPECT_TRUE(reg.has_ros2_to_cortex(MessageKind::StringMessage, "std_msgs/msg/String")); + EXPECT_TRUE(reg.has_ros2_to_cortex(MessageKind::HeaderMessage, "std_msgs/msg/Header")); +} + +TEST(RegisterPrimitives, IsIdempotent) +{ + AdapterRegistry reg; + cortex_ros2_bridge::adapters::register_primitives(reg); + // Second call adds nothing. + EXPECT_EQ(cortex_ros2_bridge::adapters::register_primitives(reg), 0u); +} diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_registry.cpp b/ros2_bridge/cortex_ros2_bridge/test/test_registry.cpp new file mode 100644 index 0000000..4a9f46e --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/test/test_registry.cpp @@ -0,0 +1,178 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/registry.hpp" + +#include +#include +#include + +#include + +using cortex_ros2_bridge::AdapterRegistry; +using cortex_ros2_bridge::BidirectionalAdapter; +using cortex_ros2_bridge::CortexInbound; +using cortex_ros2_bridge::CortexOutbound; +using cortex_ros2_bridge::CortexToRos2Adapter; +using cortex_ros2_bridge::Ros2ToCortexAdapter; +using cortex_wire::MessageKind; + +namespace +{ + +class FakeStringAdapter : public BidirectionalAdapter +{ +public: + cortex_wire::MessageKind cortex_kind() const override {return MessageKind::StringMessage;} + std::string_view ros2_type_name() const override {return "std_msgs/msg/String";} + std::unique_ptr to_ros2(const CortexInbound &) const override + { + auto m = std::make_unique(); + m->data = "from_cortex"; + return m; + } + CortexOutbound to_cortex( + const std_msgs::msg::String &, std::uint64_t, + const cortex_ros2_bridge::BridgeEntry &) const override + { + return CortexOutbound{}; + } +}; + +class FakeIntAdapter : public BidirectionalAdapter +{ +public: + cortex_wire::MessageKind cortex_kind() const override {return MessageKind::IntMessage;} + std::string_view ros2_type_name() const override {return "std_msgs/msg/Int64";} + std::unique_ptr to_ros2(const CortexInbound &) const override + { + return std::make_unique(); + } + CortexOutbound to_cortex( + const std_msgs::msg::Int64 &, std::uint64_t, + const cortex_ros2_bridge::BridgeEntry &) const override + { + return CortexOutbound{}; + } +}; + +} // namespace + +TEST(Registry, RegisterAndLookupBidirectional) +{ + AdapterRegistry reg; + EXPECT_TRUE(reg.register_bidirectional( + std::make_shared())); + EXPECT_TRUE(reg.has_cortex_to_ros2(MessageKind::StringMessage, "std_msgs/msg/String")); + EXPECT_TRUE(reg.has_ros2_to_cortex(MessageKind::StringMessage, "std_msgs/msg/String")); + + auto fwd = reg.find_cortex_to_ros2( + MessageKind::StringMessage, "std_msgs/msg/String"); + ASSERT_NE(fwd, nullptr); + cortex_ros2_bridge::BridgeEntry cfg; + cortex_wire::DecodedMetadata empty = []() { + msgpack::sbuffer s; msgpack::packer p(s); p.pack_array(0); + return cortex_wire::DecodedMetadata::from_bytes(s.data(), s.size()); + }(); + std::vector no_oob; + cortex_wire::MessageHeader hdr{}; + CortexInbound in{hdr, empty, no_oob, cfg}; + auto msg = fwd->to_ros2(in); + ASSERT_NE(msg, nullptr); + EXPECT_EQ(msg->data, "from_cortex"); +} + +TEST(Registry, MismatchedRos2TypeReturnsNull) +{ + AdapterRegistry reg; + reg.register_bidirectional(std::make_shared()); + + // Look up with the wrong template type but the right (kind, type_name). + // The typeid check inside find_* must catch this. + auto wrong = reg.find_cortex_to_ros2( + MessageKind::StringMessage, "std_msgs/msg/String"); + EXPECT_EQ(wrong, nullptr); +} + +TEST(Registry, DistinctRos2TypesShareSameCortexKind) +{ + // Two adapters claiming the same Cortex kind but different ROS 2 types + // must coexist — the kind is not unique in the registry, the (kind, + // type_name) pair is. + AdapterRegistry reg; + + class A : public BidirectionalAdapter + { +public: + cortex_wire::MessageKind cortex_kind() const override + { + return MessageKind::HeaderMessage; + } + std::string_view ros2_type_name() const override {return "std_msgs/msg/String";} + std::unique_ptr to_ros2(const CortexInbound &) const override + { + return std::make_unique(); + } + CortexOutbound to_cortex( + const std_msgs::msg::String &, std::uint64_t, + const cortex_ros2_bridge::BridgeEntry &) const override + { + return CortexOutbound{}; + } + }; + class B : public BidirectionalAdapter + { +public: + cortex_wire::MessageKind cortex_kind() const override + { + return MessageKind::HeaderMessage; + } + std::string_view ros2_type_name() const override {return "std_msgs/msg/Int64";} + std::unique_ptr to_ros2(const CortexInbound &) const override + { + return std::make_unique(); + } + CortexOutbound to_cortex( + const std_msgs::msg::Int64 &, std::uint64_t, + const cortex_ros2_bridge::BridgeEntry &) const override + { + return CortexOutbound{}; + } + }; + + EXPECT_TRUE(reg.register_bidirectional(std::make_shared())); + EXPECT_TRUE(reg.register_bidirectional(std::make_shared())); + + EXPECT_NE( + reg.find_cortex_to_ros2( + MessageKind::HeaderMessage, "std_msgs/msg/String"), + nullptr); + EXPECT_NE( + reg.find_cortex_to_ros2( + MessageKind::HeaderMessage, "std_msgs/msg/Int64"), + nullptr); +} + +TEST(Registry, DuplicateRegistrationRejected) +{ + AdapterRegistry reg; + EXPECT_TRUE(reg.register_bidirectional( + std::make_shared())); + // Second registration with the same key returns false on each direction; + // register_bidirectional returns false because at least one failed. + EXPECT_FALSE(reg.register_bidirectional( + std::make_shared())); +} + +TEST(Registry, MissingLookupReturnsNull) +{ + AdapterRegistry reg; + EXPECT_FALSE(reg.has_cortex_to_ros2(MessageKind::ImageMessage, "sensor_msgs/msg/Image")); + auto p = reg.find_cortex_to_ros2( + MessageKind::ImageMessage, "sensor_msgs/msg/Image"); + EXPECT_EQ(p, nullptr); +} + +TEST(Registry, GlobalIsSharedAcrossCalls) +{ + // Sanity: AdapterRegistry::global() returns the same instance. + EXPECT_EQ(&AdapterRegistry::global(), &AdapterRegistry::global()); +} From 4e81b78deff82119000579e9ca7fb3c8331fe5f7 Mon Sep 17 00:00:00 2001 From: Richeek Das Date: Mon, 11 May 2026 17:16:39 -0400 Subject: [PATCH 05/10] cortex to ros2 and ros2 to cortex node implemented --- ros2_bridge/cortex_ros2_bridge/CMakeLists.txt | 61 ++- .../include/cortex_ros2_bridge/binding.hpp | 288 +++++++++++ .../cortex_ros2_bridge/binding_factory.hpp | 102 ++++ .../cortex_to_ros2_node.hpp | 49 ++ .../include/cortex_ros2_bridge/qos.hpp | 17 + .../ros2_to_cortex_node.hpp | 50 ++ .../launch/composable_container.launch.py | 47 ++ .../launch/cortex_to_ros2.launch.py | 34 ++ .../launch/ros2_to_cortex.launch.py | 34 ++ ros2_bridge/cortex_ros2_bridge/package.xml | 2 + .../src/binding_factory.cpp | 85 ++++ .../src/cortex_to_ros2_node.cpp | 164 ++++++ ros2_bridge/cortex_ros2_bridge/src/qos.cpp | 30 ++ .../src/ros2_to_cortex_node.cpp | 214 ++++++++ .../test/test_binding_factory.cpp | 46 ++ .../test/test_components_e2e.cpp | 472 ++++++++++++++++++ .../cortex_ros2_bridge/test/test_qos.cpp | 56 +++ 17 files changed, 1741 insertions(+), 10 deletions(-) create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_to_ros2_node.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/ros2_to_cortex_node.hpp create mode 100644 ros2_bridge/cortex_ros2_bridge/launch/composable_container.launch.py create mode 100644 ros2_bridge/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py create mode 100644 ros2_bridge/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py create mode 100644 ros2_bridge/cortex_ros2_bridge/src/binding_factory.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/src/qos.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_binding_factory.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_components_e2e.cpp create mode 100644 ros2_bridge/cortex_ros2_bridge/test/test_qos.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt b/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt index ca4fea7..6ea614d 100644 --- a/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt +++ b/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt @@ -13,6 +13,8 @@ endif() find_package(ament_cmake REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) @@ -62,6 +64,39 @@ ament_target_dependencies(cortex_ros2_bridge_adapters PUBLIC builtin_interfaces ) +# Components library — registered with rclcpp_components so the +# component_container_mt executable can load it as composable nodes. +add_library(cortex_ros2_bridge_components SHARED + src/qos.cpp + src/binding_factory.cpp + src/cortex_to_ros2_node.cpp + src/ros2_to_cortex_node.cpp +) +target_include_directories(cortex_ros2_bridge_components PUBLIC + $ + $ +) +target_link_libraries(cortex_ros2_bridge_components PUBLIC + cortex_ros2_bridge_config + cortex_ros2_bridge_adapters + cortex_wire::cortex_wire +) +ament_target_dependencies(cortex_ros2_bridge_components PUBLIC + rclcpp + rclcpp_components + std_msgs + builtin_interfaces +) + +rclcpp_components_register_node(cortex_ros2_bridge_components + PLUGIN "cortex_ros2_bridge::CortexToRos2Bridge" + EXECUTABLE cortex_to_ros2 +) +rclcpp_components_register_node(cortex_ros2_bridge_components + PLUGIN "cortex_ros2_bridge::Ros2ToCortexBridge" + EXECUTABLE ros2_to_cortex +) + # ---- install -------------------------------------------------------------- install(DIRECTORY include/ DESTINATION include) @@ -72,12 +107,9 @@ install(TARGETS cortex_ros2_bridge_config LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -# cortex_ros2_bridge_adapters is internal to this package: consumed by the -# bridge components in PR4 but not exported downstream (it transitively -# depends on cortex_wire which is brought in via add_subdirectory and is -# therefore not installable from here). Re-evaluate when a downstream -# package needs to link against the adapter library directly. -install(TARGETS cortex_ros2_bridge_adapters +# Internal-to-this-package libraries (not exported downstream because they +# transitively pin cortex_wire which is brought in via add_subdirectory). +install(TARGETS cortex_ros2_bridge_adapters cortex_ros2_bridge_components ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -88,7 +120,7 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) ament_export_include_directories(include) ament_export_targets(export_cortex_ros2_bridge HAS_LIBRARY_TARGET) -ament_export_dependencies(yaml-cpp std_msgs builtin_interfaces) +ament_export_dependencies(yaml-cpp rclcpp rclcpp_components std_msgs builtin_interfaces) # ---- tests ---------------------------------------------------------------- @@ -96,9 +128,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) - # Skip the opinionated style linters; keep the substantive ones (cppcheck, - # lint_cmake, xmllint). Style is enforced by clang-format separately if/when - # we add one. + # Skip the opinionated style linters; keep cppcheck, lint_cmake, xmllint. set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) set(ament_cmake_uncrustify_FOUND TRUE) @@ -114,6 +144,17 @@ if(BUILD_TESTING) ament_add_gtest(test_primitives test/test_primitives.cpp) target_link_libraries(test_primitives cortex_ros2_bridge_adapters) + + ament_add_gtest(test_qos test/test_qos.cpp) + target_link_libraries(test_qos cortex_ros2_bridge_components) + + ament_add_gtest(test_binding_factory test/test_binding_factory.cpp) + target_link_libraries(test_binding_factory cortex_ros2_bridge_components) + + # End-to-end smoke test: spins up a mock discovery daemon + a Python-style + # publisher / subscriber, exercises both components, and asserts delivery. + ament_add_gtest(test_components_e2e test/test_components_e2e.cpp) + target_link_libraries(test_components_e2e cortex_ros2_bridge_components) endif() ament_package() diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp new file mode 100644 index 0000000..45bf7cb --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp @@ -0,0 +1,288 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__BINDING_HPP_ +#define CORTEX_ROS2_BRIDGE__BINDING_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cortex_ros2_bridge/adapter.hpp" +#include "cortex_ros2_bridge/config.hpp" + +namespace cortex_ros2_bridge +{ + +// Non-template binding interfaces used by the bridge components to manage +// the bridges uniformly. The component holds a vector of these. + +class CortexToRos2BindingBase +{ +public: + virtual ~CortexToRos2BindingBase() = default; + virtual void start() = 0; + virtual void stop() = 0; + virtual const std::string & entry_name() const = 0; +}; + +class Ros2ToCortexBindingBase +{ +public: + virtual ~Ros2ToCortexBindingBase() = default; + // start() is a no-op for ros2_to_cortex (the rclcpp subscription is created + // at construction time and starts receiving immediately when the executor + // spins). It is kept symmetric with the other direction for the component's + // benefit. + virtual void start() = 0; + virtual void stop() = 0; + virtual const std::string & entry_name() const = 0; +}; + +// ---- CortexToRos2BindingImpl --------------------------------------------- + +// One-per-bridge-entry: owns a SUB socket on its own thread, decodes incoming +// multipart frames, looks up the adapter, and publishes the resulting ROS 2 +// message via a templated rclcpp::Publisher. +template +class CortexToRos2BindingImpl : public CortexToRos2BindingBase +{ +public: + CortexToRos2BindingImpl( + rclcpp::Node * node, + zmq::context_t * ctx, + BridgeEntry cfg, + cortex_wire::TopicInfo topic_info, + std::shared_ptr> adapter, + const rclcpp::QoS & qos) + : node_(node), + ctx_(ctx), + cfg_(std::move(cfg)), + topic_info_(std::move(topic_info)), + adapter_(std::move(adapter)), + publisher_(node_->create_publisher(cfg_.ros2.topic, qos)) + { + } + + ~CortexToRos2BindingImpl() override + { + stop(); + } + + void start() override + { + if (thread_.joinable()) {return;} + running_.store(true); + thread_ = std::thread(&CortexToRos2BindingImpl::recv_loop, this); + } + + void stop() override + { + if (!running_.exchange(false)) {return;} + if (thread_.joinable()) {thread_.join();} + } + + const std::string & entry_name() const override {return cfg_.name;} + +private: + void recv_loop() + { + zmq::socket_t sub(*ctx_, zmq::socket_type::sub); + sub.set(zmq::sockopt::linger, 0); + // 100 ms recv timeout so stop() makes us notice promptly. + sub.set(zmq::sockopt::rcvtimeo, 100); + try { + sub.connect(topic_info_.address); + } catch (const zmq::error_t & e) { + RCLCPP_ERROR( + node_->get_logger(), "[%s] connect(%s) failed: %s", + cfg_.name.c_str(), topic_info_.address.c_str(), e.what()); + return; + } + sub.set(zmq::sockopt::subscribe, cfg_.cortex.topic); + + std::vector frames; + while (running_.load()) { + frames.clear(); + bool got_one = false; + // Read a full multipart message. + while (true) { + zmq::message_t f; + zmq::recv_result_t r; + try { + r = sub.recv(f, zmq::recv_flags::none); + } catch (const zmq::error_t & e) { + if (e.num() == ETERM) {return;} + RCLCPP_WARN(node_->get_logger(), "[%s] recv error: %s", cfg_.name.c_str(), e.what()); + break; + } + if (!r) {break;} // timeout + const bool has_more = f.more(); + frames.emplace_back(std::move(f)); + got_one = true; + if (!has_more) {break;} + } + if (!got_one) {continue;} + if (frames.size() < 3) { + RCLCPP_WARN( + node_->get_logger(), "[%s] short message: %zu frames", cfg_.name.c_str(), + frames.size()); + continue; + } + + try { + const auto header = cortex_wire::MessageHeader::from_bytes( + frames[1].data(), frames[1].size()); + if (header.fingerprint != topic_info_.fingerprint) { + RCLCPP_WARN( + node_->get_logger(), + "[%s] fingerprint mismatch: header=0x%016lx expected=0x%016lx — dropping", + cfg_.name.c_str(), + static_cast(header.fingerprint), + static_cast(topic_info_.fingerprint)); + continue; + } + const auto metadata = cortex_wire::DecodedMetadata::from_bytes( + frames[2].data(), frames[2].size()); + + std::vector oob; + oob.reserve(frames.size() > 3 ? frames.size() - 3 : 0); + for (std::size_t i = 3; i < frames.size(); ++i) { + oob.push_back(cortex_wire::make_owned(std::move(frames[i]))); + } + + const CortexInbound in{header, metadata, oob, cfg_}; + auto msg = adapter_->to_ros2(in); + if (msg) { + publisher_->publish(std::move(msg)); + } + } catch (const std::exception & e) { + RCLCPP_WARN(node_->get_logger(), "[%s] decode error: %s", cfg_.name.c_str(), e.what()); + } + } + } + + rclcpp::Node * node_; + zmq::context_t * ctx_; + BridgeEntry cfg_; + cortex_wire::TopicInfo topic_info_; + std::shared_ptr> adapter_; + typename rclcpp::Publisher::SharedPtr publisher_; + std::atomic running_{false}; + std::thread thread_; +}; + +// ---- Ros2ToCortexBindingImpl --------------------------------------------- + +// One-per-bridge-entry: subscribes via rclcpp, packs Cortex frames in the +// callback, and emits over a single PUB socket. The rclcpp executor +// guarantees serialised callbacks per subscription (single-threaded callback +// group by default), so the PUB socket is only touched from one thread. +template +class Ros2ToCortexBindingImpl : public Ros2ToCortexBindingBase +{ +public: + Ros2ToCortexBindingImpl( + rclcpp::Node * node, + zmq::context_t * ctx, + BridgeEntry cfg, + std::shared_ptr> adapter, + std::string pub_endpoint, + std::uint64_t fingerprint, + const rclcpp::QoS & qos) + : node_(node), + ctx_(ctx), + cfg_(std::move(cfg)), + adapter_(std::move(adapter)), + pub_endpoint_(std::move(pub_endpoint)), + fingerprint_(fingerprint), + pub_socket_(*ctx_, zmq::socket_type::pub) + { + pub_socket_.set(zmq::sockopt::linger, 0); + pub_socket_.bind(pub_endpoint_); + + subscription_ = node_->create_subscription( + cfg_.ros2.topic, qos, + [this](std::shared_ptr msg) {on_ros2(*msg);}); + } + + ~Ros2ToCortexBindingImpl() override + { + stop(); + } + + void start() override {} + void stop() override + { + // Drop the subscription first — rclcpp guarantees no further callbacks + // touch this object after the SharedPtr is reset. + subscription_.reset(); + } + + const std::string & entry_name() const override {return cfg_.name;} + const std::string & pub_endpoint() const {return pub_endpoint_;} + +private: + void on_ros2(const Ros2Msg & msg) + { + try { + const auto seq = sequence_.fetch_add(1); + auto out = adapter_->to_cortex(msg, seq, cfg_); + + const std::uint64_t now_ns = static_cast( + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count()); + cortex_wire::MessageHeader header{fingerprint_, now_ns, seq}; + + std::array header_bytes{}; + header.to_bytes(header_bytes.data()); + + // Multipart: [topic, header, metadata, *oob] + const std::size_t frame_count = 3 + out.oob_buffers.size(); + std::size_t i = 0; + + auto send = [&](const void * data, std::size_t size) { + zmq::message_t m(size); + std::memcpy(m.data(), data, size); + const auto flags = (i + 1 < frame_count) ? + zmq::send_flags::sndmore : zmq::send_flags::none; + (void)pub_socket_.send(m, flags); + ++i; + }; + + send(cfg_.cortex.topic.data(), cfg_.cortex.topic.size()); + send(header_bytes.data(), header_bytes.size()); + send(out.metadata.data(), out.metadata.size()); + for (const auto & buf : out.oob_buffers) { + send(buf.data(), buf.size()); + } + } catch (const std::exception & e) { + RCLCPP_WARN(node_->get_logger(), "[%s] publish error: %s", cfg_.name.c_str(), e.what()); + } + } + + rclcpp::Node * node_; + zmq::context_t * ctx_; + BridgeEntry cfg_; + std::shared_ptr> adapter_; + std::string pub_endpoint_; + std::uint64_t fingerprint_; + zmq::socket_t pub_socket_; + typename rclcpp::Subscription::SharedPtr subscription_; + std::atomic sequence_{0}; +}; + +} // namespace cortex_ros2_bridge + +#endif // CORTEX_ROS2_BRIDGE__BINDING_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp new file mode 100644 index 0000000..9a0ebaa --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp @@ -0,0 +1,102 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__BINDING_FACTORY_HPP_ +#define CORTEX_ROS2_BRIDGE__BINDING_FACTORY_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "cortex_ros2_bridge/binding.hpp" +#include "cortex_ros2_bridge/registry.hpp" + +namespace cortex_ros2_bridge +{ + +// Factories produce a typed binding from a string-keyed dispatch on the ROS 2 +// type name. The component looks up the factory once per YAML entry; the +// returned binding hides the templated rclcpp::Publisher / Subscription +// behind the non-templated BindingBase interface. + +using CortexToRos2BindingFactory = std::function< + std::unique_ptr( + rclcpp::Node *, zmq::context_t *, + const BridgeEntry &, const cortex_wire::TopicInfo &, + cortex_wire::MessageKind, const AdapterRegistry &, + const rclcpp::QoS &)>; + +using Ros2ToCortexBindingFactory = std::function< + std::unique_ptr( + rclcpp::Node *, zmq::context_t *, + const BridgeEntry &, cortex_wire::MessageKind, + std::uint64_t /*fingerprint*/, std::string /*pub_endpoint*/, + const AdapterRegistry &, const rclcpp::QoS &)>; + +class BindingFactoryRegistry +{ +public: + bool register_cortex_to_ros2(std::string ros2_type_name, CortexToRos2BindingFactory factory); + bool register_ros2_to_cortex(std::string ros2_type_name, Ros2ToCortexBindingFactory factory); + + // Returns an empty std::function if not registered. + CortexToRos2BindingFactory get_cortex_to_ros2(std::string_view ros2_type_name) const; + Ros2ToCortexBindingFactory get_ros2_to_cortex(std::string_view ros2_type_name) const; + + static BindingFactoryRegistry & global(); + +private: + mutable std::mutex mu_; + std::unordered_map c2r_; + std::unordered_map r2c_; +}; + +// ---- helpers --------------------------------------------------------------- + +template +inline bool register_cortex_to_ros2_binding( + BindingFactoryRegistry & reg, std::string ros2_type_name) +{ + return reg.register_cortex_to_ros2( + ros2_type_name, + [](rclcpp::Node * node, zmq::context_t * ctx, + const BridgeEntry & cfg, const cortex_wire::TopicInfo & info, + cortex_wire::MessageKind kind, const AdapterRegistry & adapters, + const rclcpp::QoS & qos) -> std::unique_ptr { + const auto ros2_type = cfg.ros2.type.value_or(std::string{}); + auto adapter = adapters.find_cortex_to_ros2(kind, ros2_type); + if (!adapter) {return nullptr;} + return std::make_unique>( + node, ctx, cfg, info, std::move(adapter), qos); + }); +} + +template +inline bool register_ros2_to_cortex_binding( + BindingFactoryRegistry & reg, std::string ros2_type_name) +{ + return reg.register_ros2_to_cortex( + ros2_type_name, + [](rclcpp::Node * node, zmq::context_t * ctx, + const BridgeEntry & cfg, cortex_wire::MessageKind kind, + std::uint64_t fingerprint, std::string pub_endpoint, + const AdapterRegistry & adapters, const rclcpp::QoS & qos) + -> std::unique_ptr { + const auto ros2_type = cfg.ros2.type.value_or(std::string{}); + auto adapter = adapters.find_ros2_to_cortex(kind, ros2_type); + if (!adapter) {return nullptr;} + return std::make_unique>( + node, ctx, cfg, std::move(adapter), + std::move(pub_endpoint), fingerprint, qos); + }); +} + +// Registers binding factories for the primitive catalogue. Pairs with +// adapters::register_primitives() — together they wire up every primitive +// in both directions. +std::size_t register_primitive_bindings(BindingFactoryRegistry & reg); + +} // namespace cortex_ros2_bridge + +#endif // CORTEX_ROS2_BRIDGE__BINDING_FACTORY_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_to_ros2_node.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_to_ros2_node.hpp new file mode 100644 index 0000000..2d3c081 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_to_ros2_node.hpp @@ -0,0 +1,49 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__CORTEX_TO_ROS2_NODE_HPP_ +#define CORTEX_ROS2_BRIDGE__CORTEX_TO_ROS2_NODE_HPP_ + +#include +#include +#include + +#include +#include + +#include "cortex_ros2_bridge/binding.hpp" +#include "cortex_ros2_bridge/binding_factory.hpp" +#include "cortex_ros2_bridge/config.hpp" +#include "cortex_ros2_bridge/registry.hpp" + +namespace cortex_ros2_bridge +{ + +// Composable rclcpp::Node that bridges every `cortex_to_ros2` entry in its +// YAML config from Cortex IPC to a ROS 2 topic. One SUB socket and one +// recv thread per entry. +// +// Parameters: +// config_path (string) — path to the bridge YAML config (required). +// +// Adapters and binding factories are looked up in the process-global +// registries. The primitive set is auto-registered on first instantiation. +class CortexToRos2Bridge : public rclcpp::Node +{ +public: + explicit CortexToRos2Bridge(const rclcpp::NodeOptions & options); + ~CortexToRos2Bridge() override; + + // Visible for tests: number of bridge entries successfully wired up. + std::size_t num_active_bindings() const noexcept; + +private: + void initialize(); + + BridgeConfig config_; + std::shared_ptr ctx_; + std::unique_ptr discovery_; + std::vector> bindings_; +}; + +} // namespace cortex_ros2_bridge + +#endif // CORTEX_ROS2_BRIDGE__CORTEX_TO_ROS2_NODE_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp new file mode 100644 index 0000000..1870eb2 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp @@ -0,0 +1,17 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__QOS_HPP_ +#define CORTEX_ROS2_BRIDGE__QOS_HPP_ + +#include + +#include "cortex_ros2_bridge/config.hpp" + +namespace cortex_ros2_bridge +{ + +// Translate a YAML-derived QosSettings into an rclcpp::QoS instance. +rclcpp::QoS make_qos(const QosSettings & q); + +} // namespace cortex_ros2_bridge + +#endif // CORTEX_ROS2_BRIDGE__QOS_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/ros2_to_cortex_node.hpp b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/ros2_to_cortex_node.hpp new file mode 100644 index 0000000..695a76e --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/ros2_to_cortex_node.hpp @@ -0,0 +1,50 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#ifndef CORTEX_ROS2_BRIDGE__ROS2_TO_CORTEX_NODE_HPP_ +#define CORTEX_ROS2_BRIDGE__ROS2_TO_CORTEX_NODE_HPP_ + +#include +#include +#include + +#include +#include + +#include "cortex_ros2_bridge/binding.hpp" +#include "cortex_ros2_bridge/binding_factory.hpp" +#include "cortex_ros2_bridge/config.hpp" +#include "cortex_ros2_bridge/registry.hpp" + +namespace cortex_ros2_bridge +{ + +// Composable rclcpp::Node that bridges every `ros2_to_cortex` entry in its +// YAML config from a ROS 2 topic to a Cortex IPC publisher. One PUB socket +// per entry; the discovery daemon is told about each on startup and +// unregistered on shutdown. +// +// Parameters: +// config_path (string) — path to the bridge YAML config (required). +class Ros2ToCortexBridge : public rclcpp::Node +{ +public: + explicit Ros2ToCortexBridge(const rclcpp::NodeOptions & options); + ~Ros2ToCortexBridge() override; + + std::size_t num_active_bindings() const noexcept; + +private: + void initialize(); + std::string make_pub_endpoint(const std::string & entry_name) const; + + BridgeConfig config_; + std::shared_ptr ctx_; + std::unique_ptr discovery_; + std::vector> bindings_; + // Topics we successfully registered with discovery, so we can unregister + // them in the destructor. + std::vector registered_topics_; +}; + +} // namespace cortex_ros2_bridge + +#endif // CORTEX_ROS2_BRIDGE__ROS2_TO_CORTEX_NODE_HPP_ diff --git a/ros2_bridge/cortex_ros2_bridge/launch/composable_container.launch.py b/ros2_bridge/cortex_ros2_bridge/launch/composable_container.launch.py new file mode 100644 index 0000000..4e5f9e4 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/launch/composable_container.launch.py @@ -0,0 +1,47 @@ +"""Launch both bridge components into a single composable-node container. + +This is the production launch file: downstream ROS 2 consumers that want +intra-process zero-copy delivery from the bridge should compose into the same +container by extending this file (add more ComposableNode entries) or by +referencing the same container name from their own launch graph. +""" +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + config = LaunchConfiguration("config") + container_name = LaunchConfiguration("container_name") + + return LaunchDescription([ + DeclareLaunchArgument("config", description="Path to bridge YAML config"), + DeclareLaunchArgument( + "container_name", default_value="cortex_bridge_container" + ), + ComposableNodeContainer( + name=container_name, + namespace="", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="cortex_ros2_bridge", + plugin="cortex_ros2_bridge::CortexToRos2Bridge", + name="cortex_to_ros2", + parameters=[{"config_path": config}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="cortex_ros2_bridge", + plugin="cortex_ros2_bridge::Ros2ToCortexBridge", + name="ros2_to_cortex", + parameters=[{"config_path": config}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + ), + ]) diff --git a/ros2_bridge/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py b/ros2_bridge/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py new file mode 100644 index 0000000..08aa3c1 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py @@ -0,0 +1,34 @@ +"""Launch a single CortexToRos2Bridge composable node in its own container.""" +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + config = LaunchConfiguration("config") + container_name = LaunchConfiguration("container_name") + + return LaunchDescription([ + DeclareLaunchArgument("config", description="Path to bridge YAML config"), + DeclareLaunchArgument( + "container_name", default_value="cortex_to_ros2_container" + ), + ComposableNodeContainer( + name=container_name, + namespace="", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="cortex_ros2_bridge", + plugin="cortex_ros2_bridge::CortexToRos2Bridge", + name="cortex_to_ros2", + parameters=[{"config_path": config}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + ), + ]) diff --git a/ros2_bridge/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py b/ros2_bridge/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py new file mode 100644 index 0000000..fe92f0e --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py @@ -0,0 +1,34 @@ +"""Launch a single Ros2ToCortexBridge composable node in its own container.""" +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + config = LaunchConfiguration("config") + container_name = LaunchConfiguration("container_name") + + return LaunchDescription([ + DeclareLaunchArgument("config", description="Path to bridge YAML config"), + DeclareLaunchArgument( + "container_name", default_value="ros2_to_cortex_container" + ), + ComposableNodeContainer( + name=container_name, + namespace="", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="cortex_ros2_bridge", + plugin="cortex_ros2_bridge::Ros2ToCortexBridge", + name="ros2_to_cortex", + parameters=[{"config_path": config}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + ), + ]) diff --git a/ros2_bridge/cortex_ros2_bridge/package.xml b/ros2_bridge/cortex_ros2_bridge/package.xml index 9279bda..9bcdc7c 100644 --- a/ros2_bridge/cortex_ros2_bridge/package.xml +++ b/ros2_bridge/cortex_ros2_bridge/package.xml @@ -13,6 +13,8 @@ ament_cmake yaml-cpp + rclcpp + rclcpp_components std_msgs builtin_interfaces diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/arrays.cpp b/ros2_bridge/cortex_ros2_bridge/src/adapters/arrays.cpp new file mode 100644 index 0000000..ddd8a22 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/adapters/arrays.cpp @@ -0,0 +1,180 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/adapters/arrays.hpp" +#include "cortex_ros2_bridge/adapters/oob_helpers.hpp" + +#include + +#include +#include +#include +#include + +namespace cortex_ros2_bridge::adapters +{ + +namespace +{ + +// Common decode path for ArrayMessage. Returns (data_bytes_view, shape, name, +// frame_id) after validating field shape; the caller plugs the bytes into +// the typed MultiArray. +struct DecodedArray +{ + OobByteView data; + std::vector shape; + std::string name; + std::string frame_id; +}; + +DecodedArray decode_array_metadata( + const CortexInbound & in, std::string_view adapter, std::string_view expected_dtype) +{ + require_field_count(in.metadata, 3, adapter); + auto bytes = oob_bytes(in.metadata.field(0), in.oob_frames, adapter); + if (bytes.descriptor.dtype != expected_dtype) { + throw cortex_wire::WireDecodeError( + std::string(adapter) + ": dtype mismatch (got '" + bytes.descriptor.dtype + + "', expected '" + std::string(expected_dtype) + "')"); + } + DecodedArray d; + d.data = bytes; + d.shape = std::move(bytes.descriptor.shape); + d.name = read_str_field(in.metadata, 1, adapter); + d.frame_id = read_str_field(in.metadata, 2, adapter); + return d; +} + +// Populate a std_msgs/MultiArrayLayout from a shape vector. Strides count +// elements, not bytes (per std_msgs convention). +void fill_layout( + std_msgs::msg::MultiArrayLayout & layout, const std::vector & shape) +{ + layout.dim.clear(); + layout.dim.reserve(shape.size()); + std::size_t total = 1; + for (auto d : shape) { + total *= static_cast(d); + } + std::size_t stride = total; + for (std::size_t i = 0; i < shape.size(); ++i) { + std_msgs::msg::MultiArrayDimension dim; + dim.label.clear(); + dim.size = static_cast(shape[i]); + dim.stride = static_cast(stride); + layout.dim.push_back(std::move(dim)); + if (dim.size > 0) {stride /= static_cast(shape[i]);} + } + layout.data_offset = 0; +} + +// Recover the logical shape from a MultiArray layout. Returns the shape; +// empty if layout has no dimensions (which is legal for a 1-D array — the +// caller falls back to data.size() in that case). +std::vector layout_to_shape(const std_msgs::msg::MultiArrayLayout & layout) +{ + std::vector shape; + shape.reserve(layout.dim.size()); + for (const auto & d : layout.dim) { + shape.push_back(static_cast(d.size)); + } + return shape; +} + +template +void copy_oob_to_data(const OobByteView & oob, std::vector & out) +{ + const std::size_t n = oob.size / sizeof(Elem); + out.resize(n); + std::memcpy(out.data(), oob.data, n * sizeof(Elem)); +} + +template +CortexOutbound pack_array_outbound( + std::string_view dtype, + const std::vector & data, + const std::vector & shape, + std::string_view name, std::string_view frame_id) +{ + cortex_wire::MetadataBuilder b(3); + b.pack_numpy_oob(dtype, shape, data.data(), data.size() * sizeof(Elem)); + b.pack_str(name); + b.pack_str(frame_id); + auto frames = std::move(b).finish(); + return CortexOutbound{std::move(frames.metadata), std::move(frames.oob_buffers)}; +} + +} // namespace + +// ---- ArrayFloat32Adapter ------------------------------------------------- + +cortex_wire::MessageKind ArrayFloat32Adapter::cortex_kind() const +{ + return cortex_wire::MessageKind::ArrayMessage; +} +std::string_view ArrayFloat32Adapter::ros2_type_name() const +{ + return "std_msgs/msg/Float32MultiArray"; +} + +std::unique_ptr ArrayFloat32Adapter::to_ros2( + const CortexInbound & in) const +{ + auto d = decode_array_metadata(in, "ArrayFloat32Adapter", "(); + fill_layout(out->layout, d.shape); + copy_oob_to_data(d.data, out->data); + return out; +} + +CortexOutbound ArrayFloat32Adapter::to_cortex( + const std_msgs::msg::Float32MultiArray & msg, std::uint64_t /*sequence*/, + const BridgeEntry & /*cfg*/) const +{ + auto shape = layout_to_shape(msg.layout); + if (shape.empty()) {shape.push_back(static_cast(msg.data.size()));} + return pack_array_outbound(" ArrayFloat64Adapter::to_ros2( + const CortexInbound & in) const +{ + auto d = decode_array_metadata(in, "ArrayFloat64Adapter", "(); + fill_layout(out->layout, d.shape); + copy_oob_to_data(d.data, out->data); + return out; +} + +CortexOutbound ArrayFloat64Adapter::to_cortex( + const std_msgs::msg::Float64MultiArray & msg, std::uint64_t, + const BridgeEntry &) const +{ + auto shape = layout_to_shape(msg.layout); + if (shape.empty()) {shape.push_back(static_cast(msg.data.size()));} + return pack_array_outbound("( + std::make_shared()) ? 2 : 0; + added += reg.register_bidirectional( + std::make_shared()) ? 2 : 0; + return added; +} + +} // namespace cortex_ros2_bridge::adapters diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/image.cpp b/ros2_bridge/cortex_ros2_bridge/src/adapters/image.cpp new file mode 100644 index 0000000..b2ec1f0 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/adapters/image.cpp @@ -0,0 +1,117 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/adapters/image.hpp" +#include "cortex_ros2_bridge/adapters/oob_helpers.hpp" + +#include + +#include +#include +#include +#include + +namespace cortex_ros2_bridge::adapters +{ + +namespace +{ + +// Channels per pixel for the encodings Cortex's ImageMessage commonly uses. +// Returns 0 for unknown encodings — the adapter then falls back to inferring +// from shape (H×W×C) and trusts the wire. +std::uint32_t channels_for_encoding(std::string_view enc) +{ + if (enc == "mono8" || enc == "mono16" || enc == "8UC1" || enc == "16UC1") {return 1;} + if (enc == "rgb8" || enc == "bgr8" || enc == "8UC3" || enc == "16UC3") {return 3;} + if (enc == "rgba8" || enc == "bgra8" || enc == "8UC4" || enc == "16UC4") {return 4;} + return 0; +} + +std::uint32_t bytes_per_pixel(std::string_view enc, std::uint32_t channels) +{ + if (enc == "mono16" || enc == "16UC1" || enc == "16UC3" || enc == "16UC4") { + return channels * 2; + } + return channels; +} + +} // namespace + +cortex_wire::MessageKind ImageAdapter::cortex_kind() const +{ + return cortex_wire::MessageKind::ImageMessage; +} +std::string_view ImageAdapter::ros2_type_name() const {return "sensor_msgs/msg/Image";} + +std::unique_ptr ImageAdapter::to_ros2( + const CortexInbound & in) const +{ + require_field_count(in.metadata, 4, "ImageAdapter"); + + auto oob = oob_bytes(in.metadata.field(0), in.oob_frames, "ImageAdapter"); + const auto encoding = read_str_field(in.metadata, 1, "ImageAdapter"); + const auto width = static_cast( + read_int_field(in.metadata, 2, "ImageAdapter")); + const auto height = static_cast( + read_int_field(in.metadata, 3, "ImageAdapter")); + + auto out = std::make_unique(); + out->header.stamp.sec = static_cast(in.header.timestamp_ns / 1'000'000'000ULL); + out->header.stamp.nanosec = static_cast( + in.header.timestamp_ns % 1'000'000'000ULL); + out->header.frame_id = in.cfg.ros2.frame_id.value_or(""); + out->encoding = encoding; + out->is_bigendian = 0; + out->width = width; + out->height = height; + + std::uint32_t channels = channels_for_encoding(encoding); + if (channels == 0 && oob.descriptor.shape.size() >= 3) { + channels = static_cast(oob.descriptor.shape[2]); + } + out->step = width * bytes_per_pixel(encoding, channels); + + // One memcpy from the OOB frame into the freshly-allocated Image::data. + out->data.resize(oob.size); + std::memcpy(out->data.data(), oob.data, oob.size); + return out; +} + +CortexOutbound ImageAdapter::to_cortex( + const sensor_msgs::msg::Image & msg, std::uint64_t /*sequence*/, + const BridgeEntry & /*cfg*/) const +{ + // Reconstruct an H×W or H×W×C shape that round-trips to the same + // ImageMessage layout when decoded by Python cortex. We use the channel + // hint from `step / width` (bytes per row / image width = bytes/pixel, + // then / dtype_size = channels). 8-bit data assumed unless the encoding + // string says otherwise. + cortex_wire::MetadataBuilder b(4); + + const std::uint32_t bytes_per_row = msg.width > 0 ? msg.step : msg.width; + const std::uint32_t bpp = msg.width > 0 ? bytes_per_row / msg.width : 0; + const std::string dtype = + (msg.encoding == "mono16" || msg.encoding.find("16U") != std::string::npos) ? + " 0 ? bpp / bytes_per_elem : 1; + + std::vector shape; + shape.push_back(msg.height); + shape.push_back(msg.width); + if (channels > 1) {shape.push_back(channels);} + + b.pack_numpy_oob(dtype, shape, msg.data.data(), msg.data.size()); + b.pack_str(msg.encoding); + b.pack_uint(msg.width); + b.pack_uint(msg.height); + auto frames = std::move(b).finish(); + return CortexOutbound{std::move(frames.metadata), std::move(frames.oob_buffers)}; +} + +std::size_t register_image_adapters(AdapterRegistry & reg) +{ + return reg.register_bidirectional( + std::make_shared()) ? 2 : 0; +} + +} // namespace cortex_ros2_bridge::adapters diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/pointcloud.cpp b/ros2_bridge/cortex_ros2_bridge/src/adapters/pointcloud.cpp new file mode 100644 index 0000000..6a696b7 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/adapters/pointcloud.cpp @@ -0,0 +1,269 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/adapters/pointcloud.hpp" +#include "cortex_ros2_bridge/adapters/oob_helpers.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace cortex_ros2_bridge::adapters +{ + +namespace +{ + +// Reads an optional OOB field at metadata index `i`. Returns nullopt if the +// field is msgpack NIL; throws otherwise via oob_bytes() if malformed. +std::optional optional_oob( + const CortexInbound & in, std::size_t i, std::string_view adapter) +{ + if (is_nil_field(in.metadata, i)) {return std::nullopt;} + return oob_bytes(in.metadata.field(i), in.oob_frames, adapter); +} + +// One PointCloud2 PointField record. +sensor_msgs::msg::PointField make_field( + const std::string & name, std::uint32_t offset, std::uint8_t datatype, + std::uint32_t count) +{ + sensor_msgs::msg::PointField f; + f.name = name; + f.offset = offset; + f.datatype = datatype; + f.count = count; + return f; +} + +} // namespace + +cortex_wire::MessageKind PointCloudAdapter::cortex_kind() const +{ + return cortex_wire::MessageKind::PointCloudMessage; +} +std::string_view PointCloudAdapter::ros2_type_name() const +{ + return "sensor_msgs/msg/PointCloud2"; +} + +std::unique_ptr PointCloudAdapter::to_ros2( + const CortexInbound & in) const +{ + require_field_count(in.metadata, 5, "PointCloudAdapter"); + + auto points = oob_bytes(in.metadata.field(0), in.oob_frames, "PointCloudAdapter"); + if (points.descriptor.dtype != "(points.descriptor.shape[0]); + + auto colors_opt = optional_oob(in, 1, "PointCloudAdapter"); + auto intensity_opt = optional_oob(in, 2, "PointCloudAdapter"); + auto normals_opt = optional_oob(in, 3, "PointCloudAdapter"); + const auto frame_id = read_str_field(in.metadata, 4, "PointCloudAdapter"); + + auto out = std::make_unique(); + out->header.stamp.sec = static_cast(in.header.timestamp_ns / 1'000'000'000ULL); + out->header.stamp.nanosec = static_cast( + in.header.timestamp_ns % 1'000'000'000ULL); + out->header.frame_id = in.cfg.ros2.frame_id.value_or(frame_id); + out->height = 1; + out->width = num_points; + out->is_bigendian = false; + out->is_dense = true; + + // Build the field layout. Each point's record is concatenated in this order. + std::uint32_t offset = 0; + out->fields.push_back(make_field("x", offset, sensor_msgs::msg::PointField::FLOAT32, 1)); + offset += 4; + out->fields.push_back(make_field("y", offset, sensor_msgs::msg::PointField::FLOAT32, 1)); + offset += 4; + out->fields.push_back(make_field("z", offset, sensor_msgs::msg::PointField::FLOAT32, 1)); + offset += 4; + + if (colors_opt) { + // RGB packed into a 4-byte field, padded — matches the PCL "rgb" convention. + out->fields.push_back(make_field("rgb", offset, sensor_msgs::msg::PointField::UINT32, 1)); + offset += 4; + } + if (intensity_opt) { + out->fields.push_back( + make_field("intensity", offset, sensor_msgs::msg::PointField::FLOAT32, 1)); + offset += 4; + } + if (normals_opt) { + out->fields.push_back( + make_field("normal_x", offset, sensor_msgs::msg::PointField::FLOAT32, 1)); + offset += 4; + out->fields.push_back( + make_field("normal_y", offset, sensor_msgs::msg::PointField::FLOAT32, 1)); + offset += 4; + out->fields.push_back( + make_field("normal_z", offset, sensor_msgs::msg::PointField::FLOAT32, 1)); + offset += 4; + } + out->point_step = offset; + out->row_step = out->point_step * out->width; + + out->data.resize(static_cast(out->row_step)); + + // Interleave the per-point records. We trust the OOB dtype/shape on + // colors/intensity/normals; mismatches throw. + if (colors_opt) { + if (colors_opt->descriptor.dtype != "descriptor.shape.size() != 2 || + colors_opt->descriptor.shape[0] != num_points || + colors_opt->descriptor.shape[1] != 3) + { + throw cortex_wire::WireDecodeError( + "PointCloudAdapter: 'colors' must be N×3 uint8"); + } + } + if (intensity_opt) { + if (intensity_opt->descriptor.dtype != "descriptor.shape) != num_points) + { + throw cortex_wire::WireDecodeError( + "PointCloudAdapter: 'intensity' must be float32 with N elements"); + } + } + if (normals_opt) { + if (normals_opt->descriptor.dtype != "descriptor.shape.size() != 2 || + normals_opt->descriptor.shape[0] != num_points || + normals_opt->descriptor.shape[1] != 3) + { + throw cortex_wire::WireDecodeError( + "PointCloudAdapter: 'normals' must be N×3 float32"); + } + } + + const auto * pts = points.data; + const auto * cols = colors_opt ? colors_opt->data : nullptr; + const auto * inten = intensity_opt ? intensity_opt->data : nullptr; + const auto * nrm = normals_opt ? normals_opt->data : nullptr; + for (std::uint32_t i = 0; i < num_points; ++i) { + std::uint8_t * dst = out->data.data() + i * out->point_step; + std::memcpy(dst, pts + i * 12, 12); // x,y,z + std::uint32_t o = 12; + if (cols) { + // rgb packed: r<<16 | g<<8 | b, stored as little-endian uint32. + const auto r = cols[i * 3 + 0]; + const auto g = cols[i * 3 + 1]; + const auto b = cols[i * 3 + 2]; + const std::uint32_t packed = + (static_cast(r) << 16) | + (static_cast(g) << 8) | + static_cast(b); + std::memcpy(dst + o, &packed, 4); + o += 4; + } + if (inten) { + std::memcpy(dst + o, inten + i * 4, 4); + o += 4; + } + if (nrm) { + std::memcpy(dst + o, nrm + i * 12, 12); + o += 12; + } + } + return out; +} + +CortexOutbound PointCloudAdapter::to_cortex( + const sensor_msgs::msg::PointCloud2 & msg, std::uint64_t /*sequence*/, + const BridgeEntry & cfg) const +{ + const auto num_points = static_cast(msg.width * msg.height); + + // Walk fields to find which channels are present and their offsets. + std::optional off_x, off_y, off_z, off_rgb, off_intensity; + std::optional off_nx, off_ny, off_nz; + for (const auto & f : msg.fields) { + if (f.name == "x") {off_x = f.offset;} + else if (f.name == "y") {off_y = f.offset;} + else if (f.name == "z") {off_z = f.offset;} + else if (f.name == "rgb" || f.name == "rgba") {off_rgb = f.offset;} + else if (f.name == "intensity") {off_intensity = f.offset;} + else if (f.name == "normal_x") {off_nx = f.offset;} + else if (f.name == "normal_y") {off_ny = f.offset;} + else if (f.name == "normal_z") {off_nz = f.offset;} + } + if (!off_x || !off_y || !off_z) { + throw cortex_wire::WireDecodeError( + "PointCloudAdapter::to_cortex: PointCloud2 missing x/y/z fields"); + } + + const bool has_colors = off_rgb.has_value(); + const bool has_intensity = off_intensity.has_value(); + const bool has_normals = off_nx && off_ny && off_nz; + + std::vector points(num_points * 3); + std::vector colors(has_colors ? num_points * 3 : 0); + std::vector intensity(has_intensity ? num_points : 0); + std::vector normals(has_normals ? num_points * 3 : 0); + + for (std::uint32_t i = 0; i < num_points; ++i) { + const std::uint8_t * src = msg.data.data() + i * msg.point_step; + std::memcpy(&points[i * 3 + 0], src + *off_x, 4); + std::memcpy(&points[i * 3 + 1], src + *off_y, 4); + std::memcpy(&points[i * 3 + 2], src + *off_z, 4); + if (has_colors) { + std::uint32_t packed; + std::memcpy(&packed, src + *off_rgb, 4); + colors[i * 3 + 0] = static_cast((packed >> 16) & 0xff); + colors[i * 3 + 1] = static_cast((packed >> 8) & 0xff); + colors[i * 3 + 2] = static_cast(packed & 0xff); + } + if (has_intensity) { + std::memcpy(&intensity[i], src + *off_intensity, 4); + } + if (has_normals) { + std::memcpy(&normals[i * 3 + 0], src + *off_nx, 4); + std::memcpy(&normals[i * 3 + 1], src + *off_ny, 4); + std::memcpy(&normals[i * 3 + 2], src + *off_nz, 4); + } + } + + cortex_wire::MetadataBuilder b(5); + b.pack_numpy_oob("( + std::make_shared()) ? 2 : 0; +} + +} // namespace cortex_ros2_bridge::adapters diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/pose.cpp b/ros2_bridge/cortex_ros2_bridge/src/adapters/pose.cpp new file mode 100644 index 0000000..8c2f750 --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/adapters/pose.cpp @@ -0,0 +1,120 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/adapters/pose.hpp" +#include "cortex_ros2_bridge/adapters/oob_helpers.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace cortex_ros2_bridge::adapters +{ + +namespace +{ + +// Read a fixed-length float vector from an OOB descriptor. Accepts both +// read_fixed_floats( + const msgpack::object & field, + const std::vector & frames, + std::size_t expected_len, std::string_view adapter) +{ + auto oob = oob_bytes(field, frames, adapter); + if (product(oob.descriptor.shape) != expected_len) { + throw cortex_wire::WireDecodeError( + std::string(adapter) + ": expected " + std::to_string(expected_len) + + " elements, got " + std::to_string(product(oob.descriptor.shape))); + } + std::array out{}; + if (oob.descriptor.dtype == "(v); + } + } else { + throw cortex_wire::WireDecodeError( + std::string(adapter) + ": expected float dtype, got '" + + oob.descriptor.dtype + "'"); + } + return out; +} + +} // namespace + +cortex_wire::MessageKind PoseAdapter::cortex_kind() const +{ + return cortex_wire::MessageKind::PoseMessage; +} +std::string_view PoseAdapter::ros2_type_name() const +{ + return "geometry_msgs/msg/PoseStamped"; +} + +std::unique_ptr PoseAdapter::to_ros2( + const CortexInbound & in) const +{ + require_field_count(in.metadata, 4, "PoseAdapter"); + const auto pos = read_fixed_floats(in.metadata.field(0), in.oob_frames, 3, "PoseAdapter"); + const auto orient = read_fixed_floats(in.metadata.field(1), in.oob_frames, 4, "PoseAdapter"); + const auto frame_id = read_str_field(in.metadata, 2, "PoseAdapter"); + // field(3) is child_frame_id — read but discard (no equivalent in PoseStamped). + (void)read_str_field(in.metadata, 3, "PoseAdapter"); + + auto out = std::make_unique(); + out->header.stamp.sec = static_cast(in.header.timestamp_ns / 1'000'000'000ULL); + out->header.stamp.nanosec = static_cast( + in.header.timestamp_ns % 1'000'000'000ULL); + out->header.frame_id = in.cfg.ros2.frame_id.value_or(frame_id); + out->pose.position.x = pos[0]; + out->pose.position.y = pos[1]; + out->pose.position.z = pos[2]; + out->pose.orientation.x = orient[0]; + out->pose.orientation.y = orient[1]; + out->pose.orientation.z = orient[2]; + out->pose.orientation.w = orient[3]; + return out; +} + +CortexOutbound PoseAdapter::to_cortex( + const geometry_msgs::msg::PoseStamped & msg, std::uint64_t /*sequence*/, + const BridgeEntry & /*cfg*/) const +{ + std::array position{ + msg.pose.position.x, msg.pose.position.y, msg.pose.position.z}; + std::array orientation{ + msg.pose.orientation.x, msg.pose.orientation.y, + msg.pose.orientation.z, msg.pose.orientation.w}; + + cortex_wire::MetadataBuilder b(4); + b.pack_numpy_oob("( + std::make_shared()) ? 2 : 0; +} + +} // namespace cortex_ros2_bridge::adapters diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/tensor.cpp b/ros2_bridge/cortex_ros2_bridge/src/adapters/tensor.cpp new file mode 100644 index 0000000..5dacabb --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/adapters/tensor.cpp @@ -0,0 +1,72 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/adapters/tensor.hpp" +#include "cortex_ros2_bridge/adapters/oob_helpers.hpp" + +#include + +#include +#include +#include + +namespace cortex_ros2_bridge::adapters +{ + +cortex_wire::MessageKind TensorFloat32Adapter::cortex_kind() const +{ + return cortex_wire::MessageKind::TensorMessage; +} +std::string_view TensorFloat32Adapter::ros2_type_name() const +{ + return "std_msgs/msg/Float32MultiArray"; +} + +std::unique_ptr TensorFloat32Adapter::to_ros2( + const CortexInbound & in) const +{ + require_field_count(in.metadata, 2, "TensorFloat32Adapter"); + auto oob = oob_bytes(in.metadata.field(0), in.oob_frames, "TensorFloat32Adapter"); + if (oob.descriptor.dtype != "(); + out->layout.dim.clear(); + for (auto d : oob.descriptor.shape) { + std_msgs::msg::MultiArrayDimension dim; + dim.size = static_cast(d); + out->layout.dim.push_back(std::move(dim)); + } + const std::size_t n = oob.size / sizeof(float); + out->data.resize(n); + std::memcpy(out->data.data(), oob.data, n * sizeof(float)); + return out; +} + +CortexOutbound TensorFloat32Adapter::to_cortex( + const std_msgs::msg::Float32MultiArray & msg, std::uint64_t /*sequence*/, + const BridgeEntry & /*cfg*/) const +{ + std::vector shape; + shape.reserve(msg.layout.dim.size()); + for (const auto & d : msg.layout.dim) { + shape.push_back(static_cast(d.size)); + } + if (shape.empty()) {shape.push_back(static_cast(msg.data.size()));} + + cortex_wire::MetadataBuilder b(2); + b.pack_torch_oob( + "( + std::make_shared()) ? 2 : 0; +} + +} // namespace cortex_ros2_bridge::adapters diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/transform.cpp b/ros2_bridge/cortex_ros2_bridge/src/adapters/transform.cpp new file mode 100644 index 0000000..0edb23f --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/src/adapters/transform.cpp @@ -0,0 +1,184 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +#include "cortex_ros2_bridge/adapters/transform.hpp" +#include "cortex_ros2_bridge/adapters/oob_helpers.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace cortex_ros2_bridge::adapters +{ + +namespace +{ + +// Read a 4x4 matrix from an OOB descriptor. Accepts read_matrix_4x4( + const msgpack::object & field, + const std::vector & frames, + std::string_view adapter) +{ + auto oob = oob_bytes(field, frames, adapter); + if (oob.descriptor.shape.size() != 2 || + oob.descriptor.shape[0] != 4 || oob.descriptor.shape[1] != 4) + { + throw cortex_wire::WireDecodeError( + std::string(adapter) + ": expected 4×4 matrix"); + } + std::array out{}; + if (oob.descriptor.dtype == "(v); + } + } else { + throw cortex_wire::WireDecodeError( + std::string(adapter) + ": matrix dtype must be rotation_to_quaternion(const std::array & R) +{ + const double trace = R[0] + R[4] + R[8]; + double qx, qy, qz, qw; + if (trace > 0.0) { + const double s = std::sqrt(trace + 1.0) * 2.0; + qw = 0.25 * s; + qx = (R[7] - R[5]) / s; + qy = (R[2] - R[6]) / s; + qz = (R[3] - R[1]) / s; + } else if (R[0] > R[4] && R[0] > R[8]) { + const double s = std::sqrt(1.0 + R[0] - R[4] - R[8]) * 2.0; + qw = (R[7] - R[5]) / s; + qx = 0.25 * s; + qy = (R[1] + R[3]) / s; + qz = (R[2] + R[6]) / s; + } else if (R[4] > R[8]) { + const double s = std::sqrt(1.0 + R[4] - R[0] - R[8]) * 2.0; + qw = (R[2] - R[6]) / s; + qx = (R[1] + R[3]) / s; + qy = 0.25 * s; + qz = (R[5] + R[7]) / s; + } else { + const double s = std::sqrt(1.0 + R[8] - R[0] - R[4]) * 2.0; + qw = (R[3] - R[1]) / s; + qx = (R[2] + R[6]) / s; + qy = (R[5] + R[7]) / s; + qz = 0.25 * s; + } + return {qx, qy, qz, qw}; +} + +// Quaternion → 3x3 row-major rotation matrix. +std::array quaternion_to_rotation(double qx, double qy, double qz, double qw) +{ + std::array R; + R[0] = 1.0 - 2.0 * (qy * qy + qz * qz); + R[1] = 2.0 * (qx * qy - qz * qw); + R[2] = 2.0 * (qx * qz + qy * qw); + R[3] = 2.0 * (qx * qy + qz * qw); + R[4] = 1.0 - 2.0 * (qx * qx + qz * qz); + R[5] = 2.0 * (qy * qz - qx * qw); + R[6] = 2.0 * (qx * qz - qy * qw); + R[7] = 2.0 * (qy * qz + qx * qw); + R[8] = 1.0 - 2.0 * (qx * qx + qy * qy); + return R; +} + +} // namespace + +cortex_wire::MessageKind TransformAdapter::cortex_kind() const +{ + return cortex_wire::MessageKind::TransformMessage; +} +std::string_view TransformAdapter::ros2_type_name() const +{ + return "geometry_msgs/msg/TransformStamped"; +} + +std::unique_ptr TransformAdapter::to_ros2( + const CortexInbound & in) const +{ + require_field_count(in.metadata, 3, "TransformAdapter"); + const auto matrix = read_matrix_4x4(in.metadata.field(0), in.oob_frames, "TransformAdapter"); + const auto frame_id = read_str_field(in.metadata, 1, "TransformAdapter"); + const auto child_frame_id = read_str_field(in.metadata, 2, "TransformAdapter"); + + auto out = std::make_unique(); + out->header.stamp.sec = static_cast(in.header.timestamp_ns / 1'000'000'000ULL); + out->header.stamp.nanosec = static_cast( + in.header.timestamp_ns % 1'000'000'000ULL); + out->header.frame_id = in.cfg.ros2.frame_id.value_or(frame_id); + out->child_frame_id = child_frame_id; + // matrix is row-major 4x4; index [r*4 + c]. + out->transform.translation.x = matrix[0 * 4 + 3]; + out->transform.translation.y = matrix[1 * 4 + 3]; + out->transform.translation.z = matrix[2 * 4 + 3]; + + std::array R{ + matrix[0], matrix[1], matrix[2], + matrix[4], matrix[5], matrix[6], + matrix[8], matrix[9], matrix[10]}; + const auto q = rotation_to_quaternion(R); + out->transform.rotation.x = q[0]; + out->transform.rotation.y = q[1]; + out->transform.rotation.z = q[2]; + out->transform.rotation.w = q[3]; + return out; +} + +CortexOutbound TransformAdapter::to_cortex( + const geometry_msgs::msg::TransformStamped & msg, std::uint64_t /*sequence*/, + const BridgeEntry & /*cfg*/) const +{ + const auto R = quaternion_to_rotation( + msg.transform.rotation.x, msg.transform.rotation.y, + msg.transform.rotation.z, msg.transform.rotation.w); + + std::array matrix{}; + matrix[0] = R[0]; matrix[1] = R[1]; matrix[2] = R[2]; + matrix[4] = R[3]; matrix[5] = R[4]; matrix[6] = R[5]; + matrix[8] = R[6]; matrix[9] = R[7]; matrix[10] = R[8]; + matrix[3] = msg.transform.translation.x; + matrix[7] = msg.transform.translation.y; + matrix[11] = msg.transform.translation.z; + matrix[15] = 1.0; + + cortex_wire::MetadataBuilder b(3); + b.pack_numpy_oob("( + std::make_shared()) ? 2 : 0; +} + +} // namespace cortex_ros2_bridge::adapters diff --git a/ros2_bridge/cortex_ros2_bridge/src/binding_factory.cpp b/ros2_bridge/cortex_ros2_bridge/src/binding_factory.cpp index 95ac494..b856cf5 100644 --- a/ros2_bridge/cortex_ros2_bridge/src/binding_factory.cpp +++ b/ros2_bridge/cortex_ros2_bridge/src/binding_factory.cpp @@ -2,8 +2,14 @@ #include "cortex_ros2_bridge/binding_factory.hpp" #include +#include +#include +#include +#include #include +#include #include +#include #include #include #include @@ -82,4 +88,35 @@ std::size_t register_primitive_bindings(BindingFactoryRegistry & reg) return added; } +std::size_t register_standard_bindings(BindingFactoryRegistry & reg) +{ + std::size_t added = 0; + added += register_cortex_to_ros2_binding( + reg, "std_msgs/msg/Float32MultiArray") ? 1 : 0; + added += register_cortex_to_ros2_binding( + reg, "std_msgs/msg/Float64MultiArray") ? 1 : 0; + added += register_cortex_to_ros2_binding( + reg, "sensor_msgs/msg/Image") ? 1 : 0; + added += register_cortex_to_ros2_binding( + reg, "sensor_msgs/msg/PointCloud2") ? 1 : 0; + added += register_cortex_to_ros2_binding( + reg, "geometry_msgs/msg/PoseStamped") ? 1 : 0; + added += register_cortex_to_ros2_binding( + reg, "geometry_msgs/msg/TransformStamped") ? 1 : 0; + + added += register_ros2_to_cortex_binding( + reg, "std_msgs/msg/Float32MultiArray") ? 1 : 0; + added += register_ros2_to_cortex_binding( + reg, "std_msgs/msg/Float64MultiArray") ? 1 : 0; + added += register_ros2_to_cortex_binding( + reg, "sensor_msgs/msg/Image") ? 1 : 0; + added += register_ros2_to_cortex_binding( + reg, "sensor_msgs/msg/PointCloud2") ? 1 : 0; + added += register_ros2_to_cortex_binding( + reg, "geometry_msgs/msg/PoseStamped") ? 1 : 0; + added += register_ros2_to_cortex_binding( + reg, "geometry_msgs/msg/TransformStamped") ? 1 : 0; + return added; +} + } // namespace cortex_ros2_bridge diff --git a/ros2_bridge/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp b/ros2_bridge/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp index 6976884..4df9c85 100644 --- a/ros2_bridge/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp +++ b/ros2_bridge/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp @@ -7,7 +7,13 @@ #include #include +#include "cortex_ros2_bridge/adapters/arrays.hpp" +#include "cortex_ros2_bridge/adapters/image.hpp" +#include "cortex_ros2_bridge/adapters/pointcloud.hpp" +#include "cortex_ros2_bridge/adapters/pose.hpp" #include "cortex_ros2_bridge/adapters/primitives.hpp" +#include "cortex_ros2_bridge/adapters/tensor.hpp" +#include "cortex_ros2_bridge/adapters/transform.hpp" #include "cortex_ros2_bridge/qos.hpp" namespace cortex_ros2_bridge @@ -21,8 +27,17 @@ namespace void ensure_primitives_registered() { static bool done = []() { - adapters::register_primitives(AdapterRegistry::global()); - register_primitive_bindings(BindingFactoryRegistry::global()); + auto & ar = AdapterRegistry::global(); + auto & br = BindingFactoryRegistry::global(); + adapters::register_primitives(ar); + adapters::register_array_adapters(ar); + adapters::register_image_adapters(ar); + adapters::register_pointcloud_adapters(ar); + adapters::register_pose_adapters(ar); + adapters::register_transform_adapters(ar); + adapters::register_tensor_adapters(ar); + register_primitive_bindings(br); + register_standard_bindings(br); return true; }(); (void)done; diff --git a/ros2_bridge/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp b/ros2_bridge/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp index 6f08e29..c30710d 100644 --- a/ros2_bridge/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp +++ b/ros2_bridge/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp @@ -11,7 +11,13 @@ #include #include +#include "cortex_ros2_bridge/adapters/arrays.hpp" +#include "cortex_ros2_bridge/adapters/image.hpp" +#include "cortex_ros2_bridge/adapters/pointcloud.hpp" +#include "cortex_ros2_bridge/adapters/pose.hpp" #include "cortex_ros2_bridge/adapters/primitives.hpp" +#include "cortex_ros2_bridge/adapters/tensor.hpp" +#include "cortex_ros2_bridge/adapters/transform.hpp" #include "cortex_ros2_bridge/qos.hpp" namespace cortex_ros2_bridge @@ -23,8 +29,17 @@ namespace void ensure_primitives_registered() { static bool done = []() { - adapters::register_primitives(AdapterRegistry::global()); - register_primitive_bindings(BindingFactoryRegistry::global()); + auto & ar = AdapterRegistry::global(); + auto & br = BindingFactoryRegistry::global(); + adapters::register_primitives(ar); + adapters::register_array_adapters(ar); + adapters::register_image_adapters(ar); + adapters::register_pointcloud_adapters(ar); + adapters::register_pose_adapters(ar); + adapters::register_transform_adapters(ar); + adapters::register_tensor_adapters(ar); + register_primitive_bindings(br); + register_standard_bindings(br); return true; }(); (void)done; diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_intra_process.cpp b/ros2_bridge/cortex_ros2_bridge/test/test_intra_process.cpp new file mode 100644 index 0000000..d9dcfac --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/test/test_intra_process.cpp @@ -0,0 +1,373 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +// +// PR8 — intra-process composability test. Verifies that: +// 1. CortexToRos2Bridge composed into a shared executor with +// use_intra_process_comms=True delivers messages to a colocated +// consumer subscription using rclcpp's intra-process path. +// 2. The consumer callback can take a std::unique_ptr, which +// rclcpp's intra-process manager only delivers when the publisher +// published via unique_ptr (our binding does) and exactly one +// compatible subscription is wired up. unique_ptr delivery is the +// signal that the message is *moved* across the boundary rather +// than serialised + deserialised. +// +// We don't assert raw pointer equality between the bridge's emitted +// unique_ptr and the consumer's received unique_ptr because rclcpp doesn't +// expose the publisher-side pointer through any public API. The +// delivery-via-unique_ptr signal is the conventional intra-process check. +#include "cortex_ros2_bridge/cortex_to_ros2_node.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; +using cortex_wire::DiscoveryCommand; +using cortex_wire::DiscoveryStatus; + +namespace +{ + +// ---- mock daemon (reused shape from test_components_e2e) ----------------- + +class MockDiscoveryDaemon +{ +public: + MockDiscoveryDaemon(zmq::context_t & ctx, std::string endpoint) + : ctx_(ctx), endpoint_(std::move(endpoint)) + { + socket_ = zmq::socket_t(ctx_, zmq::socket_type::rep); + socket_.set(zmq::sockopt::linger, 0); + socket_.set(zmq::sockopt::rcvtimeo, 100); + socket_.set(zmq::sockopt::sndtimeo, 100); + socket_.bind(endpoint_); + thread_ = std::thread(&MockDiscoveryDaemon::run, this); + } + ~MockDiscoveryDaemon() + { + running_.store(false); + if (thread_.joinable()) {thread_.join();} + } + void register_topic(const cortex_wire::TopicInfo & info) + { + std::lock_guard g(mu_); + topics_[info.name] = info; + } + +private: + void run() + { + while (running_.load()) { + zmq::message_t req; + zmq::recv_result_t r; + try {r = socket_.recv(req, zmq::recv_flags::none);} catch (...) {return;} + if (!r) {continue;} + const auto reply = handle(req.data(), req.size()); + try { + zmq::message_t out(reply.data(), reply.size()); + (void)socket_.send(out, zmq::send_flags::none); + } catch (...) {} + } + } + std::vector handle(const void * data, std::size_t size) + { + msgpack::object_handle oh; + try {oh = msgpack::unpack(static_cast(data), size);} catch (...) { + return pack_response(DiscoveryStatus::Error, "bad msgpack"); + } + const auto & root = oh.get(); + if (root.type != msgpack::type::MAP) { + return pack_response(DiscoveryStatus::Error, "expected map"); + } + DiscoveryCommand cmd = DiscoveryCommand::Ping; + std::string topic_name; + for (std::uint32_t i = 0; i < root.via.map.size; ++i) { + const auto & k = root.via.map.ptr[i].key; + const auto & v = root.via.map.ptr[i].val; + if (k.type != msgpack::type::STR) {continue;} + const std::string key(k.via.str.ptr, k.via.str.size); + if (key == "command" && v.type == msgpack::type::POSITIVE_INTEGER) { + cmd = static_cast(v.via.u64); + } else if (key == "topic_name" && v.type == msgpack::type::STR) { + topic_name.assign(v.via.str.ptr, v.via.str.size); + } + } + std::lock_guard g(mu_); + if (cmd == DiscoveryCommand::LookupTopic) { + auto it = topics_.find(topic_name); + if (it == topics_.end()) { + return pack_response(DiscoveryStatus::NotFound, "missing"); + } + return pack_response(DiscoveryStatus::Ok, "ok", &it->second); + } + return pack_response(DiscoveryStatus::Ok, "ignored"); + } + static std::vector pack_response( + DiscoveryStatus status, const std::string & msg, + const cortex_wire::TopicInfo * info = nullptr) + { + msgpack::sbuffer buf; + msgpack::packer pk(buf); + const std::uint32_t map_size = 2 + (info ? 1 : 0); + pk.pack_map(map_size); + pk.pack(std::string("status")); pk.pack(static_cast(status)); + pk.pack(std::string("message")); pk.pack(msg); + if (info) { + msgpack::sbuffer info_buf; + msgpack::packer ipk(info_buf); + ipk.pack_map(5); + ipk.pack(std::string("name")); ipk.pack(info->name); + ipk.pack(std::string("address")); ipk.pack(info->address); + ipk.pack(std::string("message_type")); ipk.pack(info->message_type); + ipk.pack(std::string("fingerprint")); ipk.pack(info->fingerprint); + ipk.pack(std::string("publisher_node")); ipk.pack(info->publisher_node); + pk.pack(std::string("topic_info")); + pk.pack_bin(static_cast(info_buf.size())); + pk.pack_bin_body(info_buf.data(), info_buf.size()); + } + return std::vector( + reinterpret_cast(buf.data()), + reinterpret_cast(buf.data()) + buf.size()); + } + zmq::context_t & ctx_; + std::string endpoint_; + zmq::socket_t socket_; + std::thread thread_; + std::atomic running_{true}; + std::mutex mu_; + std::unordered_map topics_; +}; + +std::string unique_ipc_path() +{ + static std::atomic counter{0}; + return "ipc:///tmp/cortex_ipc_test_" + std::to_string(::getpid()) + "_" + + std::to_string(counter.fetch_add(1)); +} + +std::string write_temp_yaml(const std::string & body) +{ + const std::string path = std::string(std::tmpnam(nullptr)) + ".yaml"; + std::ofstream f(path); + f << body; + return path; +} + +struct RclcppEnv : ::testing::Environment +{ + void SetUp() override {rclcpp::init(0, nullptr);} + void TearDown() override {rclcpp::shutdown();} +}; +::testing::Environment * const kRclcppEnv = + ::testing::AddGlobalTestEnvironment(new RclcppEnv); + +void send_cortex_frame( + zmq::socket_t & sock, const std::string & topic, std::uint64_t fingerprint, + std::uint64_t sequence, const std::vector & metadata, + const std::vector> & oob) +{ + cortex_wire::MessageHeader header{fingerprint, 0, sequence}; + std::array header_bytes{}; + header.to_bytes(header_bytes.data()); + + zmq::message_t f0(topic.data(), topic.size()); + (void)sock.send(f0, zmq::send_flags::sndmore); + zmq::message_t f1(header_bytes.data(), header_bytes.size()); + (void)sock.send(f1, zmq::send_flags::sndmore); + zmq::message_t f2(metadata.data(), metadata.size()); + zmq::send_flags meta_flag = oob.empty() ? zmq::send_flags::none : zmq::send_flags::sndmore; + (void)sock.send(f2, meta_flag); + for (std::size_t i = 0; i < oob.size(); ++i) { + zmq::message_t fi(oob[i].data(), oob[i].size()); + const auto flag = (i + 1 < oob.size()) ? + zmq::send_flags::sndmore : zmq::send_flags::none; + (void)sock.send(fi, flag); + } +} + +template +bool wait_for(Pred pred, std::chrono::milliseconds timeout = 3s) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) { + if (pred()) {return true;} + std::this_thread::sleep_for(20ms); + } + return pred(); +} + +} // namespace + +TEST(IntraProcess, ImageDeliversAsUniquePtr) +{ + zmq::context_t pub_ctx(1); + const auto daemon_addr = unique_ipc_path(); + const auto pub_addr = unique_ipc_path(); + MockDiscoveryDaemon daemon(pub_ctx, daemon_addr); + + const auto * fp = cortex_wire::find_by_name("ImageMessage"); + ASSERT_NE(fp, nullptr); + daemon.register_topic({ + "/cam/rgb", pub_addr, "ImageMessage", fp->fingerprint, "test_publisher", + }); + + zmq::socket_t pub(pub_ctx, zmq::socket_type::pub); + pub.set(zmq::sockopt::linger, 0); + pub.bind(pub_addr); + + const auto config_path = write_temp_yaml( + "version: 1\n" + "cortex:\n" + " discovery_address: \"" + daemon_addr + "\"\n" + "cortex_to_ros2:\n" + " - name: cam\n" + " cortex: {topic: \"/cam/rgb\", type: ImageMessage}\n" + " ros2: {topic: \"/cam/image_raw\", type: \"sensor_msgs/msg/Image\"}\n"); + + // Build both the bridge node and the consumer node with intra-process + // comms enabled. Putting them in the same executor activates rclcpp's + // intra-process manager. + rclcpp::NodeOptions bridge_opts; + bridge_opts.use_intra_process_comms(true); + bridge_opts.parameter_overrides({{"config_path", config_path}}); + auto bridge = std::make_shared(bridge_opts); + ASSERT_EQ(bridge->num_active_bindings(), 1u); + + rclcpp::NodeOptions consumer_opts; + consumer_opts.use_intra_process_comms(true); + auto consumer = std::make_shared("ipc_consumer", consumer_opts); + + std::atomic received_unique{0}; + std::atomic last_data_size{0}; + // Subscription with the `unique_ptr` signature — rclcpp delivers + // via unique_ptr only on the intra-process path with a single matching + // subscription, so this callback firing is the intra-process signal. + auto sub = consumer->create_subscription( + "/cam/image_raw", rclcpp::QoS(10), + [&](std::unique_ptr msg) { + last_data_size.store(msg->data.size()); + received_unique.fetch_add(1); + }); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(bridge); + exec.add_node(consumer); + + std::atomic spinning{true}; + std::thread spinner([&]() { + while (spinning.load()) {exec.spin_some(50ms);} + }); + + // Feed a 32×24×3 RGB frame. + const std::uint32_t W = 32, H = 24; + std::vector pixels(W * H * 3); + for (std::size_t i = 0; i < pixels.size(); ++i) { + pixels[i] = static_cast(i & 0xff); + } + cortex_wire::MetadataBuilder mb(4); + mb.pack_numpy_oob("fingerprint, i, frames.metadata, frames.oob_buffers); + std::this_thread::sleep_for(100ms); + } + + EXPECT_TRUE(wait_for([&] {return received_unique.load() > 0;})); + EXPECT_EQ(last_data_size.load(), pixels.size()); + + spinning.store(false); + spinner.join(); + std::remove(config_path.c_str()); +} + +// Same test, but with use_intra_process_comms=false on both nodes — the +// inter-process delivery path still works (rclcpp falls back to shared_ptr +// in the unique_ptr callback by copying on construction). Confirms our +// bridge is not silently coupled to intra-process being on. +TEST(IntraProcess, InterProcessFallbackStillDelivers) +{ + zmq::context_t pub_ctx(1); + const auto daemon_addr = unique_ipc_path(); + const auto pub_addr = unique_ipc_path(); + MockDiscoveryDaemon daemon(pub_ctx, daemon_addr); + + const auto * fp = cortex_wire::find_by_name("StringMessage"); + ASSERT_NE(fp, nullptr); + daemon.register_topic({ + "/test/str", pub_addr, "StringMessage", fp->fingerprint, "test_publisher", + }); + + zmq::socket_t pub(pub_ctx, zmq::socket_type::pub); + pub.set(zmq::sockopt::linger, 0); + pub.bind(pub_addr); + + const auto config_path = write_temp_yaml( + "version: 1\n" + "cortex:\n" + " discovery_address: \"" + daemon_addr + "\"\n" + "cortex_to_ros2:\n" + " - name: msg\n" + " cortex: {topic: \"/test/str\", type: StringMessage}\n" + " ros2: {topic: \"/test/out\", type: \"std_msgs/msg/String\"}\n"); + + rclcpp::NodeOptions opts; // default: intra-process OFF + opts.parameter_overrides({{"config_path", config_path}}); + auto bridge = std::make_shared(opts); + + rclcpp::NodeOptions consumer_opts; // intra-process OFF + auto consumer = std::make_shared("ipc_off_consumer", consumer_opts); + std::atomic received{0}; + auto sub = consumer->create_subscription( + "/test/out", rclcpp::QoS(10), + [&](const std_msgs::msg::String &) {received.fetch_add(1);}); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(bridge); + exec.add_node(consumer); + + std::atomic spinning{true}; + std::thread spinner([&]() { + while (spinning.load()) {exec.spin_some(50ms);} + }); + + cortex_wire::MetadataBuilder b(1); + b.pack_str("ping"); + auto frames = std::move(b).finish(); + std::this_thread::sleep_for(200ms); + for (int i = 0; i < 5 && received.load() == 0; ++i) { + send_cortex_frame(pub, "/test/str", fp->fingerprint, i, frames.metadata, frames.oob_buffers); + std::this_thread::sleep_for(100ms); + } + EXPECT_TRUE(wait_for([&] {return received.load() > 0;})); + + spinning.store(false); + spinner.join(); + std::remove(config_path.c_str()); +} diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_standard_adapters.cpp b/ros2_bridge/cortex_ros2_bridge/test/test_standard_adapters.cpp new file mode 100644 index 0000000..84105ee --- /dev/null +++ b/ros2_bridge/cortex_ros2_bridge/test/test_standard_adapters.cpp @@ -0,0 +1,409 @@ +// Copyright (c) 2026, Cortex contributors. Apache-2.0. +// +// Round-trip tests for the standard catalogue adapters added in PR6+PR7: +// Array (Float32/Float64 MultiArray), Image, PointCloud2, Pose, Transform, +// Tensor. +#include "cortex_ros2_bridge/adapters/arrays.hpp" +#include "cortex_ros2_bridge/adapters/image.hpp" +#include "cortex_ros2_bridge/adapters/pointcloud.hpp" +#include "cortex_ros2_bridge/adapters/pose.hpp" +#include "cortex_ros2_bridge/adapters/primitives.hpp" +#include "cortex_ros2_bridge/adapters/tensor.hpp" +#include "cortex_ros2_bridge/adapters/transform.hpp" +#include "cortex_ros2_bridge/binding_factory.hpp" +#include "cortex_ros2_bridge/registry.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +using namespace cortex_ros2_bridge; +using namespace cortex_ros2_bridge::adapters; + +namespace +{ + +// Build an inbound carrying a metadata frame plus a list of OOB byte buffers, +// each owned through a fresh zmq::message_t. +struct Inbound +{ + std::vector metadata_bytes; + std::vector> oob_owned; + std::vector oob_frames; + cortex_wire::DecodedMetadata metadata; + cortex_wire::MessageHeader header{}; + BridgeEntry cfg; + + Inbound(std::vector meta, std::vector> oob) + : metadata_bytes(std::move(meta)), + oob_owned(std::move(oob)), + metadata(cortex_wire::DecodedMetadata::from_bytes( + metadata_bytes.data(), metadata_bytes.size())) + { + oob_frames.reserve(oob_owned.size()); + for (auto & buf : oob_owned) { + zmq::message_t m(buf.size()); + std::memcpy(m.data(), buf.data(), buf.size()); + oob_frames.push_back(cortex_wire::make_owned(std::move(m))); + } + } + + CortexInbound view() const {return CortexInbound{header, metadata, oob_frames, cfg};} +}; + +} // namespace + +// ---- ArrayFloat32 -------------------------------------------------------- + +TEST(ArrayFloat32Adapter, RoundTrip) +{ + std_msgs::msg::Float32MultiArray src; + src.data = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f}; + std_msgs::msg::MultiArrayDimension d0; d0.size = 2; d0.stride = 6; + std_msgs::msg::MultiArrayDimension d1; d1.size = 3; d1.stride = 3; + src.layout.dim = {d0, d1}; + + ArrayFloat32Adapter adapter; + BridgeEntry cfg; + auto out = adapter.to_cortex(src, 0, cfg); + ASSERT_EQ(out.oob_buffers.size(), 1u); + + Inbound in(std::move(out.metadata), std::move(out.oob_buffers)); + auto round = adapter.to_ros2(in.view()); + EXPECT_EQ(round->data, src.data); + ASSERT_EQ(round->layout.dim.size(), 2u); + EXPECT_EQ(round->layout.dim[0].size, 2u); + EXPECT_EQ(round->layout.dim[1].size, 3u); +} + +TEST(ArrayFloat32Adapter, RejectsWrongDtype) +{ + // Build a metadata frame with data{1.0, 2.0}; + cortex_wire::MetadataBuilder b(3); + b.pack_numpy_oob("data, src.data); +} + +// ---- Image ---------------------------------------------------------------- + +TEST(ImageAdapter, RoundTripRgb8) +{ + // Synthesize a tiny 4×3×3 RGB image. + const std::uint32_t W = 3, H = 4, C = 3; + std::vector pixels(W * H * C); + for (std::size_t i = 0; i < pixels.size(); ++i) { + pixels[i] = static_cast(i & 0xff); + } + + cortex_wire::MetadataBuilder b(4); + b.pack_numpy_oob("width, W); + EXPECT_EQ(img->height, H); + EXPECT_EQ(img->encoding, "rgb8"); + EXPECT_EQ(img->step, W * C); + ASSERT_EQ(img->data.size(), pixels.size()); + EXPECT_EQ(std::memcmp(img->data.data(), pixels.data(), pixels.size()), 0); + + // Reverse: pack the same image back to Cortex. + BridgeEntry cfg; + auto back = adapter.to_cortex(*img, 0, cfg); + ASSERT_EQ(back.oob_buffers.size(), 1u); + EXPECT_EQ(back.oob_buffers[0].size(), pixels.size()); + EXPECT_EQ( + std::memcmp(back.oob_buffers[0].data(), pixels.data(), pixels.size()), 0); +} + +TEST(ImageAdapter, FrameIdYamlOverride) +{ + cortex_wire::MetadataBuilder b(4); + std::vector px(2 * 2 * 1, 0xff); + b.pack_numpy_oob("header.frame_id, "cam_optical"); +} + +// ---- PointCloud2 --------------------------------------------------------- + +TEST(PointCloudAdapter, XyzOnlyRoundTrip) +{ + // Three points. + const std::vector pts = { + 0.0f, 0.0f, 0.0f, + 1.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 2.0f, + }; + + cortex_wire::MetadataBuilder b(5); + b.pack_numpy_oob("width, 3u); + EXPECT_EQ(pc->height, 1u); + EXPECT_EQ(pc->fields.size(), 3u); + EXPECT_EQ(pc->point_step, 12u); + ASSERT_EQ(pc->data.size(), 36u); + // Spot-check x of point 1 + float x1; + std::memcpy(&x1, pc->data.data() + 12 + 0, 4); + EXPECT_FLOAT_EQ(x1, 1.0f); + EXPECT_EQ(pc->header.frame_id, "lidar"); + + // Reverse. + BridgeEntry cfg; + auto back = adapter.to_cortex(*pc, 0, cfg); + // points + 3 nils for colors/intensity/normals → 1 OOB buffer total. + EXPECT_EQ(back.oob_buffers.size(), 1u); +} + +TEST(PointCloudAdapter, FullAttributesRoundTrip) +{ + const std::uint32_t N = 2; + const std::vector pts = {1, 2, 3, 4, 5, 6}; + const std::vector col = {255, 0, 0, 0, 255, 0}; + const std::vector inten = {10.0f, 20.0f}; + const std::vector nrm = {0, 0, 1, 1, 0, 0}; + + cortex_wire::MetadataBuilder b(5); + b.pack_numpy_oob("fields.size(), 8u); + EXPECT_EQ(pc->point_step, 12u + 4u + 4u + 12u); + + BridgeEntry cfg; + auto back = adapter.to_cortex(*pc, 0, cfg); + EXPECT_EQ(back.oob_buffers.size(), 4u); // points, colors, intensity, normals +} + +// ---- PoseStamped --------------------------------------------------------- + +TEST(PoseAdapter, RoundTrip) +{ + std::array pos{1.0, 2.0, 3.0}; + std::array orient{0.0, 0.0, 0.0, 1.0}; + + cortex_wire::MetadataBuilder b(4); + b.pack_numpy_oob("pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(p->pose.position.y, 2.0); + EXPECT_DOUBLE_EQ(p->pose.position.z, 3.0); + EXPECT_DOUBLE_EQ(p->pose.orientation.w, 1.0); + EXPECT_EQ(p->header.frame_id, "map"); + + // Reverse — child_frame_id is dropped (PoseStamped has none). + BridgeEntry cfg; + auto back = adapter.to_cortex(*p, 0, cfg); + EXPECT_EQ(back.oob_buffers.size(), 2u); +} + +TEST(PoseAdapter, AcceptsFloat32Wire) +{ + std::array pos{0.5f, 1.5f, 2.5f}; + std::array orient{0.0f, 0.0f, 0.0f, 1.0f}; + cortex_wire::MetadataBuilder b(4); + b.pack_numpy_oob("pose.position.y, 1.5); +} + +// ---- TransformStamped ---------------------------------------------------- + +TEST(TransformAdapter, IdentityRoundTrip) +{ + std::array identity{ + 1, 0, 0, 5, + 0, 1, 0, 6, + 0, 0, 1, 7, + 0, 0, 0, 1, + }; + + cortex_wire::MetadataBuilder b(3); + b.pack_numpy_oob("transform.translation.x, 5); + EXPECT_DOUBLE_EQ(t->transform.translation.y, 6); + EXPECT_DOUBLE_EQ(t->transform.translation.z, 7); + EXPECT_DOUBLE_EQ(t->transform.rotation.w, 1.0); + EXPECT_DOUBLE_EQ(t->transform.rotation.x, 0.0); + EXPECT_EQ(t->header.frame_id, "world"); + EXPECT_EQ(t->child_frame_id, "robot"); + + // Reverse round-trips through quaternion → matrix. + BridgeEntry cfg; + auto back = adapter.to_cortex(*t, 0, cfg); + Inbound rt(std::move(back.metadata), std::move(back.oob_buffers)); + auto t2 = adapter.to_ros2(rt.view()); + EXPECT_DOUBLE_EQ(t2->transform.translation.x, 5); + EXPECT_DOUBLE_EQ(t2->transform.translation.y, 6); + EXPECT_DOUBLE_EQ(t2->transform.translation.z, 7); +} + +TEST(TransformAdapter, RotationQuaternionRoundTrip) +{ + // 90 degree rotation about Z, no translation. + const double c = 0.0; + const double s = 1.0; + std::array R{ + c, -s, 0, 0, + s, c, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1, + }; + + cortex_wire::MetadataBuilder b(3); + b.pack_numpy_oob("transform.rotation.x, 0.0, 1e-9); + EXPECT_NEAR(t->transform.rotation.y, 0.0, 1e-9); + EXPECT_NEAR(t->transform.rotation.z, std::sqrt(0.5), 1e-9); + EXPECT_NEAR(t->transform.rotation.w, std::sqrt(0.5), 1e-9); +} + +// ---- Tensor -------------------------------------------------------------- + +TEST(TensorFloat32Adapter, RoundTrip) +{ + std::vector data{1.0f, 2.0f, 3.0f, 4.0f}; + + // Construct a torch OOB descriptor by hand. + cortex_wire::MetadataBuilder b(2); + b.pack_torch_oob( + "data, data); + ASSERT_EQ(m->layout.dim.size(), 2u); + EXPECT_EQ(m->layout.dim[0].size, 2u); + EXPECT_EQ(m->layout.dim[1].size, 2u); + + BridgeEntry cfg; + auto back = adapter.to_cortex(*m, 0, cfg); + // Verify reverse emits a torch OOB descriptor by decoding the metadata. + auto md = cortex_wire::DecodedMetadata::from_bytes( + back.metadata.data(), back.metadata.size()); + ASSERT_EQ(md.field_count(), 2u); + auto desc = cortex_wire::DecodedMetadata::as_oob(md.field(0)); + ASSERT_TRUE(desc.has_value()); + EXPECT_EQ(desc->kind, cortex_wire::OobKind::Torch); + EXPECT_EQ(desc->device, "cpu"); + EXPECT_FALSE(desc->requires_grad); +} + +// ---- Registration -------------------------------------------------------- + +TEST(RegisterStandard, AllAdaptersAndBindingsLandInRegistries) +{ + AdapterRegistry ar; + BindingFactoryRegistry br; + + const auto a_pri = register_primitives(ar); + const auto a_arr = register_array_adapters(ar); + const auto a_img = register_image_adapters(ar); + const auto a_pc = register_pointcloud_adapters(ar); + const auto a_pose = register_pose_adapters(ar); + const auto a_tf = register_transform_adapters(ar); + const auto a_ten = register_tensor_adapters(ar); + + // Adapters: 12 primitive + 4 array (2 × 2 dirs) + 2 image + 2 pointcloud + + // 2 pose + 2 transform + 2 tensor. + EXPECT_EQ(a_pri + a_arr + a_img + a_pc + a_pose + a_tf + a_ten, 12u + 4u + 2u + 2u + 2u + 2u + 2u); + + const auto b_pri = register_primitive_bindings(br); + const auto b_std = register_standard_bindings(br); + EXPECT_EQ(b_pri + b_std, 12u + 12u); + + EXPECT_TRUE(br.get_cortex_to_ros2("sensor_msgs/msg/Image")); + EXPECT_TRUE(br.get_ros2_to_cortex("sensor_msgs/msg/PointCloud2")); + EXPECT_TRUE(br.get_cortex_to_ros2("geometry_msgs/msg/PoseStamped")); +} From 2e5028be1b180426ad402d1c3f5a68c895cad2b1 Mon Sep 17 00:00:00 2001 From: Richeek Das Date: Mon, 11 May 2026 18:20:03 -0400 Subject: [PATCH 07/10] a proper readme for cortex ros2 bridge --- ros2_bridge/cortex_ros2_bridge/README.md | 269 +++++++++++++++++------ 1 file changed, 205 insertions(+), 64 deletions(-) diff --git a/ros2_bridge/cortex_ros2_bridge/README.md b/ros2_bridge/cortex_ros2_bridge/README.md index 22680f3..060eef8 100644 --- a/ros2_bridge/cortex_ros2_bridge/README.md +++ b/ros2_bridge/cortex_ros2_bridge/README.md @@ -1,101 +1,242 @@ # cortex_ros2_bridge -ROS 2 package that bridges Cortex pub/sub topics to ROS 2 topics, in both directions. +A ROS 2 package that bridges Cortex pub/sub topics to ROS 2 topics, in both directions, configured by a single YAML file. -Status: **PR1 + PR2 landed** out of the 10 PRs in [the implementation plan](../PLAN.md). +The bridge ships as two composable rclcpp nodes — one per direction — that can be loaded into a `component_container_mt` together with downstream ROS 2 consumers for **intra-process zero-copy delivery**. They also run as plain executables for development. -- **PR1** — package skeleton + YAML config loader (`cortex_ros2_bridge_config`). -- **PR2** — Cortex wire library (`cortex_ros2_bridge_wire`): header decode, msgpack metadata + OOB descriptors, OobBuffer/ZmqAllocator, discovery REQ/REP client, generated fingerprint table. +``` +┌───────────────────────────┐ ┌─────────────────────────────┐ +│ Cortex publisher (Py) │ │ rclcpp consumer (C++) │ +│ /sensor/camera/rgb │ │ /camera/image_raw │ +└──────────┬────────────────┘ └─────────────┬───────────────┘ + │ ▲ + │ ZMQ multipart over ipc:// │ rclcpp intra-process + │ [topic, header, metadata, *oob_frames] │ or DDS + ▼ │ +┌─────────────────────────────────────────────────────────────────────────┐ +│ ComposableNodeContainer │ +│ ┌────────────────────────┐ ┌────────────────────────────┐ │ +│ │ CortexToRos2Bridge │ │ Ros2ToCortexBridge │ │ +│ │ - SUB socket per topic │ │ - PUB socket per topic │ │ +│ │ - adapter -> ROS 2 msg │ │ - rclcpp sub -> adapter │ │ +│ └────────────────────────┘ └────────────────────────────┘ │ +└──────────┬─────────────────────────────────────────────────────┬────────┘ + │ discovery REQ/REP (ipc:///tmp/cortex_discovery) │ + ▼ ▼ + ┌───────────────────────┐ + │ cortex-discovery │ + │ (Python daemon) │ + └───────────────────────┘ +``` -Subsequent PRs add the adapter registry, the two `rclcpp` components, intra-process composability, and loaned-message support. +## Prerequisites -## Layout +- ROS 2 Humble or later (ament_cmake, rclcpp, rclcpp_components, sensor_msgs, geometry_msgs, std_msgs, builtin_interfaces). +- A C++17 compiler. +- System packages: `libzmq3-dev`, `libmsgpack-dev`, `libyaml-cpp-dev`, `libcppzmq-dev`. +- [`cortex_wire_cpp`](../../cortex_wire_cpp) — a pure-CMake sibling library that this package depends on. If it isn't installed, our `CMakeLists.txt` falls back to `add_subdirectory` against the in-tree source at `../../cortex_wire_cpp`. +- A running Cortex discovery daemon (`cortex-discovery` from the [Python cortex package](../../)). -``` -include/cortex_ros2_bridge/ -├── config.hpp ← YAML schema + loader (PR1) -└── cortex_wire/ - ├── header.hpp ← 24-byte MessageHeader (PR2) - ├── oob_buffer.hpp ← OobBuffer, ZmqAllocator (PR2) - ├── metadata.hpp ← msgpack metadata + OobDescriptor (PR2) - ├── discovery_client.hpp ← sync REQ/REP client (PR2) - └── fingerprint_table.hpp ← auto-generated by scripts/ (PR2) - -src/ -├── config.cpp -└── cortex_wire/ - ├── header.cpp - ├── metadata.cpp - └── discovery_client.cpp - -scripts/gen_fingerprint_table.py ← regenerates fingerprint_table.hpp -config/example_*.yaml ← schema examples -test/ ← 6 gtest binaries, 59 cases +## Build + +From a colcon workspace that contains this package under `src/`: + +```bash +colcon build --packages-select cortex_ros2_bridge +source install/setup.bash ``` -## Build +On Anaconda-based ROS containers (where the default `python3` lacks `catkin_pkg`), point CMake at the system Python: ```bash -# from a colcon workspace whose src/ contains this package colcon build --packages-select cortex_ros2_bridge \ - --cmake-args -DPython3_EXECUTABLE=/usr/bin/python3 -colcon test --packages-select cortex_ros2_bridge + --cmake-args -DPython3_EXECUTABLE=/usr/bin/python3 ``` -Dependencies: `yaml-cpp`, `cppzmq` (uses installed `libzmq` + `zmq.hpp`), `libmsgpack-dev` (header-only msgpack-cxx). +The package also includes the test suite: -The `-DPython3_EXECUTABLE` override is needed on Anaconda-based ROS containers (e.g. NVIDIA's `neurosim:ros`) where the default `python3` does not have `catkin_pkg` installed. +```bash +colcon test --packages-select cortex_ros2_bridge +colcon test-result --verbose +``` -## Regenerating the fingerprint table +## Configuration -The header at `include/cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp` is committed, so the package builds without a Python `cortex` install. Regenerate after any change to `cortex.messages.standard`: +Every bridged topic is one entry in a YAML file. The complete schema: + +```yaml +version: 1 # required, currently 1 + +cortex: + discovery_address: "ipc:///tmp/cortex_discovery" # daemon REQ/REP endpoint + node_name_prefix: "cortex_bridge" # used to derive PUB endpoints + +defaults: # applied to entries that omit qos + qos: + reliability: reliable # reliable | best_effort + durability: volatile # volatile | transient_local + history: keep_last # keep_last | keep_all + depth: 10 + loaned_messages: auto # auto | force | never (reserved for future use) + +# Cortex publishers -> ROS 2 subscribers +cortex_to_ros2: + - name: camera + cortex: + topic: "/sensor/camera/rgb" + type: ImageMessage # name from cortex.messages.standard + ros2: + topic: "/camera/image_raw" + type: "sensor_msgs/msg/Image" # explicit; default-inference is deferred + frame_id: "camera_optical" # optional; overrides any wire frame_id + qos: + reliability: best_effort + depth: 5 + +# ROS 2 publishers -> Cortex subscribers +ros2_to_cortex: + - name: cmd + ros2: + topic: "/cmd_string" + type: "std_msgs/msg/String" + cortex: + topic: "/control/cmd" + type: StringMessage + qos: + reliability: reliable + depth: 1 +``` + +Validation rules: + +- `version` must be `1`. +- Each entry `name` is unique across both directions. +- `ros2.type` is **required** for `ros2_to_cortex` entries (the mapping ROS→Cortex is ambiguous; we won't guess). It is also required for `cortex_to_ros2` entries today, although default-by-Cortex-kind lookup is on the roadmap. +- Unknown `cortex.type` names, missing adapters, and discovery fingerprint mismatches fail the affected entry loudly and the bridge keeps the others alive. + +Examples ship in [config/example_minimal.yaml](config/example_minimal.yaml) and [config/example_full.yaml](config/example_full.yaml). + +## Launch + +Three launch files are installed under `share/cortex_ros2_bridge/launch/`: + +| File | Purpose | +| --- | --- | +| `cortex_to_ros2.launch.py` | One container, one component: Cortex → ROS 2 only. | +| `ros2_to_cortex.launch.py` | One container, one component: ROS 2 → Cortex only. | +| `composable_container.launch.py` | One container, both components. **Use this for production** so downstream ROS 2 consumers can compose into the same container and receive bridge-published messages via intra-process delivery. | + +Run a bridge: ```bash -python3 -m venv /tmp/cortex_venv -/tmp/cortex_venv/bin/pip install -e /path/to/cortex -/tmp/cortex_venv/bin/python scripts/gen_fingerprint_table.py +# discovery daemon (separate terminal) +cortex-discovery + +# both directions, both composable nodes in one container with +# use_intra_process_comms enabled +ros2 launch cortex_ros2_bridge composable_container.launch.py \ + config:=/path/to/bridge.yaml ``` -Pass `--check` in CI to verify it stays in sync. +The launch argument `container_name` is exposed if you want to load additional composable nodes into the same container later (`container_name:=cortex_bridge_container` is the default). -## Configuration +For development without a container (no intra-process zero-copy, easier to attach a debugger): + +```bash +ros2 run cortex_ros2_bridge cortex_to_ros2 \ + --ros-args -p config_path:=/path/to/bridge.yaml + +ros2 run cortex_ros2_bridge ros2_to_cortex \ + --ros-args -p config_path:=/path/to/bridge.yaml +``` + +## Supported message types + +| Cortex message | ROS 2 message | Notes | +| --- | --- | --- | +| `StringMessage` | `std_msgs/msg/String` | | +| `IntMessage` | `std_msgs/msg/Int64` | | +| `FloatMessage` | `std_msgs/msg/Float64` | | +| `BytesMessage` | `std_msgs/msg/ByteMultiArray` | Cortex side is msgpack `BIN`, not `STR`. | +| `TimestampMessage` | `builtin_interfaces/msg/Time` | | +| `HeaderMessage` | `std_msgs/msg/Header` | Sequence field is dropped (no equivalent in ROS 2's stripped Header). | +| `ArrayMessage` | `std_msgs/msg/Float32MultiArray` | Strict dtype `` from the bridge directly to a colocated subscription without DDS serialisation, when: + +1. Both the bridge component and the subscriber are loaded into the **same** `component_container_mt`. +2. Both use `use_intra_process_comms: true` (the launch files set this). +3. The subscriber takes the message via `std::unique_ptr` or `std::shared_ptr`. -A YAML file describes every bridged topic. See [config/example_minimal.yaml](config/example_minimal.yaml) and [config/example_full.yaml](config/example_full.yaml). The schema is documented in [../PLAN.md](../PLAN.md) §5. +On the ZMQ ingress side, the OOB frames that carry arrays are not memcpy'd until they hit the adapter; the adapter's `to_ros2` is where data ends up in the published ROS message. For `sensor_msgs/Image`, that adapter performs **one memcpy** from the ZMQ frame into a freshly-allocated `Image::data`. Eliminating that memcpy requires either a loaned-message-capable RMW (Iceoryx) or a custom intra-process message type, both of which are out of scope for v1. -The C++ loader lives in [include/cortex_ros2_bridge/config.hpp](include/cortex_ros2_bridge/config.hpp); use `cortex_ros2_bridge::load_config(path)` from any node. +## Discovery daemon -## Wire library +The bridge does not manage the daemon. Bring it up out of band: -`cortex_ros2_bridge_wire` is a standalone library that can be linked into any consumer (the future bridge components are the obvious one, but anything that wants to talk Cortex's IPC wire format from C++ can use it). +```bash +cortex-discovery # default endpoint +cortex-discovery --address ipc:///tmp/my_disc # custom endpoint, also set in YAML +``` + +`Ros2ToCortexBridge` registers itself with the daemon at startup and unregisters on shutdown. `CortexToRos2Bridge` looks up its source topics' endpoints once at startup; topics that aren't yet registered cause that entry to be skipped (other entries keep running). -```cpp -#include "cortex_ros2_bridge/cortex_wire/header.hpp" -#include "cortex_ros2_bridge/cortex_wire/metadata.hpp" -#include "cortex_ros2_bridge/cortex_wire/discovery_client.hpp" -#include "cortex_ros2_bridge/cortex_wire/fingerprint_table.hpp" +## Troubleshooting -// Look up a topic -zmq::context_t ctx(1); -cortex_wire::DiscoveryClient discovery(ctx, "ipc:///tmp/cortex_discovery"); -auto info = discovery.lookup("/sensor/camera/rgb"); // -> optional +| Symptom | Cause | +| --- | --- | +| `discovery: topic '/foo' not registered — skipping` | The Cortex publisher hasn't started yet, or `discovery_address` in YAML doesn't match the daemon. | +| `fingerprint mismatch: daemon=0x…, expected=0x… — refusing to bridge` | The publisher is running a Cortex `messages.standard` version different from the one this bridge was built against. Rebuild after regenerating `fingerprint_table.hpp` (see *Development* below). | +| `cannot create parent dir for ipc:///tmp/...` | Filesystem permissions on `/tmp/cortex/topics/` are wrong. | +| `unknown cortex type 'XMessage' — skipping` | YAML references a Cortex message we don't have an adapter for. | +| `factory returned null binding` | An adapter is registered for the cortex kind, but not for the requested `ros2.type` combination. Check the supported types table above. | -// Verify fingerprint -const auto * entry = cortex_wire::find_by_fingerprint(info->fingerprint); -assert(entry && entry->kind == cortex_wire::MessageKind::ImageMessage); +## Development -// Decode an incoming multipart message -auto header = cortex_wire::MessageHeader::from_bytes(frames[0].data(), frames[0].size()); -auto meta = cortex_wire::DecodedMetadata::from_bytes(frames[1].data(), frames[1].size()); -auto oob = cortex_wire::DecodedMetadata::as_oob(meta.field(0)); // numpy descriptor +The package layout: + +``` +cortex_ros2_bridge/ +├── include/cortex_ros2_bridge/ +│ ├── config.hpp # YAML schema + loader +│ ├── qos.hpp # QosSettings -> rclcpp::QoS +│ ├── adapter.hpp # adapter ABCs + Inbound/Outbound +│ ├── registry.hpp # type-erased adapter registry +│ ├── binding.hpp # templated per-topic plumbing +│ ├── binding_factory.hpp # string -> binding factory +│ ├── cortex_to_ros2_node.hpp # composable component +│ ├── ros2_to_cortex_node.hpp # composable component +│ └── adapters/ # one header per Cortex message +├── src/ # matching implementations +├── launch/ # 3 launch files +├── config/ # example YAMLs +└── test/ # gtest suite ``` -## Test status +The Cortex wire format library lives separately at [`cortex_wire_cpp/`](../../cortex_wire_cpp) as a pure-CMake project (no ROS dependency). It is consumed via `find_package(cortex_wire_cpp)`; the bridge's CMakeLists falls back to `add_subdirectory` for monorepo development. -Run from inside the package's colcon workspace after build: +The C++ side fingerprint table (`cortex_wire_cpp/include/cortex_wire/fingerprint_table.hpp`) is generated from the Python `cortex.messages.standard` catalogue. Regenerate after any change to that module: ```bash -colcon test --packages-select cortex_ros2_bridge -# Summary: 86 tests, 0 errors, 0 failures, 16 skipped +python3 -m venv /tmp/cortex_venv +/tmp/cortex_venv/bin/pip install -e /path/to/cortex +/tmp/cortex_venv/bin/python ../../cortex_wire_cpp/scripts/gen_fingerprint_table.py ``` -The 16 skipped entries are opinionated style linters (copyright, cpplint, uncrustify, pep257, flake8) intentionally disabled in `CMakeLists.txt`; substantive lints (cppcheck, lint_cmake, xmllint) and 59 functional gtest cases all pass. +Pass `--check` in CI to verify the committed header matches the installed cortex package. + +For a full code walkthrough of how everything fits together, see [STUDY_GUIDE.md](../STUDY_GUIDE.md). + +## License + +Apache-2.0. See [../../LICENSE](../../LICENSE). From ff7398158ed27b37be4c4cf3486c96023499dc4b Mon Sep 17 00:00:00 2001 From: Richeek Das Date: Mon, 11 May 2026 18:31:58 -0400 Subject: [PATCH 08/10] lint --- .../scripts/gen_fingerprint_table.py | 3 +- .../launch/composable_container.launch.py | 61 ++++++++++--------- .../launch/cortex_to_ros2.launch.py | 47 +++++++------- .../launch/ros2_to_cortex.launch.py | 47 +++++++------- 4 files changed, 84 insertions(+), 74 deletions(-) diff --git a/cortex_wire_cpp/scripts/gen_fingerprint_table.py b/cortex_wire_cpp/scripts/gen_fingerprint_table.py index 2f8a475..100b238 100755 --- a/cortex_wire_cpp/scripts/gen_fingerprint_table.py +++ b/cortex_wire_cpp/scripts/gen_fingerprint_table.py @@ -8,6 +8,7 @@ Usage: scripts/gen_fingerprint_table.py [--out PATH] """ + from __future__ import annotations import argparse @@ -108,7 +109,7 @@ def collect_message_classes() -> list[type[Message]]: def render(classes: list[type[Message]]) -> str: enum_entries = "\n".join(f" {c.__name__}," for c in classes) table_entries = "\n".join( - f' {{0x{c.fingerprint():016x}ULL, MessageKind::{c.__name__}, ' + f" {{0x{c.fingerprint():016x}ULL, MessageKind::{c.__name__}, " f'"{c.__name__}", "{c.__module__}.{c.__qualname__}"}},' for c in classes ) diff --git a/ros2_bridge/cortex_ros2_bridge/launch/composable_container.launch.py b/ros2_bridge/cortex_ros2_bridge/launch/composable_container.launch.py index 4e5f9e4..5f794ac 100644 --- a/ros2_bridge/cortex_ros2_bridge/launch/composable_container.launch.py +++ b/ros2_bridge/cortex_ros2_bridge/launch/composable_container.launch.py @@ -5,6 +5,7 @@ container by extending this file (add more ComposableNode entries) or by referencing the same container name from their own launch graph. """ + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -16,32 +17,34 @@ def generate_launch_description(): config = LaunchConfiguration("config") container_name = LaunchConfiguration("container_name") - return LaunchDescription([ - DeclareLaunchArgument("config", description="Path to bridge YAML config"), - DeclareLaunchArgument( - "container_name", default_value="cortex_bridge_container" - ), - ComposableNodeContainer( - name=container_name, - namespace="", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=[ - ComposableNode( - package="cortex_ros2_bridge", - plugin="cortex_ros2_bridge::CortexToRos2Bridge", - name="cortex_to_ros2", - parameters=[{"config_path": config}], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ComposableNode( - package="cortex_ros2_bridge", - plugin="cortex_ros2_bridge::Ros2ToCortexBridge", - name="ros2_to_cortex", - parameters=[{"config_path": config}], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ], - output="screen", - ), - ]) + return LaunchDescription( + [ + DeclareLaunchArgument("config", description="Path to bridge YAML config"), + DeclareLaunchArgument( + "container_name", default_value="cortex_bridge_container" + ), + ComposableNodeContainer( + name=container_name, + namespace="", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="cortex_ros2_bridge", + plugin="cortex_ros2_bridge::CortexToRos2Bridge", + name="cortex_to_ros2", + parameters=[{"config_path": config}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="cortex_ros2_bridge", + plugin="cortex_ros2_bridge::Ros2ToCortexBridge", + name="ros2_to_cortex", + parameters=[{"config_path": config}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + ), + ] + ) diff --git a/ros2_bridge/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py b/ros2_bridge/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py index 08aa3c1..f69f47a 100644 --- a/ros2_bridge/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py +++ b/ros2_bridge/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py @@ -1,4 +1,5 @@ """Launch a single CortexToRos2Bridge composable node in its own container.""" + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -10,25 +11,27 @@ def generate_launch_description(): config = LaunchConfiguration("config") container_name = LaunchConfiguration("container_name") - return LaunchDescription([ - DeclareLaunchArgument("config", description="Path to bridge YAML config"), - DeclareLaunchArgument( - "container_name", default_value="cortex_to_ros2_container" - ), - ComposableNodeContainer( - name=container_name, - namespace="", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=[ - ComposableNode( - package="cortex_ros2_bridge", - plugin="cortex_ros2_bridge::CortexToRos2Bridge", - name="cortex_to_ros2", - parameters=[{"config_path": config}], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ], - output="screen", - ), - ]) + return LaunchDescription( + [ + DeclareLaunchArgument("config", description="Path to bridge YAML config"), + DeclareLaunchArgument( + "container_name", default_value="cortex_to_ros2_container" + ), + ComposableNodeContainer( + name=container_name, + namespace="", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="cortex_ros2_bridge", + plugin="cortex_ros2_bridge::CortexToRos2Bridge", + name="cortex_to_ros2", + parameters=[{"config_path": config}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + ), + ] + ) diff --git a/ros2_bridge/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py b/ros2_bridge/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py index fe92f0e..a964f26 100644 --- a/ros2_bridge/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py +++ b/ros2_bridge/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py @@ -1,4 +1,5 @@ """Launch a single Ros2ToCortexBridge composable node in its own container.""" + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -10,25 +11,27 @@ def generate_launch_description(): config = LaunchConfiguration("config") container_name = LaunchConfiguration("container_name") - return LaunchDescription([ - DeclareLaunchArgument("config", description="Path to bridge YAML config"), - DeclareLaunchArgument( - "container_name", default_value="ros2_to_cortex_container" - ), - ComposableNodeContainer( - name=container_name, - namespace="", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=[ - ComposableNode( - package="cortex_ros2_bridge", - plugin="cortex_ros2_bridge::Ros2ToCortexBridge", - name="ros2_to_cortex", - parameters=[{"config_path": config}], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ], - output="screen", - ), - ]) + return LaunchDescription( + [ + DeclareLaunchArgument("config", description="Path to bridge YAML config"), + DeclareLaunchArgument( + "container_name", default_value="ros2_to_cortex_container" + ), + ComposableNodeContainer( + name=container_name, + namespace="", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="cortex_ros2_bridge", + plugin="cortex_ros2_bridge::Ros2ToCortexBridge", + name="ros2_to_cortex", + parameters=[{"config_path": config}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + ), + ] + ) From 39c23ffbef8452df4775b54f7d9299eadd3a5f54 Mon Sep 17 00:00:00 2001 From: Richeek Das Date: Mon, 11 May 2026 19:26:28 -0400 Subject: [PATCH 09/10] move it inside src for easier ros2 work --- ros2_bridge/{ => src}/cortex_ros2_bridge/CMakeLists.txt | 8 ++++++-- ros2_bridge/{ => src}/cortex_ros2_bridge/README.md | 8 +------- .../{ => src}/cortex_ros2_bridge/config/example_full.yaml | 0 .../cortex_ros2_bridge/config/example_minimal.yaml | 0 .../include/cortex_ros2_bridge/adapter.hpp | 0 .../include/cortex_ros2_bridge/adapters/arrays.hpp | 0 .../include/cortex_ros2_bridge/adapters/image.hpp | 0 .../include/cortex_ros2_bridge/adapters/oob_helpers.hpp | 0 .../include/cortex_ros2_bridge/adapters/pointcloud.hpp | 0 .../include/cortex_ros2_bridge/adapters/pose.hpp | 0 .../include/cortex_ros2_bridge/adapters/primitives.hpp | 0 .../include/cortex_ros2_bridge/adapters/tensor.hpp | 0 .../include/cortex_ros2_bridge/adapters/transform.hpp | 0 .../include/cortex_ros2_bridge/binding.hpp | 0 .../include/cortex_ros2_bridge/binding_factory.hpp | 0 .../include/cortex_ros2_bridge/config.hpp | 0 .../include/cortex_ros2_bridge/cortex_to_ros2_node.hpp | 0 .../cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp | 0 .../include/cortex_ros2_bridge/registry.hpp | 0 .../include/cortex_ros2_bridge/ros2_to_cortex_node.hpp | 0 .../launch/composable_container.launch.py | 0 .../cortex_ros2_bridge/launch/cortex_to_ros2.launch.py | 0 .../cortex_ros2_bridge/launch/ros2_to_cortex.launch.py | 0 ros2_bridge/{ => src}/cortex_ros2_bridge/package.xml | 0 .../{ => src}/cortex_ros2_bridge/src/adapters/arrays.cpp | 0 .../{ => src}/cortex_ros2_bridge/src/adapters/image.cpp | 0 .../cortex_ros2_bridge/src/adapters/pointcloud.cpp | 0 .../{ => src}/cortex_ros2_bridge/src/adapters/pose.cpp | 0 .../cortex_ros2_bridge/src/adapters/primitives.cpp | 0 .../{ => src}/cortex_ros2_bridge/src/adapters/tensor.cpp | 0 .../cortex_ros2_bridge/src/adapters/transform.cpp | 0 .../{ => src}/cortex_ros2_bridge/src/binding_factory.cpp | 0 ros2_bridge/{ => src}/cortex_ros2_bridge/src/config.cpp | 0 .../cortex_ros2_bridge/src/cortex_to_ros2_node.cpp | 0 ros2_bridge/{ => src}/cortex_ros2_bridge/src/qos.cpp | 0 ros2_bridge/{ => src}/cortex_ros2_bridge/src/registry.cpp | 0 .../cortex_ros2_bridge/src/ros2_to_cortex_node.cpp | 0 .../cortex_ros2_bridge/test/test_binding_factory.cpp | 0 .../cortex_ros2_bridge/test/test_components_e2e.cpp | 0 .../{ => src}/cortex_ros2_bridge/test/test_config.cpp | 0 .../cortex_ros2_bridge/test/test_intra_process.cpp | 0 .../{ => src}/cortex_ros2_bridge/test/test_primitives.cpp | 0 .../{ => src}/cortex_ros2_bridge/test/test_qos.cpp | 0 .../{ => src}/cortex_ros2_bridge/test/test_registry.cpp | 0 .../cortex_ros2_bridge/test/test_standard_adapters.cpp | 0 45 files changed, 7 insertions(+), 9 deletions(-) rename ros2_bridge/{ => src}/cortex_ros2_bridge/CMakeLists.txt (93%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/README.md (98%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/config/example_full.yaml (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/config/example_minimal.yaml (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/arrays.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/image.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/oob_helpers.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pointcloud.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pose.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/tensor.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/transform.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_to_ros2_node.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/include/cortex_ros2_bridge/ros2_to_cortex_node.hpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/launch/composable_container.launch.py (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/package.xml (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/adapters/arrays.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/adapters/image.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/adapters/pointcloud.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/adapters/pose.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/adapters/primitives.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/adapters/tensor.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/adapters/transform.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/binding_factory.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/config.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/qos.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/registry.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/test/test_binding_factory.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/test/test_components_e2e.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/test/test_config.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/test/test_intra_process.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/test/test_primitives.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/test/test_qos.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/test/test_registry.cpp (100%) rename ros2_bridge/{ => src}/cortex_ros2_bridge/test/test_standard_adapters.cpp (100%) diff --git a/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt b/ros2_bridge/src/cortex_ros2_bridge/CMakeLists.txt similarity index 93% rename from ros2_bridge/cortex_ros2_bridge/CMakeLists.txt rename to ros2_bridge/src/cortex_ros2_bridge/CMakeLists.txt index 1a392c3..3caaa78 100644 --- a/ros2_bridge/cortex_ros2_bridge/CMakeLists.txt +++ b/ros2_bridge/src/cortex_ros2_bridge/CMakeLists.txt @@ -25,7 +25,7 @@ find_package(geometry_msgs REQUIRED) # cortex_wire_cpp has been installed). find_package(cortex_wire_cpp QUIET) if(NOT cortex_wire_cpp_FOUND) - set(_cwc_dir "${CMAKE_CURRENT_SOURCE_DIR}/../../cortex_wire_cpp") + set(_cwc_dir "${CMAKE_CURRENT_SOURCE_DIR}/../../../cortex_wire_cpp") if(EXISTS "${_cwc_dir}/CMakeLists.txt") message(STATUS "cortex_wire_cpp: using in-tree source at ${_cwc_dir}") set(CORTEX_WIRE_BUILD_TESTS OFF CACHE BOOL "" FORCE) @@ -140,12 +140,16 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) - # Skip the opinionated style linters; keep cppcheck, lint_cmake, xmllint. + # Skip the opinionated style linters; keep cppcheck and lint_cmake. + # xmllint is also disabled because it fetches the package.xml XSD over the + # network at test time, which fails on offline builders; package.xml is + # already validated structurally by ament_cmake_core at configure time. set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) set(ament_cmake_uncrustify_FOUND TRUE) set(ament_cmake_pep257_FOUND TRUE) set(ament_cmake_flake8_FOUND TRUE) + set(ament_cmake_xmllint_FOUND TRUE) ament_lint_auto_find_test_dependencies() ament_add_gtest(test_config test/test_config.cpp) diff --git a/ros2_bridge/cortex_ros2_bridge/README.md b/ros2_bridge/src/cortex_ros2_bridge/README.md similarity index 98% rename from ros2_bridge/cortex_ros2_bridge/README.md rename to ros2_bridge/src/cortex_ros2_bridge/README.md index 060eef8..73a8194 100644 --- a/ros2_bridge/cortex_ros2_bridge/README.md +++ b/ros2_bridge/src/cortex_ros2_bridge/README.md @@ -41,16 +41,10 @@ The bridge ships as two composable rclcpp nodes — one per direction — that c From a colcon workspace that contains this package under `src/`: -```bash -colcon build --packages-select cortex_ros2_bridge -source install/setup.bash -``` - -On Anaconda-based ROS containers (where the default `python3` lacks `catkin_pkg`), point CMake at the system Python: - ```bash colcon build --packages-select cortex_ros2_bridge \ --cmake-args -DPython3_EXECUTABLE=/usr/bin/python3 +source install/setup.bash ``` The package also includes the test suite: diff --git a/ros2_bridge/cortex_ros2_bridge/config/example_full.yaml b/ros2_bridge/src/cortex_ros2_bridge/config/example_full.yaml similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/config/example_full.yaml rename to ros2_bridge/src/cortex_ros2_bridge/config/example_full.yaml diff --git a/ros2_bridge/cortex_ros2_bridge/config/example_minimal.yaml b/ros2_bridge/src/cortex_ros2_bridge/config/example_minimal.yaml similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/config/example_minimal.yaml rename to ros2_bridge/src/cortex_ros2_bridge/config/example_minimal.yaml diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/arrays.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/arrays.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/arrays.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/arrays.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/image.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/image.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/image.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/image.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/oob_helpers.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/oob_helpers.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/oob_helpers.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/oob_helpers.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pointcloud.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pointcloud.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pointcloud.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pointcloud.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pose.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pose.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pose.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pose.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/tensor.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/tensor.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/tensor.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/tensor.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/transform.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/transform.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/transform.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/transform.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_to_ros2_node.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_to_ros2_node.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_to_ros2_node.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_to_ros2_node.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/ros2_to_cortex_node.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/ros2_to_cortex_node.hpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/include/cortex_ros2_bridge/ros2_to_cortex_node.hpp rename to ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/ros2_to_cortex_node.hpp diff --git a/ros2_bridge/cortex_ros2_bridge/launch/composable_container.launch.py b/ros2_bridge/src/cortex_ros2_bridge/launch/composable_container.launch.py similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/launch/composable_container.launch.py rename to ros2_bridge/src/cortex_ros2_bridge/launch/composable_container.launch.py diff --git a/ros2_bridge/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py b/ros2_bridge/src/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py rename to ros2_bridge/src/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py diff --git a/ros2_bridge/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py b/ros2_bridge/src/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py rename to ros2_bridge/src/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py diff --git a/ros2_bridge/cortex_ros2_bridge/package.xml b/ros2_bridge/src/cortex_ros2_bridge/package.xml similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/package.xml rename to ros2_bridge/src/cortex_ros2_bridge/package.xml diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/arrays.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/arrays.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/adapters/arrays.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/adapters/arrays.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/image.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/image.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/adapters/image.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/adapters/image.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/pointcloud.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/pointcloud.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/adapters/pointcloud.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/adapters/pointcloud.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/pose.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/pose.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/adapters/pose.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/adapters/pose.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/primitives.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/primitives.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/adapters/primitives.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/adapters/primitives.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/tensor.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/tensor.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/adapters/tensor.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/adapters/tensor.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/adapters/transform.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/transform.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/adapters/transform.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/adapters/transform.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/binding_factory.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/binding_factory.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/binding_factory.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/binding_factory.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/config.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/config.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/config.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/config.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/qos.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/qos.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/qos.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/qos.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/registry.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/registry.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/registry.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/registry.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp rename to ros2_bridge/src/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_binding_factory.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_binding_factory.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/test/test_binding_factory.cpp rename to ros2_bridge/src/cortex_ros2_bridge/test/test_binding_factory.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_components_e2e.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_components_e2e.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/test/test_components_e2e.cpp rename to ros2_bridge/src/cortex_ros2_bridge/test/test_components_e2e.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_config.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_config.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/test/test_config.cpp rename to ros2_bridge/src/cortex_ros2_bridge/test/test_config.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_intra_process.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_intra_process.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/test/test_intra_process.cpp rename to ros2_bridge/src/cortex_ros2_bridge/test/test_intra_process.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_primitives.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_primitives.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/test/test_primitives.cpp rename to ros2_bridge/src/cortex_ros2_bridge/test/test_primitives.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_qos.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_qos.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/test/test_qos.cpp rename to ros2_bridge/src/cortex_ros2_bridge/test/test_qos.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_registry.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_registry.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/test/test_registry.cpp rename to ros2_bridge/src/cortex_ros2_bridge/test/test_registry.cpp diff --git a/ros2_bridge/cortex_ros2_bridge/test/test_standard_adapters.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_standard_adapters.cpp similarity index 100% rename from ros2_bridge/cortex_ros2_bridge/test/test_standard_adapters.cpp rename to ros2_bridge/src/cortex_ros2_bridge/test/test_standard_adapters.cpp From 9c40e190897cad8d793d70c9eb2ca64101b1765a Mon Sep 17 00:00:00 2001 From: Richeek Das Date: Tue, 12 May 2026 17:42:05 -0400 Subject: [PATCH 10/10] add publisher and subscriber in cpp for easy cpp interfacing --- cortex_wire_cpp/CMakeLists.txt | 6 + cortex_wire_cpp/DOCS.md | 375 ++++++++++++++++ cortex_wire_cpp/README.md | 55 ++- .../cmake/cortex_wire_cppConfig.cmake.in | 11 +- .../include/cortex_wire/context.hpp | 57 +++ .../include/cortex_wire/publisher.hpp | 92 ++++ .../include/cortex_wire/subscriber.hpp | 111 +++++ cortex_wire_cpp/src/publisher.cpp | 164 +++++++ cortex_wire_cpp/src/subscriber.cpp | 158 +++++++ .../test/test_pub_sub_roundtrip.cpp | 414 ++++++++++++++++++ 10 files changed, 1422 insertions(+), 21 deletions(-) create mode 100644 cortex_wire_cpp/DOCS.md create mode 100644 cortex_wire_cpp/include/cortex_wire/context.hpp create mode 100644 cortex_wire_cpp/include/cortex_wire/publisher.hpp create mode 100644 cortex_wire_cpp/include/cortex_wire/subscriber.hpp create mode 100644 cortex_wire_cpp/src/publisher.cpp create mode 100644 cortex_wire_cpp/src/subscriber.cpp create mode 100644 cortex_wire_cpp/test/test_pub_sub_roundtrip.cpp diff --git a/cortex_wire_cpp/CMakeLists.txt b/cortex_wire_cpp/CMakeLists.txt index 367059e..04e1a70 100644 --- a/cortex_wire_cpp/CMakeLists.txt +++ b/cortex_wire_cpp/CMakeLists.txt @@ -42,9 +42,14 @@ add_library(cortex_wire src/header.cpp src/metadata.cpp src/discovery_client.cpp + src/publisher.cpp + src/subscriber.cpp ) add_library(cortex_wire::cortex_wire ALIAS cortex_wire) +find_package(Threads REQUIRED) +target_link_libraries(cortex_wire PUBLIC Threads::Threads) + target_include_directories(cortex_wire PUBLIC $ $ @@ -133,6 +138,7 @@ if(CORTEX_WIRE_BUILD_TESTS) test_oob_buffer test_discovery_client test_fingerprint_table + test_pub_sub_roundtrip ) foreach(t ${_cortex_wire_tests}) add_executable(${t} test/${t}.cpp) diff --git a/cortex_wire_cpp/DOCS.md b/cortex_wire_cpp/DOCS.md new file mode 100644 index 0000000..4ec7621 --- /dev/null +++ b/cortex_wire_cpp/DOCS.md @@ -0,0 +1,375 @@ +# cortex_wire_cpp — reference docs + +C++ implementation of the Cortex IPC stack: wire codec, discovery client, and +a small high-level pub/sub client. Standalone pure-CMake library, no ROS 2 or +Python dependency. + +This document defines what's in the library, what's missing on purpose, and +how the feature surface compares to the Python reference. + +## Contents + +- [cortex\_wire\_cpp — reference docs](#cortex_wire_cpp--reference-docs) + - [Contents](#contents) + - [Scope](#scope) + - [Wire codec primitives](#wire-codec-primitives) + - [Discovery client](#discovery-client) + - [Pub/Sub client](#pubsub-client) + - [`Context`](#context) + - [`Subscriber`](#subscriber) + - [`Publisher`](#publisher) + - [Feature parity with Python](#feature-parity-with-python) + - [OOB scope, explicit](#oob-scope-explicit) + - [Examples](#examples) + - [Inline-only primitive (zero OOB)](#inline-only-primitive-zero-oob) + - [OOB-bearing array (one numpy buffer)](#oob-bearing-array-one-numpy-buffer) + - [Bypassing discovery for tests](#bypassing-discovery-for-tests) + - [Roadmap / non-goals](#roadmap--non-goals) + +--- + +## Scope + +cortex_wire_cpp is a **deliberate subset** of the Python `cortex/core/` package +focused on the needs of plain-C++ consumers — the neurosim ROS 2 +bridge, debug tools, sub-100µs control-loop consumers, alternative-language +bridges. It does not try to replicate Python's executor, timer, or +async-event-loop model; a C++ user is expected to bring their own runtime +(rclcpp, std::thread, asio, custom scheduler). + +What it provides today: + +| Layer | Components | +|--------------|--------------------------------------------------------------------| +| Codec | `MessageHeader`, `DecodedMetadata`, `MetadataBuilder`, `OobBuffer`, `ZmqAllocator` | +| Discovery | `DiscoveryClient` (REQ/REP, sync) | +| Pub/Sub | `Context`, `Publisher`, `Subscriber` | +| Type catalog | `fingerprint_table` (generated from Python `cortex.messages.standard`) | + +## Wire codec primitives + +The cortex on-the-wire format is a ZMQ multipart message: + +``` +frame 0: topic (bytes) +frame 1: MessageHeader (24 bytes, big-endian: fp | ts_ns | seq) +frame 2: metadata (msgpack array of dataclass field values) +frame 3..N: OOB buffers (raw numpy / torch payloads, one per OOB descriptor + referenced inside the metadata) +``` + +`MessageHeader` ([header.hpp](include/cortex_wire/header.hpp)) +: 24-byte fixed header. `from_bytes` / `to_bytes` for codec, public POD layout + for direct construction. `WireDecodeError` is thrown on under-sized input. + +`DecodedMetadata` ([metadata.hpp](include/cortex_wire/metadata.hpp)) +: Owns a `msgpack::object_handle` tree decoded from the metadata frame. Random + access via `field(i)`, plus `as_oob(obj)` to recognise the + `{"__cortex_oob__": ...}` descriptor maps the Python serialiser emits when a + numpy/torch value is hoisted out of band. `walk_oob(obj, visitor)` recurses + into nested dicts/lists for adapters that need every OOB referenced from a + top-level field (e.g. DictMessage with nested arrays). + +`MetadataBuilder` ([metadata.hpp](include/cortex_wire/metadata.hpp)) +: Encoder counterpart. `pack_*` helpers for primitives; `pack_numpy_oob` / + `pack_torch_oob` write the descriptor map inline AND queue the raw buffer + for emission as the next OOB frame. `finish()` returns + `{metadata, oob_buffers}`. + +`OobBuffer` ([oob_buffer.hpp](include/cortex_wire/oob_buffer.hpp)) +: Typed read-only view into a ZMQ frame. Holds a `shared_ptr` + so the bytes outlive any caller holding the view — no raw-pointer lifetime + games. `data()`, `size()`, `size_bytes()`, `operator[]`, iterators. Used by + decoder code to access OOB payloads without copying. + +`ZmqAllocator` ([oob_buffer.hpp](include/cortex_wire/oob_buffer.hpp)) +: Stateful std-allocator that backs a `std::vector>` with + an existing ZMQ frame. Useful only when you control the vector's allocator + type. See [oob_buffer.hpp](include/cortex_wire/oob_buffer.hpp) header + comment for the value-init caveat; **not usable** with stock + `sensor_msgs/Image::data` (default allocator hard-coded into the generated + message type). + +`fingerprint_table` ([fingerprint_table.hpp](include/cortex_wire/fingerprint_table.hpp)) +: Generated map from Cortex's `cortex.messages.standard` catalogue. `find_by_name("DictMessage")` returns `{kind, fingerprint}`. Regenerate with + [scripts/gen_fingerprint_table.py](scripts/gen_fingerprint_table.py) after + upstream catalogue changes. + +## Discovery client + +`DiscoveryClient` ([discovery_client.hpp](include/cortex_wire/discovery_client.hpp)) +is a sync REQ/REP client for the Python `cortex-discovery` daemon. One +round-trip per call; sockets are recycled on REQ timeout to recover from the +stuck-after-missed-reply state ZMQ REQ falls into. Supported operations: + +- `lookup(topic_name)` — returns `std::optional` (nullopt on + NOT_FOUND). `TopicInfo` carries name, endpoint, message type, fingerprint, + publisher node id. +- `register_topic(info)` — used by publishers in their ctor. +- `unregister_topic(name)` — tolerates NOT_FOUND; used in publisher dtors. + +Low-level `encode_request` / `decode_response` are exposed for unit tests. + +## Pub/Sub client + +Three classes form the high-level client: [`Context`](include/cortex_wire/context.hpp), +[`Publisher`](include/cortex_wire/publisher.hpp), +[`Subscriber`](include/cortex_wire/subscriber.hpp). + +### `Context` + +```cpp +class Context { + public: + Context(); // owns a fresh zmq::context_t(1) + explicit Context(zmq::context_t& external); // wrap externally-owned, no dtor cleanup + zmq::context_t& raw() noexcept; +}; +``` + +Copyable, refcounted shared ownership. Last destruction runs +`shutdown()` + `close()` in the right order to unblock recv()s. + +### `Subscriber` + +```cpp +class Subscriber { + public: + struct Inbound { + MessageHeader header; + const DecodedMetadata& metadata; + const std::vector& oob_frames; // possibly empty + }; + using MessageCallback = std::function; + using ErrorCallback = std::function; + + // Direct ctor — caller has done discovery lookup or knows the endpoint. + Subscriber(Context ctx, + std::string endpoint, + std::string topic, + std::uint64_t expected_fingerprint, + MessageCallback on_message, + ErrorCallback on_error = {}); + + // Factory — does the discovery lookup, throws on not-found or fp mismatch. + static Subscriber connect(Context ctx, + DiscoveryClient& discovery, + std::string topic, + std::uint64_t expected_fingerprint, + MessageCallback on_message, + ErrorCallback on_error = {}); + + ~Subscriber(); // joins recv thread (≤100 ms) + // non-copy, non-move (thread-owning) +}; +``` + +Semantics: + +- Ctor spawns a dedicated recv thread; it is running on return. +- Always **strict fingerprint**. Messages whose wire header fingerprint + differs from `expected_fingerprint` are dropped and reported via + `on_error`. Python's "lax" mode is intentionally omitted. +- The callback runs on the recv thread, one message at a time. Exceptions + thrown by the callback are caught and forwarded to `on_error` (the recv + thread keeps running). +- `oob_frames` may be empty for inline-only payloads (primitives, all-inline + dicts). No special API path. + +### `Publisher` + +```cpp +class Publisher { + public: + Publisher(Context ctx, + DiscoveryClient& discovery, + std::string topic, + std::string cortex_type_name, // e.g. "DictMessage" + std::uint64_t fingerprint, + std::string publisher_node_id, + std::uint32_t queue_size = 1000, + std::string endpoint_prefix = "ipc:///tmp/cortex/topics/"); + + ~Publisher(); // unregisters from discovery + + // Single-threaded send. Caller serialises. + // Returns false if ZMQ HWM drop occurred (sequence advances regardless, + // matching Python). + bool publish(MetadataBuilder::Frames frames); + // non-copy, non-move (socket-owning) +}; +``` + +Semantics: + +- Ctor slugifies `{node_id}__{topic}.sock` under `endpoint_prefix`, creates + the parent directory if missing, binds the PUB socket, and registers with + the discovery daemon. Any failure throws. +- Dtor calls `unregister_topic` and swallows errors (the daemon may be down + on process exit). +- Each `publish()` injects a fresh `MessageHeader{fingerprint, ns_now, + seq++}`, then emits the multipart message. Inline-only payloads send 3 + frames; OOB-bearing payloads send 3 + N. +- ZMQ PUB is single-producer. Callers must serialise; do not call from + multiple threads. + +## Feature parity with Python + +Legend: ✅ supported, ⏭ deliberately omitted, ⚠ planned for a later phase, +n/a doesn't apply to C++. + +| Feature | Python `cortex/core/` | C++ `cortex_wire_cpp` | +|------------------------------------------------------|:---:|:---:| +| **Publisher** | | | +| Single-producer PUB | ✅ | ✅ | +| Auto-register / unregister with discovery | ✅ | ✅ | +| `auto_register=False` manual mode | ✅ | ⏭ | +| Per-publisher monotonic sequence counter | ✅ | ✅ | +| Non-blocking send (drop on HWM) | ✅ | ✅ | +| Configurable SNDHWM (queue size) | ✅ | ✅ (ctor arg) | +| Custom serialiser hooks | ✅ | ⏭ (use `MetadataBuilder` directly) | +| `publish_count` / `dropped_count` properties | ✅ | ⚠ later | +| Thread-safe `publish()` | ❌ | ❌ (same) | +| **Subscriber** | | | +| Async (event-loop) subscriber | ✅ | n/a | +| Threaded subscriber | ✅ | ✅ (default and only mode) | +| Blocking-poll `receive()` | ✅ | ⏭ | +| Discovery lookup at construct (fail-fast) | ✅ | ✅ | +| `wait_for_topic` with timeout | ✅ | ⚠ later | +| Strict fingerprint validation | ✅ | ✅ (always) | +| Lax fingerprint validation | ✅ | ⏭ | +| Drop-detection via sequence gap | ✅ | ⚠ later | +| Late-joining publisher (reconnect) | ❌ | ❌ | +| CPU affinity / SCHED_FIFO | ✅ | ⏭ (pin thread externally) | +| **Wire format / serialisation** | | | +| 24-byte big-endian header | ✅ | ✅ | +| msgpack metadata frame | ✅ | ✅ | +| OOB frames for numpy arrays | ✅ | ✅ (decoder + encoder) | +| OOB frames for torch tensors | ✅ | ✅ (decoder; encoder via `pack_torch_oob`, device round-trip best effort) | +| Inline-only payloads (primitives) | ✅ | ✅ | +| `to_bytes()` inline-only single-blob encoder | ✅ | ⏭ | +| `to_frames()` OOB-aware encoder | ✅ | ✅ (via `MetadataBuilder`) | +| Nested OOB walking (Dict / List) | ✅ | ✅ (`DecodedMetadata::walk_oob`) | +| **Runtime composition** | | | +| `Node` composition unit | ✅ | ⏭ (bring your own) | +| Async timers (`create_timer`) | ✅ | ⏭ | +| Spawned managed threads | ✅ | ⏭ | +| `run()` / `spin()` lifecycle | ✅ | ⏭ | +| Shared ZMQ context across pubs/subs | ✅ | ✅ (`Context`) | +| **Discovery** | | | +| REQ/REP client | ✅ | ✅ | +| Register / Unregister / Lookup / List / Ping | ✅ | partial — Register / Unregister / Lookup | +| Async lookup | ✅ | ⏭ (sync only) | +| Auto-reconnect on REQ timeout | ✅ | ✅ | +| **Observability** | | | +| `CORTEX_TRACE_LATENCY` ns-resolution staged timing | ✅ | ⚠ later | +| Standard logging | ✅ | via user-supplied `ErrorCallback` | +| `publish_count` / `receive_count` properties | ✅ | ⚠ later | +| Free-threaded CPython advisory | ✅ | n/a | + +## OOB scope, explicit + +**OOB frames are optional and variadic.** The library treats the multipart +message as `header + metadata + zero-or-more OOB`, never as a required +OOB-bearing protocol. + +- A `Subscriber` callback receives `oob_frames` as a (possibly empty) + `std::vector`. Decoders consume zero, one, or many. +- `Publisher::publish()` accepts a `MetadataBuilder::Frames` whose + `oob_buffers` may be empty. The publisher emits exactly 3 frames in that + case. +- Inline-only messages (`StringMessage "hello"`, `FloatMessage 3.14`, + `DictMessage {"a": 1, "b": [1,2,3]}` with no numpy fields) round-trip with + zero OOB. Nested numpy values inside a DictMessage each generate one OOB + descriptor + one frame, and `DecodedMetadata::walk_oob` recursively + enumerates them. + +So: **the C++ client is no more or less OOB-restricted than the Python +client.** The same set of `cortex.messages.standard` types round-trips +either way. + +## Examples + +### Inline-only primitive (zero OOB) + +```cpp +cortex_wire::Context ctx; +cortex_wire::DiscoveryClient disc(ctx.raw(), "ipc:///tmp/cortex/discovery.sock"); + +// Subscriber side — print each FloatMessage as it arrives. +auto sub = cortex_wire::Subscriber::connect( + ctx, disc, "battery_voltage", + cortex_wire::find_by_name("FloatMessage")->fingerprint, + [](const cortex_wire::Subscriber::Inbound& in) { + const auto& f = in.metadata.field(0); + double v = (f.type == msgpack::type::FLOAT64) ? f.via.f64 + : static_cast(f.via.u64); + std::printf("battery_voltage = %.3f V (seq=%lu)\n", + v, in.header.sequence); + }); + +// Publisher side. +cortex_wire::Publisher pub( + ctx, disc, "battery_voltage", "FloatMessage", + cortex_wire::find_by_name("FloatMessage")->fingerprint, + "/my_node"); + +cortex_wire::MetadataBuilder b(1); // one msgpack field +b.pack_double(12.7); +pub.publish(std::move(b).finish()); // 3 frames total: [topic, header, metadata] +``` + +### OOB-bearing array (one numpy buffer) + +```cpp +// Publisher: ArrayMessage with a float32 vector. +cortex_wire::MetadataBuilder b(3); // ArrayMessage = data, name, frame_id +const std::vector values{1.0F, 2.0F, 3.0F, 4.0F}; +b.pack_numpy_oob("dtype != " view( + in.oob_frames[desc->buffer_index], + /*count=*/desc->shape[0]); + for (float x : view) { /* consume */ } +} +``` + +### Bypassing discovery for tests + +The direct `Subscriber` constructor accepts an endpoint string. Useful when +you're running against a manually-bound PUB socket (e.g. unit tests; see +[test/test_pub_sub_roundtrip.cpp](test/test_pub_sub_roundtrip.cpp)). + +## Roadmap / non-goals + +Deferred (⚠ later in the table), rough priority: + +1. `Subscriber::wait_for_topic(timeout)` — poll discovery for a topic that + may register after the subscriber starts. +2. Drop-detection counters on the subscriber side, exposed via accessors. +3. `CORTEX_TRACE_LATENCY` parity (env-var-gated ns-staged timing on + recv/decode/callback). +4. Publish/receive count accessors. +5. `DiscoveryClient::list_topics()` and `ping()` wrappers (encode/decode + already supports them; just no public accessors yet). + +Out of scope indefinitely: + +- A `Node` analogue. C++ users have their own composition models; imposing a + Cortex-flavoured Node would fight every one of them. +- Late-joining publisher reconnect logic. Python doesn't do this either. +- Custom serialiser plug points. The codec is fixed at msgpack + Cortex's + OOB descriptor format; consumers can construct any payload via + `MetadataBuilder` directly. +- Async-runtime variants of Subscriber. Justified only if cortex_wire_cpp + ever embeds into asio/coroutine runtimes; the threaded model is sufficient + for rclcpp and plain-thread use. diff --git a/cortex_wire_cpp/README.md b/cortex_wire_cpp/README.md index fe00db9..e8c5448 100644 --- a/cortex_wire_cpp/README.md +++ b/cortex_wire_cpp/README.md @@ -1,44 +1,61 @@ # cortex_wire_cpp -C++ port of Cortex's IPC wire format. Standalone CMake library, no ROS 2 dependency. +C++ implementation of the Cortex IPC stack: wire codec, discovery client, and +a small pub/sub client. Standalone pure-CMake library, no ROS 2 dependency. -Used by the ROS 2 bridge ([ros2_bridge/cortex_ros2_bridge](../ros2_bridge/cortex_ros2_bridge)) but designed for any C++ consumer that wants to read or write Cortex's pub/sub IPC format: debug tools (LCM-spy style), other language bridges, or sub-100µs control-loop consumers that don't want to go through Python. +Used by the ROS 2 bridge +([`ros2_bridge/cortex_ros2_bridge`](../ros2_bridge/cortex_ros2_bridge)) but +designed for any C++ consumer that needs to read or write Cortex's pub/sub +IPC format — debug tools, alternative-language bridges, sub-100µs control +loops that don't want to go through Python. -## What it provides +## What's inside -- `cortex_wire::MessageHeader` — 24-byte big-endian fingerprint/timestamp/sequence header. -- `cortex_wire::DecodedMetadata` — msgpack-cxx decoder for the metadata frame, with `OobDescriptor` parsing for numpy/torch out-of-band buffers. -- `cortex_wire::OobBuffer` — zero-copy read-only view into a ZMQ frame. -- `cortex_wire::ZmqAllocator` — std-allocator that backs a `std::vector` with a ZMQ frame's buffer (zero allocation; not zero-copy for content — see header for caveats). -- `cortex_wire::DiscoveryClient` — sync REQ/REP client for the discovery daemon. -- `cortex_wire::fingerprint_table` — generated map from Cortex's standard catalogue. +| Layer | Components | +|--------------|-------------------------------------------------------------------------| +| Codec | `MessageHeader`, `DecodedMetadata`, `MetadataBuilder`, `OobBuffer` | +| Discovery | `DiscoveryClient` (REQ/REP, sync) | +| Pub/Sub | `Context`, `Publisher`, `Subscriber` | +| Type catalog | `fingerprint_table` (generated from Python `cortex.messages.standard`) | -Layout: +See **[DOCS.md](DOCS.md)** for the full API surface, Python feature-parity +table, OOB-scope explainer, and examples. + +## Layout ``` cortex_wire_cpp/ ├── CMakeLists.txt +├── DOCS.md ├── include/cortex_wire/ │ ├── header.hpp │ ├── metadata.hpp │ ├── oob_buffer.hpp │ ├── discovery_client.hpp -│ └── fingerprint_table.hpp ← auto-generated +│ ├── context.hpp ← shared ZMQ context handle +│ ├── publisher.hpp ← PUB + discovery register +│ ├── subscriber.hpp ← SUB recv thread + fingerprint check +│ └── fingerprint_table.hpp ← auto-generated ├── src/ │ ├── header.cpp │ ├── metadata.cpp -│ └── discovery_client.cpp +│ ├── discovery_client.cpp +│ ├── publisher.cpp +│ └── subscriber.cpp ├── scripts/ -│ └── gen_fingerprint_table.py ← regen tool +│ └── gen_fingerprint_table.py ← regen tool ├── cmake/ │ └── cortex_wire_cppConfig.cmake.in -└── test/ ← 5 gtest binaries +└── test/ ← 6 gtest binaries ``` ## Dependencies - `cppzmq` — header-only C++ wrapper over `libzmq` (system). -- `msgpack-cxx` — header-only msgpack C++ library. On Ubuntu 22.04 install `libmsgpack-dev`. Newer distros provide a CMake config under `msgpack-cxx` which is picked up automatically. +- `msgpack-cxx` — header-only msgpack C++ library. On Ubuntu 22.04 install + `libmsgpack-dev`. Newer distros provide a CMake config under `msgpack-cxx` + which is picked up automatically. +- POSIX threads (linked publicly; the pub/sub client owns a recv thread). - C++17. ## Build & install @@ -61,11 +78,15 @@ add_executable(my_tool my_tool.cpp) target_link_libraries(my_tool PRIVATE cortex_wire::cortex_wire) ``` -For monorepo consumers (e.g. `ros2_bridge`) the same target is available via `add_subdirectory(../../cortex_wire_cpp ...)` — no install step needed for dev iteration. +For monorepo consumers (e.g. `ros2_bridge`) the same target is available via +`add_subdirectory(../../cortex_wire_cpp ...)` — no install step needed for +dev iteration. ## Regenerating the fingerprint table -`include/cortex_wire/fingerprint_table.hpp` is committed so the library builds without a Python `cortex` install. Regenerate after any change to `cortex.messages.standard`: +`include/cortex_wire/fingerprint_table.hpp` is committed so the library +builds without a Python `cortex` install. Regenerate after any change to +`cortex.messages.standard`: ```bash python3 -m venv /tmp/cortex_venv diff --git a/cortex_wire_cpp/cmake/cortex_wire_cppConfig.cmake.in b/cortex_wire_cpp/cmake/cortex_wire_cppConfig.cmake.in index ffee709..01e8b49 100644 --- a/cortex_wire_cpp/cmake/cortex_wire_cppConfig.cmake.in +++ b/cortex_wire_cpp/cmake/cortex_wire_cppConfig.cmake.in @@ -3,10 +3,13 @@ include(CMakeFindDependencyMacro) find_dependency(cppzmq) -find_dependency(msgpack-cxx QUIET) -# msgpack-cxx may be header-only with no CMake config (Ubuntu 22.04). In that -# case the target's public include directory has already been baked into the -# install interface so consumers do not need to re-find it. +find_dependency(Threads) +# Plain find_package (CONFIG mode, QUIET) so REQUIRED is not propagated from +# the consumer's find_package(cortex_wire_cpp REQUIRED) call. Ubuntu 22.04's +# libmsgpack-dev ships header-only without a CMake config — the target's +# install-interface include directory carries the msgpack headers in that +# case, so consumers don't need to re-find them. +find_package(msgpack-cxx QUIET CONFIG) include("${CMAKE_CURRENT_LIST_DIR}/cortex_wire_cppTargets.cmake") diff --git a/cortex_wire_cpp/include/cortex_wire/context.hpp b/cortex_wire_cpp/include/cortex_wire/context.hpp new file mode 100644 index 0000000..c3555f5 --- /dev/null +++ b/cortex_wire_cpp/include/cortex_wire/context.hpp @@ -0,0 +1,57 @@ +#ifndef CORTEX_WIRE__CONTEXT_HPP_ +#define CORTEX_WIRE__CONTEXT_HPP_ + +#include + +#include + +namespace cortex_wire +{ + +// Shared ownership wrapper around `zmq::context_t`. Copying a Context is cheap +// (a refcount bump) and lets multiple Publisher / Subscriber instances share +// the same underlying ZMQ context. +// +// The context lives as long as any Context copy referencing it does. On the +// last destruction we run shutdown + close in the right order to unblock any +// recv()s and tear the context down cleanly. +class Context +{ +public: + // Construct a fresh context with one IO thread (matches the Python default). + Context() + : ctx_(std::shared_ptr( + new zmq::context_t(1), + [](zmq::context_t * c) { + c->shutdown(); + c->close(); + delete c; + })) + { + } + + // Wrap an externally-owned context. The Context will NOT shutdown / close + // the underlying context on destruction; that remains the caller's job. + // Useful for interop with code that already owns a zmq::context_t. + explicit Context(zmq::context_t & external) noexcept + : ctx_(std::shared_ptr(&external, [](zmq::context_t *) {})) + { + } + + // Copyable and movable. + Context(const Context &) = default; + Context(Context &&) noexcept = default; + Context & operator=(const Context &) = default; + Context & operator=(Context &&) noexcept = default; + ~Context() = default; + + zmq::context_t & raw() noexcept {return *ctx_;} + const zmq::context_t & raw() const noexcept {return *ctx_;} + +private: + std::shared_ptr ctx_; +}; + +} // namespace cortex_wire + +#endif // CORTEX_WIRE__CONTEXT_HPP_ diff --git a/cortex_wire_cpp/include/cortex_wire/publisher.hpp b/cortex_wire_cpp/include/cortex_wire/publisher.hpp new file mode 100644 index 0000000..0865673 --- /dev/null +++ b/cortex_wire_cpp/include/cortex_wire/publisher.hpp @@ -0,0 +1,92 @@ +#ifndef CORTEX_WIRE__PUBLISHER_HPP_ +#define CORTEX_WIRE__PUBLISHER_HPP_ + +#include +#include +#include + +#include + +#include "cortex_wire/context.hpp" +#include "cortex_wire/discovery_client.hpp" +#include "cortex_wire/metadata.hpp" + +namespace cortex_wire +{ + +// PUB-side cortex client. Single-producer (ZMQ PUB is inherently +// single-producer; callers must serialize publish() calls). +// +// Lifetime: +// - ctor builds a deterministic ipc:// endpoint under `endpoint_prefix`, +// creates the parent directory if missing, binds a PUB socket, and +// registers the topic with the discovery daemon. +// - dtor unregisters the topic and closes the socket. +// +// Each publish() injects a fresh header (fp, monotonic seq, timestamp_ns) +// and emits a multipart message: [topic, header, metadata, *oob_buffers]. +// Inline-only messages (oob_buffers empty) send 3 frames; OOB-bearing +// messages send 3 + N. +class Publisher +{ +public: + // Throws std::runtime_error on bind / mkdir / discovery-register failure. + Publisher( + Context ctx, + DiscoveryClient & discovery, + std::string topic, + std::string cortex_type_name, + std::uint64_t fingerprint, + std::string publisher_node_id, + std::uint32_t queue_size = 1000, + std::string endpoint_prefix = "ipc:///tmp/cortex/topics/"); + + ~Publisher(); + + Publisher(const Publisher &) = delete; + Publisher & operator=(const Publisher &) = delete; + Publisher(Publisher &&) = delete; + Publisher & operator=(Publisher &&) = delete; + + // Send one message. The caller is responsible for serialising calls. + // + // Returns true on send, false if ZMQ reported the message was dropped + // (e.g. queue overflow under NOBLOCK semantics, which is the default for + // PUB sockets — same behaviour as the Python client). The sequence + // counter advances on every call regardless of return value, mirroring + // Python. + bool publish(MetadataBuilder::Frames frames); + + const std::string & topic() const noexcept {return topic_;} + const std::string & endpoint() const noexcept {return endpoint_;} + const std::string & cortex_type_name() const noexcept {return cortex_type_;} + std::uint64_t fingerprint() const noexcept {return fingerprint_;} + std::uint64_t sequence() const noexcept + { + return sequence_.load(std::memory_order_relaxed); + } + +private: + static std::string slugify(std::string s); + static std::string build_endpoint( + const std::string & prefix, + const std::string & node_id, + const std::string & topic); + static void ensure_parent_dir(const std::string & endpoint); + + Context ctx_; // shared ownership of the ZMQ context + DiscoveryClient * discovery_; // non-owning; caller outlives Publisher + std::string topic_; + std::string endpoint_; + std::string cortex_type_; + std::uint64_t fingerprint_; + std::string node_id_; + + zmq::socket_t sock_; + std::atomic sequence_{0}; + bool registered_ = false; +}; + +} // namespace cortex_wire + +#endif // CORTEX_WIRE__PUBLISHER_HPP_ diff --git a/cortex_wire_cpp/include/cortex_wire/subscriber.hpp b/cortex_wire_cpp/include/cortex_wire/subscriber.hpp new file mode 100644 index 0000000..cc591b3 --- /dev/null +++ b/cortex_wire_cpp/include/cortex_wire/subscriber.hpp @@ -0,0 +1,111 @@ +#ifndef CORTEX_WIRE__SUBSCRIBER_HPP_ +#define CORTEX_WIRE__SUBSCRIBER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "cortex_wire/context.hpp" +#include "cortex_wire/discovery_client.hpp" +#include "cortex_wire/header.hpp" +#include "cortex_wire/metadata.hpp" +#include "cortex_wire/oob_buffer.hpp" + +namespace cortex_wire +{ + +// Threaded SUB-side cortex client. Owns a ZMQ SUB socket and a recv thread +// that decodes incoming multipart messages (header + msgpack metadata + zero +// or more OOB frames) and delivers them to the user callback. +// +// Lifetime: +// - ctor opens the SUB socket, subscribes to `topic`, spawns the recv +// thread. The thread is running on return. +// - dtor signals the thread to stop and joins it. Any in-flight callback +// completes before the dtor returns. +// +// Threading: the user `on_message` callback is invoked on the recv thread, +// one message at a time. If the callback throws, the exception is caught +// and forwarded to `on_error` (which defaults to silent); the recv thread +// keeps running. +// +// Fingerprint policy: always strict. Messages whose wire header fingerprint +// differs from `expected_fingerprint` are dropped and reported via on_error. +// +// Inline-only payloads (no OOB descriptors at all) are handled transparently: +// the callback receives an empty `oob_frames` vector. +class Subscriber +{ +public: + struct Inbound + { + MessageHeader header; + const DecodedMetadata & metadata; + const std::vector & oob_frames; + }; + + using MessageCallback = std::function; + using ErrorCallback = std::function; + + // Direct constructor. Caller has already done the discovery lookup and + // owns the endpoint + fingerprint contract. + // + // Throws std::runtime_error if connecting to `endpoint` fails. Otherwise + // the recv thread is running on return. + Subscriber( + Context ctx, + std::string endpoint, + std::string topic, + std::uint64_t expected_fingerprint, + MessageCallback on_message, + ErrorCallback on_error = {}); + + // Convenience factory: look up the topic via the supplied DiscoveryClient + // and delegate to the direct constructor. Throws if the topic isn't + // registered or its registered fingerprint doesn't match. + static Subscriber connect( + Context ctx, + DiscoveryClient & discovery, + std::string topic, + std::uint64_t expected_fingerprint, + MessageCallback on_message, + ErrorCallback on_error = {}); + + ~Subscriber(); + + Subscriber(const Subscriber &) = delete; + Subscriber & operator=(const Subscriber &) = delete; + // Non-movable: the recv thread captures `this` via the running_ flag and + // socket reference. Wrap in std::unique_ptr if you need to move ownership. + Subscriber(Subscriber &&) = delete; + Subscriber & operator=(Subscriber &&) = delete; + + const std::string & topic() const noexcept {return topic_;} + const std::string & endpoint() const noexcept {return endpoint_;} + std::uint64_t expected_fingerprint() const noexcept {return expected_fp_;} + +private: + void recv_loop(); + void report_error(std::string what); + + Context ctx_; // shared ownership of the ZMQ context + std::string endpoint_; + std::string topic_; + std::uint64_t expected_fp_; + MessageCallback on_message_; + ErrorCallback on_error_; + + zmq::socket_t sock_; + std::atomic running_{false}; + std::thread thread_; +}; + +} // namespace cortex_wire + +#endif // CORTEX_WIRE__SUBSCRIBER_HPP_ diff --git a/cortex_wire_cpp/src/publisher.cpp b/cortex_wire_cpp/src/publisher.cpp new file mode 100644 index 0000000..4ce1741 --- /dev/null +++ b/cortex_wire_cpp/src/publisher.cpp @@ -0,0 +1,164 @@ +#include "cortex_wire/publisher.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cortex_wire/header.hpp" + +namespace cortex_wire +{ + +namespace +{ + +constexpr std::string_view kIpcPrefix = "ipc://"; + +} // namespace + +std::string Publisher::slugify(std::string s) +{ + for (auto & c : s) { + if (!std::isalnum(static_cast(c)) && c != '-' && c != '_') { + c = '_'; + } + } + return s; +} + +std::string Publisher::build_endpoint( + const std::string & prefix, + const std::string & node_id, + const std::string & topic) +{ + // Mirrors the slugging convention the Python publisher and the + // ros2 bridge already use: __.sock when + // prefix ends with a directory separator, else __. + std::string ep = prefix; + if (!ep.empty() && ep.back() != '/' && ep.back() != '_') { + ep.push_back('_'); + } + ep += slugify(node_id); + ep += "__"; + ep += slugify(topic); + if (prefix.rfind(kIpcPrefix, 0) == 0) { + ep += ".sock"; + } + return ep; +} + +void Publisher::ensure_parent_dir(const std::string & endpoint) +{ + if (endpoint.rfind(kIpcPrefix, 0) != 0) { + return; // tcp:// etc — no fs needed + } + std::filesystem::path p(endpoint.substr(kIpcPrefix.size())); + std::error_code ec; + std::filesystem::create_directories(p.parent_path(), ec); + if (ec) { + throw std::runtime_error( + "cortex_wire::Publisher: cannot create parent dir for " + + endpoint + ": " + ec.message()); + } +} + +Publisher::Publisher( + Context ctx, + DiscoveryClient & discovery, + std::string topic, + std::string cortex_type_name, + std::uint64_t fingerprint, + std::string publisher_node_id, + std::uint32_t queue_size, + std::string endpoint_prefix) +: ctx_(std::move(ctx)), + discovery_(&discovery), + topic_(std::move(topic)), + cortex_type_(std::move(cortex_type_name)), + fingerprint_(fingerprint), + node_id_(std::move(publisher_node_id)), + sock_(ctx_.raw(), zmq::socket_type::pub) +{ + endpoint_ = build_endpoint(endpoint_prefix, node_id_, topic_); + ensure_parent_dir(endpoint_); + + sock_.set(zmq::sockopt::linger, 0); + sock_.set(zmq::sockopt::sndhwm, static_cast(queue_size)); + try { + sock_.bind(endpoint_); + } catch (const zmq::error_t & e) { + throw std::runtime_error( + "cortex_wire::Publisher: bind(" + endpoint_ + ") failed: " + + e.what()); + } + + TopicInfo info{topic_, endpoint_, cortex_type_, fingerprint_, node_id_}; + try { + discovery_->register_topic(info); + registered_ = true; + } catch (const std::exception & e) { + // The bind succeeded; close the socket on the way out. + sock_.close(); + throw std::runtime_error( + std::string("cortex_wire::Publisher: discovery register('") + + topic_ + "') failed: " + e.what()); + } +} + +Publisher::~Publisher() +{ + if (registered_ && discovery_) { + try { + discovery_->unregister_topic(topic_); + } catch (...) { + // Swallow — we are in a destructor; the daemon may also be down on + // process exit. Leaking a stale entry is preferable to terminate(). + } + } +} + +bool Publisher::publish(MetadataBuilder::Frames frames) +{ + // Build the wire header. + const std::uint64_t seq = sequence_.fetch_add(1, std::memory_order_relaxed); + const std::uint64_t now_ns = static_cast( + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count()); + MessageHeader hdr{fingerprint_, now_ns, seq}; + + std::array hdr_bytes{}; + hdr.to_bytes(hdr_bytes.data()); + + // Multipart layout: [topic, header, metadata, *oob_buffers] + const std::size_t frame_count = 3 + frames.oob_buffers.size(); + std::size_t i = 0; + bool all_ok = true; + + auto send_one = [&](const void * data, std::size_t size) { + zmq::message_t m(size); + std::memcpy(m.data(), data, size); + const auto flags = (i + 1 < frame_count) ? + (zmq::send_flags::sndmore | zmq::send_flags::dontwait) : + zmq::send_flags::dontwait; + const auto r = sock_.send(m, flags); + if (!r) { + all_ok = false; + } + ++i; + }; + + send_one(topic_.data(), topic_.size()); + send_one(hdr_bytes.data(), hdr_bytes.size()); + send_one(frames.metadata.data(), frames.metadata.size()); + for (const auto & buf : frames.oob_buffers) { + send_one(buf.data(), buf.size()); + } + return all_ok; +} + +} // namespace cortex_wire diff --git a/cortex_wire_cpp/src/subscriber.cpp b/cortex_wire_cpp/src/subscriber.cpp new file mode 100644 index 0000000..cb188a4 --- /dev/null +++ b/cortex_wire_cpp/src/subscriber.cpp @@ -0,0 +1,158 @@ +#include "cortex_wire/subscriber.hpp" + +#include +#include + +namespace cortex_wire +{ + +namespace +{ + +// 100 ms wakeup gives the dtor a bounded time to stop the recv thread. +constexpr int kRecvTimeoutMs = 100; + +} // namespace + +Subscriber::Subscriber( + Context ctx, + std::string endpoint, + std::string topic, + std::uint64_t expected_fingerprint, + MessageCallback on_message, + ErrorCallback on_error) +: ctx_(std::move(ctx)), + endpoint_(std::move(endpoint)), + topic_(std::move(topic)), + expected_fp_(expected_fingerprint), + on_message_(std::move(on_message)), + on_error_(std::move(on_error)), + sock_(ctx_.raw(), zmq::socket_type::sub) +{ + if (!on_message_) { + throw std::invalid_argument("cortex_wire::Subscriber: on_message is empty"); + } + sock_.set(zmq::sockopt::linger, 0); + sock_.set(zmq::sockopt::rcvtimeo, kRecvTimeoutMs); + try { + sock_.connect(endpoint_); + } catch (const zmq::error_t & e) { + throw std::runtime_error( + "cortex_wire::Subscriber: connect(" + endpoint_ + ") failed: " + + e.what()); + } + sock_.set(zmq::sockopt::subscribe, topic_); + + running_.store(true); + thread_ = std::thread(&Subscriber::recv_loop, this); +} + +Subscriber Subscriber::connect( + Context ctx, + DiscoveryClient & discovery, + std::string topic, + std::uint64_t expected_fingerprint, + MessageCallback on_message, + ErrorCallback on_error) +{ + auto info = discovery.lookup(topic); + if (!info) { + throw std::runtime_error( + "cortex_wire::Subscriber::connect: topic '" + topic + + "' is not registered with discovery"); + } + if (info->fingerprint != expected_fingerprint) { + throw std::runtime_error( + "cortex_wire::Subscriber::connect: fingerprint mismatch for topic '" + + topic + "' (daemon reports a different message type than expected)"); + } + return Subscriber( + std::move(ctx), + std::move(info->address), + std::move(topic), + expected_fingerprint, + std::move(on_message), + std::move(on_error)); +} + +Subscriber::~Subscriber() +{ + if (running_.exchange(false)) { + if (thread_.joinable()) { + thread_.join(); + } + } +} + +void Subscriber::report_error(std::string what) +{ + if (on_error_) { + on_error_(what); + } +} + +void Subscriber::recv_loop() +{ + std::vector frames; + while (running_.load(std::memory_order_relaxed)) { + frames.clear(); + + // Pull a full multipart message. First recv may time out (so we can + // notice running_); subsequent ones use the same timeout, which is fine + // because frames within a single multipart arrive together. + bool got_one = false; + while (true) { + zmq::message_t f; + zmq::recv_result_t r; + try { + r = sock_.recv(f, zmq::recv_flags::none); + } catch (const zmq::error_t & e) { + if (e.num() == ETERM) {return;} + report_error(std::string("recv: ") + e.what()); + break; + } + if (!r) {break;} // timeout, retry outer loop + const bool has_more = f.more(); + frames.emplace_back(std::move(f)); + got_one = true; + if (!has_more) {break;} + } + if (!got_one) {continue;} + + // [topic, header, metadata, *oob] + if (frames.size() < 3) { + report_error("short message: <3 frames"); + continue; + } + + try { + const auto header = MessageHeader::from_bytes( + frames[1].data(), frames[1].size()); + if (header.fingerprint != expected_fp_) { + report_error("fingerprint mismatch on wire — dropping"); + continue; + } + const auto metadata = DecodedMetadata::from_bytes( + frames[2].data(), frames[2].size()); + + std::vector oob; + oob.reserve(frames.size() > 3 ? frames.size() - 3 : 0); + for (std::size_t i = 3; i < frames.size(); ++i) { + oob.push_back(make_owned(std::move(frames[i]))); + } + + const Inbound in{header, metadata, oob}; + try { + on_message_(in); + } catch (const std::exception & e) { + report_error(std::string("callback: ") + e.what()); + } catch (...) { + report_error("callback: unknown exception"); + } + } catch (const std::exception & e) { + report_error(std::string("decode: ") + e.what()); + } + } +} + +} // namespace cortex_wire diff --git a/cortex_wire_cpp/test/test_pub_sub_roundtrip.cpp b/cortex_wire_cpp/test/test_pub_sub_roundtrip.cpp new file mode 100644 index 0000000..01baf91 --- /dev/null +++ b/cortex_wire_cpp/test/test_pub_sub_roundtrip.cpp @@ -0,0 +1,414 @@ +// End-to-end Publisher/Subscriber round-trip test. +// +// Two modes of operation are covered: +// 1. Direct constructor (no discovery) — Subscriber connects to a known +// endpoint, exercises the recv-thread, fingerprint check, and OOB-bearing +// payload. +// 2. Convenience factory `Subscriber::connect()` — backed by a tiny mock +// discovery REP daemon, exercises the lookup-via-discovery path. +// +// We deliberately mock the discovery daemon in-process rather than depending +// on `cortex-discovery` being on PATH. +#include "cortex_wire/context.hpp" +#include "cortex_wire/discovery_client.hpp" +#include "cortex_wire/metadata.hpp" +#include "cortex_wire/publisher.hpp" +#include "cortex_wire/subscriber.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +using cortex_wire::Context; +using cortex_wire::DiscoveryClient; +using cortex_wire::DiscoveryCommand; +using cortex_wire::DiscoveryStatus; +using cortex_wire::MetadataBuilder; +using cortex_wire::Publisher; +using cortex_wire::Subscriber; +using cortex_wire::TopicInfo; + +// A tiny in-process discovery daemon. Speaks the same REQ/REP protocol the +// real daemon does, just enough for register + lookup. Multi-request loop so +// it can serve a register-then-lookup sequence. +class MockDaemon +{ +public: + MockDaemon(zmq::context_t & ctx, const std::string & address) + : socket_(ctx, zmq::socket_type::rep) + { + socket_.set(zmq::sockopt::linger, 0); + socket_.set(zmq::sockopt::rcvtimeo, 100); + socket_.set(zmq::sockopt::sndtimeo, 2000); + socket_.bind(address); + running_.store(true); + thread_ = std::thread(&MockDaemon::loop, this); + } + + ~MockDaemon() + { + running_.store(false); + if (thread_.joinable()) {thread_.join();} + } + + std::optional get(const std::string & name) + { + std::lock_guard lk(mu_); + auto it = topics_.find(name); + if (it == topics_.end()) {return std::nullopt;} + return it->second; + } + +private: + void loop() + { + while (running_.load()) { + zmq::message_t req; + auto r = socket_.recv(req, zmq::recv_flags::none); + if (!r) {continue;} + respond(req); + } + } + + void respond(const zmq::message_t & req) + { + msgpack::object_handle oh; + try { + oh = msgpack::unpack(static_cast(req.data()), req.size()); + } catch (...) { + send_status(DiscoveryStatus::Error, "bad msgpack"); + return; + } + const auto & root = oh.get(); + if (root.type != msgpack::type::MAP) { + send_status(DiscoveryStatus::Error, "not a map"); + return; + } + + std::int32_t cmd = 0; + std::string topic_name; + std::optional info; + for (std::uint32_t i = 0; i < root.via.map.size; ++i) { + const auto & k = root.via.map.ptr[i].key; + const auto & v = root.via.map.ptr[i].val; + std::string_view key(k.via.str.ptr, k.via.str.size); + if (key == "command") {cmd = static_cast(v.via.i64);} + if (key == "topic_name" && v.type == msgpack::type::STR) { + topic_name.assign(v.via.str.ptr, v.via.str.size); + } + if (key == "topic_info" && v.type == msgpack::type::BIN) { + info = decode_topic_info(v.via.bin.ptr, v.via.bin.size); + } + } + + if (cmd == static_cast(DiscoveryCommand::RegisterTopic) && info) { + std::lock_guard lk(mu_); + topics_[info->name] = *info; + send_status(DiscoveryStatus::Ok, "ok"); + return; + } + if (cmd == static_cast(DiscoveryCommand::UnregisterTopic)) { + std::lock_guard lk(mu_); + topics_.erase(topic_name); + send_status(DiscoveryStatus::Ok, "ok"); + return; + } + if (cmd == static_cast(DiscoveryCommand::LookupTopic)) { + std::optional found; + { + std::lock_guard lk(mu_); + auto it = topics_.find(topic_name); + if (it != topics_.end()) {found = it->second;} + } + if (!found) { + send_status(DiscoveryStatus::NotFound, "topic not registered"); + } else { + send_topic_info(DiscoveryStatus::Ok, "ok", *found); + } + return; + } + send_status(DiscoveryStatus::Error, "unsupported command"); + } + + static TopicInfo decode_topic_info(const char * data, std::size_t size) + { + auto oh = msgpack::unpack(data, size); + const auto & m = oh.get(); + TopicInfo info{}; + for (std::uint32_t i = 0; i < m.via.map.size; ++i) { + const auto & k = m.via.map.ptr[i].key; + const auto & v = m.via.map.ptr[i].val; + std::string_view key(k.via.str.ptr, k.via.str.size); + if (key == "name") {info.name.assign(v.via.str.ptr, v.via.str.size);} + else if (key == "address") {info.address.assign(v.via.str.ptr, v.via.str.size);} + else if (key == "message_type") {info.message_type.assign(v.via.str.ptr, v.via.str.size);} + else if (key == "fingerprint") {info.fingerprint = v.via.u64;} + else if (key == "publisher_node") {info.publisher_node.assign(v.via.str.ptr, v.via.str.size);} + } + return info; + } + + void send_status(DiscoveryStatus status, const std::string & msg) + { + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_map(2); + pk.pack(std::string("status")); pk.pack(static_cast(status)); + pk.pack(std::string("message")); pk.pack(msg); + zmq::message_t out(buf.data(), buf.size()); + (void)socket_.send(out, zmq::send_flags::none); + } + + void send_topic_info(DiscoveryStatus status, const std::string & msg, + const TopicInfo & info) + { + msgpack::sbuffer info_buf; + msgpack::packer info_pk(info_buf); + info_pk.pack_map(5); + info_pk.pack(std::string("name")); info_pk.pack(info.name); + info_pk.pack(std::string("address")); info_pk.pack(info.address); + info_pk.pack(std::string("message_type")); info_pk.pack(info.message_type); + info_pk.pack(std::string("fingerprint")); info_pk.pack(info.fingerprint); + info_pk.pack(std::string("publisher_node")); info_pk.pack(info.publisher_node); + + msgpack::sbuffer buf; + msgpack::packer pk(buf); + pk.pack_map(3); + pk.pack(std::string("status")); pk.pack(static_cast(status)); + pk.pack(std::string("message")); pk.pack(msg); + pk.pack(std::string("topic_info")); + pk.pack_bin(static_cast(info_buf.size())); + pk.pack_bin_body(info_buf.data(), info_buf.size()); + + zmq::message_t out(buf.data(), buf.size()); + (void)socket_.send(out, zmq::send_flags::none); + } + + zmq::socket_t socket_; + std::atomic running_{false}; + std::thread thread_; + std::mutex mu_; + std::unordered_map topics_; +}; + +// Pump messages until the predicate is true or we hit a timeout. +template +bool wait_for(Pred pred, std::chrono::milliseconds timeout = std::chrono::milliseconds(2000)) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) { + if (pred()) {return true;} + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + return pred(); +} + +std::string unique_inproc(const std::string & prefix) +{ + static std::atomic counter{0}; + return "inproc://" + prefix + "_" + std::to_string(counter.fetch_add(1)); +} + +std::string unique_ipc_dir() +{ + static std::atomic counter{0}; + auto p = std::filesystem::temp_directory_path() / + ("cortex_wire_test_" + std::to_string(counter.fetch_add(1))); + std::filesystem::create_directories(p); + return "ipc://" + p.string() + "/"; +} + +} // namespace + +// Direct ctor — bypasses discovery, exercises just the recv path. We bind a +// PUB socket manually instead of going through Publisher so we can isolate +// the subscriber under test. +TEST(SubscriberDirect, ReceivesMultiPartFrameAndDecodesHeader) +{ + Context ctx; + const auto endpoint = unique_inproc("subdirect"); + + zmq::socket_t pub(ctx.raw(), zmq::socket_type::pub); + pub.set(zmq::sockopt::linger, 0); + pub.bind(endpoint); + + std::atomic seen{0}; + std::atomic last_seq{0}; + Subscriber sub( + ctx, endpoint, "topic", /*fingerprint=*/0xabcdef0011223344ULL, + [&](const Subscriber::Inbound & in) { + last_seq.store(in.header.sequence, std::memory_order_relaxed); + seen.fetch_add(1, std::memory_order_relaxed); + }); + + // Give the SUB time to subscribe before we publish. inproc transport + // doesn't have late-joiner semantics; missed messages are dropped. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + // Pack a 1-field DictMessage-ish payload (a single str). The metadata + // body shape doesn't matter for this test — we just verify the framing + // and header reach the callback. + MetadataBuilder b(1); + b.pack_str("hello"); + auto frames = std::move(b).finish(); + + // Build the wire header manually so we can verify the sequence the + // subscriber sees. + cortex_wire::MessageHeader hdr{0xabcdef0011223344ULL, 1'700'000'000ULL, 7ULL}; + std::array hdr_bytes{}; + hdr.to_bytes(hdr_bytes.data()); + + auto send_frame = [&](const void * data, std::size_t size, bool more) { + zmq::message_t m(size); + std::memcpy(m.data(), data, size); + pub.send(m, more ? zmq::send_flags::sndmore : zmq::send_flags::none); + }; + const std::string topic = "topic"; + send_frame(topic.data(), topic.size(), true); + send_frame(hdr_bytes.data(), hdr_bytes.size(), true); + send_frame(frames.metadata.data(), frames.metadata.size(), false); + + ASSERT_TRUE(wait_for([&]() {return seen.load() >= 1;})); + EXPECT_EQ(last_seq.load(), 7u); +} + +TEST(SubscriberDirect, DropsMessagesWithMismatchedFingerprint) +{ + Context ctx; + const auto endpoint = unique_inproc("subfpmismatch"); + + zmq::socket_t pub(ctx.raw(), zmq::socket_type::pub); + pub.set(zmq::sockopt::linger, 0); + pub.bind(endpoint); + + std::atomic delivered{0}; + std::atomic errors{0}; + Subscriber sub( + ctx, endpoint, "topic", /*expected_fp=*/0xAAAAAAAAAAAAAAAAULL, + [&](const Subscriber::Inbound &) {delivered.fetch_add(1);}, + [&](std::string_view) {errors.fetch_add(1);}); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + MetadataBuilder b(1); + b.pack_int(42); + auto frames = std::move(b).finish(); + + // Wrong fingerprint on the wire. + cortex_wire::MessageHeader hdr{0xBBBBBBBBBBBBBBBBULL, 0ULL, 0ULL}; + std::array hdr_bytes{}; + hdr.to_bytes(hdr_bytes.data()); + + auto send_frame = [&](const void * data, std::size_t size, bool more) { + zmq::message_t m(size); + std::memcpy(m.data(), data, size); + pub.send(m, more ? zmq::send_flags::sndmore : zmq::send_flags::none); + }; + const std::string topic = "topic"; + send_frame(topic.data(), topic.size(), true); + send_frame(hdr_bytes.data(), hdr_bytes.size(), true); + send_frame(frames.metadata.data(), frames.metadata.size(), false); + + // Wait a bit longer than the rcvtimeo so the wire pump has run. + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + EXPECT_EQ(delivered.load(), 0); + EXPECT_GE(errors.load(), 1); +} + +// End-to-end via discovery — Publisher registers, Subscriber::connect() +// looks up, messages round-trip including the OOB-bearing case (one numpy- +// shaped buffer) and the inline-only case (primitive-style payload). +TEST(PubSubViaDiscovery, RoundTripsInlineAndOobMessages) +{ + Context ctx; + const auto daemon_addr = unique_inproc("daemon"); + MockDaemon daemon(ctx.raw(), daemon_addr); + + DiscoveryClient discovery(ctx.raw(), daemon_addr); + const std::uint64_t fp = 0xDECAFBADCAFEC0DEULL; + + // Use a dedicated ipc:// dir so the bound socket doesn't collide with the + // host's real /tmp/cortex/topics state. + const auto ipc_prefix = unique_ipc_dir(); + + Publisher pub( + ctx, discovery, /*topic=*/"unit_test_topic", + /*cortex_type_name=*/"DictMessage", /*fingerprint=*/fp, + /*publisher_node_id=*/"test_node", + /*queue_size=*/100, + ipc_prefix); + + std::mutex mu; + std::condition_variable cv; + std::vector seqs; + std::vector oob_counts; + + auto sub = Subscriber::connect( + ctx, discovery, "unit_test_topic", fp, + [&](const Subscriber::Inbound & in) { + std::lock_guard lk(mu); + seqs.push_back(in.header.sequence); + oob_counts.push_back(in.oob_frames.size()); + cv.notify_all(); + }); + + // SUB sockets need a beat to actually subscribe. + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // --- Inline-only message (no OOB) --- + { + MetadataBuilder b(1); + b.pack_str("inline_only"); + EXPECT_TRUE(pub.publish(std::move(b).finish())); + } + + // --- OOB-bearing message (one numpy buffer) --- + { + MetadataBuilder b(1); + const std::vector arr{1.0F, 2.0F, 3.0F, 4.0F}; + b.pack_numpy_oob(" lk(mu); + EXPECT_TRUE( + cv.wait_for(lk, std::chrono::seconds(2), + [&] {return seqs.size() >= 2;})); + } + + ASSERT_EQ(seqs.size(), 2u); + EXPECT_EQ(seqs[0], 0u); + EXPECT_EQ(seqs[1], 1u); + EXPECT_EQ(oob_counts[0], 0u); // inline-only + EXPECT_EQ(oob_counts[1], 1u); // one OOB frame + + // Cleanup: subscriber dtor joins thread; publisher dtor unregisters. +} + +TEST(PubSubViaDiscovery, ConnectFailsLoudlyOnUnknownTopic) +{ + Context ctx; + const auto daemon_addr = unique_inproc("daemon_unknown"); + MockDaemon daemon(ctx.raw(), daemon_addr); + DiscoveryClient discovery(ctx.raw(), daemon_addr); + + EXPECT_THROW( + Subscriber::connect( + ctx, discovery, "nope", 1ULL, + [](const Subscriber::Inbound &) {}), + std::runtime_error); +}