From 8c2ac4f650120d7dbe3a02a162ef513d4b6e395f Mon Sep 17 00:00:00 2001 From: Richeek Das Date: Wed, 13 May 2026 00:51:54 -0400 Subject: [PATCH] move ros2_bridge to its own branch The ROS2 bridge is still in beta and lives on the ros2_bridge branch until it stabilizes. --- ros2_bridge/PLAN.md | 603 ------------------ .../src/cortex_ros2_bridge/CMakeLists.txt | 184 ------ ros2_bridge/src/cortex_ros2_bridge/README.md | 236 ------- .../config/example_full.yaml | 67 -- .../config/example_minimal.yaml | 10 - .../include/cortex_ros2_bridge/adapter.hpp | 95 --- .../cortex_ros2_bridge/adapters/arrays.hpp | 52 -- .../cortex_ros2_bridge/adapters/image.hpp | 34 - .../adapters/oob_helpers.hpp | 154 ----- .../adapters/pointcloud.hpp | 38 -- .../cortex_ros2_bridge/adapters/pose.hpp | 34 - .../adapters/primitives.hpp | 91 --- .../cortex_ros2_bridge/adapters/tensor.hpp | 37 -- .../cortex_ros2_bridge/adapters/transform.hpp | 39 -- .../include/cortex_ros2_bridge/binding.hpp | 288 --------- .../cortex_ros2_bridge/binding_factory.hpp | 107 ---- .../include/cortex_ros2_bridge/config.hpp | 99 --- .../cortex_to_ros2_node.hpp | 49 -- .../include/cortex_ros2_bridge/qos.hpp | 17 - .../include/cortex_ros2_bridge/registry.hpp | 166 ----- .../ros2_to_cortex_node.hpp | 50 -- .../launch/composable_container.launch.py | 50 -- .../launch/cortex_to_ros2.launch.py | 37 -- .../launch/ros2_to_cortex.launch.py | 37 -- .../src/cortex_ros2_bridge/package.xml | 35 - .../src/adapters/arrays.cpp | 180 ------ .../cortex_ros2_bridge/src/adapters/image.cpp | 117 ---- .../src/adapters/pointcloud.cpp | 269 -------- .../cortex_ros2_bridge/src/adapters/pose.cpp | 120 ---- .../src/adapters/primitives.cpp | 298 --------- .../src/adapters/tensor.cpp | 72 --- .../src/adapters/transform.cpp | 184 ------ .../src/binding_factory.cpp | 122 ---- .../src/cortex_ros2_bridge/src/config.cpp | 312 --------- .../src/cortex_to_ros2_node.cpp | 179 ------ .../src/cortex_ros2_bridge/src/qos.cpp | 30 - .../src/cortex_ros2_bridge/src/registry.cpp | 28 - .../src/ros2_to_cortex_node.cpp | 229 ------- .../test/test_binding_factory.cpp | 46 -- .../test/test_components_e2e.cpp | 472 -------------- .../cortex_ros2_bridge/test/test_config.cpp | 324 ---------- .../test/test_intra_process.cpp | 373 ----------- .../test/test_primitives.cpp | 266 -------- .../src/cortex_ros2_bridge/test/test_qos.cpp | 56 -- .../cortex_ros2_bridge/test/test_registry.cpp | 178 ------ .../test/test_standard_adapters.cpp | 409 ------------ 46 files changed, 6873 deletions(-) delete mode 100644 ros2_bridge/PLAN.md delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/CMakeLists.txt delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/README.md delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/config/example_full.yaml delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/config/example_minimal.yaml delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/arrays.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/image.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/oob_helpers.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pointcloud.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pose.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/tensor.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/transform.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_to_ros2_node.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/ros2_to_cortex_node.hpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/launch/composable_container.launch.py delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/package.xml delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/adapters/arrays.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/adapters/image.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/adapters/pointcloud.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/adapters/pose.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/adapters/primitives.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/adapters/tensor.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/adapters/transform.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/binding_factory.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/config.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/qos.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/registry.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/test/test_binding_factory.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/test/test_components_e2e.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/test/test_config.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/test/test_intra_process.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/test/test_primitives.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/test/test_qos.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/test/test_registry.cpp delete mode 100644 ros2_bridge/src/cortex_ros2_bridge/test/test_standard_adapters.cpp diff --git a/ros2_bridge/PLAN.md b/ros2_bridge/PLAN.md deleted file mode 100644 index 8b6c641..0000000 --- a/ros2_bridge/PLAN.md +++ /dev/null @@ -1,603 +0,0 @@ -# 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. 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. - -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. *Resolved in PR2:* ported as-is; one fragility flagged in §14. - ---- - -## 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. -- 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/src/cortex_ros2_bridge/CMakeLists.txt b/ros2_bridge/src/cortex_ros2_bridge/CMakeLists.txt deleted file mode 100644 index 3caaa78..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/CMakeLists.txt +++ /dev/null @@ -1,184 +0,0 @@ -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) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(std_msgs REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs 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 ------------------------------------------------------------ - -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) - -add_library(cortex_ros2_bridge_adapters SHARED - src/registry.cpp - src/adapters/primitives.cpp - src/adapters/arrays.cpp - src/adapters/image.cpp - src/adapters/pointcloud.cpp - src/adapters/pose.cpp - src/adapters/transform.cpp - src/adapters/tensor.cpp -) -target_include_directories(cortex_ros2_bridge_adapters PUBLIC - $ - $ -) -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 - sensor_msgs - geometry_msgs -) - -# 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 - sensor_msgs - geometry_msgs -) - -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) - -install(TARGETS cortex_ros2_bridge_config - EXPORT export_cortex_ros2_bridge - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -# 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 -) - -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 rclcpp rclcpp_components std_msgs builtin_interfaces sensor_msgs geometry_msgs) - -# ---- tests ---------------------------------------------------------------- - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_gtest REQUIRED) - - # 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) - target_link_libraries(test_config cortex_ros2_bridge_config) - - ament_add_gtest(test_registry test/test_registry.cpp) - target_link_libraries(test_registry cortex_ros2_bridge_adapters) - - ament_add_gtest(test_primitives test/test_primitives.cpp) - target_link_libraries(test_primitives cortex_ros2_bridge_adapters) - - ament_add_gtest(test_standard_adapters test/test_standard_adapters.cpp) - target_link_libraries(test_standard_adapters cortex_ros2_bridge_components) - - 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) - - # PR8: intra-process composability proof — verifies bridge-published - # messages reach a colocated subscriber via rclcpp's intra-process path. - ament_add_gtest(test_intra_process test/test_intra_process.cpp) - target_link_libraries(test_intra_process cortex_ros2_bridge_components) -endif() - -ament_package() diff --git a/ros2_bridge/src/cortex_ros2_bridge/README.md b/ros2_bridge/src/cortex_ros2_bridge/README.md deleted file mode 100644 index 73a8194..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/README.md +++ /dev/null @@ -1,236 +0,0 @@ -# cortex_ros2_bridge - -A ROS 2 package that bridges Cortex pub/sub topics to ROS 2 topics, in both directions, configured by a single YAML file. - -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. - -``` -┌───────────────────────────┐ ┌─────────────────────────────┐ -│ 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) │ - └───────────────────────┘ -``` - -## Prerequisites - -- 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](../../)). - -## Build - -From a colcon workspace that contains this package under `src/`: - -```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: - -```bash -colcon test --packages-select cortex_ros2_bridge -colcon test-result --verbose -``` - -## Configuration - -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 -# 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 -``` - -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). - -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`. - -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. - -## Discovery daemon - -The bridge does not manage the daemon. Bring it up out of band: - -```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). - -## Troubleshooting - -| 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. | - -## Development - -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 -``` - -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. - -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 -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 -``` - -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). diff --git a/ros2_bridge/src/cortex_ros2_bridge/config/example_full.yaml b/ros2_bridge/src/cortex_ros2_bridge/config/example_full.yaml deleted file mode 100644 index 1bcca4a..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/config/example_full.yaml +++ /dev/null @@ -1,67 +0,0 @@ -# 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/src/cortex_ros2_bridge/config/example_minimal.yaml b/ros2_bridge/src/cortex_ros2_bridge/config/example_minimal.yaml deleted file mode 100644 index e993e44..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/config/example_minimal.yaml +++ /dev/null @@ -1,10 +0,0 @@ -# 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/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp deleted file mode 100644 index 4df1062..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapter.hpp +++ /dev/null @@ -1,95 +0,0 @@ -// 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/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/arrays.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/arrays.hpp deleted file mode 100644 index 3682db8..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/arrays.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#ifndef CORTEX_ROS2_BRIDGE__ADAPTERS__ARRAYS_HPP_ -#define CORTEX_ROS2_BRIDGE__ADAPTERS__ARRAYS_HPP_ - -#include -#include - -#include "cortex_ros2_bridge/adapter.hpp" -#include "cortex_ros2_bridge/registry.hpp" - -namespace cortex_ros2_bridge::adapters -{ - -// ArrayMessage(data: ndarray, name: str = "", frame_id: str = "") <-> -// std_msgs/msg/Float32MultiArray | Float64MultiArray -// -// We provide one adapter per numeric dtype the user can plausibly want as a -// ROS 2 MultiArray. The YAML's ros2.type selects which one is wired up; if -// the wire dtype doesn't match the adapter's expected dtype, the adapter -// throws a decode error rather than silently misinterpreting bytes. - -class ArrayFloat32Adapter - : 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::Float32MultiArray & msg, std::uint64_t sequence, - const BridgeEntry & cfg) const override; -}; - -class ArrayFloat64Adapter - : 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::Float64MultiArray & msg, std::uint64_t sequence, - const BridgeEntry & cfg) const override; -}; - -std::size_t register_array_adapters(AdapterRegistry & registry); - -} // namespace cortex_ros2_bridge::adapters - -#endif // CORTEX_ROS2_BRIDGE__ADAPTERS__ARRAYS_HPP_ diff --git a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/image.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/image.hpp deleted file mode 100644 index 73c5391..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/image.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#ifndef CORTEX_ROS2_BRIDGE__ADAPTERS__IMAGE_HPP_ -#define CORTEX_ROS2_BRIDGE__ADAPTERS__IMAGE_HPP_ - -#include - -#include "cortex_ros2_bridge/adapter.hpp" -#include "cortex_ros2_bridge/registry.hpp" - -namespace cortex_ros2_bridge::adapters -{ - -// ImageMessage(data: ndarray, encoding: str, width: int, height: int) <-> -// sensor_msgs/msg/Image -// -// Forward path: one memcpy from the ZMQ OOB frame into Image::data (we cannot -// alias the buffer because std::vector's sizing constructor value- -// initialises). The frame is then released. See PLAN.md §13.2. -class ImageAdapter : 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 sensor_msgs::msg::Image & msg, std::uint64_t sequence, - const BridgeEntry & cfg) const override; -}; - -std::size_t register_image_adapters(AdapterRegistry & registry); - -} // namespace cortex_ros2_bridge::adapters - -#endif // CORTEX_ROS2_BRIDGE__ADAPTERS__IMAGE_HPP_ diff --git a/ros2_bridge/src/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 deleted file mode 100644 index 2c7faa2..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/oob_helpers.hpp +++ /dev/null @@ -1,154 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#ifndef CORTEX_ROS2_BRIDGE__ADAPTERS__OOB_HELPERS_HPP_ -#define CORTEX_ROS2_BRIDGE__ADAPTERS__OOB_HELPERS_HPP_ - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cortex_ros2_bridge::adapters -{ - -// Parse the byte width from a numpy dtype string like "i8". -// Returns 0 if the string doesn't follow the {endian}{kind}{byte_count} -// pattern (e.g. complex/datetime types we don't support yet). -inline std::size_t dtype_size_bytes(std::string_view dtype) -{ - if (dtype.size() < 3) {return 0;} - std::size_t n = 0; - for (std::size_t i = 2; i < dtype.size(); ++i) { - const char c = dtype[i]; - if (!std::isdigit(static_cast(c))) {return 0;} - n = n * 10 + static_cast(c - '0'); - } - return n; -} - -// Compute total element count from a shape vector. -inline std::size_t product(const std::vector & shape) -{ - std::size_t n = 1; - for (auto d : shape) { - if (d < 0) {return 0;} - n *= static_cast(d); - } - return n; -} - -// Look up an OOB descriptor under `field` and return a typed view over its -// underlying ZMQ frame. Throws WireDecodeError if the field isn't an OOB -// descriptor, the buffer index is out of range, or dtype/shape don't match -// the expected width. -template -cortex_wire::OobBuffer oob_view( - const msgpack::object & field, - const std::vector & frames, - std::string_view adapter, std::string_view expected_dtype) -{ - auto desc = cortex_wire::DecodedMetadata::as_oob(field); - if (!desc) { - throw cortex_wire::WireDecodeError( - std::string(adapter) + ": expected OOB descriptor field"); - } - if (desc->dtype != expected_dtype) { - throw cortex_wire::WireDecodeError( - std::string(adapter) + ": dtype mismatch (got '" + desc->dtype + - "', expected '" + std::string(expected_dtype) + "')"); - } - if (desc->buffer_index >= frames.size()) { - throw cortex_wire::WireDecodeError( - std::string(adapter) + ": OOB buffer index " + - std::to_string(desc->buffer_index) + " >= frames.size() " + - std::to_string(frames.size())); - } - const auto & frame = frames[desc->buffer_index]; - const std::size_t expected_bytes = product(desc->shape) * sizeof(T); - if (frame->size() < expected_bytes) { - throw cortex_wire::WireDecodeError( - std::string(adapter) + ": OOB frame too small (" + - std::to_string(frame->size()) + " < " + std::to_string(expected_bytes) + ")"); - } - return cortex_wire::OobBuffer(frame, product(desc->shape)); -} - -// Like oob_view but also returns the shape; useful for adapters that need -// to inspect dimensions (e.g. ImageMessage validating H×W×C). -struct OobByteView -{ - cortex_wire::OobDescriptor descriptor; - const std::uint8_t * data = nullptr; - std::size_t size = 0; - cortex_wire::ZmqFramePtr frame; -}; - -inline OobByteView oob_bytes( - const msgpack::object & field, - const std::vector & frames, - std::string_view adapter) -{ - auto desc = cortex_wire::DecodedMetadata::as_oob(field); - if (!desc) { - throw cortex_wire::WireDecodeError( - std::string(adapter) + ": expected OOB descriptor field"); - } - if (desc->buffer_index >= frames.size()) { - throw cortex_wire::WireDecodeError( - std::string(adapter) + ": OOB buffer index out of range"); - } - OobByteView out; - out.descriptor = *desc; - out.frame = frames[desc->buffer_index]; - out.data = static_cast(out.frame->data()); - out.size = out.frame->size(); - return out; -} - -// Pull a string field; throws on type mismatch. -inline std::string read_str_field( - 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 cortex_wire::WireDecodeError( - std::string(adapter) + ": field[" + std::to_string(i) + "] expected str"); - } - return std::string(f.via.str.ptr, f.via.str.size); -} - -inline std::int64_t read_int_field( - 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 cortex_wire::WireDecodeError( - std::string(adapter) + ": field[" + std::to_string(i) + "] expected int"); -} - -inline bool is_nil_field(const cortex_wire::DecodedMetadata & m, std::size_t i) -{ - return m.field(i).type == msgpack::type::NIL; -} - -inline void require_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 cortex_ros2_bridge::adapters - -#endif // CORTEX_ROS2_BRIDGE__ADAPTERS__OOB_HELPERS_HPP_ diff --git a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pointcloud.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pointcloud.hpp deleted file mode 100644 index 7424a14..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pointcloud.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#ifndef CORTEX_ROS2_BRIDGE__ADAPTERS__POINTCLOUD_HPP_ -#define CORTEX_ROS2_BRIDGE__ADAPTERS__POINTCLOUD_HPP_ - -#include - -#include "cortex_ros2_bridge/adapter.hpp" -#include "cortex_ros2_bridge/registry.hpp" - -namespace cortex_ros2_bridge::adapters -{ - -// PointCloudMessage(points, colors, intensity, normals, frame_id) <-> -// sensor_msgs/msg/PointCloud2 -// -// v1 maps points (N×3 float32) <-> PointCloud2 with x/y/z PointFields. The -// optional `colors` / `intensity` / `normals` channels round-trip when set -// — colors as rgb (UINT8x3 packed into 4 bytes with one byte of padding), -// intensity and normals as float32. Missing channels round-trip as msgpack -// nil on the Cortex side and are simply omitted from PointCloud2.fields on -// the ROS side. -class PointCloudAdapter : 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 sensor_msgs::msg::PointCloud2 & msg, std::uint64_t sequence, - const BridgeEntry & cfg) const override; -}; - -std::size_t register_pointcloud_adapters(AdapterRegistry & registry); - -} // namespace cortex_ros2_bridge::adapters - -#endif // CORTEX_ROS2_BRIDGE__ADAPTERS__POINTCLOUD_HPP_ diff --git a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pose.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pose.hpp deleted file mode 100644 index e853af5..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/pose.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#ifndef CORTEX_ROS2_BRIDGE__ADAPTERS__POSE_HPP_ -#define CORTEX_ROS2_BRIDGE__ADAPTERS__POSE_HPP_ - -#include - -#include "cortex_ros2_bridge/adapter.hpp" -#include "cortex_ros2_bridge/registry.hpp" - -namespace cortex_ros2_bridge::adapters -{ - -// PoseMessage(position, orientation, frame_id, child_frame_id) <-> -// geometry_msgs/msg/PoseStamped -// -// Forward: position is [x,y,z] (float64), orientation is [qx,qy,qz,qw] -// (float64). child_frame_id is dropped (PoseStamped has no such field). -class PoseAdapter : 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 geometry_msgs::msg::PoseStamped & msg, std::uint64_t sequence, - const BridgeEntry & cfg) const override; -}; - -std::size_t register_pose_adapters(AdapterRegistry & registry); - -} // namespace cortex_ros2_bridge::adapters - -#endif // CORTEX_ROS2_BRIDGE__ADAPTERS__POSE_HPP_ diff --git a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp deleted file mode 100644 index 8efa066..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/primitives.hpp +++ /dev/null @@ -1,91 +0,0 @@ -// 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/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/tensor.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/tensor.hpp deleted file mode 100644 index 63cec5e..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/tensor.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#ifndef CORTEX_ROS2_BRIDGE__ADAPTERS__TENSOR_HPP_ -#define CORTEX_ROS2_BRIDGE__ADAPTERS__TENSOR_HPP_ - -#include - -#include "cortex_ros2_bridge/adapter.hpp" -#include "cortex_ros2_bridge/registry.hpp" - -namespace cortex_ros2_bridge::adapters -{ - -// TensorMessage(data: torch.Tensor, name: str = "") <-> -// std_msgs/msg/Float32MultiArray -// -// Forward: drops the torch-specific device / requires_grad metadata. The -// receiver (ROS side) gets a CPU array. Reverse: emits a torch OOB -// descriptor with device="cpu", requires_grad=false (the Python receiver -// reconstructs a CPU tensor). -class TensorFloat32Adapter - : 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::Float32MultiArray & msg, std::uint64_t sequence, - const BridgeEntry & cfg) const override; -}; - -std::size_t register_tensor_adapters(AdapterRegistry & registry); - -} // namespace cortex_ros2_bridge::adapters - -#endif // CORTEX_ROS2_BRIDGE__ADAPTERS__TENSOR_HPP_ diff --git a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/transform.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/transform.hpp deleted file mode 100644 index e1ca2f2..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/adapters/transform.hpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#ifndef CORTEX_ROS2_BRIDGE__ADAPTERS__TRANSFORM_HPP_ -#define CORTEX_ROS2_BRIDGE__ADAPTERS__TRANSFORM_HPP_ - -#include - -#include "cortex_ros2_bridge/adapter.hpp" -#include "cortex_ros2_bridge/registry.hpp" - -namespace cortex_ros2_bridge::adapters -{ - -// TransformMessage(matrix: 4x4 float, frame_id, child_frame_id) <-> -// geometry_msgs/msg/TransformStamped -// -// Forward: decomposes the 4x4 into translation (matrix[0:3, 3]) and the -// quaternion of matrix[0:3, 0:3] via Shepperd's method. Reverse: builds the -// 4x4 from the ROS translation+rotation. -// -// `/tf` broadcasting is deliberately out of scope for v1 — the YAML's -// broadcast_tf hook is unwired here. -class TransformAdapter - : 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 geometry_msgs::msg::TransformStamped & msg, std::uint64_t sequence, - const BridgeEntry & cfg) const override; -}; - -std::size_t register_transform_adapters(AdapterRegistry & registry); - -} // namespace cortex_ros2_bridge::adapters - -#endif // CORTEX_ROS2_BRIDGE__ADAPTERS__TRANSFORM_HPP_ diff --git a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp deleted file mode 100644 index 45bf7cb..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding.hpp +++ /dev/null @@ -1,288 +0,0 @@ -// 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/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp deleted file mode 100644 index d414d2a..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/binding_factory.hpp +++ /dev/null @@ -1,107 +0,0 @@ -// 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); - -// Registers binding factories for every ROS 2 type used by the standard -// catalogue beyond primitives (multi-array, image, pointcloud, pose, -// transform). Idempotent — repeated calls add nothing. -std::size_t register_standard_bindings(BindingFactoryRegistry & reg); - -} // namespace cortex_ros2_bridge - -#endif // CORTEX_ROS2_BRIDGE__BINDING_FACTORY_HPP_ diff --git a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp deleted file mode 100644 index fd98b01..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/config.hpp +++ /dev/null @@ -1,99 +0,0 @@ -// 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/src/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 deleted file mode 100644 index 2d3c081..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/cortex_to_ros2_node.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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/src/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp deleted file mode 100644 index 1870eb2..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/qos.hpp +++ /dev/null @@ -1,17 +0,0 @@ -// 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/src/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp b/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp deleted file mode 100644 index 31b9e3e..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/registry.hpp +++ /dev/null @@ -1,166 +0,0 @@ -// 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/src/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 deleted file mode 100644 index 695a76e..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/include/cortex_ros2_bridge/ros2_to_cortex_node.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// 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/src/cortex_ros2_bridge/launch/composable_container.launch.py b/ros2_bridge/src/cortex_ros2_bridge/launch/composable_container.launch.py deleted file mode 100644 index 5f794ac..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/launch/composable_container.launch.py +++ /dev/null @@ -1,50 +0,0 @@ -"""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/src/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py b/ros2_bridge/src/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py deleted file mode 100644 index f69f47a..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/launch/cortex_to_ros2.launch.py +++ /dev/null @@ -1,37 +0,0 @@ -"""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/src/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py b/ros2_bridge/src/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py deleted file mode 100644 index a964f26..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/launch/ros2_to_cortex.launch.py +++ /dev/null @@ -1,37 +0,0 @@ -"""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/src/cortex_ros2_bridge/package.xml b/ros2_bridge/src/cortex_ros2_bridge/package.xml deleted file mode 100644 index 456d7f1..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - 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 - rclcpp - rclcpp_components - std_msgs - builtin_interfaces - sensor_msgs - geometry_msgs - - cppzmq - libmsgpack-dev - - ament_cmake_gtest - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ros2_bridge/src/cortex_ros2_bridge/src/adapters/arrays.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/arrays.cpp deleted file mode 100644 index ddd8a22..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/adapters/arrays.cpp +++ /dev/null @@ -1,180 +0,0 @@ -// 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/src/cortex_ros2_bridge/src/adapters/image.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/image.cpp deleted file mode 100644 index b2ec1f0..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/adapters/image.cpp +++ /dev/null @@ -1,117 +0,0 @@ -// 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/src/cortex_ros2_bridge/src/adapters/pointcloud.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/pointcloud.cpp deleted file mode 100644 index 6a696b7..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/adapters/pointcloud.cpp +++ /dev/null @@ -1,269 +0,0 @@ -// 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/src/cortex_ros2_bridge/src/adapters/pose.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/pose.cpp deleted file mode 100644 index 8c2f750..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/adapters/pose.cpp +++ /dev/null @@ -1,120 +0,0 @@ -// 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/src/cortex_ros2_bridge/src/adapters/primitives.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/primitives.cpp deleted file mode 100644 index 97f3560..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/adapters/primitives.cpp +++ /dev/null @@ -1,298 +0,0 @@ -// 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/src/cortex_ros2_bridge/src/adapters/tensor.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/tensor.cpp deleted file mode 100644 index 5dacabb..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/adapters/tensor.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// 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/src/cortex_ros2_bridge/src/adapters/transform.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/adapters/transform.cpp deleted file mode 100644 index 0edb23f..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/adapters/transform.cpp +++ /dev/null @@ -1,184 +0,0 @@ -// 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/src/cortex_ros2_bridge/src/binding_factory.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/binding_factory.cpp deleted file mode 100644 index b856cf5..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/binding_factory.cpp +++ /dev/null @@ -1,122 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/binding_factory.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cortex_ros2_bridge -{ - -bool BindingFactoryRegistry::register_cortex_to_ros2( - std::string ros2_type_name, CortexToRos2BindingFactory factory) -{ - if (!factory) {return false;} - std::lock_guard g(mu_); - return c2r_.emplace(std::move(ros2_type_name), std::move(factory)).second; -} - -bool BindingFactoryRegistry::register_ros2_to_cortex( - std::string ros2_type_name, Ros2ToCortexBindingFactory factory) -{ - if (!factory) {return false;} - std::lock_guard g(mu_); - return r2c_.emplace(std::move(ros2_type_name), std::move(factory)).second; -} - -CortexToRos2BindingFactory BindingFactoryRegistry::get_cortex_to_ros2( - std::string_view ros2_type_name) const -{ - std::lock_guard g(mu_); - const auto it = c2r_.find(std::string(ros2_type_name)); - if (it == c2r_.end()) {return {};} - return it->second; -} - -Ros2ToCortexBindingFactory BindingFactoryRegistry::get_ros2_to_cortex( - std::string_view ros2_type_name) const -{ - std::lock_guard g(mu_); - const auto it = r2c_.find(std::string(ros2_type_name)); - if (it == r2c_.end()) {return {};} - return it->second; -} - -BindingFactoryRegistry & BindingFactoryRegistry::global() -{ - static BindingFactoryRegistry instance; - return instance; -} - -std::size_t register_primitive_bindings(BindingFactoryRegistry & reg) -{ - std::size_t added = 0; - added += register_cortex_to_ros2_binding( - reg, "std_msgs/msg/String") ? 1 : 0; - added += register_cortex_to_ros2_binding( - reg, "std_msgs/msg/Int64") ? 1 : 0; - added += register_cortex_to_ros2_binding( - reg, "std_msgs/msg/Float64") ? 1 : 0; - added += register_cortex_to_ros2_binding( - reg, "std_msgs/msg/ByteMultiArray") ? 1 : 0; - added += register_cortex_to_ros2_binding( - reg, "builtin_interfaces/msg/Time") ? 1 : 0; - added += register_cortex_to_ros2_binding( - reg, "std_msgs/msg/Header") ? 1 : 0; - - added += register_ros2_to_cortex_binding( - reg, "std_msgs/msg/String") ? 1 : 0; - added += register_ros2_to_cortex_binding( - reg, "std_msgs/msg/Int64") ? 1 : 0; - added += register_ros2_to_cortex_binding( - reg, "std_msgs/msg/Float64") ? 1 : 0; - added += register_ros2_to_cortex_binding( - reg, "std_msgs/msg/ByteMultiArray") ? 1 : 0; - added += register_ros2_to_cortex_binding( - reg, "builtin_interfaces/msg/Time") ? 1 : 0; - added += register_ros2_to_cortex_binding( - reg, "std_msgs/msg/Header") ? 1 : 0; - 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/src/cortex_ros2_bridge/src/config.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/config.cpp deleted file mode 100644 index ecfc653..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/config.cpp +++ /dev/null @@ -1,312 +0,0 @@ -// 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/src/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp deleted file mode 100644 index 4df9c85..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/cortex_to_ros2_node.cpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/cortex_to_ros2_node.hpp" - -#include -#include - -#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 -{ - -namespace -{ - -// Register the primitive adapters + bindings into the process globals the -// first time we instantiate any bridge component. Idempotent. -void ensure_primitives_registered() -{ - static bool done = []() { - 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; -} - -} // namespace - -CortexToRos2Bridge::CortexToRos2Bridge(const rclcpp::NodeOptions & options) -: rclcpp::Node("cortex_to_ros2", options), - ctx_(std::make_shared(1)) -{ - ensure_primitives_registered(); - this->declare_parameter("config_path", ""); - initialize(); -} - -CortexToRos2Bridge::~CortexToRos2Bridge() -{ - for (auto & b : bindings_) { - if (b) {b->stop();} - } - bindings_.clear(); - discovery_.reset(); - if (ctx_) { - ctx_->shutdown(); - ctx_->close(); - } -} - -std::size_t CortexToRos2Bridge::num_active_bindings() const noexcept -{ - return bindings_.size(); -} - -void CortexToRos2Bridge::initialize() -{ - const auto config_path = this->get_parameter("config_path").as_string(); - if (config_path.empty()) { - throw std::runtime_error( - "CortexToRos2Bridge: required parameter 'config_path' is empty"); - } - config_ = load_config(config_path); - - discovery_ = std::make_unique( - *ctx_, config_.cortex.discovery_address); - - const auto & adapters = AdapterRegistry::global(); - const auto & factories = BindingFactoryRegistry::global(); - - for (const auto & entry : config_.entries) { - if (entry.direction != Direction::CortexToRos2) {continue;} - - const auto * fp_entry = cortex_wire::find_by_name(entry.cortex.type); - if (!fp_entry) { - RCLCPP_ERROR( - get_logger(), "[%s] unknown cortex type '%s' — skipping", - entry.name.c_str(), entry.cortex.type.c_str()); - continue; - } - const auto kind = fp_entry->kind; - const auto expected_fingerprint = fp_entry->fingerprint; - - // PR4 requires ros2.type to be explicit; defaulting (look up the - // canonical ROS 2 type for the Cortex kind via the registry) is a - // follow-up — see PLAN.md §13 follow-ups. - if (!entry.ros2.type) { - RCLCPP_ERROR( - get_logger(), - "[%s] cortex_to_ros2 entry must specify ros2.type — skipping", - entry.name.c_str()); - continue; - } - const auto & ros2_type_name = *entry.ros2.type; - - if (!adapters.has_cortex_to_ros2(kind, ros2_type_name)) { - RCLCPP_ERROR( - get_logger(), - "[%s] no adapter registered for (%s -> %s)", - entry.name.c_str(), entry.cortex.type.c_str(), ros2_type_name.c_str()); - continue; - } - auto factory = factories.get_cortex_to_ros2(ros2_type_name); - if (!factory) { - RCLCPP_ERROR( - get_logger(), - "[%s] no binding factory for ros2 type '%s'", - entry.name.c_str(), ros2_type_name.c_str()); - continue; - } - - cortex_wire::TopicInfo topic_info; - try { - auto lookup = discovery_->lookup(entry.cortex.topic); - if (!lookup) { - RCLCPP_ERROR( - get_logger(), - "[%s] discovery: topic '%s' not registered — skipping", - entry.name.c_str(), entry.cortex.topic.c_str()); - continue; - } - topic_info = std::move(*lookup); - } catch (const std::exception & e) { - RCLCPP_ERROR( - get_logger(), "[%s] discovery lookup failed: %s — skipping", - entry.name.c_str(), e.what()); - continue; - } - - if (topic_info.fingerprint != expected_fingerprint) { - RCLCPP_ERROR( - get_logger(), - "[%s] fingerprint mismatch: daemon=0x%016lx, expected=0x%016lx (for %s) — " - "refusing to bridge", - entry.name.c_str(), - static_cast(topic_info.fingerprint), - static_cast(expected_fingerprint), - entry.cortex.type.c_str()); - continue; - } - - auto binding = factory( - this, ctx_.get(), entry, topic_info, kind, adapters, make_qos(entry.qos)); - if (!binding) { - RCLCPP_ERROR( - get_logger(), "[%s] factory returned null binding — skipping", - entry.name.c_str()); - continue; - } - binding->start(); - bindings_.push_back(std::move(binding)); - RCLCPP_INFO( - get_logger(), "[%s] bridging Cortex(%s) -> ROS2(%s)", - entry.name.c_str(), entry.cortex.topic.c_str(), entry.ros2.topic.c_str()); - } -} - -} // namespace cortex_ros2_bridge - -RCLCPP_COMPONENTS_REGISTER_NODE(cortex_ros2_bridge::CortexToRos2Bridge) diff --git a/ros2_bridge/src/cortex_ros2_bridge/src/qos.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/qos.cpp deleted file mode 100644 index aa4b8c6..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/qos.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/qos.hpp" - -namespace cortex_ros2_bridge -{ - -rclcpp::QoS make_qos(const QosSettings & q) -{ - // History first — the depth ctor variants of rclcpp::QoS encode "keep last - // with depth N"; KeepAll is built via KeepAll() (which sets the underlying - // RMW history to RMW_QOS_POLICY_HISTORY_KEEP_ALL and ignores depth). - rclcpp::QoS qos = (q.history == History::KeepAll) - ? rclcpp::QoS(rclcpp::KeepAll()) - : rclcpp::QoS(rclcpp::KeepLast(q.depth)); - - if (q.reliability == Reliability::BestEffort) { - qos.best_effort(); - } else { - qos.reliable(); - } - - if (q.durability == Durability::TransientLocal) { - qos.transient_local(); - } else { - qos.durability_volatile(); - } - return qos; -} - -} // namespace cortex_ros2_bridge diff --git a/ros2_bridge/src/cortex_ros2_bridge/src/registry.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/registry.cpp deleted file mode 100644 index bef9353..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/registry.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// 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/src/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp b/ros2_bridge/src/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp deleted file mode 100644 index c30710d..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/src/ros2_to_cortex_node.cpp +++ /dev/null @@ -1,229 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/ros2_to_cortex_node.hpp" - -#include -#include - -#include -#include -#include -#include -#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 -{ - -namespace -{ - -void ensure_primitives_registered() -{ - static bool done = []() { - 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; -} - -// Build a deterministic IPC endpoint for a bridge entry. The slugify pass -// makes the path filesystem-friendly: any character that isn't alnum, dash -// or underscore becomes underscore. Two bridge processes with identical -// configs on the same host would collide here — that's already a bug at the -// config level, and the bind() call will surface it loudly. -std::string slugify(std::string s) -{ - for (auto & c : s) { - if (!std::isalnum(static_cast(c)) && c != '-' && c != '_') { - c = '_'; - } - } - return s; -} - -} // namespace - -std::string Ros2ToCortexBridge::make_pub_endpoint(const std::string & entry_name) const -{ - return "ipc:///tmp/cortex/topics/" + - slugify(config_.cortex.node_name_prefix) + "__" + slugify(entry_name); -} - -Ros2ToCortexBridge::Ros2ToCortexBridge(const rclcpp::NodeOptions & options) -: rclcpp::Node("ros2_to_cortex", options), - ctx_(std::make_shared(1)) -{ - ensure_primitives_registered(); - this->declare_parameter("config_path", ""); - initialize(); -} - -Ros2ToCortexBridge::~Ros2ToCortexBridge() -{ - // Stop binding callbacks first — drops the rclcpp subscription so no more - // ROS messages can race the unregister. - for (auto & b : bindings_) { - if (b) {b->stop();} - } - bindings_.clear(); - - if (discovery_) { - for (const auto & topic : registered_topics_) { - try { - discovery_->unregister_topic(topic); - } catch (const std::exception & e) { - RCLCPP_WARN( - get_logger(), "discovery unregister('%s') failed: %s — continuing", - topic.c_str(), e.what()); - } - } - } - registered_topics_.clear(); - discovery_.reset(); - - if (ctx_) { - ctx_->shutdown(); - ctx_->close(); - } -} - -std::size_t Ros2ToCortexBridge::num_active_bindings() const noexcept -{ - return bindings_.size(); -} - -void Ros2ToCortexBridge::initialize() -{ - const auto config_path = this->get_parameter("config_path").as_string(); - if (config_path.empty()) { - throw std::runtime_error( - "Ros2ToCortexBridge: required parameter 'config_path' is empty"); - } - config_ = load_config(config_path); - - discovery_ = std::make_unique( - *ctx_, config_.cortex.discovery_address); - - const auto & adapters = AdapterRegistry::global(); - const auto & factories = BindingFactoryRegistry::global(); - - for (const auto & entry : config_.entries) { - if (entry.direction != Direction::Ros2ToCortex) {continue;} - - const auto * fp_entry = cortex_wire::find_by_name(entry.cortex.type); - if (!fp_entry) { - RCLCPP_ERROR( - get_logger(), "[%s] unknown cortex type '%s' — skipping", - entry.name.c_str(), entry.cortex.type.c_str()); - continue; - } - const auto kind = fp_entry->kind; - const auto fingerprint = fp_entry->fingerprint; - - // ros2.type is required for this direction by the config loader (see - // §5.2 / config.cpp). Defensive recheck: - if (!entry.ros2.type) { - RCLCPP_ERROR( - get_logger(), - "[%s] ros2_to_cortex entries must specify ros2.type — skipping", - entry.name.c_str()); - continue; - } - const auto & ros2_type_name = *entry.ros2.type; - - if (!adapters.has_ros2_to_cortex(kind, ros2_type_name)) { - RCLCPP_ERROR( - get_logger(), - "[%s] no adapter registered for (%s <- %s)", - entry.name.c_str(), entry.cortex.type.c_str(), ros2_type_name.c_str()); - continue; - } - auto factory = factories.get_ros2_to_cortex(ros2_type_name); - if (!factory) { - RCLCPP_ERROR( - get_logger(), - "[%s] no binding factory for ros2 type '%s'", - entry.name.c_str(), ros2_type_name.c_str()); - continue; - } - - const auto pub_endpoint = make_pub_endpoint(entry.name); - - // ZMQ's ipc:// bind() requires the parent directory to exist. - // make_pub_endpoint() puts sockets under /tmp/cortex/topics/; create it - // if missing. Stripping the "ipc://" prefix gives the filesystem path. - constexpr std::string_view kIpcPrefix = "ipc://"; - if (pub_endpoint.rfind(kIpcPrefix, 0) == 0) { - const std::filesystem::path sock_path(pub_endpoint.substr(kIpcPrefix.size())); - std::error_code ec; - std::filesystem::create_directories(sock_path.parent_path(), ec); - if (ec) { - RCLCPP_ERROR( - get_logger(), - "[%s] cannot create parent dir for %s: %s — skipping", - entry.name.c_str(), pub_endpoint.c_str(), ec.message().c_str()); - continue; - } - } - - auto binding = factory( - this, ctx_.get(), entry, kind, fingerprint, pub_endpoint, adapters, make_qos(entry.qos)); - if (!binding) { - RCLCPP_ERROR( - get_logger(), "[%s] factory returned null binding — skipping", - entry.name.c_str()); - continue; - } - - // Register with discovery so Cortex subscribers can find this endpoint. - cortex_wire::TopicInfo info{ - entry.cortex.topic, - pub_endpoint, - entry.cortex.type, - fingerprint, - this->get_fully_qualified_name(), - }; - try { - discovery_->register_topic(info); - registered_topics_.push_back(entry.cortex.topic); - } catch (const std::exception & e) { - RCLCPP_ERROR( - get_logger(), - "[%s] discovery register('%s') failed: %s — dropping binding", - entry.name.c_str(), entry.cortex.topic.c_str(), e.what()); - // Let `binding` destruct here, which closes the PUB socket. - continue; - } - - binding->start(); - bindings_.push_back(std::move(binding)); - RCLCPP_INFO( - get_logger(), - "[%s] bridging ROS2(%s) -> Cortex(%s @ %s)", - entry.name.c_str(), entry.ros2.topic.c_str(), - entry.cortex.topic.c_str(), pub_endpoint.c_str()); - } -} - -} // namespace cortex_ros2_bridge - -RCLCPP_COMPONENTS_REGISTER_NODE(cortex_ros2_bridge::Ros2ToCortexBridge) diff --git a/ros2_bridge/src/cortex_ros2_bridge/test/test_binding_factory.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_binding_factory.cpp deleted file mode 100644 index c2e6de1..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/test/test_binding_factory.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/binding_factory.hpp" - -#include - -using cortex_ros2_bridge::BindingFactoryRegistry; -using cortex_ros2_bridge::register_primitive_bindings; - -TEST(BindingFactoryRegistry, RegistersAllTwelveDirections) -{ - BindingFactoryRegistry reg; - EXPECT_EQ(register_primitive_bindings(reg), 12u); - - const std::vector primitive_types = { - "std_msgs/msg/String", - "std_msgs/msg/Int64", - "std_msgs/msg/Float64", - "std_msgs/msg/ByteMultiArray", - "builtin_interfaces/msg/Time", - "std_msgs/msg/Header", - }; - for (const auto & t : primitive_types) { - EXPECT_TRUE(static_cast(reg.get_cortex_to_ros2(t))) << t; - EXPECT_TRUE(static_cast(reg.get_ros2_to_cortex(t))) << t; - } -} - -TEST(BindingFactoryRegistry, IsIdempotent) -{ - BindingFactoryRegistry reg; - register_primitive_bindings(reg); - EXPECT_EQ(register_primitive_bindings(reg), 0u); -} - -TEST(BindingFactoryRegistry, UnknownTypeReturnsEmpty) -{ - BindingFactoryRegistry reg; - register_primitive_bindings(reg); - EXPECT_FALSE(static_cast(reg.get_cortex_to_ros2("sensor_msgs/msg/NotAType"))); - EXPECT_FALSE(static_cast(reg.get_ros2_to_cortex("sensor_msgs/msg/NotAType"))); -} - -TEST(BindingFactoryRegistry, GlobalIsSingleton) -{ - EXPECT_EQ(&BindingFactoryRegistry::global(), &BindingFactoryRegistry::global()); -} diff --git a/ros2_bridge/src/cortex_ros2_bridge/test/test_components_e2e.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_components_e2e.cpp deleted file mode 100644 index 246e1df..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/test/test_components_e2e.cpp +++ /dev/null @@ -1,472 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -// -// End-to-end smoke tests for the CortexToRos2Bridge / Ros2ToCortexBridge -// composable nodes. These exercise the full path through: -// - a hand-rolled REP mock standing in for the Python discovery daemon -// - a raw zmq PUB/SUB pair playing the role of the Cortex publisher/sub -// - the real bridge component + a real rclcpp::Node consumer -// -// We avoid launch_testing here so the test stays in-process and self-contained -// (launch_testing brings in a python orchestration layer that is hard to -// wire up cleanly inside ament_add_gtest). -#include "cortex_ros2_bridge/cortex_to_ros2_node.hpp" -#include "cortex_ros2_bridge/ros2_to_cortex_node.hpp" - -#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 discovery daemon ----------------------------------------------- - -// A minimal REP responder that mimics the Python discovery daemon: handles -// REGISTER_TOPIC, UNREGISTER_TOPIC, and LOOKUP_TOPIC. State is in-memory. -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; - } - - std::size_t topic_count() - { - std::lock_guard g(mu_); - return topics_.size(); - } - - bool has_topic(const std::string & name) - { - std::lock_guard g(mu_); - return topics_.count(name) > 0; - } - -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 (const zmq::error_t &) { - 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 (const zmq::error_t &) { - } - } - } - - 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; - cortex_wire::TopicInfo info; - bool have_info = 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) {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); - } else if (key == "topic_info" && v.type == msgpack::type::BIN) { - info = unpack_topic_info(v.via.bin.ptr, v.via.bin.size); - have_info = true; - } - } - - std::lock_guard g(mu_); - switch (cmd) { - case DiscoveryCommand::RegisterTopic: { - if (!have_info) {return pack_response(DiscoveryStatus::Error, "no info");} - topics_[info.name] = info; - return pack_response(DiscoveryStatus::Ok, "ok"); - } - case DiscoveryCommand::UnregisterTopic: { - topics_.erase(topic_name); - return pack_response(DiscoveryStatus::Ok, "ok"); - } - case 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); - } - default: - return pack_response(DiscoveryStatus::Ok, "ignored"); - } - } - - static cortex_wire::TopicInfo unpack_topic_info(const void * data, std::size_t size) - { - msgpack::object_handle oh = msgpack::unpack(static_cast(data), size); - cortex_wire::TopicInfo info; - const auto & m = oh.get(); - if (m.type != msgpack::type::MAP) {return 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; - if (k.type != msgpack::type::STR) {continue;} - const std::string key(k.via.str.ptr, k.via.str.size); - if (key == "name" && v.type == msgpack::type::STR) { - info.name.assign(v.via.str.ptr, v.via.str.size); - } else if (key == "address" && v.type == msgpack::type::STR) { - info.address.assign(v.via.str.ptr, v.via.str.size); - } else if (key == "message_type" && v.type == msgpack::type::STR) { - info.message_type.assign(v.via.str.ptr, v.via.str.size); - } 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.assign(v.via.str.ptr, v.via.str.size); - } - } - return info; - } - - 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_; -}; - -// ---- helpers -------------------------------------------------------------- - -std::string unique_ipc_path() -{ - static std::atomic counter{0}; - const int pid = static_cast(::getpid()); - return "ipc:///tmp/cortex_bridge_test_" + std::to_string(pid) + "_" + - std::to_string(counter.fetch_add(1)); -} - -std::string write_temp_yaml(const std::string & body) -{ - // tmpnam is fine for tests — we own the namespace. - std::string path = std::string(std::tmpnam(nullptr)) + ".yaml"; - std::ofstream f(path); - f << body; - return path; -} - -// Globals shared by the rclcpp::init / shutdown lifecycle. -struct RclcppEnv : ::testing::Environment -{ - void SetUp() override - { - rclcpp::init(0, nullptr); - } - void TearDown() override - { - rclcpp::shutdown(); - } -}; -::testing::Environment * const kRclcppEnv = - ::testing::AddGlobalTestEnvironment(new RclcppEnv); - -// Pack one Cortex multipart message [topic, header, metadata, ...] onto `sock`. -void send_cortex_frame( - zmq::socket_t & sock, const std::string & topic, std::uint64_t fingerprint, - std::uint64_t sequence, const std::vector & metadata) -{ - 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()); - zmq::message_t f1(header_bytes.data(), header_bytes.size()); - zmq::message_t f2(metadata.data(), metadata.size()); - (void)sock.send(f0, zmq::send_flags::sndmore); - (void)sock.send(f1, zmq::send_flags::sndmore); - (void)sock.send(f2, zmq::send_flags::none); -} - -template -bool wait_for(Pred pred, std::chrono::milliseconds timeout = 2s) -{ - 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 - -// ---- CortexToRos2 end-to-end ---------------------------------------------- - -TEST(CortexToRos2BridgeE2E, StringMessageFlowsToRosTopic) -{ - // Mock daemon endpoint and a Cortex publisher endpoint. - 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); - - // Pre-register the "publisher" topic so the bridge can look it up. - const auto * fp = cortex_wire::find_by_name("StringMessage"); - ASSERT_NE(fp, nullptr); - daemon.register_topic({ - "/test/string", pub_addr, "StringMessage", fp->fingerprint, "test_publisher", - }); - - // Bind a raw PUB socket as the "Cortex publisher". - zmq::socket_t pub(pub_ctx, zmq::socket_type::pub); - pub.set(zmq::sockopt::linger, 0); - pub.bind(pub_addr); - - // Write the bridge YAML. - const auto config_path = write_temp_yaml( - "version: 1\n" - "cortex:\n" - " discovery_address: \"" + daemon_addr + "\"\n" - "cortex_to_ros2:\n" - " - name: hello\n" - " cortex: {topic: \"/test/string\", type: StringMessage}\n" - " ros2: {topic: \"/ros/hello\", type: \"std_msgs/msg/String\"}\n"); - - // Bring up the bridge component. - rclcpp::NodeOptions opts; - opts.parameter_overrides({{"config_path", config_path}}); - auto bridge = std::make_shared(opts); - ASSERT_EQ(bridge->num_active_bindings(), 1u); - - // Subscribe a Node to the ROS topic. - auto consumer = std::make_shared("ros_consumer"); - std::atomic received{0}; - std::string last_payload; - auto sub = consumer->create_subscription( - "/ros/hello", rclcpp::QoS(10), - [&](const std_msgs::msg::String & msg) { - last_payload = msg.data; - received.fetch_add(1); - }); - - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(bridge); - exec.add_node(consumer); - - // Spin in a worker thread while we feed messages. - std::atomic spinning{true}; - std::thread spinner([&]() { - while (spinning.load()) {exec.spin_some(50ms);} - }); - - // Emit one Cortex-format multipart message via the raw PUB socket. - std::this_thread::sleep_for(200ms); // let SUB connect - cortex_wire::MetadataBuilder b(1); - b.pack_str("hello from cortex"); - auto frames = std::move(b).finish(); - - // The bridge SUBs by topic prefix; emit the same topic string here. - for (int i = 0; i < 5 && received.load() == 0; ++i) { - send_cortex_frame(pub, "/test/string", fp->fingerprint, i, frames.metadata); - std::this_thread::sleep_for(100ms); - } - - EXPECT_TRUE(wait_for([&] {return received.load() > 0;})); - EXPECT_EQ(last_payload, "hello from cortex"); - - spinning.store(false); - spinner.join(); - std::remove(config_path.c_str()); -} - -// ---- Ros2ToCortex end-to-end ---------------------------------------------- - -TEST(Ros2ToCortexBridgeE2E, StringMessageFlowsToCortexSocket) -{ - zmq::context_t sub_ctx(1); - const auto daemon_addr = unique_ipc_path(); - MockDiscoveryDaemon daemon(sub_ctx, daemon_addr); - - // YAML: a ros2_to_cortex entry that binds to a ROS topic and emits to - // Cortex topic "/test/echo". - const auto config_path = write_temp_yaml( - "version: 1\n" - "cortex:\n" - " discovery_address: \"" + daemon_addr + "\"\n" - " node_name_prefix: \"e2e_test\"\n" - "ros2_to_cortex:\n" - " - name: echo\n" - " ros2: {topic: \"/ros/in\", type: \"std_msgs/msg/String\"}\n" - " cortex: {topic: \"/test/echo\", type: StringMessage}\n"); - - rclcpp::NodeOptions opts; - opts.parameter_overrides({{"config_path", config_path}}); - auto bridge = std::make_shared(opts); - ASSERT_EQ(bridge->num_active_bindings(), 1u); - - // The bridge should have registered with the daemon. - EXPECT_TRUE(wait_for([&] {return daemon.has_topic("/test/echo");})); - - // Look up the endpoint the bridge bound to. - cortex_wire::DiscoveryClient client(sub_ctx, daemon_addr, 1s); - const auto info = client.lookup("/test/echo"); - ASSERT_TRUE(info.has_value()); - EXPECT_EQ(info->message_type, "StringMessage"); - EXPECT_NE(info->fingerprint, 0u); - - // Subscribe to that endpoint with a raw SUB socket. - zmq::socket_t sub(sub_ctx, zmq::socket_type::sub); - sub.set(zmq::sockopt::linger, 0); - sub.set(zmq::sockopt::rcvtimeo, 100); - sub.connect(info->address); - sub.set(zmq::sockopt::subscribe, std::string("/test/echo")); - - // Spin the bridge executor and publish a ROS message. - auto producer = std::make_shared("ros_producer"); - auto pub = producer->create_publisher( - "/ros/in", rclcpp::QoS(10)); - - rclcpp::executors::SingleThreadedExecutor exec; - exec.add_node(bridge); - exec.add_node(producer); - - std::atomic spinning{true}; - std::thread spinner([&]() { - while (spinning.load()) {exec.spin_some(20ms);} - }); - - // Give the SUB time to connect to the PUB before we publish. - std::this_thread::sleep_for(200ms); - - std_msgs::msg::String msg; - msg.data = "echo me"; - - // Retry the publish a few times — SUB connect on IPC can drop the first - // sends if the SUB hasn't completed its handshake. - std::vector recv_frames; - for (int attempt = 0; attempt < 10 && recv_frames.empty(); ++attempt) { - pub->publish(msg); - std::this_thread::sleep_for(100ms); - - while (true) { - zmq::message_t frame; - zmq::recv_result_t r; - try { - r = sub.recv(frame, zmq::recv_flags::none); - } catch (const zmq::error_t &) { - break; - } - if (!r) {break;} - const bool more = frame.more(); - recv_frames.emplace_back(std::move(frame)); - if (!more) {break;} - } - } - - ASSERT_GE(recv_frames.size(), 3u); - - const auto header = cortex_wire::MessageHeader::from_bytes( - recv_frames[1].data(), recv_frames[1].size()); - EXPECT_EQ(header.fingerprint, info->fingerprint); - - const auto md = cortex_wire::DecodedMetadata::from_bytes( - recv_frames[2].data(), recv_frames[2].size()); - ASSERT_EQ(md.field_count(), 1u); - ASSERT_EQ(md.field(0).type, msgpack::type::STR); - EXPECT_EQ( - std::string(md.field(0).via.str.ptr, md.field(0).via.str.size), "echo me"); - - spinning.store(false); - spinner.join(); - std::remove(config_path.c_str()); -} diff --git a/ros2_bridge/src/cortex_ros2_bridge/test/test_config.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_config.cpp deleted file mode 100644 index d3e88e0..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/test/test_config.cpp +++ /dev/null @@ -1,324 +0,0 @@ -// 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); -} diff --git a/ros2_bridge/src/cortex_ros2_bridge/test/test_intra_process.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_intra_process.cpp deleted file mode 100644 index d9dcfac..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/test/test_intra_process.cpp +++ /dev/null @@ -1,373 +0,0 @@ -// 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/src/cortex_ros2_bridge/test/test_primitives.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_primitives.cpp deleted file mode 100644 index baef4b2..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/test/test_primitives.cpp +++ /dev/null @@ -1,266 +0,0 @@ -// 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/src/cortex_ros2_bridge/test/test_qos.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_qos.cpp deleted file mode 100644 index 8261b57..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/test/test_qos.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright (c) 2026, Cortex contributors. Apache-2.0. -#include "cortex_ros2_bridge/qos.hpp" - -#include - -using cortex_ros2_bridge::Durability; -using cortex_ros2_bridge::History; -using cortex_ros2_bridge::QosSettings; -using cortex_ros2_bridge::Reliability; -using cortex_ros2_bridge::make_qos; - -TEST(MakeQos, DefaultsMapToReliableVolatileKeepLast) -{ - QosSettings q; - const auto qos = make_qos(q); - const auto p = qos.get_rmw_qos_profile(); - EXPECT_EQ(p.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); - EXPECT_EQ(p.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); - EXPECT_EQ(p.history, RMW_QOS_POLICY_HISTORY_KEEP_LAST); - EXPECT_EQ(p.depth, 10u); -} - -TEST(MakeQos, BestEffortIsApplied) -{ - QosSettings q; - q.reliability = Reliability::BestEffort; - const auto qos = make_qos(q); - EXPECT_EQ( - qos.get_rmw_qos_profile().reliability, RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); -} - -TEST(MakeQos, TransientLocalIsApplied) -{ - QosSettings q; - q.durability = Durability::TransientLocal; - const auto qos = make_qos(q); - EXPECT_EQ( - qos.get_rmw_qos_profile().durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); -} - -TEST(MakeQos, KeepAllHistory) -{ - QosSettings q; - q.history = History::KeepAll; - const auto qos = make_qos(q); - EXPECT_EQ( - qos.get_rmw_qos_profile().history, RMW_QOS_POLICY_HISTORY_KEEP_ALL); -} - -TEST(MakeQos, DepthIsPropagated) -{ - QosSettings q; - q.depth = 7; - const auto qos = make_qos(q); - EXPECT_EQ(qos.get_rmw_qos_profile().depth, 7u); -} diff --git a/ros2_bridge/src/cortex_ros2_bridge/test/test_registry.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_registry.cpp deleted file mode 100644 index 4a9f46e..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/test/test_registry.cpp +++ /dev/null @@ -1,178 +0,0 @@ -// 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()); -} diff --git a/ros2_bridge/src/cortex_ros2_bridge/test/test_standard_adapters.cpp b/ros2_bridge/src/cortex_ros2_bridge/test/test_standard_adapters.cpp deleted file mode 100644 index 84105ee..0000000 --- a/ros2_bridge/src/cortex_ros2_bridge/test/test_standard_adapters.cpp +++ /dev/null @@ -1,409 +0,0 @@ -// 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")); -}