diff --git a/.clang-format b/.clang-format index 4c78d3a..67d78ee 100644 --- a/.clang-format +++ b/.clang-format @@ -21,3 +21,10 @@ AlignAfterOpenBracket: AlwaysBreak AllowShortBlocksOnASingleLine: Empty AllowShortFunctionsOnASingleLine: Empty AllowShortIfStatementsOnASingleLine: Never + +--- +# Same column limit and indent as C++ for the protobuf schemas under pj_base/proto/. +Language: Proto +BasedOnStyle: Google +ColumnLimit: 120 +IndentWidth: 2 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 245b5a3..bf0c5fc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -17,6 +17,7 @@ repos: - id: check-xml - id: check-yaml exclude: .gitlab-ci.yml + args: ['--allow-multiple-documents'] - id: debug-statements - id: end-of-file-fixer exclude_types: [svg] @@ -30,5 +31,5 @@ repos: rev: v22.1.0 hooks: - id: clang-format - types_or: [c++, c] + types_or: [c++, c, proto] args: ['-fallback-style=none', '-i'] diff --git a/docs/builtin_type.md b/docs/builtin_type.md index 0c87f32..c9c8947 100644 --- a/docs/builtin_type.md +++ b/docs/builtin_type.md @@ -14,12 +14,29 @@ image message schema can both become `PJ::sdk::Image`; a ROS The public headers live under: ```cpp -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// Codecs — one per type, all share the canonical PJ. wire format under pj_base/proto/pj/. +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include ``` @@ -66,14 +83,16 @@ Builtin objects fall into two serialization families: | Family | Current types | Storage model | Codec policy | |--------|---------------|---------------|--------------| -| Byte-backed views | `Image`, `DepthImage`, `PointCloud` | Header fields live in the SDK struct; payload bytes live behind `Span` plus `BufferAnchor`. | No mandatory canonical codec; preserve zero-copy views over ROS, MCAP, compressed image, point-cloud, or plugin-owned payloads. If conversion is unavoidable, allocate a new payload and anchor it. | -| Owned values | `ImageAnnotations`, `FrameTransforms`; future marker types | SDK structs own their vectors/strings/scalars directly. | Add explicit codecs when canonical bytes are needed. Codecs serialize the owned value to the protobuf-wire payload described by the `.proto` contract, using shared private wire primitives. | +| Byte-backed views | `Image`, `DepthImage`, `PointCloud`, `CompressedPointCloud`, `OccupancyGrid`, `Mesh3D`, `VideoFrame` | Header fields live in the SDK struct; payload bytes live behind `Span` plus `BufferAnchor`. | No mandatory canonical codec; preserve zero-copy views over ROS, MCAP, compressed image, point-cloud, or plugin-owned payloads. If conversion is unavoidable, allocate a new payload and anchor it. | +| Owned values | `ImageAnnotations`, `FrameTransforms`, `SceneEntities`, `AssetVideo`, `RobotDescription`; future marker types | SDK structs own their vectors/strings/scalars directly. | Add explicit codecs when canonical bytes are needed. Codecs serialize the owned value to the protobuf-wire payload described by the `.proto` contract, using shared private wire primitives. `RobotDescription` carries source-format text as-is (no canonical codec) — the format hint distinguishes URDF / SDF / MJCF. | Canonical `.proto` files live under `pj_base/proto/pj` and act as the wire -format contract. `PJ.ImageAnnotations` describes the annotation codec payload. -`PJ.FrameTransforms` describes the transform codec payload. Shared geometry -primitives are grouped in `Geometry.proto`: `Point2`, `Point3`, `Vector2`, -`Vector3`, and `Quaternion`. +format contract. One file per top-level message, each named after its message +(`Image.proto`, `SceneEntities.proto`, `FrameTransforms.proto`, …). Shared +geometry primitives are grouped in `Geometry.proto`: `Point2`, `Point3`, +`Vector2`, `Vector3`, `Quaternion`, and `Pose`. See +[`pj_base/proto/pj/README.md`](../pj_base/proto/pj/README.md) for the family +grouping (raster, point-cloud, scene, 2D annotation, …). The codecs do not expose generated Protobuf types in public SDK headers. The current implementation does not require generated Protobuf code or a Protobuf @@ -94,6 +113,13 @@ annotations, frame transforms, or no builtin object. | `kDepthImage` | `PJ::sdk::DepthImage` | Depth pixels plus camera intrinsics. | | `kImageAnnotations` | `PJ::sdk::ImageAnnotations` | Pixel-space overlay primitives. | | `kFrameTransforms` | `PJ::sdk::FrameTransforms` | Named 3D frame relationships. | +| `kOccupancyGrid` | `PJ::sdk::OccupancyGrid` | 2D metric occupancy grid (maps, costmaps) in world coordinates. | +| `kCompressedPointCloud` | `PJ::sdk::CompressedPointCloud` | Point cloud delivered in a format-specific compressed binary (e.g. Draco). | +| `kMesh3D` | `PJ::sdk::Mesh3D` | 3D mesh asset in its native binary format (GLTF/GLB/STL/PLY/OBJ/USD/DAE). | +| `kVideoFrame` | `PJ::sdk::VideoFrame` | One frame of an inter-frame-coded video stream (h264/h265/vp9/av1). | +| `kSceneEntities` | `PJ::sdk::SceneEntities` | Procedural 3D scene primitives (arrows, cubes, lines, text, …). | +| `kAssetVideo` | `PJ::sdk::AssetVideo` | File-backed video reference plus typed playback metadata. | +| `kRobotDescription` | `PJ::sdk::RobotDescription` | Raw URDF/SDF/MJCF text + format hint. | `BuiltinObject` is `std::any`. Producers store a concrete builtin value in it; consumers recover the concrete type with `std::any_cast(&object)` or ask @@ -182,6 +208,7 @@ or any source that produces a packed point buffer. | `row_step` | `uint32_t` | Bytes per row. Usually `point_step * width` when tightly packed. | | `is_bigendian` | `bool` | Whether packed field values are big-endian. | | `is_dense` | `bool` | `false` when some points may be invalid, typically NaN-filled. | +| `frame_id` | `std::string` | Source coordinate frame for the points; needed by 3D consumers to resolve TF to a fixed frame. | | `fields` | `std::vector` | Channel layout for each point. | | `data` | `Span` | Packed point bytes. | | `anchor` | `BufferAnchor` | Keeps `data` alive when it references shared storage. | @@ -263,6 +290,191 @@ Each `FrameTransform` contains: `pj_base/builtin/frame_transforms_codec.hpp` serializes and deserializes this type using the canonical `PJ.FrameTransforms` protobuf wire format. +## OccupancyGrid + +`OccupancyGrid` is a 2D metric occupancy grid placed in world coordinates. It +covers ROS-style nav maps, costmaps, and any rasterized 2D probability / +cost layer with a metric resolution and world placement. + +Use `OccupancyGrid` when the value is a regular 2D grid whose cells carry +8-bit signed occupancy (`-1` unknown, `0..100` percent occupied). Frame +graph navigation builtins use this rather than `Image` because the +renderer cares about cell-to-world placement, not pixel layout. + +| Field | Type | Notes | +|-------|------|-------| +| `timestamp_ns` | `Timestamp` | Timestamp associated with the map. | +| `frame_id` | `std::string` | Frame in which `origin` is expressed. | +| `origin` | `Pose` | World pose of cell (0, 0). The grid lies in `origin`'s local xy-plane. | +| `resolution` | `double` | Cell size in meters (square cells). | +| `width` | `uint32_t` | Number of columns (cells along x). | +| `height` | `uint32_t` | Number of rows (cells along y). | +| `data` | `Span` | Row-major cell bytes; size must equal `width * height`. | +| `anchor` | `BufferAnchor` | Keeps `data` alive when it references shared storage. | + +`pj_base/builtin/occupancy_grid_codec.hpp` serializes and deserializes this +type using the canonical `PJ.OccupancyGrid` protobuf wire format. + +## CompressedPointCloud + +`CompressedPointCloud` carries a point cloud delivered in a format-specific +compressed binary (e.g. Draco). It is distinct from `PointCloud` because +the wire layout is opaque to PlotJuggler — `data` plus `format` must be +handed to the matching decoder library, which produces a decompressed +point set on the host side. Same reasoning that separates `VideoFrame` +from `Image`. + +| Field | Type | Notes | +|-------|------|-------| +| `timestamp_ns` | `Timestamp` | Timestamp associated with the cloud. | +| `frame_id` | `std::string` | Frame in which the cloud is expressed once decoded. | +| `format` | `std::string` | Codec identifier, lowercase. Recognized values include `"draco"`. | +| `data` | `Span` | Compressed payload bytes. | +| `anchor` | `BufferAnchor` | Keeps `data` alive when it references shared storage. | + +`pj_base/builtin/compressed_point_cloud_codec.hpp` serializes and deserializes +this type using the canonical `PJ.CompressedPointCloud` wire format. + +## Mesh3D + +`Mesh3D` references a 3D mesh asset delivered in its native binary format. +The renderer hands `data` + `format` (or the contents at `url`) to a +mesh-loader library (Assimp, tinygltf, …); PlotJuggler does not parse the +asset itself. Distinct from `SceneEntities`'s `TrianglePrimitive` because +asset formats can carry richer scene content — materials, textures, +skinning, animations — that is not expressible as raw triangle soup. + +Asset source: exactly one of `data` (with `anchor` keeping the bytes +alive) or `url` should be populated. When `data` is used, `format` is +required; when `url` is used, `format` may be inferred from the file +extension. + +| Field | Type | Notes | +|-------|------|-------| +| `timestamp_ns` | `Timestamp` | Timestamp associated with the asset. | +| `frame_id` | `std::string` | Frame in which `pose` is expressed. | +| `id` | `std::string` | Republishing with the same id replaces the previous entry on the topic. | +| `pose` | `Pose` | Placement of the asset's local origin in `frame_id`. | +| `scale` | `Vector3` | Per-axis scale factor. Defaults to `(1, 1, 1)`. | +| `format` | `std::string` | `"gltf"`, `"glb"`, `"stl"`, `"ply"`, `"obj"`, `"usd"`, `"dae"`. | +| `data` | `Span` | Embedded asset bytes; non-empty implies `format` is required. | +| `anchor` | `BufferAnchor` | Keeps `data` alive when it references shared storage. | +| `url` | `std::string` | External URL to the asset; used when `data` is empty. | +| `color` | `ColorRGBA` | Applied when `override_color` is true. | +| `override_color` | `bool` | When true, ignore embedded material color and tint with `color`. | + +`pj_base/builtin/mesh3d_codec.hpp` serializes and deserializes this type +using the canonical `PJ.Mesh3D` protobuf wire format. + +## VideoFrame + +`VideoFrame` carries a single frame of a compressed video stream +(h264/h265/vp9/av1) when per-frame `Image` payloads would be wasteful. +Unlike `Image`, a video frame may have inter-frame dependencies +(P-frames, B-frames, etc.); consumers must maintain decoder state across +frames within a stream. + +| Field | Type | Notes | +|-------|------|-------| +| `timestamp_ns` | `Timestamp` | Frame presentation timestamp. | +| `frame_id` | `std::string` | Camera frame. Optical axis: `+x` right, `+y` down, `+z` into scene. | +| `format` | `std::string` | Codec identifier, lowercase. `"h264"`, `"h265"`, `"vp9"`, `"av1"`. | +| `data` | `Span` | Bitstream bytes for this frame. | +| `anchor` | `BufferAnchor` | Keeps `data` alive when it references shared storage. | + +`pj_base/builtin/video_frame_codec.hpp` serializes and deserializes this +type using the canonical `PJ.VideoFrame` protobuf wire format. + +## AssetVideo + +`AssetVideo` is the entry-point handle for video assets ingested by data +loaders that point at an external media file — LeRobot datasets, MP4 +loaders, and similar. Producers push exactly one `AssetVideo` per topic; +the ObjectStore timestamp of that entry equals `time_origin_ns` so +timeline UIs naturally see the asset's start instant. + +Unlike `VideoFrame` (a single frame of a streamed payload), `AssetVideo` +carries no pixel data — it references the file by path and surfaces +decode-routing metadata (media type, dimensions, frame rate) without +forcing the consumer to open the file just to size a playback window. + +| Field | Type | Notes | +|-------|------|-------| +| `time_origin_ns` | `std::optional` | Wall-clock instant of the first frame. Absent means the asset is not aligned to wall clock. | +| `duration_ns` | `std::optional` | Total playable duration. Absent means probe the file. | +| `file_path` | `std::string` | Absolute path or path relative to a consumer-known root. | +| `media_type` | `std::string` | MIME type hint. Empty means probe the file. | +| `width` | `uint32_t` | Pixel width. `0` means unknown. | +| `height` | `uint32_t` | Pixel height. `0` means unknown. | +| `frame_rate` | `double` | Nominal FPS. `0` or NaN means unknown. | + +`pj_base/builtin/asset_video_codec.hpp` serializes and deserializes this +type using the canonical `PJ.AssetVideo` protobuf wire format. + +## SceneEntities + +`SceneEntities` is the workhorse for marker-style 3D visualization — the +equivalent of ROS's `visualization_msgs/MarkerArray`. A `SceneEntity` +bundles heterogeneous primitives sharing a `frame_id` and timestamp; +`SceneEntities` is the batch container shipped on a topic. + +Use `SceneEntities` when the value is procedural 3D scene content +expressible as a small set of primitives: arrows, cubes, spheres, +cylinders, line strips/loops/lists, triangles, text labels, or coordinate +axes glyphs. + +| Field on `SceneEntity` | Type | Notes | +|------------------------|------|-------| +| `timestamp` | `Timestamp` | Stamp used together with `lifetime_ns` to control expiry. | +| `frame_id` | `std::string` | Frame the entity's primitives are expressed in. | +| `id` | `std::string` | Republishing with the same `(topic, id)` replaces the previous entity. | +| `lifetime_ns` | `int64_t` | `0` means persist until replaced; otherwise expire `lifetime_ns` after `timestamp`. | +| `frame_locked` | `bool` | When true, track `frame_id` as it moves; when false, stamp into the fixed frame at publish time. | +| `arrows` / `cubes` / `spheres` / `cylinders` / `lines` / `triangles` / `texts` / `axes` | `std::vector<…Primitive>` | Heterogeneous primitive lists. | + +Each primitive carries its own `Pose`, geometry-specific size or shape +fields, and color (or per-vertex colors, where applicable). See +`pj_base/include/pj_base/builtin/scene_entities.hpp` for the per-primitive +fields and `pj_base/proto/pj/SceneEntities.proto` for the wire contract. + +`pj_base/builtin/scene_entities_codec.hpp` serializes and deserializes +this type using the canonical `PJ.SceneEntities` protobuf wire format. + +## RobotDescription + +`RobotDescription` carries a robot kinematic + visual model as the raw source- +format text plus a `format` hint string. The SDK does not parse the document; +downstream consumers (notably the 3D viewer) do the format-specific parsing +and asset resolution. + +Use `RobotDescription` when the message represents a kinematic / visual model +description: a ROS `/robot_description` topic with `std_msgs/String` payload +containing URDF XML, an SDF world, an MJCF model, or any future textual robot- +description format. + +| Field | Type | Notes | +|-------|------|-------| +| `timestamp_ns` | `Timestamp` | Timestamp the description was observed. | +| `topic` | `std::string` | Source topic name (e.g. `/robot_description`). Empty if not topic-sourced. | +| `format` | `std::string` | Format hint set by the producer after validation. Examples: `urdf`, `sdf`, `mjcf`. Open-ended like `Image::encoding`. | +| `text` | `std::string` | Raw source text. Consumers parse according to `format`. | + +Design notes: + +- **No canonical codec.** The format space is open and growing; embedding a + format-specific codec in the SDK would multiply schemas without payoff. + Consumers parse the text directly with format-specific libraries (e.g. + TinyXML for URDF / SDF / COLLADA, mjcf parsers for MJCF). +- **No embedded mesh bytes.** URDF/SDF reference meshes via `package://` URIs + or relative paths; mesh resolution is consumer-side (search paths, MCAP + attachments, sidecar directories). Embedding meshes in the SDK type would + force assumptions about that resolution and bloat ObjectStore for the + common case of a single robot referenced by thousands of TF samples. +- **Producer responsibility.** A parser emitting `RobotDescription` should + validate the text matches `format` (e.g. for URDF, that the root element is + ``) before emission. Generic `std_msgs/String` payloads on unrelated + topics should not surface as RobotDescription. + ## Conversion Examples | Source type | Canonical builtin type | Conversion intent | @@ -270,8 +482,16 @@ using the canonical `PJ.FrameTransforms` protobuf wire format. | ROS `sensor_msgs/Image` | `Image` or `DepthImage` | Choose `DepthImage` when the semantic value is metric depth; otherwise use `Image`. | | ROS `sensor_msgs/CompressedImage` | `Image` | Preserve compressed bytes and set `encoding` to the codec. | | ROS `sensor_msgs/PointCloud2` | `PointCloud` | Map point fields, strides, density, endianness, and packed bytes. | +| Draco-compressed cloud | `CompressedPointCloud` | Forward the opaque blob plus `"draco"` format; decoding happens on the host. | +| ROS `nav_msgs/OccupancyGrid` | `OccupancyGrid` | Map metadata (resolution, origin) into the struct; keep cell bytes zero-copy. | +| URDF / `visualization_msgs/Marker` mesh resource | `Mesh3D` | Embed `data` (with `format`) or point at `url`; preserve `pose` and `scale`. | +| ROS `nav_msgs/Path`, marker arrays | `SceneEntities` | Map polylines to `LinePrimitive`, arrows to `ArrowPrimitive`, etc. | +| H.264/H.265/VP9/AV1 stream frame | `VideoFrame` | Forward one frame's bitstream bytes plus the codec identifier. | +| MP4 / MKV / AV1 dataset file | `AssetVideo` | Push once per topic with the file path and metadata; consumers seek into the file by tracker time. | | Detection or tracking message | `ImageAnnotations` | Convert boxes, points, circles, and labels into pixel-space primitives. | | ROS `tf2_msgs/TFMessage` | `FrameTransforms` | Convert transform batches into named parent/child frame relationships. | +| ROS `std_msgs/String` on `/robot_description` (or matching name) carrying URDF XML | `RobotDescription` | Validate root element matches `format`, then carry the raw text + format hint. No mesh resolution at parse time. | +| ROS `std_msgs/String` on `/robot_description` (or matching name) carrying URDF XML | `RobotDescription` | Validate root element matches `format`, then carry the raw text + format hint. No mesh resolution at parse time. | The builtin type is the boundary object. After conversion, consumers should not need to know which third-party schema produced it. diff --git a/pj_base/CMakeLists.txt b/pj_base/CMakeLists.txt index 64d22dd..0ca38a1 100644 --- a/pj_base/CMakeLists.txt +++ b/pj_base/CMakeLists.txt @@ -3,8 +3,17 @@ # --------------------------------------------------------------------------- add_library(pj_base STATIC + src/builtin/asset_video_codec.cpp + src/builtin/compressed_point_cloud_codec.cpp + src/builtin/depth_image_codec.cpp src/builtin/frame_transforms_codec.cpp src/builtin/image_annotations_codec.cpp + src/builtin/image_codec.cpp + src/builtin/mesh3d_codec.cpp + src/builtin/occupancy_grid_codec.cpp + src/builtin/point_cloud_codec.cpp + src/builtin/scene_entities_codec.cpp + src/builtin/video_frame_codec.cpp src/type_tree.cpp ) target_include_directories(pj_base PUBLIC @@ -63,6 +72,15 @@ if(PJ_BUILD_TESTS) tests/image_annotations_decoder_test.cpp tests/media_metadata_test.cpp tests/push_message_v2_test.cpp + tests/image_codec_test.cpp + tests/depth_image_codec_test.cpp + tests/point_cloud_codec_test.cpp + tests/compressed_point_cloud_codec_test.cpp + tests/occupancy_grid_codec_test.cpp + tests/mesh3d_codec_test.cpp + tests/video_frame_codec_test.cpp + tests/scene_entities_codec_test.cpp + tests/asset_video_codec_test.cpp ) foreach(test_src ${PJ_BASE_TESTS}) diff --git a/pj_base/include/pj_base/builtin/asset_video.hpp b/pj_base/include/pj_base/builtin/asset_video.hpp new file mode 100644 index 0000000..9949222 --- /dev/null +++ b/pj_base/include/pj_base/builtin/asset_video.hpp @@ -0,0 +1,60 @@ +/** + * @file asset_video.hpp + * @brief File-backed video reference + typed playback metadata. + * + * AssetVideo is the entry-point handle for video assets ingested by data + * loaders that point at an external media file (LeRobot datasets, MP4 + * loaders, etc.). Producers push exactly one AssetVideo per topic; the + * ObjectStore timestamp of that entry equals `time_origin_ns` so timeline + * UIs naturally see the asset's start instant. + * + * Unlike VideoFrame (a single frame of a streamed payload), AssetVideo + * carries no pixel data — it references the file by path and surfaces + * decode-routing metadata (media type, dimensions, frame rate) without + * forcing the consumer to open the file just to size a playback window. + */ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +#include "pj_base/types.hpp" + +namespace PJ { +namespace sdk { + +/// File-backed video reference with typed metadata. +/// +/// `time_origin_ns` carries the wall-clock instant of the first frame. +/// Consumers subtract this from a tracker time to derive a file-relative +/// seek position. An absent value means the asset is not aligned to wall +/// clock and should not advance with the tracker. +/// +/// `duration_ns` is the total playable duration when known; absent means +/// consumers may probe the file. +/// +/// `media_type` is a MIME-type hint ("video/mp4", "video/x-matroska", +/// "video/av1"). Empty string means "probe the file". +/// +/// `width` / `height` are pixel dimensions; zero in either field means +/// "unknown — probe the file". +/// +/// `frame_rate` is nominal frames per second; zero or NaN means "unknown — +/// probe the file". For variable-frame-rate video this is an advisory +/// average; actual per-frame timestamps come from the decoder. +struct AssetVideo { + std::optional time_origin_ns; ///< Wall-clock instant of the first frame. + std::optional duration_ns; ///< Total playable duration in nanoseconds. + std::string file_path; ///< Absolute path or path relative to a consumer-known root. + std::string media_type; ///< MIME type. Empty string means "probe the file". + uint32_t width = 0; ///< Pixel width. 0 means unknown. + uint32_t height = 0; ///< Pixel height. 0 means unknown. + double frame_rate = 0.0; ///< Nominal frames per second. 0 or NaN means unknown. +}; + +} // namespace sdk +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/asset_video_codec.hpp b/pj_base/include/pj_base/builtin/asset_video_codec.hpp new file mode 100644 index 0000000..16358c4 --- /dev/null +++ b/pj_base/include/pj_base/builtin/asset_video_codec.hpp @@ -0,0 +1,24 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include + +#include "pj_base/builtin/asset_video.hpp" +#include "pj_base/expected.hpp" + +namespace PJ { + +inline constexpr std::string_view kSchemaAssetVideo = "PJ.AssetVideo"; + +/// Serializes sdk::AssetVideo to canonical PJ.AssetVideo wire bytes (see +/// pj_base/proto/pj/AssetVideo.proto). +[[nodiscard]] std::vector serializeAssetVideo(const sdk::AssetVideo& asset); + +/// Decodes canonical PJ.AssetVideo wire bytes into sdk::AssetVideo. +[[nodiscard]] Expected deserializeAssetVideo(const uint8_t* data, size_t size); + +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/BuiltinObject.hpp b/pj_base/include/pj_base/builtin/builtin_object.hpp similarity index 51% rename from pj_base/include/pj_base/builtin/BuiltinObject.hpp rename to pj_base/include/pj_base/builtin/builtin_object.hpp index cd10ec9..a6f7055 100644 --- a/pj_base/include/pj_base/builtin/BuiltinObject.hpp +++ b/pj_base/include/pj_base/builtin/builtin_object.hpp @@ -1,5 +1,5 @@ /** - * @file BuiltinObject.h + * @file builtin_object.hpp * @brief Type-erased holder for any builtin object a MessageParser may produce. * * BuiltinObject is `std::any`. A producer constructs it by passing a @@ -26,24 +26,36 @@ #include #include -#include "pj_base/builtin/DepthImage.hpp" -#include "pj_base/builtin/FrameTransforms.hpp" -#include "pj_base/builtin/Image.hpp" -#include "pj_base/builtin/ImageAnnotations.hpp" -#include "pj_base/builtin/PointCloud.hpp" +#include "pj_base/builtin/asset_video.hpp" +#include "pj_base/builtin/compressed_point_cloud.hpp" +#include "pj_base/builtin/depth_image.hpp" +#include "pj_base/builtin/frame_transforms.hpp" +#include "pj_base/builtin/image.hpp" +#include "pj_base/builtin/image_annotations.hpp" +#include "pj_base/builtin/mesh3d.hpp" +#include "pj_base/builtin/occupancy_grid.hpp" +#include "pj_base/builtin/point_cloud.hpp" +#include "pj_base/builtin/robot_description.hpp" +#include "pj_base/builtin/scene_entities.hpp" +#include "pj_base/builtin/video_frame.hpp" namespace PJ { namespace sdk { enum class BuiltinObjectType : uint16_t { kNone = 0, - kImage = 1, ///< sdk::Image — raw or compressed, distinguished by encoding string. - kPointCloud = 3, ///< sdk::PointCloud — packed points + per-channel field layout. - kDepthImage = 4, ///< sdk::DepthImage — depth pixels + camera intrinsics. - kImageAnnotations = 5, ///< sdk::ImageAnnotations — 2D overlays (points, lines, text). - kFrameTransforms = 6, ///< sdk::FrameTransforms — named 3D frame relationships. - // Reserved for future types; keep numeric values stable across releases. - // kOccupancyGrid = 7, + kImage = 1, ///< sdk::Image — raw or compressed, distinguished by encoding string. + kPointCloud = 3, ///< sdk::PointCloud — packed points + per-channel field layout. + kDepthImage = 4, ///< sdk::DepthImage — depth pixels + camera intrinsics. + kImageAnnotations = 5, ///< sdk::ImageAnnotations — 2D overlays (points, lines, text). + kFrameTransforms = 6, ///< sdk::FrameTransforms — named 3D frame relationships. + kOccupancyGrid = 7, ///< sdk::OccupancyGrid — 2D metric grid (maps, costmaps). + kCompressedPointCloud = 8, ///< sdk::CompressedPointCloud — opaque compressed cloud (Draco, ...). + kMesh3D = 9, ///< sdk::Mesh3D — binary mesh asset (GLTF/STL/PLY/OBJ/USD/DAE). + kVideoFrame = 10, ///< sdk::VideoFrame — single frame of h264/h265/vp9/av1 stream. + kSceneEntities = 11, ///< sdk::SceneEntities — procedural 3D scene primitives. + kAssetVideo = 12, ///< sdk::AssetVideo — file-backed video reference + playback metadata. + kRobotDescription = 13, ///< sdk::RobotDescription — raw URDF/SDF/MJCF text + format hint. }; /// A-priori classification of a schema. Currently carries only the type; @@ -68,6 +80,20 @@ struct SchemaClassification { return "kImageAnnotations"; case BuiltinObjectType::kFrameTransforms: return "kFrameTransforms"; + case BuiltinObjectType::kOccupancyGrid: + return "kOccupancyGrid"; + case BuiltinObjectType::kCompressedPointCloud: + return "kCompressedPointCloud"; + case BuiltinObjectType::kMesh3D: + return "kMesh3D"; + case BuiltinObjectType::kVideoFrame: + return "kVideoFrame"; + case BuiltinObjectType::kSceneEntities: + return "kSceneEntities"; + case BuiltinObjectType::kAssetVideo: + return "kAssetVideo"; + case BuiltinObjectType::kRobotDescription: + return "kRobotDescription"; } return "kNone"; } @@ -93,6 +119,27 @@ struct SchemaClassification { if (s == "kFrameTransforms") { return BuiltinObjectType::kFrameTransforms; } + if (s == "kOccupancyGrid") { + return BuiltinObjectType::kOccupancyGrid; + } + if (s == "kCompressedPointCloud") { + return BuiltinObjectType::kCompressedPointCloud; + } + if (s == "kMesh3D") { + return BuiltinObjectType::kMesh3D; + } + if (s == "kVideoFrame") { + return BuiltinObjectType::kVideoFrame; + } + if (s == "kSceneEntities") { + return BuiltinObjectType::kSceneEntities; + } + if (s == "kAssetVideo") { + return BuiltinObjectType::kAssetVideo; + } + if (s == "kRobotDescription") { + return BuiltinObjectType::kRobotDescription; + } return std::nullopt; } @@ -121,6 +168,27 @@ using BuiltinObject = std::any; if (t == typeid(FrameTransforms)) { return BuiltinObjectType::kFrameTransforms; } + if (t == typeid(OccupancyGrid)) { + return BuiltinObjectType::kOccupancyGrid; + } + if (t == typeid(CompressedPointCloud)) { + return BuiltinObjectType::kCompressedPointCloud; + } + if (t == typeid(Mesh3D)) { + return BuiltinObjectType::kMesh3D; + } + if (t == typeid(VideoFrame)) { + return BuiltinObjectType::kVideoFrame; + } + if (t == typeid(SceneEntities)) { + return BuiltinObjectType::kSceneEntities; + } + if (t == typeid(AssetVideo)) { + return BuiltinObjectType::kAssetVideo; + } + if (t == typeid(RobotDescription)) { + return BuiltinObjectType::kRobotDescription; + } return BuiltinObjectType::kNone; } diff --git a/pj_base/include/pj_base/builtin/compressed_point_cloud.hpp b/pj_base/include/pj_base/builtin/compressed_point_cloud.hpp new file mode 100644 index 0000000..c685f3e --- /dev/null +++ b/pj_base/include/pj_base/builtin/compressed_point_cloud.hpp @@ -0,0 +1,41 @@ +/** + * @file compressed_point_cloud.hpp + * @brief Point cloud delivered in a compressed binary format (Draco, ...). + */ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#include "pj_base/buffer_anchor.hpp" +#include "pj_base/span.hpp" +#include "pj_base/types.hpp" + +namespace PJ { +namespace sdk { + +/// Point cloud delivered in a compressed binary format. Unlike PointCloud, +/// the wire layout is opaque to PlotJuggler — `data` + `format` must be +/// handed to the matching decoder library (e.g. Draco), which produces a +/// decompressed point set on the host side. +/// +/// This type is distinct from PointCloud because per-format decoders carry +/// their own attribute table and layout semantics. Same reasoning that +/// separates VideoFrame from Image. +/// +/// `anchor` keeps the underlying buffer alive — the producer may have made +/// `data` a view into the source payload (zero-copy) or into a freshly +/// allocated vector; consumers don't need to know which. +struct CompressedPointCloud { + Timestamp timestamp_ns = 0; + std::string frame_id; + std::string format; ///< Codec identifier, lowercase. Recognized values include "draco". + Span data; + BufferAnchor anchor; +}; + +} // namespace sdk +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/compressed_point_cloud_codec.hpp b/pj_base/include/pj_base/builtin/compressed_point_cloud_codec.hpp new file mode 100644 index 0000000..2388fd1 --- /dev/null +++ b/pj_base/include/pj_base/builtin/compressed_point_cloud_codec.hpp @@ -0,0 +1,25 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include + +#include "pj_base/builtin/compressed_point_cloud.hpp" +#include "pj_base/expected.hpp" + +namespace PJ { + +inline constexpr std::string_view kSchemaCompressedPointCloud = "PJ.CompressedPointCloud"; + +/// Serializes sdk::CompressedPointCloud to canonical PJ.CompressedPointCloud +/// wire bytes (see pj_base/proto/pj/CompressedPointCloud.proto). +[[nodiscard]] std::vector serializeCompressedPointCloud(const sdk::CompressedPointCloud& cloud); + +/// Decodes canonical PJ.CompressedPointCloud wire bytes. The returned object +/// owns its bytes via `anchor`. +[[nodiscard]] Expected deserializeCompressedPointCloud(const uint8_t* data, size_t size); + +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/DepthImage.hpp b/pj_base/include/pj_base/builtin/depth_image.hpp similarity index 98% rename from pj_base/include/pj_base/builtin/DepthImage.hpp rename to pj_base/include/pj_base/builtin/depth_image.hpp index 13279bc..f97adf4 100644 --- a/pj_base/include/pj_base/builtin/DepthImage.hpp +++ b/pj_base/include/pj_base/builtin/depth_image.hpp @@ -1,5 +1,5 @@ /** - * @file DepthImage.h + * @file depth_image.hpp * @brief Depth image with camera intrinsics. */ // Copyright 2026 Davide Faconti diff --git a/pj_base/include/pj_base/builtin/depth_image_codec.hpp b/pj_base/include/pj_base/builtin/depth_image_codec.hpp new file mode 100644 index 0000000..6fedb2a --- /dev/null +++ b/pj_base/include/pj_base/builtin/depth_image_codec.hpp @@ -0,0 +1,25 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include + +#include "pj_base/builtin/depth_image.hpp" +#include "pj_base/expected.hpp" + +namespace PJ { + +inline constexpr std::string_view kSchemaDepthImage = "PJ.DepthImage"; + +/// Serializes sdk::DepthImage to canonical PJ.DepthImage wire bytes (see +/// pj_base/proto/pj/DepthImage.proto). +[[nodiscard]] std::vector serializeDepthImage(const sdk::DepthImage& depth); + +/// Decodes canonical PJ.DepthImage wire bytes. The returned object owns +/// its depth pixel bytes via `anchor`. +[[nodiscard]] Expected deserializeDepthImage(const uint8_t* data, size_t size); + +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/depth_image_utils.hpp b/pj_base/include/pj_base/builtin/depth_image_utils.hpp index b5076aa..bf4e008 100644 --- a/pj_base/include/pj_base/builtin/depth_image_utils.hpp +++ b/pj_base/include/pj_base/builtin/depth_image_utils.hpp @@ -1,5 +1,5 @@ /** - * @file depth_image_utils.h + * @file depth_image_utils.hpp * @brief Free-function helpers that derive conventional matrices (R, P) * from sdk::DepthImage's intrinsics. * @@ -16,7 +16,7 @@ #include -#include "pj_base/builtin/DepthImage.hpp" +#include "pj_base/builtin/depth_image.hpp" namespace PJ { namespace sdk { diff --git a/pj_base/include/pj_base/builtin/FrameTransforms.hpp b/pj_base/include/pj_base/builtin/frame_transforms.hpp similarity index 81% rename from pj_base/include/pj_base/builtin/FrameTransforms.hpp rename to pj_base/include/pj_base/builtin/frame_transforms.hpp index e1b9e65..f6f669c 100644 --- a/pj_base/include/pj_base/builtin/FrameTransforms.hpp +++ b/pj_base/include/pj_base/builtin/frame_transforms.hpp @@ -1,5 +1,5 @@ /** - * @file FrameTransforms.h + * @file frame_transforms.hpp * @brief Time-stamped 3D transforms between named reference frames. * * FrameTransforms is a small owned builtin for TF-style frame relationships. @@ -35,6 +35,15 @@ struct Quaternion { bool operator==(const Quaternion&) const = default; }; +/// Rigid transform in 3D space: position + orientation. +/// Used by Mesh3D, OccupancyGrid, SceneEntities, and other types that +/// place data in a frame of reference. +struct Pose { + Vector3 position; + Quaternion orientation; + bool operator==(const Pose&) const = default; +}; + /// Transform from `parent_frame_id` to `child_frame_id`. struct FrameTransform { Timestamp timestamp = 0; diff --git a/pj_base/include/pj_base/builtin/frame_transforms_codec.hpp b/pj_base/include/pj_base/builtin/frame_transforms_codec.hpp index c81f183..fe5d119 100644 --- a/pj_base/include/pj_base/builtin/frame_transforms_codec.hpp +++ b/pj_base/include/pj_base/builtin/frame_transforms_codec.hpp @@ -7,7 +7,7 @@ #include #include -#include "pj_base/builtin/FrameTransforms.hpp" +#include "pj_base/builtin/frame_transforms.hpp" #include "pj_base/expected.hpp" namespace PJ { diff --git a/pj_base/include/pj_base/builtin/Image.hpp b/pj_base/include/pj_base/builtin/image.hpp similarity index 99% rename from pj_base/include/pj_base/builtin/Image.hpp rename to pj_base/include/pj_base/builtin/image.hpp index 090c36a..e78aec7 100644 --- a/pj_base/include/pj_base/builtin/Image.hpp +++ b/pj_base/include/pj_base/builtin/image.hpp @@ -1,5 +1,5 @@ /** - * @file Image.h + * @file image.hpp * @brief Image built-in object: raw or compressed, identified by an * encoding string. */ diff --git a/pj_base/include/pj_base/builtin/ImageAnnotations.hpp b/pj_base/include/pj_base/builtin/image_annotations.hpp similarity index 99% rename from pj_base/include/pj_base/builtin/ImageAnnotations.hpp rename to pj_base/include/pj_base/builtin/image_annotations.hpp index 75b88fd..83cd016 100644 --- a/pj_base/include/pj_base/builtin/ImageAnnotations.hpp +++ b/pj_base/include/pj_base/builtin/image_annotations.hpp @@ -1,5 +1,5 @@ /** - * @file ImageAnnotations.h + * @file image_annotations.hpp * @brief Vector primitives (points, lines, circles, text) overlaid on a * specific image at a specific timestamp. * diff --git a/pj_base/include/pj_base/builtin/image_annotations_codec.hpp b/pj_base/include/pj_base/builtin/image_annotations_codec.hpp index b1865f1..c26b7a3 100644 --- a/pj_base/include/pj_base/builtin/image_annotations_codec.hpp +++ b/pj_base/include/pj_base/builtin/image_annotations_codec.hpp @@ -7,7 +7,7 @@ #include #include -#include "pj_base/builtin/ImageAnnotations.hpp" +#include "pj_base/builtin/image_annotations.hpp" #include "pj_base/expected.hpp" namespace PJ { diff --git a/pj_base/include/pj_base/builtin/image_codec.hpp b/pj_base/include/pj_base/builtin/image_codec.hpp new file mode 100644 index 0000000..c249387 --- /dev/null +++ b/pj_base/include/pj_base/builtin/image_codec.hpp @@ -0,0 +1,25 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include + +#include "pj_base/builtin/image.hpp" +#include "pj_base/expected.hpp" + +namespace PJ { + +inline constexpr std::string_view kSchemaImage = "PJ.Image"; + +/// Serializes sdk::Image to canonical PJ.Image wire bytes (see +/// pj_base/proto/pj/Image.proto). +[[nodiscard]] std::vector serializeImage(const sdk::Image& image); + +/// Decodes canonical PJ.Image wire bytes. The returned image owns its +/// pixel/compressed bytes via `anchor`. +[[nodiscard]] Expected deserializeImage(const uint8_t* data, size_t size); + +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/mesh3d.hpp b/pj_base/include/pj_base/builtin/mesh3d.hpp new file mode 100644 index 0000000..b0d4721 --- /dev/null +++ b/pj_base/include/pj_base/builtin/mesh3d.hpp @@ -0,0 +1,52 @@ +/** + * @file mesh3d.hpp + * @brief 3D mesh asset in its native binary format (GLTF/STL/PLY/OBJ/USD/DAE). + */ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#include "pj_base/buffer_anchor.hpp" +#include "pj_base/builtin/frame_transforms.hpp" // for Pose, Vector3 +#include "pj_base/builtin/image_annotations.hpp" // for ColorRGBA +#include "pj_base/span.hpp" +#include "pj_base/types.hpp" + +namespace PJ { +namespace sdk { + +/// 3D mesh asset delivered in its native binary format. The renderer hands +/// `data` + `format` (or the contents at `url`) to a mesh-loader library +/// (Assimp, tinygltf, ...); PlotJuggler does not parse the asset itself. +/// +/// Distinct from SceneEntities' TrianglePrimitive because asset formats can +/// carry richer scene content (materials, textures, skinning, animations) +/// that is not expressible as raw triangle soup. +/// +/// Asset source: exactly one of `data` (with `anchor` keeping the bytes +/// alive) or `url` should be populated. When `data` is used, `format` is +/// required; when `url` is used, `format` may be inferred from the file +/// extension. +struct Mesh3D { + Timestamp timestamp_ns = 0; + std::string frame_id; + std::string id; ///< Republishing with the same id replaces the previous entry on the topic. + + Pose pose; + Vector3 scale{1.0, 1.0, 1.0}; + + std::string format; ///< "gltf", "glb", "stl", "ply", "obj", "usd", "dae" + Span data; ///< Embedded asset bytes; when non-empty, `format` is required. + BufferAnchor anchor; + std::string url; ///< External URL to the asset. Used when `data` is empty. + + ColorRGBA color; ///< Applied when `override_color` is true. + bool override_color = false; +}; + +} // namespace sdk +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/mesh3d_codec.hpp b/pj_base/include/pj_base/builtin/mesh3d_codec.hpp new file mode 100644 index 0000000..2f912c1 --- /dev/null +++ b/pj_base/include/pj_base/builtin/mesh3d_codec.hpp @@ -0,0 +1,25 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include + +#include "pj_base/builtin/mesh3d.hpp" +#include "pj_base/expected.hpp" + +namespace PJ { + +inline constexpr std::string_view kSchemaMesh3D = "PJ.Mesh3D"; + +/// Serializes sdk::Mesh3D to canonical PJ.Mesh3D wire bytes (see +/// pj_base/proto/pj/Mesh3D.proto). +[[nodiscard]] std::vector serializeMesh3D(const sdk::Mesh3D& mesh); + +/// Decodes canonical PJ.Mesh3D wire bytes. The returned mesh owns its +/// embedded bytes via `anchor` (or `data` is empty when only `url` was set). +[[nodiscard]] Expected deserializeMesh3D(const uint8_t* data, size_t size); + +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/occupancy_grid.hpp b/pj_base/include/pj_base/builtin/occupancy_grid.hpp new file mode 100644 index 0000000..bd937f2 --- /dev/null +++ b/pj_base/include/pj_base/builtin/occupancy_grid.hpp @@ -0,0 +1,49 @@ +/** + * @file occupancy_grid.hpp + * @brief 2D metric occupancy grid placed in world coordinates. + */ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#include "pj_base/buffer_anchor.hpp" +#include "pj_base/builtin/frame_transforms.hpp" // for Pose +#include "pj_base/span.hpp" +#include "pj_base/types.hpp" + +namespace PJ { +namespace sdk { + +/// 2D metric occupancy grid placed in world coordinates. +/// +/// The grid lies in the local xy-plane of `origin`, with cell (0, 0) at the +/// origin's translation. Each cell holds a signed 8-bit integer: +/// -1 : unknown / no data +/// 0..100 : probability of occupied, in percent +/// other : reserved +/// +/// Cell (row r, column c) is at world position +/// origin + (c * resolution, r * resolution, 0) in `frame_id`. +/// Layout is row-major: data[r * width + c]. `data.size()` must equal +/// `width * height`. +/// +/// `anchor` keeps the underlying buffer alive — the producer may have made +/// `data` a view into the source payload (zero-copy) or into a freshly +/// allocated vector; consumers don't need to know which. +struct OccupancyGrid { + Timestamp timestamp_ns = 0; + std::string frame_id; + Pose origin; + double resolution = 0.0; ///< Cell size in meters (square cells). + uint32_t width = 0; ///< Number of columns (cells along x). + uint32_t height = 0; ///< Number of rows (cells along y). + Span data; + BufferAnchor anchor; +}; + +} // namespace sdk +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/occupancy_grid_codec.hpp b/pj_base/include/pj_base/builtin/occupancy_grid_codec.hpp new file mode 100644 index 0000000..4e10968 --- /dev/null +++ b/pj_base/include/pj_base/builtin/occupancy_grid_codec.hpp @@ -0,0 +1,25 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include + +#include "pj_base/builtin/occupancy_grid.hpp" +#include "pj_base/expected.hpp" + +namespace PJ { + +inline constexpr std::string_view kSchemaOccupancyGrid = "PJ.OccupancyGrid"; + +/// Serializes sdk::OccupancyGrid to canonical PJ.OccupancyGrid wire bytes +/// (see pj_base/proto/pj/OccupancyGrid.proto). +[[nodiscard]] std::vector serializeOccupancyGrid(const sdk::OccupancyGrid& grid); + +/// Decodes canonical PJ.OccupancyGrid wire bytes. The returned object owns +/// its cell bytes via `anchor`. +[[nodiscard]] Expected deserializeOccupancyGrid(const uint8_t* data, size_t size); + +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/PointCloud.hpp b/pj_base/include/pj_base/builtin/point_cloud.hpp similarity index 95% rename from pj_base/include/pj_base/builtin/PointCloud.hpp rename to pj_base/include/pj_base/builtin/point_cloud.hpp index 8874434..13db6e7 100644 --- a/pj_base/include/pj_base/builtin/PointCloud.hpp +++ b/pj_base/include/pj_base/builtin/point_cloud.hpp @@ -1,5 +1,5 @@ /** - * @file PointCloud.h + * @file point_cloud.hpp * @brief Packed point cloud with per-channel field layout. */ // Copyright 2026 Davide Faconti @@ -71,6 +71,7 @@ struct PointCloud { uint32_t row_step = 0; ///< Bytes per row (= point_step * width when no padding). bool is_bigendian = false; bool is_dense = true; + std::string frame_id; ///< Source coordinate frame; required for 3D TF resolution. std::vector fields; Span data; BufferAnchor anchor; diff --git a/pj_base/include/pj_base/builtin/point_cloud_codec.hpp b/pj_base/include/pj_base/builtin/point_cloud_codec.hpp new file mode 100644 index 0000000..cba4223 --- /dev/null +++ b/pj_base/include/pj_base/builtin/point_cloud_codec.hpp @@ -0,0 +1,25 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include + +#include "pj_base/builtin/point_cloud.hpp" +#include "pj_base/expected.hpp" + +namespace PJ { + +inline constexpr std::string_view kSchemaPointCloud = "PJ.PointCloud"; + +/// Serializes sdk::PointCloud to canonical PJ.PointCloud wire bytes (see +/// pj_base/proto/pj/PointCloud.proto). +[[nodiscard]] std::vector serializePointCloud(const sdk::PointCloud& cloud); + +/// Decodes canonical PJ.PointCloud wire bytes. The returned object owns +/// its packed point bytes via `anchor`. +[[nodiscard]] Expected deserializePointCloud(const uint8_t* data, size_t size); + +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/robot_description.hpp b/pj_base/include/pj_base/builtin/robot_description.hpp new file mode 100644 index 0000000..5137156 --- /dev/null +++ b/pj_base/include/pj_base/builtin/robot_description.hpp @@ -0,0 +1,60 @@ +/** + * @file robot_description.hpp + * @brief Robot kinematic + visual model carried as a raw markup document. + * + * RobotDescription is a small owned builtin for URDF-style robot descriptions + * (or other XML-based formats: SDF, MJCF, COLLADA-scene, etc.). The SDK does + * not parse the document — it carries the raw text and a format hint string, + * and downstream consumers (the 3D viewer in particular) do the format- + * specific parsing and asset resolution. + * + * Rationale for raw-text-only: + * - The format space is open (URDF, SDF, MJCF, MJCF variants, custom). + * A format-specific SDK type would multiply schemas without payoff. + * - Mesh-file resolution depends on consumer-side configuration (search + * paths, MCAP attachments, sidecar directories). Embedding parsed mesh + * references in the SDK type would force assumptions about resolution. + * - URDFs are usually 1-2 KB; the cost of carrying the string verbatim is + * negligible vs. structured re-encoding. + */ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#include "pj_base/types.hpp" + +namespace PJ { +namespace sdk { + +/// Robot description carried as the source-format text plus a hint about +/// which format the text uses. Producers (e.g. ParserROS) set `format` to +/// something like "urdf" / "sdf" / "mjcf" after validating the root element; +/// consumers route to the matching parser. +struct RobotDescription { + /// Timestamp the description was observed (usually the message-arrival + /// time, since /robot_description is rarely updated mid-recording). + Timestamp timestamp_ns = 0; + + /// Source topic the description came from. Empty if not topic-sourced. + std::string topic; + + /// Format hint set by the producer, e.g. "urdf", "sdf", "mjcf". Open-ended + /// like `Image::encoding` so new formats land without an SDK change. + std::string format; + + /// Raw text of the description (XML for URDF/SDF/COLLADA, JSON-ish for + /// MJCF wrappers, etc.). Consumers must parse according to `format`. + std::string text; + + bool operator==(const RobotDescription&) const = default; + + [[nodiscard]] bool empty() const noexcept { + return text.empty(); + } +}; + +} // namespace sdk +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/scene_entities.hpp b/pj_base/include/pj_base/builtin/scene_entities.hpp new file mode 100644 index 0000000..cb03df7 --- /dev/null +++ b/pj_base/include/pj_base/builtin/scene_entities.hpp @@ -0,0 +1,165 @@ +/** + * @file scene_entities.hpp + * @brief Procedural 3D scene primitives + SceneEntity container + batch. + * + * SceneEntities is the workhorse for marker-style 3D visualization (the + * visualization_msgs/MarkerArray equivalent). A SceneEntity bundles + * heterogeneous primitives sharing a frame_id and timestamp; SceneEntities + * is the batch container shipped on a topic. + */ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +#include "pj_base/builtin/frame_transforms.hpp" // for Pose, Vector3 +#include "pj_base/builtin/image_annotations.hpp" // for ColorRGBA +#include "pj_base/types.hpp" + +namespace PJ { +namespace sdk { + +/// Position in 3D space. +struct Point3 { + double x = 0.0; + double y = 0.0; + double z = 0.0; + bool operator==(const Point3&) const = default; +}; + +/// Topology of a LinePrimitive's vertex list. +enum class LineType : uint8_t { + kLineStrip = 0, ///< 0-1, 1-2, ..., (n-1)-n + kLineLoop = 1, ///< like LineStrip, plus closing edge n-0 + kLineList = 2, ///< 0-1, 2-3, 4-5, ... +}; + +/// Arrow primitive. Tail at pose.position; identity orientation points along +x. +struct ArrowPrimitive { + Pose pose; + double shaft_length = 0.0; + double shaft_diameter = 0.0; + double head_length = 0.0; + double head_diameter = 0.0; + ColorRGBA color; + bool operator==(const ArrowPrimitive&) const = default; +}; + +/// Cuboid (or rectangular prism) centered at pose.position with extents `size`. +struct CubePrimitive { + Pose pose; + Vector3 size; ///< Side lengths along local x, y, z. + ColorRGBA color; + bool operator==(const CubePrimitive&) const = default; +}; + +/// Sphere or ellipsoid centered at pose.position. Non-uniform `size` yields an ellipsoid. +struct SpherePrimitive { + Pose pose; + Vector3 size; ///< Diameter along local x, y, z. + ColorRGBA color; + bool operator==(const SpherePrimitive&) const = default; +}; + +/// Cylinder / cone / truncated cone. Flat faces perpendicular to local z. +/// `bottom_scale` / `top_scale` (in 0..1) collapse the respective face toward an apex. +struct CylinderPrimitive { + Pose pose; + Vector3 size; ///< Bounding box (diameter x, diameter y, height z). + double bottom_scale = 1.0; + double top_scale = 1.0; + ColorRGBA color; + bool operator==(const CylinderPrimitive&) const = default; +}; + +/// Polyline or line-list primitive. `pose` is the origin of the local +/// frame in which `points` are interpreted. +struct LinePrimitive { + LineType type = LineType::kLineStrip; + Pose pose; + double thickness = 0.0; + bool scale_invariant = false; ///< true => thickness in screen pixels. + std::vector points; + ColorRGBA color; ///< Solid color (used when `colors` is empty). + std::vector colors; ///< Per-vertex colors; size must equal points.size() when non-empty. + std::vector indices; ///< Optional vertex index buffer (GL-style). Empty => 0..N-1. + bool operator==(const LinePrimitive&) const = default; +}; + +/// Triangle-list primitive. Vertices consumed in triples: (0,1,2), (3,4,5), ... +struct TrianglePrimitive { + Pose pose; + std::vector points; + ColorRGBA color; + std::vector colors; + std::vector indices; + bool operator==(const TrianglePrimitive&) const = default; +}; + +/// Text label anchored at pose.position. Identity orientation flows along +x in the xy-plane. +struct TextPrimitive { + Pose pose; + bool billboard = false; ///< When true, always faces the camera and ignores pose.orientation. + double font_size = 0.0; + bool scale_invariant = false; ///< true => font_size in screen pixels. + ColorRGBA color; + std::string text; + bool operator==(const TextPrimitive&) const = default; +}; + +/// Coordinate-axes glyph at pose. Renders three arrows: X (red), Y (green), Z (blue). +/// Used to visualize TF frames, target poses, or any oriented reference. +struct AxesPrimitive { + Pose pose; + double length = 0.0; + double thickness = 0.0; + bool scale_invariant = false; ///< true => length in screen pixels. + bool operator==(const AxesPrimitive&) const = default; +}; + +/// A visual element in a 3D scene composed of multiple primitives, all +/// sharing the same frame of reference and timestamp. +/// +/// Identity: (topic, id) is the deduplication key. Republishing with the +/// same id on the same topic replaces the previous entity. +/// +/// Lifetime: zero means persist until replaced or deleted; otherwise the +/// entity is removed `lifetime_ns` after `timestamp`. +/// +/// frame_locked: when true, the entity tracks `frame_id` as it moves; when +/// false, it is stamped into the fixed frame at publish time. +struct SceneEntity { + Timestamp timestamp = 0; + std::string frame_id; + std::string id; + int64_t lifetime_ns = 0; ///< 0 means persist until replaced. + bool frame_locked = false; + + std::vector arrows; + std::vector cubes; + std::vector spheres; + std::vector cylinders; + std::vector lines; + std::vector triangles; + std::vector texts; + std::vector axes; + + bool operator==(const SceneEntity&) const = default; +}; + +/// Batch of scene entities published together. +struct SceneEntities { + std::vector entities; + bool operator==(const SceneEntities&) const = default; + + [[nodiscard]] bool empty() const noexcept { + return entities.empty(); + } +}; + +} // namespace sdk +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/scene_entities_codec.hpp b/pj_base/include/pj_base/builtin/scene_entities_codec.hpp new file mode 100644 index 0000000..3cd0b7d --- /dev/null +++ b/pj_base/include/pj_base/builtin/scene_entities_codec.hpp @@ -0,0 +1,24 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include + +#include "pj_base/builtin/scene_entities.hpp" +#include "pj_base/expected.hpp" + +namespace PJ { + +inline constexpr std::string_view kSchemaSceneEntities = "PJ.SceneEntities"; + +/// Serializes sdk::SceneEntities to canonical PJ.SceneEntities wire bytes +/// (see pj_base/proto/pj/SceneEntities.proto). +[[nodiscard]] std::vector serializeSceneEntities(const sdk::SceneEntities& entities); + +/// Decodes canonical PJ.SceneEntities wire bytes into sdk::SceneEntities. +[[nodiscard]] Expected deserializeSceneEntities(const uint8_t* data, size_t size); + +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/video_frame.hpp b/pj_base/include/pj_base/builtin/video_frame.hpp new file mode 100644 index 0000000..0421d83 --- /dev/null +++ b/pj_base/include/pj_base/builtin/video_frame.hpp @@ -0,0 +1,40 @@ +/** + * @file video_frame.hpp + * @brief Single frame of a compressed video stream (h264 / h265 / vp9 / av1). + */ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#include "pj_base/buffer_anchor.hpp" +#include "pj_base/span.hpp" +#include "pj_base/types.hpp" + +namespace PJ { +namespace sdk { + +/// Single frame of a compressed video stream. Unlike Image, a video frame +/// may have inter-frame dependencies (P-frames, etc.); consumers must +/// maintain decoder state across frames within a stream. +/// +/// `data` must contain enough bitstream to decode exactly one frame given +/// prior stream state. Format-specific packaging requirements live in the +/// proto schema documentation (pj_base/proto/pj/VideoFrame.proto). +/// +/// `anchor` keeps the underlying buffer alive — the producer may have made +/// `data` a view into the source payload (zero-copy) or into a freshly +/// allocated vector; consumers don't need to know which. +struct VideoFrame { + Timestamp timestamp_ns = 0; + std::string frame_id; ///< Camera frame. Optical axis: +x right, +y down, +z into scene. + std::string format; ///< Codec identifier, lowercase. "h264", "h265", "vp9", "av1". + Span data; + BufferAnchor anchor; +}; + +} // namespace sdk +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/video_frame_codec.hpp b/pj_base/include/pj_base/builtin/video_frame_codec.hpp new file mode 100644 index 0000000..5f39b87 --- /dev/null +++ b/pj_base/include/pj_base/builtin/video_frame_codec.hpp @@ -0,0 +1,28 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include + +#include "pj_base/builtin/video_frame.hpp" +#include "pj_base/expected.hpp" + +namespace PJ { + +inline constexpr std::string_view kSchemaVideoFrame = "PJ.VideoFrame"; + +/// Serializes sdk::VideoFrame to canonical PJ.VideoFrame wire bytes. +/// +/// The payload follows pj_base/proto/pj/VideoFrame.proto, but the +/// implementation uses PlotJuggler's private protobuf-wire primitives rather +/// than generated Protobuf code. +[[nodiscard]] std::vector serializeVideoFrame(const sdk::VideoFrame& frame); + +/// Decodes canonical PJ.VideoFrame wire bytes into sdk::VideoFrame. The +/// returned frame owns its bytes via `anchor`. +[[nodiscard]] Expected deserializeVideoFrame(const uint8_t* data, size_t size); + +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin_object_abi.h b/pj_base/include/pj_base/builtin_object_abi.h index 0e46740..15c55f6 100644 --- a/pj_base/include/pj_base/builtin_object_abi.h +++ b/pj_base/include/pj_base/builtin_object_abi.h @@ -7,14 +7,14 @@ * produce for that schema. The parser returns a PJ_schema_classification_t * carrying a PJ_builtin_object_type_t. * - * Canonical-object production (sdk::Image / sdk::DepthImage / - * sdk::PointCloud / sdk::ImageAnnotations / sdk::FrameTransforms) and the - * pure-functional scalar production (Expected>) are C++ SDK contracts: plugins - * inheriting from MessageParserPluginBase register handlers in - * SchemaHandler, and the in-process host consumes them via - * MessageParserPluginBase::parseObject() and parseScalars() called - * directly on the C++ pointer. Pure-C plugins emit scalars via the - * parse() slot (writing to writeHost). + * Canonical-object production (any concrete sdk::* type listed in + * BuiltinObjectType — see pj_base/builtin/builtin_object.hpp) and the + * pure-functional scalar production (Expected>) + * are C++ SDK contracts: plugins inheriting from MessageParserPluginBase + * register handlers in SchemaHandler, and the in-process host consumes + * them via MessageParserPluginBase::parseObject() and parseScalars() + * called directly on the C++ pointer. Pure-C plugins emit scalars via + * the parse() slot (writing to writeHost). */ // Copyright 2026 Davide Faconti // SPDX-License-Identifier: MIT @@ -41,12 +41,21 @@ extern "C" { typedef enum PJ_builtin_object_type_t { PJ_BUILTIN_OBJECT_TYPE_NONE = 0, PJ_BUILTIN_OBJECT_TYPE_IMAGE = 1, + /* 2 reserved — never used historically. */ PJ_BUILTIN_OBJECT_TYPE_POINTCLOUD = 3, PJ_BUILTIN_OBJECT_TYPE_DEPTH_IMAGE = 4, PJ_BUILTIN_OBJECT_TYPE_IMAGE_ANNOTATIONS = 5, PJ_BUILTIN_OBJECT_TYPE_FRAME_TRANSFORMS = 6, - /* Reserve future types; appended at the tail. */ - /* PJ_BUILTIN_OBJECT_TYPE_OCCUPANCY_GRID = 7, */ + PJ_BUILTIN_OBJECT_TYPE_OCCUPANCY_GRID = 7, + PJ_BUILTIN_OBJECT_TYPE_COMPRESSED_POINTCLOUD = 8, + PJ_BUILTIN_OBJECT_TYPE_MESH3D = 9, + PJ_BUILTIN_OBJECT_TYPE_VIDEO_FRAME = 10, + PJ_BUILTIN_OBJECT_TYPE_SCENE_ENTITIES = 11, + PJ_BUILTIN_OBJECT_TYPE_ASSET_VIDEO = 12, + PJ_BUILTIN_OBJECT_TYPE_ROBOT_DESCRIPTION = 13, + /* Reserve future types; appended at the tail. Numeric values are stable + * across releases — never renumber. Each new value here must match the + * matching kFoo entry in BuiltinObjectType (builtin_object.hpp). */ } PJ_builtin_object_type_t; /** diff --git a/pj_base/proto/pj/AssetVideo.proto b/pj_base/proto/pj/AssetVideo.proto new file mode 100644 index 0000000..a4c7a34 --- /dev/null +++ b/pj_base/proto/pj/AssetVideo.proto @@ -0,0 +1,42 @@ +// PlotJuggler canonical asset video protobuf schema. +// File-backed video reference + playback metadata. + +syntax = "proto3"; + +import "google/protobuf/duration.proto"; +import "google/protobuf/timestamp.proto"; + +package PJ; + +// A reference to a file-backed video, plus typed metadata sufficient for consumers to size playback windows and route +// to a decoder without opening the file. Producers (e.g. data_load_lerobot, data_load_mp4) push exactly one +// AssetVideo per topic, with the entry's ObjectStore timestamp equal to `time_origin` (so timeRange() of the topic +// naturally reflects the start instant). +message AssetVideo { + // Wall-clock instant of the first frame. Consumers subtract this from the global tracker time to derive the + // file-relative seek position. Required: an absent time_origin is treated as "this asset is not aligned to wall + // clock" and the asset will not advance with the tracker. + google.protobuf.Timestamp time_origin = 1; + + // Total playable duration of the video. Producers should set this when known (FFmpeg probe at registration is + // cheap). Consumers may fall back to probing the file if absent. Optional. + optional google.protobuf.Duration duration = 2; + + // Path to the video file. Resolution is consumer-side: producers should emit either an absolute path or a path + // relative to a consumer-known root (the dataset directory in LeRobot's case). Required. + string file_path = 3; + + // Codec / container hint as a MIME type — e.g. "video/mp4", "video/x-matroska", "video/av1". Open-ended like + // Image.encoding so new formats land without an SDK bump. Consumers route to the matching decoder backend. Empty + // string means "probe the file". + string media_type = 4; + + // Pixel dimensions. Zero in either field means "unknown — probe the file". Producers should populate when known + // (LeRobot dataset manifests carry these explicitly). + uint32 width = 5; + uint32 height = 6; + + // Nominal frame rate in frames per second. Zero or NaN means "unknown — probe the file". For variable-frame-rate + // video this is an advisory average; actual per-frame timestamps come from the decoder. + double frame_rate = 7; +} diff --git a/pj_base/proto/pj/CompressedPointCloud.proto b/pj_base/proto/pj/CompressedPointCloud.proto new file mode 100644 index 0000000..46b7508 --- /dev/null +++ b/pj_base/proto/pj/CompressedPointCloud.proto @@ -0,0 +1,33 @@ +// PlotJuggler canonical compressed point cloud protobuf schema. +// Follows the opaque-asset pattern shared with VideoFrame and Mesh3D. + +syntax = "proto3"; + +import "google/protobuf/timestamp.proto"; + +package PJ; + +// A point cloud delivered in a compressed binary format. Unlike `PointCloud`, the wire layout is opaque to PlotJuggler +// — the renderer hands `data` + `format` to the matching decoder library, which produces a decompressed point set on +// the host side. +// +// This type is distinct from `PointCloud` because per-format decoders carry their own attribute table and layout +// semantics (e.g. Draco encodes its own metadata). This is the same reasoning that separates `VideoFrame` from `Image`: +// stream / asset decoding is a different code path from uncompressed handling. +// +// On the SDK side, `data` is exposed as `Span` plus a `BufferAnchor` (same byte-backed view pattern as +// Image, DepthImage, PointCloud, VideoFrame, and Mesh3D). The anchor is a C++ lifetime concept with no wire-format +// equivalent. +message CompressedPointCloud { + // Timestamp associated with the point cloud + google.protobuf.Timestamp timestamp = 1; + + // Frame of reference for the point cloud + string frame_id = 2; + + // Format identifier, lowercase. Recognized values include "draco". + string format = 3; + + // Compressed payload bytes + bytes data = 4; +} diff --git a/pj_base/proto/pj/DepthImage.proto b/pj_base/proto/pj/DepthImage.proto new file mode 100644 index 0000000..6866962 --- /dev/null +++ b/pj_base/proto/pj/DepthImage.proto @@ -0,0 +1,56 @@ +// PlotJuggler canonical depth image protobuf schema. +// Wire-level contract for PJ::sdk::DepthImage in +// pj_base/include/pj_base/builtin/DepthImage.hpp. + +syntax = "proto3"; + +import "google/protobuf/timestamp.proto"; + +package PJ; + +// Depth image. The `encoding` string carries the depth representation (e.g. "16UC1" = millimeters as uint16, "32FC1" = +// meters as float). +// +// Intrinsics: `K` is the 3x3 row-major intrinsic camera matrix, exactly 9 entries. +// +// K = [ fx, 0, cx, +// 0, fy, cy, +// 0, 0, 1 ] +// +// Back-projection of pixel (u, v) with depth value z: +// +// X = (u - cx) * z / fx +// Y = (v - cy) * z / fy +// Z = z +// +// Distortion: when `distortion_model` is non-empty, `D` carries the distortion coefficients for that model +// ("plumb_bob": 5 coeffs k1, k2, p1, p2, k3; "equidistant": 4 coeffs k1, k2, k3, k4). An empty `distortion_model` means +// the image is rectified and `D` is unused. +// +// On the SDK side, `data` is exposed as `Span` plus a `BufferAnchor`. The anchor is a C++ lifetime +// concept with no wire-format equivalent. +message DepthImage { + // Timestamp associated with the depth frame + google.protobuf.Timestamp timestamp = 1; + + // Image width in pixels + uint32 width = 2; + + // Image height in pixels + uint32 height = 3; + + // Depth representation identifier (e.g. "16UC1" for uint16 millimeters, "32FC1" for float meters) + string encoding = 4; + + // Depth pixel bytes + bytes data = 5; + + // 3x3 row-major intrinsic camera matrix. Length must be 9. + repeated double K = 6; + + // Distortion model identifier; empty when the image is rectified (`D` then unused) + string distortion_model = 7; + + // Distortion coefficients for `distortion_model`. Length depends on the model. + repeated double D = 8; +} diff --git a/pj_base/proto/pj/FrameTransform.proto b/pj_base/proto/pj/FrameTransform.proto deleted file mode 100644 index c69fa81..0000000 --- a/pj_base/proto/pj/FrameTransform.proto +++ /dev/null @@ -1,34 +0,0 @@ -// PlotJuggler canonical frame transform protobuf schema. -// Field layout adapted from Foxglove SDK schemas (MIT License, -// Copyright (c) Foxglove Technologies Inc). - -syntax = "proto3"; - -import "pj/Geometry.proto"; -import "google/protobuf/timestamp.proto"; - -package PJ; - -// A transform between two reference frames in 3D space. The transform defines the position and orientation of a child frame within a parent frame. Translation moves the origin of the child frame relative to the parent origin. The rotation changes the orientation of the child frame around its origin. -// -// Examples: -// -// - With translation (x=1, y=0, z=0) and identity rotation (x=0, y=0, z=0, w=1), a point at (x=0, y=0, z=0) in the child frame maps to (x=1, y=0, z=0) in the parent frame. -// -// - With translation (x=1, y=2, z=0) and a 90-degree rotation around the z-axis (x=0, y=0, z=0.707, w=0.707), a point at (x=1, y=0, z=0) in the child frame maps to (x=-1, y=3, z=0) in the parent frame. -message FrameTransform { - // Timestamp of transform - google.protobuf.Timestamp timestamp = 1; - - // Name of the parent frame - string parent_frame_id = 2; - - // Name of the child frame - string child_frame_id = 3; - - // Translation component of the transform, representing the position of the child frame's origin in the parent frame. - PJ.Vector3 translation = 4; - - // Rotation component of the transform, representing the orientation of the child frame in the parent frame - PJ.Quaternion rotation = 5; -} diff --git a/pj_base/proto/pj/FrameTransforms.proto b/pj_base/proto/pj/FrameTransforms.proto index f6e612d..d39fb2b 100644 --- a/pj_base/proto/pj/FrameTransforms.proto +++ b/pj_base/proto/pj/FrameTransforms.proto @@ -1,13 +1,42 @@ -// PlotJuggler canonical frame transforms protobuf schema. +// PlotJuggler canonical frame transform protobuf schemas. // Field layout adapted from Foxglove SDK schemas (MIT License, // Copyright (c) Foxglove Technologies Inc). syntax = "proto3"; -import "pj/FrameTransform.proto"; +import "pj/Geometry.proto"; +import "google/protobuf/timestamp.proto"; package PJ; +// A transform between two reference frames in 3D space. The transform defines the position and orientation of a child +// frame within a parent frame. Translation moves the origin of the child frame relative to the parent origin. The +// rotation changes the orientation of the child frame around its origin. +// +// Examples: +// +// - With translation (x=1, y=0, z=0) and identity rotation (x=0, y=0, z=0, w=1), a point at (x=0, y=0, z=0) in the +// child frame maps to (x=1, y=0, z=0) in the parent frame. +// +// - With translation (x=1, y=2, z=0) and a 90-degree rotation around the z-axis (x=0, y=0, z=0.707, w=0.707), a point +// at (x=1, y=0, z=0) in the child frame maps to (x=-1, y=3, z=0) in the parent frame. +message FrameTransform { + // Timestamp of transform + google.protobuf.Timestamp timestamp = 1; + + // Name of the parent frame + string parent_frame_id = 2; + + // Name of the child frame + string child_frame_id = 3; + + // Translation component of the transform, representing the position of the child frame's origin in the parent frame. + PJ.Vector3 translation = 4; + + // Rotation component of the transform, representing the orientation of the child frame in the parent frame + PJ.Quaternion rotation = 5; +} + // An array of FrameTransform messages message FrameTransforms { // Array of transforms diff --git a/pj_base/proto/pj/Geometry.proto b/pj_base/proto/pj/Geometry.proto index 0a7a96d..fe53081 100644 --- a/pj_base/proto/pj/Geometry.proto +++ b/pj_base/proto/pj/Geometry.proto @@ -62,3 +62,12 @@ message Quaternion { // w value double w = 4; } + +// A pose in 3D space consisting of position (translation) and orientation (rotation) +message Pose { + // Position of the pose's origin in the parent frame + PJ.Vector3 position = 1; + + // Orientation of the pose relative to the parent frame's axes + PJ.Quaternion orientation = 2; +} diff --git a/pj_base/proto/pj/Image.proto b/pj_base/proto/pj/Image.proto new file mode 100644 index 0000000..08dacf2 --- /dev/null +++ b/pj_base/proto/pj/Image.proto @@ -0,0 +1,57 @@ +// PlotJuggler canonical image protobuf schema. +// Wire-level contract for PJ::sdk::Image in +// pj_base/include/pj_base/builtin/Image.hpp. + +syntax = "proto3"; + +import "google/protobuf/timestamp.proto"; + +package PJ; + +// Image. The `encoding` string distinguishes raw pixel layouts from compressed wire formats; the producer decides +// which. +// +// Raw encodings: "rgb8", "rgba8", "bgr8", "bgra8", "mono8", "mono16". `data` is `row_step * height` bytes laid out per +// the encoding. `row_step` may exceed `width * bytes_per_pixel(encoding)` when the wire format includes per-row +// padding; consumers must honor it. `is_bigendian` is meaningful only for multi-byte raw encodings (e.g. mono16). +// +// Compressed encodings: "jpeg", "png", "qoi". `data` carries the compressed payload; consumers run the appropriate +// codec to obtain decoded pixels. `row_step` and `is_bigendian` are unused. +// +// Compressed depth: "compressedDepth" (ROS-style). `data` carries a PNG payload that decodes to grayscale; +// `compressed_depth_min` and `compressed_depth_max` carry the quantization range needed to map the grayscale back to +// depth values. +// +// On the SDK side, `data` is exposed as `Span` plus a `BufferAnchor` that keeps the underlying +// allocation alive (zero-copy view into the source payload, or a freshly allocated vector when conversion was +// unavoidable). The anchor is a C++ lifetime concept with no wire-format equivalent. +message Image { + // Timestamp associated with the image + google.protobuf.Timestamp timestamp = 1; + + // Image width in pixels + uint32 width = 2; + + // Image height in pixels + uint32 height = 3; + + // Raw pixel layout or compressed-codec identifier (e.g. "rgb8", "mono16", "jpeg", "png", "qoi", "compressedDepth") + string encoding = 4; + + // Bytes per row for raw encodings; 0 for compressed encodings. + uint32 row_step = 5; + + // Big-endian byte order flag. Meaningful only for multi-byte raw encodings (e.g. mono16). + bool is_bigendian = 6; + + // Raw pixel bytes (raw encodings) or compressed payload bytes (compressed encodings). + bytes data = 7; + + // ROS compressedDepth quantization minimum; set together with `compressed_depth_max` when `encoding == + // "compressedDepth"`. + optional float compressed_depth_min = 8; + + // ROS compressedDepth quantization maximum; set together with `compressed_depth_min` when `encoding == + // "compressedDepth"`. + optional float compressed_depth_max = 9; +} diff --git a/pj_base/proto/pj/ImageAnnotations.proto b/pj_base/proto/pj/ImageAnnotations.proto index cc266d6..116199f 100644 --- a/pj_base/proto/pj/ImageAnnotations.proto +++ b/pj_base/proto/pj/ImageAnnotations.proto @@ -26,6 +26,7 @@ message ImageAnnotations { // Text annotations repeated PJ.TextAnnotation texts = 3; - // Additional user-provided metadata associated with the image annotations. Keys must be unique within this object. Per-annotation metadata takes precedence over these values. + // Additional user-provided metadata associated with the image annotations. Keys must be unique within this object. + // Per-annotation metadata takes precedence over these values. repeated PJ.KeyValuePair metadata = 4; } diff --git a/pj_base/proto/pj/Mesh3D.proto b/pj_base/proto/pj/Mesh3D.proto new file mode 100644 index 0000000..0a1e93b --- /dev/null +++ b/pj_base/proto/pj/Mesh3D.proto @@ -0,0 +1,56 @@ +// PlotJuggler canonical 3D mesh asset protobuf schema. +// Field layout adapted from Foxglove SDK schemas (MIT License, +// Copyright (c) Foxglove Technologies Inc). + +syntax = "proto3"; + +import "pj/Color.proto"; +import "pj/Geometry.proto"; +import "google/protobuf/timestamp.proto"; + +package PJ; + +// A 3D mesh asset delivered in its native binary format. The renderer hands `data` + `format` (or the contents at +// `url`) to a mesh-loader library (Assimp, tinygltf, ...); PlotJuggler does not parse the asset itself. +// +// This type is distinct from the triangle primitive in SceneEntity because asset formats can carry richer scene content +// (materials, textures, skinning, animations) that is not expressible as raw triangle soup. +// +// Asset source: exactly one of `data` or `url` should be populated. When `data` is used, `format` is required. When +// `url` is used, `format` may be inferred from the file extension if absent. +// +// On the SDK side, `data` is exposed as `Span` plus a `BufferAnchor` that keeps the underlying +// allocation alive (same byte-backed view pattern as Image, DepthImage, and PointCloud). The anchor is a C++ lifetime +// concept with no wire-format equivalent. +message Mesh3D { + // Timestamp of the mesh asset + google.protobuf.Timestamp timestamp = 1; + + // Frame of reference in which the mesh is placed + string frame_id = 2; + + // Identifier for the mesh asset. Republishing with the same id on the same topic replaces the previous one. + string id = 3; + + // Placement of the mesh's origin in `frame_id` + PJ.Pose pose = 4; + + // Per-axis scale factor applied to the mesh; (1, 1, 1) is the default when unset + PJ.Vector3 scale = 5; + + // Asset format identifier, lowercase. Recognized values include "gltf", "glb", "stl", "ply", "obj", "usd", "dae". + string format = 6; + + // Embedded asset bytes. When non-empty, `format` is required. + bytes data = 7; + + // External URL referencing the asset (e.g. "file:///path/to/model.glb"). Used when `data` is empty. + string url = 8; + + // Color tint applied to the mesh. Only takes effect when `override_color` is true; otherwise the asset's embedded + // materials are used. + PJ.Color color = 9; + + // When true, the mesh is rendered using `color` instead of its embedded materials. + bool override_color = 10; +} diff --git a/pj_base/proto/pj/OccupancyGrid.proto b/pj_base/proto/pj/OccupancyGrid.proto new file mode 100644 index 0000000..41440c2 --- /dev/null +++ b/pj_base/proto/pj/OccupancyGrid.proto @@ -0,0 +1,47 @@ +// PlotJuggler canonical occupancy grid protobuf schema. +// Wire-level contract for the byte-backed `OccupancyGrid` builtin +// (SDK struct lives in pj_base/include/pj_base/builtin/OccupancyGrid.hpp +// once implemented). + +syntax = "proto3"; + +import "pj/Geometry.proto"; +import "google/protobuf/timestamp.proto"; + +package PJ; + +// A 2D metric occupancy grid. The grid lies in the local xy-plane of `origin`, with cell (0, 0) at the origin's +// translation. +// +// Cell values are signed 8-bit integers stored row-major in `data`: +// -1 = unknown / no data +// 0..100 = probability of occupied, in percent +// Other values are reserved. +// +// The cell at row r and column c sits at world position `origin + (c * resolution, r * resolution, 0)` expressed in +// `frame_id`. `data.size()` must equal `width * height`. +// +// On the SDK side, `data` is exposed as `Span` plus a `BufferAnchor` (same byte-backed view pattern as +// Image, DepthImage, and PointCloud). The anchor is a C++ lifetime concept with no wire-format equivalent. +message OccupancyGrid { + // Timestamp of the grid + google.protobuf.Timestamp timestamp = 1; + + // Frame of reference for the grid placement + string frame_id = 2; + + // Pose of cell (0, 0) in `frame_id`. The grid lies in the local xy-plane of this pose. + PJ.Pose origin = 3; + + // Cell size in meters. Cells are square: the same spacing applies along both x and y. + double resolution = 4; + + // Number of columns (cells along the local x axis) + uint32 width = 5; + + // Number of rows (cells along the local y axis) + uint32 height = 6; + + // Row-major signed 8-bit cell values. `data.size()` must equal `width * height`. + bytes data = 7; +} diff --git a/pj_base/proto/pj/PointCloud.proto b/pj_base/proto/pj/PointCloud.proto new file mode 100644 index 0000000..a289489 --- /dev/null +++ b/pj_base/proto/pj/PointCloud.proto @@ -0,0 +1,93 @@ +// PlotJuggler canonical point cloud protobuf schemas. +// Wire-level contract for PJ::sdk::PointCloud and PJ::sdk::PointField in +// pj_base/include/pj_base/builtin/PointCloud.hpp. + +syntax = "proto3"; + +import "google/protobuf/timestamp.proto"; + +package PJ; + +// Description of one channel inside a packed point cloud (x, y, z, intensity, rgb, ring, time, ...). Mirrors the shape +// of sensor_msgs/PointField but the datatype enum is canonical PJ vocabulary, not a ROS-specific enum. +message PointField { + // Scalar data type of one channel element + enum Datatype { + // Unknown / unset datatype + UNKNOWN = 0; + + // Signed 8-bit integer + INT8 = 1; + + // Unsigned 8-bit integer + UINT8 = 2; + + // Signed 16-bit integer + INT16 = 3; + + // Unsigned 16-bit integer + UINT16 = 4; + + // Signed 32-bit integer + INT32 = 5; + + // Unsigned 32-bit integer + UINT32 = 6; + + // IEEE 754 single-precision float (32 bits) + FLOAT32 = 7; + + // IEEE 754 double-precision float (64 bits) + FLOAT64 = 8; + } + + // Channel name (e.g. "x", "y", "z", "intensity", "rgb", "ring", "time") + string name = 1; + + // Byte offset of this channel within a single point record + uint32 offset = 2; + + // Scalar data type of one element of this channel + Datatype datatype = 3; + + // Number of elements of `datatype` per point (typically 1) + uint32 count = 4; +} + +// Packed point cloud. The `data` buffer holds `width * height` points, each occupying `point_step` bytes laid out per +// `fields`. `is_dense = false` means some points may be invalid (typically NaN-filled). +// +// On the SDK side, `data` is exposed as `Span` plus a `BufferAnchor`. The anchor is a C++ lifetime +// concept with no wire-format equivalent. +message PointCloud { + // Timestamp associated with the cloud + google.protobuf.Timestamp timestamp = 1; + + // Number of points per row, or total points for unorganized clouds + uint32 width = 2; + + // Number of rows. 1 for unorganized clouds. + uint32 height = 3; + + // Bytes per point + uint32 point_step = 4; + + // Bytes per row (= point_step * width when tightly packed) + uint32 row_step = 5; + + // Big-endian byte order flag for packed numeric fields + bool is_bigendian = 6; + + // When false, some points may be invalid (typically NaN-filled) + bool is_dense = 7; + + // Channel layout for each point in `data` + repeated PJ.PointField fields = 8; + + // Packed point bytes + bytes data = 9; + + // Source coordinate frame for the points (e.g. "velodyne", "base_link"). 3D consumers TF-transform from this frame to + // a fixed render frame. Empty when unset; 2D consumers ignore this field. + string frame_id = 10; +} diff --git a/pj_base/proto/pj/PointsAnnotation.proto b/pj_base/proto/pj/PointsAnnotation.proto index 66c6590..65a57e9 100644 --- a/pj_base/proto/pj/PointsAnnotation.proto +++ b/pj_base/proto/pj/PointsAnnotation.proto @@ -44,7 +44,8 @@ message PointsAnnotation { // Outline color PJ.Color outline_color = 4; - // Per-point colors, if `type` is `POINTS`, or per-segment stroke colors, if `type` is `LINE_LIST`, `LINE_STRIP` or `LINE_LOOP`. + // Per-point colors, if `type` is `POINTS`, or per-segment stroke colors, if `type` is `LINE_LIST`, `LINE_STRIP` or + // `LINE_LOOP`. repeated PJ.Color outline_colors = 5; // Fill color diff --git a/pj_base/proto/pj/README.md b/pj_base/proto/pj/README.md new file mode 100644 index 0000000..bd09694 --- /dev/null +++ b/pj_base/proto/pj/README.md @@ -0,0 +1,64 @@ +# `pj/` protobuf schemas + +Canonical wire-format contracts for PlotJuggler's builtin object types — the shim +between source-specific message families (ROS, Protobuf, JSON, ...) and the data +shapes that PlotJuggler can classify, store, and render consistently. Each schema +defines the authoritative on-disk / on-wire layout for a corresponding SDK struct +under `pj_base/include/pj_base/builtin/`. See `docs/builtin_type.md` for design +rationale. + +## Schemas + +### Shared primitives + +- **`Color.proto`** — RGBA color (channels in `[0, 1]`). + - `Color` +- **`Geometry.proto`** — spatial primitives reused across schemas. + - `Point2`, `Point3`, `Vector2`, `Vector3`, `Quaternion`, `Pose` +- **`KeyValuePair.proto`** — opaque user-attached metadata slot. + - `KeyValuePair` + +### Frame graph + +- **`FrameTransforms.proto`** — TF-style coordinate frame relationships so consumers can place data in a common world frame. + - `FrameTransform`, `FrameTransforms` + +### Byte-backed raster builtins + +- **`Image.proto`** — single 2D image, raw (`rgb8`, `mono16`, …) or compressed (`jpeg`, `png`, `qoi`) unified under a single `encoding` string. + - `Image` +- **`DepthImage.proto`** — metric depth image with camera intrinsics + distortion so consumers can back-project pixels into 3D. + - `DepthImage` +- **`OccupancyGrid.proto`** — 2D metric occupancy grid (maps, costmaps) placed in world coordinates via an origin pose + cell resolution. + - `OccupancyGrid` +- **`VideoFrame.proto`** — one frame of an inter-frame-coded video stream (`h264`, `h265`, `vp9`, `av1`) when per-frame `Image` messages would be wasteful. + - `VideoFrame` +- **`AssetVideo.proto`** — reference to a file-backed video plus typed playback metadata (path, MIME type, dimensions, frame rate) so consumers can size playback windows without opening the file. + - `AssetVideo` + +### Point clouds + +- **`PointCloud.proto`** — uncompressed packed 3D point cloud with a self-describing per-channel field layout. + - `PointField`, `PointCloud` +- **`CompressedPointCloud.proto`** — point cloud delivered in a format-specific compressed binary (e.g. Draco) when the uncompressed schema would be too large. + - `CompressedPointCloud` + +### 3D scene + +- **`SceneEntities.proto`** — the workhorse for marker-style 3D visualization; batches procedural primitives sharing a frame and timestamp. + - Primitives: `ArrowPrimitive`, `CubePrimitive`, `SpherePrimitive`, `CylinderPrimitive`, `LinePrimitive`, `TrianglePrimitive`, `TextPrimitive`, `AxesPrimitive` + - Entity: `SceneEntity` + - Batch: `SceneEntities` +- **`Mesh3D.proto`** — 3D mesh asset delivered in its native binary format (GLTF/GLB/STL/PLY/OBJ/USD/DAE) for URDF-style or scene-mesh visualization. + - `Mesh3D` + +### 2D image annotations (vector overlays) + +- **`CircleAnnotation.proto`** — circle overlay in 2D image-pixel space. + - `CircleAnnotation` +- **`PointsAnnotation.proto`** — points / line list / line strip / line loop overlay in 2D image-pixel space. + - `PointsAnnotation` +- **`TextAnnotation.proto`** — text label overlay in 2D image-pixel space. + - `TextAnnotation` +- **`ImageAnnotations.proto`** — bundle the annotations above for one image, with a shared timestamp and image reference. + - `ImageAnnotations` diff --git a/pj_base/proto/pj/SceneEntities.proto b/pj_base/proto/pj/SceneEntities.proto new file mode 100644 index 0000000..d9c6b0c --- /dev/null +++ b/pj_base/proto/pj/SceneEntities.proto @@ -0,0 +1,243 @@ +// PlotJuggler canonical scene primitive and scene entity protobuf schemas. +// Field layout adapted from Foxglove SDK schemas (MIT License, Copyright (c) Foxglove Technologies Inc). + +syntax = "proto3"; + +import "pj/Color.proto"; +import "pj/Geometry.proto"; +import "google/protobuf/duration.proto"; +import "google/protobuf/timestamp.proto"; + +package PJ; + +// An arrow primitive in a 3D scene. The arrow's tail sits at pose.position; with identity orientation, the arrow +// points along the local +x axis. +message ArrowPrimitive { + // Position of the tail and orientation of the arrow. Identity orientation means the arrow points along +x. + PJ.Pose pose = 1; + + // Length of the arrow shaft + double shaft_length = 2; + + // Diameter of the arrow shaft + double shaft_diameter = 3; + + // Length of the arrow head + double head_length = 4; + + // Diameter of the arrow head + double head_diameter = 5; + + // Color of the arrow + PJ.Color color = 6; +} + +// A cuboid primitive in a 3D scene. Centered at pose.position with extents `size` along each local axis (non-uniform +// `size` yields a rectangular prism). +message CubePrimitive { + // Center pose of the cuboid + PJ.Pose pose = 1; + + // Size of the cuboid along its local x, y, and z axes + PJ.Vector3 size = 2; + + // Color of the cuboid + PJ.Color color = 3; +} + +// A sphere or ellipsoid primitive in a 3D scene. Centered at pose.position; non-uniform `size` values yield an +// ellipsoid. +message SpherePrimitive { + // Center pose of the sphere or ellipsoid + PJ.Pose pose = 1; + + // Diameter of the sphere along its local x, y, and z axes (ellipsoid when components differ) + PJ.Vector3 size = 2; + + // Color of the sphere + PJ.Color color = 3; +} + +// A cylinder, cone, or truncated cone primitive in a 3D scene. Flat faces lie perpendicular to the local z axis. +// `bottom_scale` and `top_scale` (each in 0..1) shrink the corresponding face toward an apex: +// 1.0/1.0 = cylinder, 1.0/0.0 = cone, 0.5/1.0 = truncated cone. +message CylinderPrimitive { + // Center pose of the cylinder; flat faces are perpendicular to the local z axis + PJ.Pose pose = 1; + + // Bounding box of the cylinder (diameter along x, diameter along y, height along z) + PJ.Vector3 size = 2; + + // Ratio of the bottom face diameter to the bounding box diameter, in 0..1. A value of 0 collapses the bottom face + // to a point. + double bottom_scale = 3; + + // Ratio of the top face diameter to the bounding box diameter, in 0..1. A value of 0 collapses the top face to a + // point. + double top_scale = 4; + + // Color of the cylinder + PJ.Color color = 5; +} + +// A polyline or line list primitive in a 3D scene. The drawing topology is selected by `type`. +message LinePrimitive { + // Topology of the line drawing + enum Type { + // Unknown line type + UNKNOWN = 0; + + // Connected line segments: 0-1, 1-2, ..., (n-1)-n + LINE_STRIP = 1; + + // Closed polyline: 0-1, 1-2, ..., (n-1)-n, n-0 + LINE_LOOP = 2; + + // Individual line segments: 0-1, 2-3, 4-5, ... + LINE_LIST = 3; + } + + // Topology of the line drawing + Type type = 1; + + // Origin pose of the primitive; `points` are interpreted in this local frame + PJ.Pose pose = 2; + + // Line thickness + double thickness = 3; + + // When true, `thickness` is in screen-space pixels (invariant to camera distance). When false, thickness is in + // world units. + bool scale_invariant = 4; + + // Vertices of the line primitive in the local frame defined by `pose` + repeated PJ.Point3 points = 5; + + // Solid color, used when `colors` is empty + PJ.Color color = 6; + + // Per-vertex colors. When non-empty, `colors.size()` must equal `points.size()` and overrides `color`. + repeated PJ.Color colors = 7; + + // Optional vertex index buffer (GL-style). When empty, vertices are visited as 0..N-1 over `points`. + repeated fixed32 indices = 8; +} + +// A triangle list primitive in a 3D scene. Vertices are consumed in triples: (0,1,2), (3,4,5), ... +message TrianglePrimitive { + // Origin pose of the primitive; `points` are interpreted in this local frame + PJ.Pose pose = 1; + + // Triangle vertices in the local frame defined by `pose` + repeated PJ.Point3 points = 2; + + // Solid color, used when `colors` is empty + PJ.Color color = 3; + + // Per-vertex colors. When non-empty, `colors.size()` must equal `points.size()` and overrides `color`. + repeated PJ.Color colors = 4; + + // Optional vertex index buffer (GL-style). When empty, vertices are visited as 0..N-1 over `points`. + repeated fixed32 indices = 5; +} + +// A text label primitive anchored at pose.position. With identity orientation, the text flows in the local +x +// direction in the xy-plane. +message TextPrimitive { + // Anchor pose of the text + PJ.Pose pose = 1; + + // When true, the text always faces the camera and ignores pose.orientation + bool billboard = 2; + + // Font size + double font_size = 3; + + // When true, `font_size` is in screen-space pixels (invariant to camera distance). When false, font size is in + // world units. + bool scale_invariant = 4; + + // Color of the text + PJ.Color color = 5; + + // Text content + string text = 6; +} + +// A coordinate-axes glyph in a 3D scene. Renders three arrows colored by convention: X = red, Y = green, Z = blue. +// Commonly used to visualize TF frames, target poses, or any oriented reference. +// (PlotJuggler addition; no Foxglove counterpart.) +message AxesPrimitive { + // Pose at which to render the axes glyph + PJ.Pose pose = 1; + + // Length of each axis arrow + double length = 2; + + // Line thickness for the axis arrows + double thickness = 3; + + // When true, `length` is in screen-space pixels (invariant to camera distance) + bool scale_invariant = 4; +} + +// A visual element in a 3D scene. An entity is composed of one or more primitives, all of which share the same frame +// of reference and timestamp. The entity itself has no top-level pose; each primitive carries its own pose. +// +// Identity: `(topic, id)` is the deduplication key. Republishing an entity with the same id on the same topic +// replaces the previous one. +// +// Lifetime: a zero duration indicates the entity persists until replaced or deleted; otherwise the entity is removed +// `lifetime` after `timestamp`. +// +// frame_locked: when true, the entity tracks `frame_id` as it moves over time. When false, the entity is stamped into +// the fixed frame at publish time. +message SceneEntity { + // Timestamp of the entity + google.protobuf.Timestamp timestamp = 1; + + // Frame of reference for all primitives in this entity + string frame_id = 2; + + // Identifier for the entity. Republishing an entity with the same id on the same topic replaces the previous one. + string id = 3; + + // Length of time (relative to `timestamp`) after which the entity should be removed automatically. A zero value + // means the entity persists until replaced or deleted. + google.protobuf.Duration lifetime = 4; + + // When true, the entity tracks `frame_id` as it moves; when false, the entity is stamped into the fixed frame at + // publish time. + bool frame_locked = 5; + + // Arrow primitives composing this entity + repeated PJ.ArrowPrimitive arrows = 6; + + // Cube primitives composing this entity + repeated PJ.CubePrimitive cubes = 7; + + // Sphere primitives composing this entity + repeated PJ.SpherePrimitive spheres = 8; + + // Cylinder primitives composing this entity + repeated PJ.CylinderPrimitive cylinders = 9; + + // Line primitives composing this entity + repeated PJ.LinePrimitive lines = 10; + + // Triangle primitives composing this entity + repeated PJ.TrianglePrimitive triangles = 11; + + // Text primitives composing this entity + repeated PJ.TextPrimitive texts = 12; + + // Axes-glyph primitives composing this entity + repeated PJ.AxesPrimitive axes = 13; +} + +// A batch of scene entities published together. Each payload is a self-contained snapshot; entity-deletion semantics +// may be introduced later if streaming sources require them. +message SceneEntities { + // Array of scene entities + repeated PJ.SceneEntity entities = 1; +} diff --git a/pj_base/proto/pj/VideoFrame.proto b/pj_base/proto/pj/VideoFrame.proto new file mode 100644 index 0000000..38bd31a --- /dev/null +++ b/pj_base/proto/pj/VideoFrame.proto @@ -0,0 +1,43 @@ +// PlotJuggler canonical video frame protobuf schema. +// Field layout adapted from Foxglove SDK schemas (MIT License, +// Copyright (c) Foxglove Technologies Inc). + +syntax = "proto3"; + +import "google/protobuf/timestamp.proto"; + +package PJ; + +// A single frame of a compressed video stream. Unlike `Image`, a video frame may have inter-frame dependencies +// (P-frames, B-frames), so consumers must maintain decoder state across frames within a stream. +// +// Each frame's `data` must contain enough bitstream to decode exactly one frame given prior stream state. +// Format-specific packaging requirements: +// +// "h264": Annex B byte stream. IDR (instantaneous decoder refresh) frames must carry SPS and PPS. B-frames are not +// supported (the encoder must be configured for low-latency / no-B-frame output). +// +// "h265": Annex B byte stream. IRAP (intra random access point) frames must carry VPS, SPS, and PPS. B-frames are not +// supported. +// +// "vp9": a single VP9 frame. +// +// "av1": Low Overhead Bitstream Format (LOBF), a single temporal unit. +// +// On the SDK side, `data` is exposed as `Span` plus a `BufferAnchor` that keeps the underlying +// allocation alive (same byte-backed view pattern as Image, DepthImage, and PointCloud). The anchor is a C++ lifetime +// concept with no wire-format equivalent. +message VideoFrame { + // Timestamp of the frame + google.protobuf.Timestamp timestamp = 1; + + // Frame of reference for the camera. The video plane is centered on the optical axis: +x right, +y down, +z away from + // the camera (into the scene). + string frame_id = 2; + + // Codec identifier, lowercase. Recognized values: "h264", "h265", "vp9", "av1". + string format = 3; + + // Compressed bitstream containing exactly one frame given prior stream state. + bytes data = 4; +} diff --git a/pj_base/src/builtin/asset_video_codec.cpp b/pj_base/src/builtin/asset_video_codec.cpp new file mode 100644 index 0000000..283d675 --- /dev/null +++ b/pj_base/src/builtin/asset_video_codec.cpp @@ -0,0 +1,119 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/asset_video_codec.hpp" + +#include +#include +#include +#include + +#include "geometry_codec.hpp" +#include "protobuf_wire.hpp" + +namespace PJ { +namespace { + +using builtin_wire::parseFields; +using builtin_wire::Reader; +using builtin_wire::Tag; +using builtin_wire::WireType; +using builtin_wire::Writer; +using sdk::AssetVideo; + +} // namespace + +std::vector serializeAssetVideo(const AssetVideo& asset) { + std::vector out; + Writer writer(out); + + // Both `time_origin` and `duration` use the seconds+nanos wire shape; + // omit the field entirely when the SDK optional is empty. + if (asset.time_origin_ns.has_value()) { + writer.message(1, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, *asset.time_origin_ns); }); + } + if (asset.duration_ns.has_value()) { + writer.message(2, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, *asset.duration_ns); }); + } + writer.string(3, asset.file_path); + writer.string(4, asset.media_type); + writer.varint(5, asset.width); + writer.varint(6, asset.height); + writer.doubleField(7, asset.frame_rate); + + return out; +} + +Expected deserializeAssetVideo(const uint8_t* data, size_t size) { + if (data == nullptr || size == 0) { + return unexpected(std::string("AssetVideo wire: empty buffer")); + } + + Reader reader(data, size); + sdk::AssetVideo asset; + + const bool ok = parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: { + if (tag.type != WireType::kLengthDelimited) { + return false; + } + Timestamp t = 0; + if (!builtin_wire::readTimestampMessage(r, t)) { + return false; + } + asset.time_origin_ns = t; + return true; + } + case 2: { + if (tag.type != WireType::kLengthDelimited) { + return false; + } + Timestamp d = 0; + if (!builtin_wire::readTimestampMessage(r, d)) { + return false; + } + asset.duration_ns = d; + return true; + } + case 3: + return tag.type == WireType::kLengthDelimited && r.readString(asset.file_path); + case 4: + return tag.type == WireType::kLengthDelimited && r.readString(asset.media_type); + case 5: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + asset.width = static_cast(v); + return true; + } + case 6: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + asset.height = static_cast(v); + return true; + } + case 7: + return tag.type == WireType::kFixed64 && r.readDouble(asset.frame_rate); + default: + return false; + } + }); + + if (!ok) { + return unexpected(std::string("AssetVideo wire: decode failed")); + } + + return asset; +} + +} // namespace PJ diff --git a/pj_base/src/builtin/compressed_point_cloud_codec.cpp b/pj_base/src/builtin/compressed_point_cloud_codec.cpp new file mode 100644 index 0000000..d3bbd28 --- /dev/null +++ b/pj_base/src/builtin/compressed_point_cloud_codec.cpp @@ -0,0 +1,84 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/compressed_point_cloud_codec.hpp" + +#include +#include +#include +#include +#include + +#include "geometry_codec.hpp" +#include "protobuf_wire.hpp" + +namespace PJ { +namespace { + +using builtin_wire::parseFields; +using builtin_wire::Reader; +using builtin_wire::Tag; +using builtin_wire::WireType; +using builtin_wire::Writer; +using sdk::CompressedPointCloud; + +bool readBytesIntoCloud(Reader& reader, CompressedPointCloud& out) { + const uint8_t* data = nullptr; + size_t size = 0; + if (!reader.readBytes(data, size)) { + return false; + } + auto owned = std::make_shared>(data, data + size); + out.data = Span(owned->data(), owned->size()); + out.anchor = owned; + return true; +} + +} // namespace + +std::vector serializeCompressedPointCloud(const CompressedPointCloud& cloud) { + std::vector out; + Writer writer(out); + + writer.message(1, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, cloud.timestamp_ns); }); + writer.string(2, cloud.frame_id); + writer.string(3, cloud.format); + writer.bytes(4, cloud.data.data(), cloud.data.size()); + + return out; +} + +Expected deserializeCompressedPointCloud(const uint8_t* data, size_t size) { + if (data == nullptr || size == 0) { + return unexpected(std::string("CompressedPointCloud wire: empty buffer")); + } + + Reader reader(data, size); + sdk::CompressedPointCloud cloud; + + const bool ok = parseFields(reader, [&](Tag tag, Reader& r) { + if (tag.type != WireType::kLengthDelimited) { + return false; + } + switch (tag.field) { + case 1: + return builtin_wire::readTimestampMessage(r, cloud.timestamp_ns); + case 2: + return r.readString(cloud.frame_id); + case 3: + return r.readString(cloud.format); + case 4: + return readBytesIntoCloud(r, cloud); + default: + return false; + } + }); + + if (!ok) { + return unexpected(std::string("CompressedPointCloud wire: decode failed")); + } + + return cloud; +} + +} // namespace PJ diff --git a/pj_base/src/builtin/depth_image_codec.cpp b/pj_base/src/builtin/depth_image_codec.cpp new file mode 100644 index 0000000..a14881b --- /dev/null +++ b/pj_base/src/builtin/depth_image_codec.cpp @@ -0,0 +1,113 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/depth_image_codec.hpp" + +#include +#include +#include +#include +#include + +#include "geometry_codec.hpp" +#include "protobuf_wire.hpp" + +namespace PJ { +namespace { + +using builtin_wire::parseFields; +using builtin_wire::Reader; +using builtin_wire::readPackedDouble; +using builtin_wire::readPackedDoubleArray; +using builtin_wire::Tag; +using builtin_wire::WireType; +using builtin_wire::Writer; +using sdk::DepthImage; + +bool readBytesIntoDepth(Reader& reader, DepthImage& out) { + const uint8_t* data = nullptr; + size_t size = 0; + if (!reader.readBytes(data, size)) { + return false; + } + auto owned = std::make_shared>(data, data + size); + out.data = Span(owned->data(), owned->size()); + out.anchor = owned; + return true; +} + +} // namespace + +std::vector serializeDepthImage(const DepthImage& depth) { + std::vector out; + Writer writer(out); + + writer.message(1, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, depth.timestamp_ns); }); + writer.varint(2, depth.width); + writer.varint(3, depth.height); + writer.string(4, depth.encoding); + writer.bytes(5, depth.data.data(), depth.data.size()); + writer.packedDouble(6, depth.K); + writer.string(7, depth.distortion_model); + writer.packedDouble(8, depth.D); + + return out; +} + +Expected deserializeDepthImage(const uint8_t* data, size_t size) { + if (data == nullptr || size == 0) { + return unexpected(std::string("DepthImage wire: empty buffer")); + } + + Reader reader(data, size); + sdk::DepthImage depth; + + const bool ok = parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + return tag.type == WireType::kLengthDelimited && builtin_wire::readTimestampMessage(r, depth.timestamp_ns); + case 2: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + depth.width = static_cast(v); + return true; + } + case 3: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + depth.height = static_cast(v); + return true; + } + case 4: + return tag.type == WireType::kLengthDelimited && r.readString(depth.encoding); + case 5: + return tag.type == WireType::kLengthDelimited && readBytesIntoDepth(r, depth); + case 6: + return tag.type == WireType::kLengthDelimited && readPackedDoubleArray(r, depth.K); + case 7: + return tag.type == WireType::kLengthDelimited && r.readString(depth.distortion_model); + case 8: + return tag.type == WireType::kLengthDelimited && readPackedDouble(r, depth.D); + default: + return false; + } + }); + + if (!ok) { + return unexpected(std::string("DepthImage wire: decode failed")); + } + + return depth; +} + +} // namespace PJ diff --git a/pj_base/src/builtin/frame_transforms_codec.cpp b/pj_base/src/builtin/frame_transforms_codec.cpp index ec2f407..e5f2b13 100644 --- a/pj_base/src/builtin/frame_transforms_codec.cpp +++ b/pj_base/src/builtin/frame_transforms_codec.cpp @@ -4,243 +4,52 @@ #include "pj_base/builtin/frame_transforms_codec.hpp" #include -#include #include #include #include +#include "geometry_codec.hpp" #include "protobuf_wire.hpp" namespace PJ { namespace { +using builtin_wire::parseFields; using builtin_wire::Reader; using builtin_wire::Tag; using builtin_wire::WireType; using builtin_wire::Writer; using sdk::FrameTransform; using sdk::FrameTransforms; -using sdk::Quaternion; -using sdk::Vector3; - -constexpr int64_t kNanosecondsPerSecond = 1000LL * 1000LL * 1000LL; - -struct TimestampParts { - int64_t seconds = 0; - int32_t nanos = 0; -}; - -TimestampParts splitTimestamp(Timestamp timestamp_ns) { - TimestampParts out; - out.seconds = timestamp_ns / kNanosecondsPerSecond; - out.nanos = static_cast(timestamp_ns % kNanosecondsPerSecond); - if (out.nanos < 0) { - --out.seconds; - out.nanos += static_cast(kNanosecondsPerSecond); - } - return out; -} - -bool combineTimestamp(const TimestampParts& parts, Timestamp& out) { - if (parts.nanos < 0 || parts.nanos >= kNanosecondsPerSecond) { - return false; - } - if (parts.seconds > std::numeric_limits::max() / kNanosecondsPerSecond || - parts.seconds < std::numeric_limits::min() / kNanosecondsPerSecond) { - return false; - } - const Timestamp seconds_ns = parts.seconds * kNanosecondsPerSecond; - if (seconds_ns > std::numeric_limits::max() - parts.nanos) { - return false; - } - out = seconds_ns + parts.nanos; - return true; -} - -void writeTimestamp(Writer& writer, Timestamp timestamp_ns) { - const auto parts = splitTimestamp(timestamp_ns); - writer.varint(1, static_cast(parts.seconds)); - writer.varint(2, static_cast(parts.nanos)); -} - -void writeVector3(Writer& writer, const Vector3& vector) { - writer.doubleField(1, vector.x); - writer.doubleField(2, vector.y); - writer.doubleField(3, vector.z); -} - -void writeQuaternion(Writer& writer, const Quaternion& quaternion) { - writer.doubleField(1, quaternion.x); - writer.doubleField(2, quaternion.y); - writer.doubleField(3, quaternion.z); - writer.doubleField(4, quaternion.w); -} void writeFrameTransform(Writer& writer, const FrameTransform& transform) { - writer.message(1, [&](Writer& nested) { writeTimestamp(nested, transform.timestamp); }); + writer.message(1, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, transform.timestamp); }); writer.string(2, transform.parent_frame_id); writer.string(3, transform.child_frame_id); - writer.message(4, [&](Writer& nested) { writeVector3(nested, transform.translation); }); - writer.message(5, [&](Writer& nested) { writeQuaternion(nested, transform.rotation); }); -} - -bool decodeTimestamp(Reader& reader, Timestamp& out) { - TimestampParts parts; - - while (!reader.eof()) { - Tag tag; - if (!reader.readTag(tag)) { - return false; - } - - if (tag.field == 1 && tag.type == WireType::kVarint) { - uint64_t value = 0; - if (!reader.readVarint(value)) { - return false; - } - parts.seconds = static_cast(value); - } else if (tag.field == 2 && tag.type == WireType::kVarint) { - uint64_t value = 0; - if (!reader.readVarint(value)) { - return false; - } - parts.nanos = static_cast(value); - } else if (!reader.skip(tag.type)) { - return false; - } - } - - return combineTimestamp(parts, out); -} - -bool decodeVector3(Reader& reader, Vector3& out) { - while (!reader.eof()) { - Tag tag; - if (!reader.readTag(tag)) { - return false; - } - - if (tag.field == 1 && tag.type == WireType::kFixed64) { - if (!reader.readDouble(out.x)) { - return false; - } - } else if (tag.field == 2 && tag.type == WireType::kFixed64) { - if (!reader.readDouble(out.y)) { - return false; - } - } else if (tag.field == 3 && tag.type == WireType::kFixed64) { - if (!reader.readDouble(out.z)) { - return false; - } - } else if (!reader.skip(tag.type)) { - return false; - } - } - return true; -} - -bool decodeQuaternion(Reader& reader, Quaternion& out) { - while (!reader.eof()) { - Tag tag; - if (!reader.readTag(tag)) { - return false; - } - - if (tag.field == 1 && tag.type == WireType::kFixed64) { - if (!reader.readDouble(out.x)) { - return false; - } - } else if (tag.field == 2 && tag.type == WireType::kFixed64) { - if (!reader.readDouble(out.y)) { - return false; - } - } else if (tag.field == 3 && tag.type == WireType::kFixed64) { - if (!reader.readDouble(out.z)) { - return false; - } - } else if (tag.field == 4 && tag.type == WireType::kFixed64) { - if (!reader.readDouble(out.w)) { - return false; - } - } else if (!reader.skip(tag.type)) { - return false; - } - } - return true; -} - -bool readTimestampMessage(Reader& reader, Timestamp& out) { - Reader nested; - return reader.readMessage(nested) && decodeTimestamp(nested, out); -} - -bool readVector3Message(Reader& reader, Vector3& out) { - Reader nested; - return reader.readMessage(nested) && decodeVector3(nested, out); -} - -bool readQuaternionMessage(Reader& reader, Quaternion& out) { - Reader nested; - return reader.readMessage(nested) && decodeQuaternion(nested, out); + writer.message(4, [&](Writer& nested) { builtin_wire::writeVector3(nested, transform.translation); }); + writer.message(5, [&](Writer& nested) { builtin_wire::writeQuaternion(nested, transform.rotation); }); } bool decodeFrameTransform(Reader& reader, FrameTransform& out) { - while (!reader.eof()) { - Tag tag; - if (!reader.readTag(tag)) { + return parseFields(reader, [&](Tag tag, Reader& r) { + if (tag.type != WireType::kLengthDelimited) { return false; } - switch (tag.field) { case 1: - if (tag.type == WireType::kLengthDelimited) { - if (!readTimestampMessage(reader, out.timestamp)) { - return false; - } - continue; - } - break; + return builtin_wire::readTimestampMessage(r, out.timestamp); case 2: - if (tag.type == WireType::kLengthDelimited) { - if (!reader.readString(out.parent_frame_id)) { - return false; - } - continue; - } - break; + return r.readString(out.parent_frame_id); case 3: - if (tag.type == WireType::kLengthDelimited) { - if (!reader.readString(out.child_frame_id)) { - return false; - } - continue; - } - break; + return r.readString(out.child_frame_id); case 4: - if (tag.type == WireType::kLengthDelimited) { - if (!readVector3Message(reader, out.translation)) { - return false; - } - continue; - } - break; + return builtin_wire::readVector3Message(r, out.translation); case 5: - if (tag.type == WireType::kLengthDelimited) { - if (!readQuaternionMessage(reader, out.rotation)) { - return false; - } - continue; - } - break; + return builtin_wire::readQuaternionMessage(r, out.rotation); default: - break; - } - - if (!reader.skip(tag.type)) { - return false; + return false; } - } - return true; + }); } } // namespace diff --git a/pj_base/src/builtin/geometry_codec.hpp b/pj_base/src/builtin/geometry_codec.hpp new file mode 100644 index 0000000..f2569b4 --- /dev/null +++ b/pj_base/src/builtin/geometry_codec.hpp @@ -0,0 +1,264 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT +// +// Internal helpers shared across builtin-object codecs. Provides write / +// decode functions for the geometric primitives reused by multiple types +// (Vector3, Point3, Quaternion, Pose), for the canonical Timestamp encoding +// (proto seconds + nanos <-> SDK int64 nanoseconds), and for Color (proto +// double [0..1] RGBA <-> SDK ColorRGBA uint8 [0..255]). +// +// Inline-only; not exposed through the public include path. + +#include +#include +#include + +#include "pj_base/builtin/frame_transforms.hpp" // Vector3, Quaternion, Pose +#include "pj_base/builtin/image_annotations.hpp" // ColorRGBA +#include "pj_base/builtin/scene_entities.hpp" // Point3 +#include "pj_base/types.hpp" +#include "protobuf_wire.hpp" + +namespace PJ::builtin_wire { + +// ---------- Timestamp ---------- + +inline constexpr int64_t kNanosecondsPerSecond = 1000LL * 1000LL * 1000LL; + +struct TimestampParts { + int64_t seconds = 0; + int32_t nanos = 0; +}; + +inline TimestampParts splitTimestamp(Timestamp timestamp_ns) { + TimestampParts out; + out.seconds = timestamp_ns / kNanosecondsPerSecond; + out.nanos = static_cast(timestamp_ns % kNanosecondsPerSecond); + if (out.nanos < 0) { + --out.seconds; + out.nanos += static_cast(kNanosecondsPerSecond); + } + return out; +} + +inline bool combineTimestamp(const TimestampParts& parts, Timestamp& out) { + if (parts.nanos < 0 || parts.nanos >= kNanosecondsPerSecond) { + return false; + } + if (parts.seconds > std::numeric_limits::max() / kNanosecondsPerSecond || + parts.seconds < std::numeric_limits::min() / kNanosecondsPerSecond) { + return false; + } + const Timestamp seconds_ns = parts.seconds * kNanosecondsPerSecond; + if (seconds_ns > std::numeric_limits::max() - parts.nanos) { + return false; + } + out = seconds_ns + parts.nanos; + return true; +} + +inline void writeTimestamp(Writer& writer, Timestamp timestamp_ns) { + const auto parts = splitTimestamp(timestamp_ns); + writer.varint(1, static_cast(parts.seconds)); + writer.varint(2, static_cast(parts.nanos)); +} + +inline bool decodeTimestamp(Reader& reader, Timestamp& out) { + TimestampParts parts; + const bool ok = parseFields(reader, [&](Tag tag, Reader& r) { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t value = 0; + if (!r.readVarint(value)) { + return false; + } + if (tag.field == 1) { + parts.seconds = static_cast(value); + return true; + } + if (tag.field == 2) { + parts.nanos = static_cast(value); + return true; + } + return false; + }); + return ok && combineTimestamp(parts, out); +} + +inline bool readTimestampMessage(Reader& reader, Timestamp& out) { + Reader nested; + return reader.readMessage(nested) && decodeTimestamp(nested, out); +} + +// ---------- Vector3 ---------- + +inline void writeVector3(Writer& writer, const sdk::Vector3& v) { + writer.doubleField(1, v.x); + writer.doubleField(2, v.y); + writer.doubleField(3, v.z); +} + +inline bool decodeVector3(Reader& reader, sdk::Vector3& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + if (tag.type != WireType::kFixed64) { + return false; + } + switch (tag.field) { + case 1: + return r.readDouble(out.x); + case 2: + return r.readDouble(out.y); + case 3: + return r.readDouble(out.z); + default: + return false; + } + }); +} + +inline bool readVector3Message(Reader& reader, sdk::Vector3& out) { + Reader nested; + return reader.readMessage(nested) && decodeVector3(nested, out); +} + +// ---------- Point3 (same wire shape as Vector3, semantically distinct) ---------- + +inline void writePoint3(Writer& writer, const sdk::Point3& p) { + writer.doubleField(1, p.x); + writer.doubleField(2, p.y); + writer.doubleField(3, p.z); +} + +inline bool decodePoint3(Reader& reader, sdk::Point3& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + if (tag.type != WireType::kFixed64) { + return false; + } + switch (tag.field) { + case 1: + return r.readDouble(out.x); + case 2: + return r.readDouble(out.y); + case 3: + return r.readDouble(out.z); + default: + return false; + } + }); +} + +inline bool readPoint3Message(Reader& reader, sdk::Point3& out) { + Reader nested; + return reader.readMessage(nested) && decodePoint3(nested, out); +} + +// ---------- Quaternion ---------- + +inline void writeQuaternion(Writer& writer, const sdk::Quaternion& q) { + writer.doubleField(1, q.x); + writer.doubleField(2, q.y); + writer.doubleField(3, q.z); + writer.doubleField(4, q.w); +} + +inline bool decodeQuaternion(Reader& reader, sdk::Quaternion& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + if (tag.type != WireType::kFixed64) { + return false; + } + switch (tag.field) { + case 1: + return r.readDouble(out.x); + case 2: + return r.readDouble(out.y); + case 3: + return r.readDouble(out.z); + case 4: + return r.readDouble(out.w); + default: + return false; + } + }); +} + +inline bool readQuaternionMessage(Reader& reader, sdk::Quaternion& out) { + Reader nested; + return reader.readMessage(nested) && decodeQuaternion(nested, out); +} + +// ---------- Pose ---------- + +inline void writePose(Writer& writer, const sdk::Pose& p) { + writer.message(1, [&](Writer& nested) { writeVector3(nested, p.position); }); + writer.message(2, [&](Writer& nested) { writeQuaternion(nested, p.orientation); }); +} + +inline bool decodePose(Reader& reader, sdk::Pose& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + if (tag.type != WireType::kLengthDelimited) { + return false; + } + switch (tag.field) { + case 1: + return readVector3Message(r, out.position); + case 2: + return readQuaternionMessage(r, out.orientation); + default: + return false; + } + }); +} + +inline bool readPoseMessage(Reader& reader, sdk::Pose& out) { + Reader nested; + return reader.readMessage(nested) && decodePose(nested, out); +} + +// ---------- Color (uint8 RGBA <-> double 0..1) ---------- + +inline uint8_t normalizedToByte(double value) { + value = std::clamp(value, 0.0, 1.0); + return static_cast(value * 255.0 + 0.5); +} + +inline void writeColor(Writer& writer, const sdk::ColorRGBA& color) { + writer.doubleField(1, static_cast(color.r) / 255.0); + writer.doubleField(2, static_cast(color.g) / 255.0); + writer.doubleField(3, static_cast(color.b) / 255.0); + writer.doubleField(4, static_cast(color.a) / 255.0); +} + +inline bool decodeColor(Reader& reader, sdk::ColorRGBA& out) { + double r = 0.0, g = 0.0, b = 0.0, a = 0.0; + const bool ok = parseFields(reader, [&](Tag tag, Reader& reader_) { + if (tag.type != WireType::kFixed64) { + return false; + } + switch (tag.field) { + case 1: + return reader_.readDouble(r); + case 2: + return reader_.readDouble(g); + case 3: + return reader_.readDouble(b); + case 4: + return reader_.readDouble(a); + default: + return false; + } + }); + if (!ok) { + return false; + } + out = {normalizedToByte(r), normalizedToByte(g), normalizedToByte(b), normalizedToByte(a)}; + return true; +} + +inline bool readColorMessage(Reader& reader, sdk::ColorRGBA& out) { + Reader nested; + return reader.readMessage(nested) && decodeColor(nested, out); +} + +} // namespace PJ::builtin_wire diff --git a/pj_base/src/builtin/image_codec.cpp b/pj_base/src/builtin/image_codec.cpp new file mode 100644 index 0000000..756658d --- /dev/null +++ b/pj_base/src/builtin/image_codec.cpp @@ -0,0 +1,154 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/image_codec.hpp" + +#include +#include +#include +#include +#include + +#include "geometry_codec.hpp" +#include "protobuf_wire.hpp" + +namespace PJ { +namespace { + +using builtin_wire::parseFields; +using builtin_wire::Reader; +using builtin_wire::Tag; +using builtin_wire::WireType; +using builtin_wire::Writer; +using sdk::Image; + +bool readBytesIntoImage(Reader& reader, Image& out) { + const uint8_t* data = nullptr; + size_t size = 0; + if (!reader.readBytes(data, size)) { + return false; + } + auto owned = std::make_shared>(data, data + size); + out.data = Span(owned->data(), owned->size()); + out.anchor = owned; + return true; +} + +} // namespace + +std::vector serializeImage(const Image& image) { + std::vector out; + Writer writer(out); + + writer.message(1, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, image.timestamp_ns); }); + writer.varint(2, image.width); + writer.varint(3, image.height); + writer.string(4, image.encoding); + writer.varint(5, image.row_step); + writer.varint(6, image.is_bigendian ? 1u : 0u); + writer.bytes(7, image.data.data(), image.data.size()); + if (image.compressed_depth_min.has_value()) { + writer.floatField(8, *image.compressed_depth_min); + } + if (image.compressed_depth_max.has_value()) { + writer.floatField(9, *image.compressed_depth_max); + } + + return out; +} + +Expected deserializeImage(const uint8_t* data, size_t size) { + if (data == nullptr || size == 0) { + return unexpected(std::string("Image wire: empty buffer")); + } + + Reader reader(data, size); + sdk::Image image; + + const bool ok = parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + return tag.type == WireType::kLengthDelimited && builtin_wire::readTimestampMessage(r, image.timestamp_ns); + case 2: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + image.width = static_cast(v); + return true; + } + case 3: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + image.height = static_cast(v); + return true; + } + case 4: + return tag.type == WireType::kLengthDelimited && r.readString(image.encoding); + case 5: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + image.row_step = static_cast(v); + return true; + } + case 6: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + image.is_bigendian = (v != 0); + return true; + } + case 7: + return tag.type == WireType::kLengthDelimited && readBytesIntoImage(r, image); + case 8: { + if (tag.type != WireType::kFixed32) { + return false; + } + float v = 0.0f; + if (!r.readFloat(v)) { + return false; + } + image.compressed_depth_min = v; + return true; + } + case 9: { + if (tag.type != WireType::kFixed32) { + return false; + } + float v = 0.0f; + if (!r.readFloat(v)) { + return false; + } + image.compressed_depth_max = v; + return true; + } + default: + return false; + } + }); + + if (!ok) { + return unexpected(std::string("Image wire: decode failed")); + } + + return image; +} + +} // namespace PJ diff --git a/pj_base/src/builtin/mesh3d_codec.cpp b/pj_base/src/builtin/mesh3d_codec.cpp new file mode 100644 index 0000000..1ca6824 --- /dev/null +++ b/pj_base/src/builtin/mesh3d_codec.cpp @@ -0,0 +1,135 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/mesh3d_codec.hpp" + +#include +#include +#include +#include +#include + +#include "geometry_codec.hpp" +#include "protobuf_wire.hpp" + +namespace PJ { +namespace { + +using builtin_wire::parseFields; +using builtin_wire::Reader; +using builtin_wire::Tag; +using builtin_wire::WireType; +using builtin_wire::Writer; +using sdk::Mesh3D; + +bool readBytesIntoMesh(Reader& reader, Mesh3D& out) { + const uint8_t* data = nullptr; + size_t size = 0; + if (!reader.readBytes(data, size)) { + return false; + } + auto owned = std::make_shared>(data, data + size); + out.data = Span(owned->data(), owned->size()); + out.anchor = owned; + return true; +} + +} // namespace + +std::vector serializeMesh3D(const Mesh3D& mesh) { + std::vector out; + Writer writer(out); + + writer.message(1, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, mesh.timestamp_ns); }); + writer.string(2, mesh.frame_id); + writer.string(3, mesh.id); + writer.message(4, [&](Writer& nested) { builtin_wire::writePose(nested, mesh.pose); }); + writer.message(5, [&](Writer& nested) { builtin_wire::writeVector3(nested, mesh.scale); }); + writer.string(6, mesh.format); + writer.bytes(7, mesh.data.data(), mesh.data.size()); + writer.string(8, mesh.url); + writer.message(9, [&](Writer& nested) { builtin_wire::writeColor(nested, mesh.color); }); + writer.varint(10, mesh.override_color ? 1u : 0u); + + return out; +} + +Expected deserializeMesh3D(const uint8_t* data, size_t size) { + if (data == nullptr || size == 0) { + return unexpected(std::string("Mesh3D wire: empty buffer")); + } + + Reader reader(data, size); + sdk::Mesh3D mesh; + + const bool ok = parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return builtin_wire::readTimestampMessage(r, mesh.timestamp_ns); + case 2: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return r.readString(mesh.frame_id); + case 3: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return r.readString(mesh.id); + case 4: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return builtin_wire::readPoseMessage(r, mesh.pose); + case 5: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return builtin_wire::readVector3Message(r, mesh.scale); + case 6: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return r.readString(mesh.format); + case 7: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return readBytesIntoMesh(r, mesh); + case 8: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return r.readString(mesh.url); + case 9: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return builtin_wire::readColorMessage(r, mesh.color); + case 10: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + mesh.override_color = (v != 0); + return true; + } + default: + return false; + } + }); + + if (!ok) { + return unexpected(std::string("Mesh3D wire: decode failed")); + } + + return mesh; +} + +} // namespace PJ diff --git a/pj_base/src/builtin/occupancy_grid_codec.cpp b/pj_base/src/builtin/occupancy_grid_codec.cpp new file mode 100644 index 0000000..6acce7f --- /dev/null +++ b/pj_base/src/builtin/occupancy_grid_codec.cpp @@ -0,0 +1,123 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/occupancy_grid_codec.hpp" + +#include +#include +#include +#include +#include + +#include "geometry_codec.hpp" +#include "protobuf_wire.hpp" + +namespace PJ { +namespace { + +using builtin_wire::parseFields; +using builtin_wire::Reader; +using builtin_wire::Tag; +using builtin_wire::WireType; +using builtin_wire::Writer; +using sdk::OccupancyGrid; + +bool readBytesIntoGrid(Reader& reader, OccupancyGrid& out) { + const uint8_t* data = nullptr; + size_t size = 0; + if (!reader.readBytes(data, size)) { + return false; + } + auto owned = std::make_shared>(data, data + size); + out.data = Span(owned->data(), owned->size()); + out.anchor = owned; + return true; +} + +} // namespace + +std::vector serializeOccupancyGrid(const OccupancyGrid& grid) { + std::vector out; + Writer writer(out); + + writer.message(1, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, grid.timestamp_ns); }); + writer.string(2, grid.frame_id); + writer.message(3, [&](Writer& nested) { builtin_wire::writePose(nested, grid.origin); }); + writer.doubleField(4, grid.resolution); + writer.varint(5, grid.width); + writer.varint(6, grid.height); + writer.bytes(7, grid.data.data(), grid.data.size()); + + return out; +} + +Expected deserializeOccupancyGrid(const uint8_t* data, size_t size) { + if (data == nullptr || size == 0) { + return unexpected(std::string("OccupancyGrid wire: empty buffer")); + } + + Reader reader(data, size); + sdk::OccupancyGrid grid; + + const bool ok = parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return builtin_wire::readTimestampMessage(r, grid.timestamp_ns); + case 2: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return r.readString(grid.frame_id); + case 3: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return builtin_wire::readPoseMessage(r, grid.origin); + case 4: + if (tag.type != WireType::kFixed64) { + return false; + } + return r.readDouble(grid.resolution); + case 5: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + grid.width = static_cast(v); + return true; + } + case 6: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + grid.height = static_cast(v); + return true; + } + case 7: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return readBytesIntoGrid(r, grid); + default: + return false; + } + }); + + if (!ok) { + return unexpected(std::string("OccupancyGrid wire: decode failed")); + } + + return grid; +} + +} // namespace PJ diff --git a/pj_base/src/builtin/point_cloud_codec.cpp b/pj_base/src/builtin/point_cloud_codec.cpp new file mode 100644 index 0000000..996f5de --- /dev/null +++ b/pj_base/src/builtin/point_cloud_codec.cpp @@ -0,0 +1,258 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/point_cloud_codec.hpp" + +#include +#include +#include +#include +#include + +#include "geometry_codec.hpp" +#include "protobuf_wire.hpp" + +namespace PJ { +namespace { + +using builtin_wire::parseFields; +using builtin_wire::Reader; +using builtin_wire::Tag; +using builtin_wire::WireType; +using builtin_wire::Writer; +using sdk::PointCloud; +using sdk::PointField; + +// ---------- PointField enum mapping ---------- +// proto: UNKNOWN=0, INT8=1, UINT8=2, INT16=3, UINT16=4, INT32=5, UINT32=6, FLOAT32=7, FLOAT64=8 +// SDK: kUnknown=0, kInt8=1, kUint8=2, kInt16=3, kUint16=4, kInt32=5, kUint32=6, kFloat32=7, kFloat64=8 +// +// Numerically identical — direct cast suffices. + +uint32_t datatypeToWire(PointField::Datatype dt) { + return static_cast(dt); +} + +PointField::Datatype datatypeFromWire(uint64_t value) { + switch (value) { + case 1: + return PointField::Datatype::kInt8; + case 2: + return PointField::Datatype::kUint8; + case 3: + return PointField::Datatype::kInt16; + case 4: + return PointField::Datatype::kUint16; + case 5: + return PointField::Datatype::kInt32; + case 6: + return PointField::Datatype::kUint32; + case 7: + return PointField::Datatype::kFloat32; + case 8: + return PointField::Datatype::kFloat64; + case 0: + default: + return PointField::Datatype::kUnknown; + } +} + +// ---------- PointField ---------- + +void writePointField(Writer& writer, const PointField& field) { + writer.string(1, field.name); + writer.varint(2, field.offset); + writer.varint(3, datatypeToWire(field.datatype)); + writer.varint(4, field.count); +} + +bool decodePointField(Reader& reader, PointField& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + return tag.type == WireType::kLengthDelimited && r.readString(out.name); + case 2: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + out.offset = static_cast(v); + return true; + } + case 3: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + out.datatype = datatypeFromWire(v); + return true; + } + case 4: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + out.count = static_cast(v); + return true; + } + default: + return false; + } + }); +} + +bool readPointFieldIntoVector(Reader& reader, std::vector& out) { + Reader nested; + if (!reader.readMessage(nested)) { + return false; + } + PointField field; + if (!decodePointField(nested, field)) { + return false; + } + out.push_back(std::move(field)); + return true; +} + +// ---------- PointCloud payload bytes ---------- + +bool readBytesIntoCloud(Reader& reader, PointCloud& out) { + const uint8_t* data = nullptr; + size_t size = 0; + if (!reader.readBytes(data, size)) { + return false; + } + auto owned = std::make_shared>(data, data + size); + out.data = Span(owned->data(), owned->size()); + out.anchor = owned; + return true; +} + +} // namespace + +std::vector serializePointCloud(const PointCloud& cloud) { + std::vector out; + Writer writer(out); + + writer.message(1, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, cloud.timestamp_ns); }); + writer.varint(2, cloud.width); + writer.varint(3, cloud.height); + writer.varint(4, cloud.point_step); + writer.varint(5, cloud.row_step); + writer.varint(6, cloud.is_bigendian ? 1u : 0u); + writer.varint(7, cloud.is_dense ? 1u : 0u); + for (const auto& field : cloud.fields) { + writer.message(8, [&](Writer& nested) { writePointField(nested, field); }); + } + writer.bytes(9, cloud.data.data(), cloud.data.size()); + writer.string(10, cloud.frame_id); + + return out; +} + +Expected deserializePointCloud(const uint8_t* data, size_t size) { + if (data == nullptr || size == 0) { + return unexpected(std::string("PointCloud wire: empty buffer")); + } + + Reader reader(data, size); + sdk::PointCloud cloud; + + const bool ok = parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + return tag.type == WireType::kLengthDelimited && builtin_wire::readTimestampMessage(r, cloud.timestamp_ns); + case 2: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + cloud.width = static_cast(v); + return true; + } + case 3: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + cloud.height = static_cast(v); + return true; + } + case 4: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + cloud.point_step = static_cast(v); + return true; + } + case 5: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + cloud.row_step = static_cast(v); + return true; + } + case 6: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + cloud.is_bigendian = (v != 0); + return true; + } + case 7: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + cloud.is_dense = (v != 0); + return true; + } + case 8: + return tag.type == WireType::kLengthDelimited && readPointFieldIntoVector(r, cloud.fields); + case 9: + return tag.type == WireType::kLengthDelimited && readBytesIntoCloud(r, cloud); + case 10: + return tag.type == WireType::kLengthDelimited && r.readString(cloud.frame_id); + default: + return false; + } + }); + + if (!ok) { + return unexpected(std::string("PointCloud wire: decode failed")); + } + + return cloud; +} + +} // namespace PJ diff --git a/pj_base/src/builtin/protobuf_wire.hpp b/pj_base/src/builtin/protobuf_wire.hpp index f3fd905..903efe3 100644 --- a/pj_base/src/builtin/protobuf_wire.hpp +++ b/pj_base/src/builtin/protobuf_wire.hpp @@ -2,6 +2,7 @@ // Copyright 2026 Davide Faconti // SPDX-License-Identifier: MIT +#include #include #include #include @@ -37,12 +38,56 @@ class Writer { appendFixed64(value); } + void fixed32(uint32_t field, uint32_t value) { + tag(field, WireType::kFixed32); + appendFixed32(value); + } + + /// Writes a packed array of fixed32 values (proto3 default packing for + /// `repeated fixed32` scalar fields). Omits the field entirely when + /// `values` is empty (matches proto3 default-field semantics). + template + void packedFixed32(uint32_t field, const Range& values) { + const size_t count = static_cast(values.size()); + if (count == 0) { + return; + } + tag(field, WireType::kLengthDelimited); + appendVarint(count * 4); + for (uint32_t v : values) { + appendFixed32(v); + } + } + void doubleField(uint32_t field, double value) { uint64_t bits = 0; std::memcpy(&bits, &value, sizeof(value)); fixed64(field, bits); } + void floatField(uint32_t field, float value) { + uint32_t bits = 0; + std::memcpy(&bits, &value, sizeof(value)); + fixed32(field, bits); + } + + /// Writes a packed array of double values (proto3 default packing for + /// `repeated double` fields). Omits the field entirely when empty. + template + void packedDouble(uint32_t field, const Range& values) { + const size_t count = static_cast(values.size()); + if (count == 0) { + return; + } + tag(field, WireType::kLengthDelimited); + appendVarint(count * 8); + for (double v : values) { + uint64_t bits = 0; + std::memcpy(&bits, &v, sizeof(v)); + appendFixed64(bits); + } + } + void string(uint32_t field, std::string_view value) { bytes(field, reinterpret_cast(value.data()), value.size()); } @@ -82,6 +127,12 @@ class Writer { } } + void appendFixed32(uint32_t value) { + for (int i = 0; i < 4; ++i) { + out_.push_back(static_cast((value >> (8 * i)) & 0xFFu)); + } + } + std::vector& out_; }; @@ -144,6 +195,18 @@ class Reader { return true; } + bool readFixed32(uint32_t& out) { + if (remaining() < 4) { + return false; + } + out = 0; + for (int i = 0; i < 4; ++i) { + out |= static_cast(p_[i]) << (8 * i); + } + p_ += 4; + return true; + } + bool readDouble(double& out) { uint64_t bits = 0; if (!readFixed64(bits)) { @@ -153,6 +216,15 @@ class Reader { return true; } + bool readFloat(float& out) { + uint32_t bits = 0; + if (!readFixed32(bits)) { + return false; + } + std::memcpy(&out, &bits, sizeof(out)); + return true; + } + bool readString(std::string& out) { const uint8_t* data = nullptr; size_t size = 0; @@ -163,6 +235,14 @@ class Reader { return true; } + /// Reads a length-delimited byte payload, returning a non-owning view into + /// the underlying buffer. The view is valid only for the lifetime of the + /// bytes the Reader was constructed over — callers that need to retain the + /// data must copy it. + bool readBytes(const uint8_t*& data, size_t& size) { + return readBytesImpl(data, size); + } + bool readMessage(Reader& out) { const uint8_t* data = nullptr; size_t size = 0; @@ -194,7 +274,7 @@ class Reader { } private: - bool readBytes(const uint8_t*& data, size_t& size) { + bool readBytesImpl(const uint8_t*& data, size_t& size) { uint64_t len = 0; if (!readVarint(len) || len > remaining()) { return false; @@ -217,4 +297,92 @@ class Reader { const uint8_t* end_ = nullptr; }; +/// Drives the standard tag-read / field-dispatch / skip-unknown loop used +/// by every per-message decoder. `handler(tag, reader)` consumes the field +/// and returns true when handled (including its own wire-type validation), +/// false to let the loop skip the field via reader.skip(). Returns false on +/// truncation / malformed input. +template +[[nodiscard]] inline bool parseFields(Reader& reader, FieldHandler&& handler) { + while (!reader.eof()) { + Tag tag; + if (!reader.readTag(tag)) { + return false; + } + if (!handler(tag, reader) && !reader.skip(tag.type)) { + return false; + } + } + return true; +} + +/// Reads a proto3-packed `repeated fixed32` payload from a length-delimited +/// field into `out`. Values are appended; the caller may pre-clear if +/// needed. Returns false on truncation or non-multiple-of-4 length. +[[nodiscard]] inline bool readPackedFixed32(Reader& reader, std::vector& out) { + const uint8_t* data = nullptr; + size_t size = 0; + if (!reader.readBytes(data, size)) { + return false; + } + if ((size % 4) != 0) { + return false; + } + Reader sub(data, size); + out.reserve(out.size() + size / 4); + while (!sub.eof()) { + uint32_t v = 0; + if (!sub.readFixed32(v)) { + return false; + } + out.push_back(v); + } + return true; +} + +/// Reads a proto3-packed `repeated double` payload into `out`. Values are +/// appended; the caller may pre-clear if needed. Returns false on +/// truncation or non-multiple-of-8 length. +[[nodiscard]] inline bool readPackedDouble(Reader& reader, std::vector& out) { + const uint8_t* data = nullptr; + size_t size = 0; + if (!reader.readBytes(data, size)) { + return false; + } + if ((size % 8) != 0) { + return false; + } + Reader sub(data, size); + out.reserve(out.size() + size / 8); + while (!sub.eof()) { + double v = 0.0; + if (!sub.readDouble(v)) { + return false; + } + out.push_back(v); + } + return true; +} + +/// Reads a proto3-packed `repeated double` payload into a fixed-size array. +/// Fails if the wire length does not match exactly `N * 8` bytes. +template +[[nodiscard]] inline bool readPackedDoubleArray(Reader& reader, std::array& out) { + const uint8_t* data = nullptr; + size_t size = 0; + if (!reader.readBytes(data, size)) { + return false; + } + if (size != N * 8) { + return false; + } + Reader sub(data, size); + for (size_t i = 0; i < N; ++i) { + if (!sub.readDouble(out[i])) { + return false; + } + } + return true; +} + } // namespace PJ::builtin_wire diff --git a/pj_base/src/builtin/scene_entities_codec.cpp b/pj_base/src/builtin/scene_entities_codec.cpp new file mode 100644 index 0000000..8b1ce29 --- /dev/null +++ b/pj_base/src/builtin/scene_entities_codec.cpp @@ -0,0 +1,527 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/scene_entities_codec.hpp" + +#include +#include +#include +#include + +#include "geometry_codec.hpp" +#include "protobuf_wire.hpp" + +namespace PJ { +namespace { + +using builtin_wire::parseFields; +using builtin_wire::Reader; +using builtin_wire::readPackedFixed32; +using builtin_wire::Tag; +using builtin_wire::WireType; +using builtin_wire::Writer; +using sdk::ArrowPrimitive; +using sdk::AxesPrimitive; +using sdk::CubePrimitive; +using sdk::CylinderPrimitive; +using sdk::LinePrimitive; +using sdk::LineType; +using sdk::SceneEntities; +using sdk::SceneEntity; +using sdk::SpherePrimitive; +using sdk::TextPrimitive; +using sdk::TrianglePrimitive; + +// ---------- LineType enum mapping ---------- +// proto: UNKNOWN=0, LINE_STRIP=1, LINE_LOOP=2, LINE_LIST=3 +// SDK: kLineStrip=0, kLineLoop=1, kLineList=2 + +uint32_t lineTypeToWire(LineType type) { + switch (type) { + case LineType::kLineStrip: + return 1; + case LineType::kLineLoop: + return 2; + case LineType::kLineList: + return 3; + } + return 1; +} + +LineType lineTypeFromWire(uint64_t value) { + switch (value) { + case 2: + return LineType::kLineLoop; + case 3: + return LineType::kLineList; + case 0: + case 1: + default: + return LineType::kLineStrip; + } +} + +// ---------- Color list helper (decoder accumulates) ---------- + +bool readColorIntoVector(Reader& reader, std::vector& out) { + sdk::ColorRGBA c; + if (!builtin_wire::readColorMessage(reader, c)) { + return false; + } + out.push_back(c); + return true; +} + +bool readPoint3IntoVector(Reader& reader, std::vector& out) { + sdk::Point3 p; + if (!builtin_wire::readPoint3Message(reader, p)) { + return false; + } + out.push_back(p); + return true; +} + +// ---------- ArrowPrimitive ---------- + +void writeArrowPrimitive(Writer& writer, const ArrowPrimitive& p) { + writer.message(1, [&](Writer& nested) { builtin_wire::writePose(nested, p.pose); }); + writer.doubleField(2, p.shaft_length); + writer.doubleField(3, p.shaft_diameter); + writer.doubleField(4, p.head_length); + writer.doubleField(5, p.head_diameter); + writer.message(6, [&](Writer& nested) { builtin_wire::writeColor(nested, p.color); }); +} + +bool decodeArrowPrimitive(Reader& reader, ArrowPrimitive& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + return tag.type == WireType::kLengthDelimited && builtin_wire::readPoseMessage(r, out.pose); + case 2: + return tag.type == WireType::kFixed64 && r.readDouble(out.shaft_length); + case 3: + return tag.type == WireType::kFixed64 && r.readDouble(out.shaft_diameter); + case 4: + return tag.type == WireType::kFixed64 && r.readDouble(out.head_length); + case 5: + return tag.type == WireType::kFixed64 && r.readDouble(out.head_diameter); + case 6: + return tag.type == WireType::kLengthDelimited && builtin_wire::readColorMessage(r, out.color); + default: + return false; + } + }); +} + +// ---------- CubePrimitive ---------- + +void writeCubePrimitive(Writer& writer, const CubePrimitive& p) { + writer.message(1, [&](Writer& nested) { builtin_wire::writePose(nested, p.pose); }); + writer.message(2, [&](Writer& nested) { builtin_wire::writeVector3(nested, p.size); }); + writer.message(3, [&](Writer& nested) { builtin_wire::writeColor(nested, p.color); }); +} + +bool decodeCubePrimitive(Reader& reader, CubePrimitive& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + if (tag.type != WireType::kLengthDelimited) { + return false; + } + switch (tag.field) { + case 1: + return builtin_wire::readPoseMessage(r, out.pose); + case 2: + return builtin_wire::readVector3Message(r, out.size); + case 3: + return builtin_wire::readColorMessage(r, out.color); + default: + return false; + } + }); +} + +// ---------- SpherePrimitive (same wire shape as Cube) ---------- + +void writeSpherePrimitive(Writer& writer, const SpherePrimitive& p) { + writer.message(1, [&](Writer& nested) { builtin_wire::writePose(nested, p.pose); }); + writer.message(2, [&](Writer& nested) { builtin_wire::writeVector3(nested, p.size); }); + writer.message(3, [&](Writer& nested) { builtin_wire::writeColor(nested, p.color); }); +} + +bool decodeSpherePrimitive(Reader& reader, SpherePrimitive& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + if (tag.type != WireType::kLengthDelimited) { + return false; + } + switch (tag.field) { + case 1: + return builtin_wire::readPoseMessage(r, out.pose); + case 2: + return builtin_wire::readVector3Message(r, out.size); + case 3: + return builtin_wire::readColorMessage(r, out.color); + default: + return false; + } + }); +} + +// ---------- CylinderPrimitive ---------- + +void writeCylinderPrimitive(Writer& writer, const CylinderPrimitive& p) { + writer.message(1, [&](Writer& nested) { builtin_wire::writePose(nested, p.pose); }); + writer.message(2, [&](Writer& nested) { builtin_wire::writeVector3(nested, p.size); }); + writer.doubleField(3, p.bottom_scale); + writer.doubleField(4, p.top_scale); + writer.message(5, [&](Writer& nested) { builtin_wire::writeColor(nested, p.color); }); +} + +bool decodeCylinderPrimitive(Reader& reader, CylinderPrimitive& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + return tag.type == WireType::kLengthDelimited && builtin_wire::readPoseMessage(r, out.pose); + case 2: + return tag.type == WireType::kLengthDelimited && builtin_wire::readVector3Message(r, out.size); + case 3: + return tag.type == WireType::kFixed64 && r.readDouble(out.bottom_scale); + case 4: + return tag.type == WireType::kFixed64 && r.readDouble(out.top_scale); + case 5: + return tag.type == WireType::kLengthDelimited && builtin_wire::readColorMessage(r, out.color); + default: + return false; + } + }); +} + +// ---------- LinePrimitive ---------- + +void writeLinePrimitive(Writer& writer, const LinePrimitive& p) { + writer.varint(1, lineTypeToWire(p.type)); + writer.message(2, [&](Writer& nested) { builtin_wire::writePose(nested, p.pose); }); + writer.doubleField(3, p.thickness); + writer.varint(4, p.scale_invariant ? 1u : 0u); + for (const auto& point : p.points) { + writer.message(5, [&](Writer& nested) { builtin_wire::writePoint3(nested, point); }); + } + writer.message(6, [&](Writer& nested) { builtin_wire::writeColor(nested, p.color); }); + for (const auto& color : p.colors) { + writer.message(7, [&](Writer& nested) { builtin_wire::writeColor(nested, color); }); + } + writer.packedFixed32(8, p.indices); +} + +bool decodeLinePrimitive(Reader& reader, LinePrimitive& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + out.type = lineTypeFromWire(v); + return true; + } + case 2: + return tag.type == WireType::kLengthDelimited && builtin_wire::readPoseMessage(r, out.pose); + case 3: + return tag.type == WireType::kFixed64 && r.readDouble(out.thickness); + case 4: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + out.scale_invariant = (v != 0); + return true; + } + case 5: + return tag.type == WireType::kLengthDelimited && readPoint3IntoVector(r, out.points); + case 6: + return tag.type == WireType::kLengthDelimited && builtin_wire::readColorMessage(r, out.color); + case 7: + return tag.type == WireType::kLengthDelimited && readColorIntoVector(r, out.colors); + case 8: + return tag.type == WireType::kLengthDelimited && readPackedFixed32(r, out.indices); + default: + return false; + } + }); +} + +// ---------- TrianglePrimitive ---------- + +void writeTrianglePrimitive(Writer& writer, const TrianglePrimitive& p) { + writer.message(1, [&](Writer& nested) { builtin_wire::writePose(nested, p.pose); }); + for (const auto& point : p.points) { + writer.message(2, [&](Writer& nested) { builtin_wire::writePoint3(nested, point); }); + } + writer.message(3, [&](Writer& nested) { builtin_wire::writeColor(nested, p.color); }); + for (const auto& color : p.colors) { + writer.message(4, [&](Writer& nested) { builtin_wire::writeColor(nested, color); }); + } + writer.packedFixed32(5, p.indices); +} + +bool decodeTrianglePrimitive(Reader& reader, TrianglePrimitive& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + if (tag.type != WireType::kLengthDelimited) { + return false; + } + switch (tag.field) { + case 1: + return builtin_wire::readPoseMessage(r, out.pose); + case 2: + return readPoint3IntoVector(r, out.points); + case 3: + return builtin_wire::readColorMessage(r, out.color); + case 4: + return readColorIntoVector(r, out.colors); + case 5: + return readPackedFixed32(r, out.indices); + default: + return false; + } + }); +} + +// ---------- TextPrimitive ---------- + +void writeTextPrimitive(Writer& writer, const TextPrimitive& p) { + writer.message(1, [&](Writer& nested) { builtin_wire::writePose(nested, p.pose); }); + writer.varint(2, p.billboard ? 1u : 0u); + writer.doubleField(3, p.font_size); + writer.varint(4, p.scale_invariant ? 1u : 0u); + writer.message(5, [&](Writer& nested) { builtin_wire::writeColor(nested, p.color); }); + writer.string(6, p.text); +} + +bool decodeTextPrimitive(Reader& reader, TextPrimitive& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + return tag.type == WireType::kLengthDelimited && builtin_wire::readPoseMessage(r, out.pose); + case 2: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + out.billboard = (v != 0); + return true; + } + case 3: + return tag.type == WireType::kFixed64 && r.readDouble(out.font_size); + case 4: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + out.scale_invariant = (v != 0); + return true; + } + case 5: + return tag.type == WireType::kLengthDelimited && builtin_wire::readColorMessage(r, out.color); + case 6: + return tag.type == WireType::kLengthDelimited && r.readString(out.text); + default: + return false; + } + }); +} + +// ---------- AxesPrimitive ---------- + +void writeAxesPrimitive(Writer& writer, const AxesPrimitive& p) { + writer.message(1, [&](Writer& nested) { builtin_wire::writePose(nested, p.pose); }); + writer.doubleField(2, p.length); + writer.doubleField(3, p.thickness); + writer.varint(4, p.scale_invariant ? 1u : 0u); +} + +bool decodeAxesPrimitive(Reader& reader, AxesPrimitive& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + return tag.type == WireType::kLengthDelimited && builtin_wire::readPoseMessage(r, out.pose); + case 2: + return tag.type == WireType::kFixed64 && r.readDouble(out.length); + case 3: + return tag.type == WireType::kFixed64 && r.readDouble(out.thickness); + case 4: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + out.scale_invariant = (v != 0); + return true; + } + default: + return false; + } + }); +} + +// ---------- Nested-primitive read helpers ---------- + +template +bool readPrimitiveIntoVector(Reader& reader, std::vector& out, Decoder&& decode) { + Reader nested; + if (!reader.readMessage(nested)) { + return false; + } + Primitive primitive; + if (!decode(nested, primitive)) { + return false; + } + out.push_back(std::move(primitive)); + return true; +} + +// ---------- SceneEntity ---------- + +void writeSceneEntity(Writer& writer, const SceneEntity& e) { + writer.message(1, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, e.timestamp); }); + writer.string(2, e.frame_id); + writer.string(3, e.id); + // Duration shares the seconds+nanos wire shape with Timestamp. + writer.message(4, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, e.lifetime_ns); }); + writer.varint(5, e.frame_locked ? 1u : 0u); + + for (const auto& a : e.arrows) { + writer.message(6, [&](Writer& nested) { writeArrowPrimitive(nested, a); }); + } + for (const auto& c : e.cubes) { + writer.message(7, [&](Writer& nested) { writeCubePrimitive(nested, c); }); + } + for (const auto& s : e.spheres) { + writer.message(8, [&](Writer& nested) { writeSpherePrimitive(nested, s); }); + } + for (const auto& c : e.cylinders) { + writer.message(9, [&](Writer& nested) { writeCylinderPrimitive(nested, c); }); + } + for (const auto& l : e.lines) { + writer.message(10, [&](Writer& nested) { writeLinePrimitive(nested, l); }); + } + for (const auto& t : e.triangles) { + writer.message(11, [&](Writer& nested) { writeTrianglePrimitive(nested, t); }); + } + for (const auto& t : e.texts) { + writer.message(12, [&](Writer& nested) { writeTextPrimitive(nested, t); }); + } + for (const auto& a : e.axes) { + writer.message(13, [&](Writer& nested) { writeAxesPrimitive(nested, a); }); + } +} + +bool decodeSceneEntity(Reader& reader, SceneEntity& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + return tag.type == WireType::kLengthDelimited && builtin_wire::readTimestampMessage(r, out.timestamp); + case 2: + return tag.type == WireType::kLengthDelimited && r.readString(out.frame_id); + case 3: + return tag.type == WireType::kLengthDelimited && r.readString(out.id); + case 4: + return tag.type == WireType::kLengthDelimited && builtin_wire::readTimestampMessage(r, out.lifetime_ns); + case 5: { + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + out.frame_locked = (v != 0); + return true; + } + case 6: + return tag.type == WireType::kLengthDelimited && readPrimitiveIntoVector(r, out.arrows, decodeArrowPrimitive); + case 7: + return tag.type == WireType::kLengthDelimited && readPrimitiveIntoVector(r, out.cubes, decodeCubePrimitive); + case 8: + return tag.type == WireType::kLengthDelimited && readPrimitiveIntoVector(r, out.spheres, decodeSpherePrimitive); + case 9: + return tag.type == WireType::kLengthDelimited && + readPrimitiveIntoVector(r, out.cylinders, decodeCylinderPrimitive); + case 10: + return tag.type == WireType::kLengthDelimited && readPrimitiveIntoVector(r, out.lines, decodeLinePrimitive); + case 11: + return tag.type == WireType::kLengthDelimited && + readPrimitiveIntoVector(r, out.triangles, decodeTrianglePrimitive); + case 12: + return tag.type == WireType::kLengthDelimited && readPrimitiveIntoVector(r, out.texts, decodeTextPrimitive); + case 13: + return tag.type == WireType::kLengthDelimited && readPrimitiveIntoVector(r, out.axes, decodeAxesPrimitive); + default: + return false; + } + }); +} + +} // namespace + +std::vector serializeSceneEntities(const SceneEntities& entities) { + std::vector out; + Writer writer(out); + + for (const auto& entity : entities.entities) { + writer.message(1, [&](Writer& nested) { writeSceneEntity(nested, entity); }); + } + + return out; +} + +Expected deserializeSceneEntities(const uint8_t* data, size_t size) { + if (data == nullptr || size == 0) { + return unexpected(std::string("SceneEntities wire: empty buffer")); + } + + Reader reader(data, size); + sdk::SceneEntities entities; + + while (!reader.eof()) { + Tag tag; + if (!reader.readTag(tag)) { + return unexpected(std::string("SceneEntities wire: bad tag")); + } + + if (tag.type != WireType::kLengthDelimited) { + if (!reader.skip(tag.type)) { + return unexpected(std::string("SceneEntities wire: skip failed")); + } + continue; + } + + Reader nested; + if (!reader.readMessage(nested)) { + return unexpected(std::string("SceneEntities wire: bad nested message length")); + } + + if (tag.field == 1) { + SceneEntity entity; + if (!decodeSceneEntity(nested, entity)) { + return unexpected(std::string("SceneEntities wire: SceneEntity decode failed")); + } + entities.entities.push_back(std::move(entity)); + } + } + + return entities; +} + +} // namespace PJ diff --git a/pj_base/src/builtin/video_frame_codec.cpp b/pj_base/src/builtin/video_frame_codec.cpp new file mode 100644 index 0000000..e52e62a --- /dev/null +++ b/pj_base/src/builtin/video_frame_codec.cpp @@ -0,0 +1,93 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/video_frame_codec.hpp" + +#include +#include +#include +#include +#include + +#include "geometry_codec.hpp" +#include "protobuf_wire.hpp" + +namespace PJ { +namespace { + +using builtin_wire::parseFields; +using builtin_wire::Reader; +using builtin_wire::Tag; +using builtin_wire::WireType; +using builtin_wire::Writer; +using sdk::VideoFrame; + +bool readBytesIntoFrame(Reader& reader, VideoFrame& out) { + const uint8_t* data = nullptr; + size_t size = 0; + if (!reader.readBytes(data, size)) { + return false; + } + auto owned = std::make_shared>(data, data + size); + out.data = Span(owned->data(), owned->size()); + out.anchor = owned; + return true; +} + +} // namespace + +std::vector serializeVideoFrame(const VideoFrame& frame) { + std::vector out; + Writer writer(out); + + writer.message(1, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, frame.timestamp_ns); }); + writer.string(2, frame.frame_id); + writer.string(3, frame.format); + writer.bytes(4, frame.data.data(), frame.data.size()); + + return out; +} + +Expected deserializeVideoFrame(const uint8_t* data, size_t size) { + if (data == nullptr || size == 0) { + return unexpected(std::string("VideoFrame wire: empty buffer")); + } + + Reader reader(data, size); + sdk::VideoFrame frame; + + const bool ok = parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return builtin_wire::readTimestampMessage(r, frame.timestamp_ns); + case 2: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return r.readString(frame.frame_id); + case 3: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return r.readString(frame.format); + case 4: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return readBytesIntoFrame(r, frame); + default: + return false; + } + }); + + if (!ok) { + return unexpected(std::string("VideoFrame wire: decode failed")); + } + + return frame; +} + +} // namespace PJ diff --git a/pj_base/tests/abi_layout_sentinels_test.cpp b/pj_base/tests/abi_layout_sentinels_test.cpp index a021eeb..ee9aa85 100644 --- a/pj_base/tests/abi_layout_sentinels_test.cpp +++ b/pj_base/tests/abi_layout_sentinels_test.cpp @@ -101,7 +101,19 @@ static_assert(PJ_TOOLBOX_MIN_VTABLE_SIZE <= sizeof(PJ_toolbox_vtable_t), "MIN mu // Public ABI types crossing the boundary for the v4 builtin-object pipeline. // Sizes and offsets are pinned; any change is a deliberate ABI revision. static_assert(sizeof(PJ_builtin_object_type_t) == 4, "enum layout pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_NONE == 0, "None type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_IMAGE == 1, "Image type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_POINTCLOUD == 3, "PointCloud type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_DEPTH_IMAGE == 4, "DepthImage type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_IMAGE_ANNOTATIONS == 5, "ImageAnnotations type id pinned"); static_assert(PJ_BUILTIN_OBJECT_TYPE_FRAME_TRANSFORMS == 6, "FrameTransforms type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_OCCUPANCY_GRID == 7, "OccupancyGrid type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_COMPRESSED_POINTCLOUD == 8, "CompressedPointCloud type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_MESH3D == 9, "Mesh3D type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_VIDEO_FRAME == 10, "VideoFrame type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_SCENE_ENTITIES == 11, "SceneEntities type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_ASSET_VIDEO == 12, "AssetVideo type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_ROBOT_DESCRIPTION == 13, "RobotDescription type id pinned"); static_assert(sizeof(PJ_schema_classification_t) == 4, "PJ_schema_classification_t layout pinned"); static_assert(offsetof(PJ_schema_classification_t, object_type) == 0, "object_type at offset 0"); static_assert(offsetof(PJ_schema_classification_t, reserved) == 2, "reserved at offset 2"); diff --git a/pj_base/tests/asset_video_codec_test.cpp b/pj_base/tests/asset_video_codec_test.cpp new file mode 100644 index 0000000..51a4fcd --- /dev/null +++ b/pj_base/tests/asset_video_codec_test.cpp @@ -0,0 +1,67 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/asset_video_codec.hpp" + +#include + +#include +#include + +#include "protobuf_wire_test_helpers.hpp" + +namespace PJ { +namespace { + +using sdk::AssetVideo; +namespace pb = ::PJ::test_pb; + +TEST(AssetVideoCodecTest, SchemaName) { + EXPECT_EQ(kSchemaAssetVideo, "PJ.AssetVideo"); +} + +TEST(AssetVideoCodecTest, EmptyBufferProducesError) { + EXPECT_FALSE(deserializeAssetVideo(nullptr, 0).has_value()); +} + +TEST(AssetVideoCodecTest, RoundTripFullyPopulated) { + AssetVideo in; + in.time_origin_ns = 1'700'000'000'000'000'000LL; + in.duration_ns = 60'000'000'000LL; // 60 s + in.file_path = "/data/2026-05-21/camera0.mp4"; + in.media_type = "video/mp4"; + in.width = 1920; + in.height = 1080; + in.frame_rate = 29.97; + + const auto bytes = serializeAssetVideo(in); + auto out = deserializeAssetVideo(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + ASSERT_TRUE(out->time_origin_ns.has_value()); + EXPECT_EQ(*out->time_origin_ns, *in.time_origin_ns); + ASSERT_TRUE(out->duration_ns.has_value()); + EXPECT_EQ(*out->duration_ns, *in.duration_ns); + EXPECT_EQ(out->file_path, in.file_path); + EXPECT_EQ(out->media_type, in.media_type); + EXPECT_EQ(out->width, in.width); + EXPECT_EQ(out->height, in.height); + EXPECT_DOUBLE_EQ(out->frame_rate, in.frame_rate); +} + +TEST(AssetVideoCodecTest, OptionalsAbsentRoundTrip) { + AssetVideo in; + in.file_path = "relative/path.mkv"; + + const auto bytes = serializeAssetVideo(in); + auto out = deserializeAssetVideo(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_FALSE(out->time_origin_ns.has_value()); + EXPECT_FALSE(out->duration_ns.has_value()); + EXPECT_EQ(out->file_path, in.file_path); + EXPECT_TRUE(out->media_type.empty()); + EXPECT_EQ(out->width, 0u); + EXPECT_EQ(out->height, 0u); +} + +} // namespace +} // namespace PJ diff --git a/pj_base/tests/builtin_object_test.cpp b/pj_base/tests/builtin_object_test.cpp index 35197aa..0bc07cd 100644 --- a/pj_base/tests/builtin_object_test.cpp +++ b/pj_base/tests/builtin_object_test.cpp @@ -1,19 +1,27 @@ // Copyright 2026 Davide Faconti // SPDX-License-Identifier: MIT -#include +#include "pj_base/builtin/builtin_object.hpp" -#include "pj_base/builtin/BuiltinObject.hpp" +#include +using PJ::sdk::AssetVideo; using PJ::sdk::BuiltinObject; using PJ::sdk::BuiltinObjectType; +using PJ::sdk::CompressedPointCloud; using PJ::sdk::DepthImage; using PJ::sdk::FrameTransforms; using PJ::sdk::Image; using PJ::sdk::ImageAnnotations; +using PJ::sdk::Mesh3D; +using PJ::sdk::name; +using PJ::sdk::OccupancyGrid; using PJ::sdk::parseBuiltinObjectType; using PJ::sdk::PointCloud; +using PJ::sdk::RobotDescription; +using PJ::sdk::SceneEntities; using PJ::sdk::typeOf; +using PJ::sdk::VideoFrame; TEST(BuiltinObjectTest, TypeOfRecognizesKnownBuiltinTypes) { EXPECT_EQ(typeOf(BuiltinObject{}), BuiltinObjectType::kNone); @@ -22,9 +30,84 @@ TEST(BuiltinObjectTest, TypeOfRecognizesKnownBuiltinTypes) { EXPECT_EQ(typeOf(BuiltinObject{DepthImage{}}), BuiltinObjectType::kDepthImage); EXPECT_EQ(typeOf(BuiltinObject{ImageAnnotations{}}), BuiltinObjectType::kImageAnnotations); EXPECT_EQ(typeOf(BuiltinObject{FrameTransforms{}}), BuiltinObjectType::kFrameTransforms); + EXPECT_EQ(typeOf(BuiltinObject{OccupancyGrid{}}), BuiltinObjectType::kOccupancyGrid); + EXPECT_EQ(typeOf(BuiltinObject{CompressedPointCloud{}}), BuiltinObjectType::kCompressedPointCloud); + EXPECT_EQ(typeOf(BuiltinObject{Mesh3D{}}), BuiltinObjectType::kMesh3D); + EXPECT_EQ(typeOf(BuiltinObject{VideoFrame{}}), BuiltinObjectType::kVideoFrame); + EXPECT_EQ(typeOf(BuiltinObject{SceneEntities{}}), BuiltinObjectType::kSceneEntities); + EXPECT_EQ(typeOf(BuiltinObject{AssetVideo{}}), BuiltinObjectType::kAssetVideo); + EXPECT_EQ(typeOf(BuiltinObject{RobotDescription{}}), BuiltinObjectType::kRobotDescription); +} + +TEST(BuiltinObjectTest, NameAndParseRoundTripForEveryEnumEntry) { + for (auto t : { + BuiltinObjectType::kNone, + BuiltinObjectType::kImage, + BuiltinObjectType::kPointCloud, + BuiltinObjectType::kDepthImage, + BuiltinObjectType::kImageAnnotations, + BuiltinObjectType::kFrameTransforms, + BuiltinObjectType::kOccupancyGrid, + BuiltinObjectType::kCompressedPointCloud, + BuiltinObjectType::kMesh3D, + BuiltinObjectType::kVideoFrame, + BuiltinObjectType::kSceneEntities, + BuiltinObjectType::kAssetVideo, + BuiltinObjectType::kRobotDescription, + }) { + const auto parsed = parseBuiltinObjectType(name(t)); + ASSERT_TRUE(parsed.has_value()) << "parseBuiltinObjectType failed for " << name(t); + EXPECT_EQ(*parsed, t) << "round-trip mismatch for " << name(t); + } } -TEST(BuiltinObjectTest, ParsesFrameTransformsTypeName) { - EXPECT_EQ(parseBuiltinObjectType("kFrameTransforms"), BuiltinObjectType::kFrameTransforms); - EXPECT_FALSE(parseBuiltinObjectType("FrameTransforms").has_value()); +TEST(BuiltinObjectTest, ParseRejectsUnknownNames) { + EXPECT_FALSE(parseBuiltinObjectType("FrameTransforms").has_value()); // missing leading 'k' + EXPECT_FALSE(parseBuiltinObjectType("").has_value()); + EXPECT_FALSE(parseBuiltinObjectType("kBogus").has_value()); +} + +TEST(BuiltinObjectTest, ParsesRobotDescriptionTypeName) { + EXPECT_EQ(parseBuiltinObjectType("kRobotDescription"), BuiltinObjectType::kRobotDescription); + EXPECT_FALSE(parseBuiltinObjectType("RobotDescription").has_value()); +} + +TEST(BuiltinObjectTest, PointCloudCarriesFrameId) { + PointCloud in; + in.width = 100; + in.height = 1; + in.point_step = 16; + in.row_step = 1600; + in.frame_id = "velodyne"; + in.timestamp_ns = 1'500'000'000; + BuiltinObject obj{in}; + ASSERT_EQ(typeOf(obj), BuiltinObjectType::kPointCloud); + + const auto* out = std::any_cast(&obj); + ASSERT_NE(out, nullptr); + EXPECT_EQ(out->frame_id, "velodyne"); + EXPECT_EQ(out->width, 100u); + EXPECT_EQ(out->timestamp_ns, 1'500'000'000); +} + +TEST(BuiltinObjectTest, RobotDescriptionRoundtripPreservesFields) { + RobotDescription in{ + .timestamp_ns = 1'500'000'000, + .topic = "/robot_description", + .format = "urdf", + .text = "", + }; + BuiltinObject obj{in}; + ASSERT_EQ(typeOf(obj), BuiltinObjectType::kRobotDescription); + + const auto* out = std::any_cast(&obj); + ASSERT_NE(out, nullptr); + EXPECT_EQ(out->timestamp_ns, in.timestamp_ns); + EXPECT_EQ(out->topic, in.topic); + EXPECT_EQ(out->format, in.format); + EXPECT_EQ(out->text, in.text); + EXPECT_FALSE(out->empty()); + + RobotDescription empty; + EXPECT_TRUE(empty.empty()); } diff --git a/pj_base/tests/compressed_point_cloud_codec_test.cpp b/pj_base/tests/compressed_point_cloud_codec_test.cpp new file mode 100644 index 0000000..89bad39 --- /dev/null +++ b/pj_base/tests/compressed_point_cloud_codec_test.cpp @@ -0,0 +1,47 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/compressed_point_cloud_codec.hpp" + +#include + +#include +#include +#include + +#include "protobuf_wire_test_helpers.hpp" + +namespace PJ { +namespace { + +using sdk::CompressedPointCloud; +namespace pb = ::PJ::test_pb; + +TEST(CompressedPointCloudCodecTest, SchemaName) { + EXPECT_EQ(kSchemaCompressedPointCloud, "PJ.CompressedPointCloud"); +} + +TEST(CompressedPointCloudCodecTest, EmptyBufferProducesError) { + EXPECT_FALSE(deserializeCompressedPointCloud(nullptr, 0).has_value()); +} + +TEST(CompressedPointCloudCodecTest, RoundTripDracoPayload) { + CompressedPointCloud in; + in.timestamp_ns = -123'456'789LL; + in.frame_id = "lidar_link"; + in.format = "draco"; + const std::vector payload = {0x44, 0x52, 0x41, 0x43, 0x4F, 0x01, 0x02, 0x03, 0x04}; + in.data = Span(payload.data(), payload.size()); + + const auto bytes = serializeCompressedPointCloud(in); + auto out = deserializeCompressedPointCloud(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_EQ(out->timestamp_ns, in.timestamp_ns); + EXPECT_EQ(out->frame_id, in.frame_id); + EXPECT_EQ(out->format, in.format); + ASSERT_EQ(out->data.size(), payload.size()); + EXPECT_EQ(std::memcmp(out->data.data(), payload.data(), payload.size()), 0); +} + +} // namespace +} // namespace PJ diff --git a/pj_base/tests/depth_image_codec_test.cpp b/pj_base/tests/depth_image_codec_test.cpp new file mode 100644 index 0000000..ca76242 --- /dev/null +++ b/pj_base/tests/depth_image_codec_test.cpp @@ -0,0 +1,72 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/depth_image_codec.hpp" + +#include + +#include +#include +#include + +#include "protobuf_wire_test_helpers.hpp" + +namespace PJ { +namespace { + +using sdk::DepthImage; +namespace pb = ::PJ::test_pb; + +TEST(DepthImageCodecTest, SchemaName) { + EXPECT_EQ(kSchemaDepthImage, "PJ.DepthImage"); +} + +TEST(DepthImageCodecTest, EmptyBufferProducesError) { + EXPECT_FALSE(deserializeDepthImage(nullptr, 0).has_value()); +} + +TEST(DepthImageCodecTest, RoundTripRectified16UC1) { + DepthImage in; + in.timestamp_ns = 1'234'000'000LL; + in.width = 2; + in.height = 2; + in.encoding = "16UC1"; + in.K = {525.0, 0.0, 319.5, 0.0, 525.0, 239.5, 0.0, 0.0, 1.0}; + // empty distortion_model => rectified + const std::vector payload = {0x10, 0x00, 0x20, 0x00, 0x30, 0x00, 0x40, 0x00}; + in.data = Span(payload.data(), payload.size()); + + const auto bytes = serializeDepthImage(in); + auto out = deserializeDepthImage(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_EQ(out->width, in.width); + EXPECT_EQ(out->height, in.height); + EXPECT_EQ(out->encoding, in.encoding); + EXPECT_EQ(out->K, in.K); + EXPECT_TRUE(out->distortion_model.empty()); + EXPECT_TRUE(out->D.empty()); + ASSERT_EQ(out->data.size(), payload.size()); + EXPECT_EQ(std::memcmp(out->data.data(), payload.data(), payload.size()), 0); +} + +TEST(DepthImageCodecTest, RoundTripPlumbBobDistortion) { + DepthImage in; + in.width = 640; + in.height = 480; + in.encoding = "32FC1"; + in.K = {525.0, 0.0, 319.5, 0.0, 525.0, 239.5, 0.0, 0.0, 1.0}; + in.distortion_model = "plumb_bob"; + in.D = {-0.1, 0.05, 0.001, -0.002, 0.0}; + + const auto bytes = serializeDepthImage(in); + auto out = deserializeDepthImage(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_EQ(out->distortion_model, "plumb_bob"); + ASSERT_EQ(out->D.size(), 5u); + for (size_t i = 0; i < in.D.size(); ++i) { + EXPECT_DOUBLE_EQ(out->D[i], in.D[i]) << "D[" << i << "]"; + } +} + +} // namespace +} // namespace PJ diff --git a/pj_base/tests/frame_transforms_codec_test.cpp b/pj_base/tests/frame_transforms_codec_test.cpp index 030eff3..5c5ec39 100644 --- a/pj_base/tests/frame_transforms_codec_test.cpp +++ b/pj_base/tests/frame_transforms_codec_test.cpp @@ -6,105 +6,29 @@ #include #include -#include -#include -#include #include +#include "protobuf_wire_test_helpers.hpp" + namespace PJ { namespace { using sdk::FrameTransform; using sdk::FrameTransforms; - -namespace pb { - -inline void appendVarint(std::vector& out, uint64_t v) { - while (v >= 0x80u) { - out.push_back(static_cast((v & 0x7Fu) | 0x80u)); - v >>= 7; - } - out.push_back(static_cast(v)); -} - -inline void appendTag(std::vector& out, uint32_t field, uint32_t wire) { - appendVarint(out, (static_cast(field) << 3) | wire); -} - -inline void appendDouble(std::vector& out, double v) { - uint64_t bits = 0; - std::memcpy(&bits, &v, sizeof(v)); - for (int i = 0; i < 8; ++i) { - out.push_back(static_cast((bits >> (8 * i)) & 0xFFu)); - } -} - -inline void appendLenDelim(std::vector& out, const std::vector& body) { - appendVarint(out, body.size()); - out.insert(out.end(), body.begin(), body.end()); -} - -inline void appendString(std::vector& out, const std::string& value) { - appendVarint(out, value.size()); - out.insert(out.end(), value.begin(), value.end()); -} - -} // namespace pb - -std::vector encodeTimestamp(Timestamp timestamp_ns) { - constexpr int64_t ns_per_second = 1000LL * 1000LL * 1000LL; - int64_t seconds = timestamp_ns / ns_per_second; - int32_t nanos = static_cast(timestamp_ns % ns_per_second); - if (nanos < 0) { - --seconds; - nanos += static_cast(ns_per_second); - } - - std::vector body; - pb::appendTag(body, 1, 0); - pb::appendVarint(body, static_cast(seconds)); - pb::appendTag(body, 2, 0); - pb::appendVarint(body, static_cast(nanos)); - return body; -} - -std::vector encodeVector3(double x, double y, double z) { - std::vector body; - pb::appendTag(body, 1, 1); - pb::appendDouble(body, x); - pb::appendTag(body, 2, 1); - pb::appendDouble(body, y); - pb::appendTag(body, 3, 1); - pb::appendDouble(body, z); - return body; -} - -std::vector encodeQuaternion(double x, double y, double z, double w) { - std::vector body; - pb::appendTag(body, 1, 1); - pb::appendDouble(body, x); - pb::appendTag(body, 2, 1); - pb::appendDouble(body, y); - pb::appendTag(body, 3, 1); - pb::appendDouble(body, z); - pb::appendTag(body, 4, 1); - pb::appendDouble(body, w); - return body; -} +namespace pb = ::PJ::test_pb; std::vector encodeFrameTransform(const FrameTransform& transform) { std::vector body; pb::appendTag(body, 1, 2); - pb::appendLenDelim(body, encodeTimestamp(transform.timestamp)); + pb::appendLenDelim(body, pb::encodeTimestamp(transform.timestamp)); pb::appendTag(body, 2, 2); pb::appendString(body, transform.parent_frame_id); pb::appendTag(body, 3, 2); pb::appendString(body, transform.child_frame_id); pb::appendTag(body, 4, 2); - pb::appendLenDelim(body, encodeVector3(transform.translation.x, transform.translation.y, transform.translation.z)); + pb::appendLenDelim(body, pb::encodeVector3(transform.translation)); pb::appendTag(body, 5, 2); - pb::appendLenDelim( - body, encodeQuaternion(transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w)); + pb::appendLenDelim(body, pb::encodeQuaternion(transform.rotation)); return body; } diff --git a/pj_base/tests/image_annotations_codec_test.cpp b/pj_base/tests/image_annotations_codec_test.cpp index f6507f7..4f390b9 100644 --- a/pj_base/tests/image_annotations_codec_test.cpp +++ b/pj_base/tests/image_annotations_codec_test.cpp @@ -6,10 +6,10 @@ #include #include -#include -#include #include +#include "protobuf_wire_test_helpers.hpp" + namespace PJ { namespace { @@ -21,38 +21,7 @@ using sdk::Point2; using sdk::PointsAnnotation; using sdk::TextAnnotation; -// ----------------------------------------------------------------------------- -// Hand-rolled Protobuf helpers. Used to build expected byte sequences for -// golden-byte tests. -// ----------------------------------------------------------------------------- -namespace pb { - -inline void appendVarint(std::vector& out, uint64_t v) { - while (v >= 0x80u) { - out.push_back(static_cast((v & 0x7Fu) | 0x80u)); - v >>= 7; - } - out.push_back(static_cast(v)); -} - -inline void appendTag(std::vector& out, uint32_t field, uint32_t wire) { - appendVarint(out, (static_cast(field) << 3) | wire); -} - -inline void appendDouble(std::vector& out, double v) { - uint64_t bits = 0; - std::memcpy(&bits, &v, 8); - for (int i = 0; i < 8; ++i) { - out.push_back(static_cast((bits >> (8 * i)) & 0xFFu)); - } -} - -inline void appendLenDelim(std::vector& out, const std::vector& body) { - appendVarint(out, body.size()); - out.insert(out.end(), body.begin(), body.end()); -} - -} // namespace pb +namespace pb = ::PJ::test_pb; // Decode the bytes produced by serializeImageAnnotations back into an // sdk::ImageAnnotations. diff --git a/pj_base/tests/image_codec_test.cpp b/pj_base/tests/image_codec_test.cpp new file mode 100644 index 0000000..c2d1285 --- /dev/null +++ b/pj_base/tests/image_codec_test.cpp @@ -0,0 +1,76 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/image_codec.hpp" + +#include + +#include +#include +#include + +#include "protobuf_wire_test_helpers.hpp" + +namespace PJ { +namespace { + +using sdk::Image; +namespace pb = ::PJ::test_pb; + +TEST(ImageCodecTest, SchemaName) { + EXPECT_EQ(kSchemaImage, "PJ.Image"); +} + +TEST(ImageCodecTest, EmptyBufferProducesError) { + EXPECT_FALSE(deserializeImage(nullptr, 0).has_value()); +} + +TEST(ImageCodecTest, RoundTripRawRGB8) { + Image in; + in.timestamp_ns = 9'000'000'000LL; + in.width = 2; + in.height = 2; + in.encoding = "rgb8"; + in.row_step = 6; // 2 px * 3 bytes + in.is_bigendian = false; + const std::vector pixels = {255, 0, 0, 0, 255, 0, 0, 0, 255, 128, 128, 128}; + in.data = Span(pixels.data(), pixels.size()); + + const auto bytes = serializeImage(in); + auto out = deserializeImage(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_EQ(out->timestamp_ns, in.timestamp_ns); + EXPECT_EQ(out->width, in.width); + EXPECT_EQ(out->height, in.height); + EXPECT_EQ(out->encoding, in.encoding); + EXPECT_EQ(out->row_step, in.row_step); + EXPECT_FALSE(out->is_bigendian); + EXPECT_FALSE(out->compressed_depth_min.has_value()); + EXPECT_FALSE(out->compressed_depth_max.has_value()); + ASSERT_EQ(out->data.size(), pixels.size()); + EXPECT_EQ(std::memcmp(out->data.data(), pixels.data(), pixels.size()), 0); +} + +TEST(ImageCodecTest, RoundTripCompressedDepthWithRange) { + Image in; + in.timestamp_ns = 0; + in.width = 320; + in.height = 240; + in.encoding = "compressedDepth"; + in.compressed_depth_min = 0.5f; + in.compressed_depth_max = 10.0f; + const std::vector payload = {0x89, 'P', 'N', 'G', 0x0D, 0x0A, 0x1A, 0x0A}; + in.data = Span(payload.data(), payload.size()); + + const auto bytes = serializeImage(in); + auto out = deserializeImage(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_EQ(out->encoding, "compressedDepth"); + ASSERT_TRUE(out->compressed_depth_min.has_value()); + ASSERT_TRUE(out->compressed_depth_max.has_value()); + EXPECT_FLOAT_EQ(*out->compressed_depth_min, 0.5f); + EXPECT_FLOAT_EQ(*out->compressed_depth_max, 10.0f); +} + +} // namespace +} // namespace PJ diff --git a/pj_base/tests/media_metadata_test.cpp b/pj_base/tests/media_metadata_test.cpp index 4e8c971..b901aef 100644 --- a/pj_base/tests/media_metadata_test.cpp +++ b/pj_base/tests/media_metadata_test.cpp @@ -23,8 +23,8 @@ TEST(MediaMetadataBuilderTest, SingleKeyRoundtrip) { } TEST(MediaMetadataBuilderTest, AllThreeKeysInCanonicalOrder) { - const auto json = MediaMetadataBuilder().mediaClass("video").encoding("h264").schema("PJ.CompressedVideo").build(); - EXPECT_EQ(json, R"({"media_class":"video","encoding":"h264","schema":"PJ.CompressedVideo"})"); + const auto json = MediaMetadataBuilder().mediaClass("video").encoding("h264").schema("PJ.VideoFrame").build(); + EXPECT_EQ(json, R"({"media_class":"video","encoding":"h264","schema":"PJ.VideoFrame"})"); } TEST(MediaMetadataBuilderTest, EmptyKeysAreOmitted) { diff --git a/pj_base/tests/mesh3d_codec_test.cpp b/pj_base/tests/mesh3d_codec_test.cpp new file mode 100644 index 0000000..bae100f --- /dev/null +++ b/pj_base/tests/mesh3d_codec_test.cpp @@ -0,0 +1,74 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/mesh3d_codec.hpp" + +#include + +#include +#include +#include + +#include "protobuf_wire_test_helpers.hpp" + +namespace PJ { +namespace { + +using sdk::Mesh3D; +namespace pb = ::PJ::test_pb; + +TEST(Mesh3DCodecTest, SchemaName) { + EXPECT_EQ(kSchemaMesh3D, "PJ.Mesh3D"); +} + +TEST(Mesh3DCodecTest, EmptyBufferProducesError) { + EXPECT_FALSE(deserializeMesh3D(nullptr, 0).has_value()); +} + +TEST(Mesh3DCodecTest, RoundTripEmbeddedAsset) { + Mesh3D in; + in.timestamp_ns = 1'000'000'000LL; + in.frame_id = "world"; + in.id = "robot_link0"; + in.pose.position = {.x = 0.0, .y = 0.0, .z = 0.5}; + in.pose.orientation = {.x = 0.0, .y = 0.0, .z = 0.0, .w = 1.0}; + in.scale = {.x = 1.0, .y = 1.0, .z = 1.0}; + in.format = "stl"; + const std::vector payload(80, 0xAA); // minimal STL header bytes + in.data = Span(payload.data(), payload.size()); + in.color = {200, 100, 50, 255}; + in.override_color = true; + + const auto bytes = serializeMesh3D(in); + auto out = deserializeMesh3D(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_EQ(out->timestamp_ns, in.timestamp_ns); + EXPECT_EQ(out->frame_id, in.frame_id); + EXPECT_EQ(out->id, in.id); + EXPECT_EQ(out->pose, in.pose); + EXPECT_EQ(out->scale, in.scale); + EXPECT_EQ(out->format, in.format); + EXPECT_TRUE(out->url.empty()); + EXPECT_TRUE(out->override_color); + ASSERT_EQ(out->data.size(), payload.size()); + EXPECT_EQ(std::memcmp(out->data.data(), payload.data(), payload.size()), 0); +} + +TEST(Mesh3DCodecTest, RoundTripUrlReference) { + Mesh3D in; + in.timestamp_ns = 0; + in.frame_id = "world"; + in.id = "wheel"; + in.pose.orientation.w = 1.0; + in.url = "file:///robots/wheel.glb"; + + const auto bytes = serializeMesh3D(in); + auto out = deserializeMesh3D(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_EQ(out->url, in.url); + EXPECT_TRUE(out->data.empty()); + EXPECT_FALSE(out->override_color); +} + +} // namespace +} // namespace PJ diff --git a/pj_base/tests/occupancy_grid_codec_test.cpp b/pj_base/tests/occupancy_grid_codec_test.cpp new file mode 100644 index 0000000..3cc5289 --- /dev/null +++ b/pj_base/tests/occupancy_grid_codec_test.cpp @@ -0,0 +1,54 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/occupancy_grid_codec.hpp" + +#include + +#include +#include +#include + +#include "protobuf_wire_test_helpers.hpp" + +namespace PJ { +namespace { + +using sdk::OccupancyGrid; +namespace pb = ::PJ::test_pb; + +TEST(OccupancyGridCodecTest, SchemaName) { + EXPECT_EQ(kSchemaOccupancyGrid, "PJ.OccupancyGrid"); +} + +TEST(OccupancyGridCodecTest, EmptyBufferProducesError) { + EXPECT_FALSE(deserializeOccupancyGrid(nullptr, 0).has_value()); +} + +TEST(OccupancyGridCodecTest, RoundTrip2x3Grid) { + OccupancyGrid in; + in.timestamp_ns = 42'000'000'000LL; + in.frame_id = "map"; + in.origin.position = {.x = 1.0, .y = 2.0, .z = 0.0}; + in.origin.orientation = {.x = 0.0, .y = 0.0, .z = 0.0, .w = 1.0}; + in.resolution = 0.05; + in.width = 3; + in.height = 2; + const std::vector cells = {0, 50, 100, 0xFF /* -1 unknown */, 25, 75}; + in.data = Span(cells.data(), cells.size()); + + const auto bytes = serializeOccupancyGrid(in); + auto out = deserializeOccupancyGrid(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_EQ(out->timestamp_ns, in.timestamp_ns); + EXPECT_EQ(out->frame_id, in.frame_id); + EXPECT_EQ(out->origin, in.origin); + EXPECT_DOUBLE_EQ(out->resolution, in.resolution); + EXPECT_EQ(out->width, in.width); + EXPECT_EQ(out->height, in.height); + ASSERT_EQ(out->data.size(), cells.size()); + EXPECT_EQ(std::memcmp(out->data.data(), cells.data(), cells.size()), 0); +} + +} // namespace +} // namespace PJ diff --git a/pj_base/tests/point_cloud_codec_test.cpp b/pj_base/tests/point_cloud_codec_test.cpp new file mode 100644 index 0000000..a9946ae --- /dev/null +++ b/pj_base/tests/point_cloud_codec_test.cpp @@ -0,0 +1,83 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/point_cloud_codec.hpp" + +#include + +#include +#include +#include + +#include "protobuf_wire_test_helpers.hpp" + +namespace PJ { +namespace { + +using sdk::PointCloud; +using sdk::PointField; +namespace pb = ::PJ::test_pb; + +TEST(PointCloudCodecTest, SchemaName) { + EXPECT_EQ(kSchemaPointCloud, "PJ.PointCloud"); +} + +TEST(PointCloudCodecTest, EmptyBufferProducesError) { + EXPECT_FALSE(deserializePointCloud(nullptr, 0).has_value()); +} + +TEST(PointCloudCodecTest, RoundTripXYZIntensity) { + PointCloud in; + in.timestamp_ns = 5'000'000'000LL; + in.width = 3; + in.height = 1; + in.point_step = 16; // 4*float32 + in.row_step = 48; // 3 * point_step + in.is_bigendian = false; + in.is_dense = true; + in.frame_id = "velodyne"; + in.fields = { + {.name = "x", .offset = 0, .datatype = PointField::Datatype::kFloat32, .count = 1}, + {.name = "y", .offset = 4, .datatype = PointField::Datatype::kFloat32, .count = 1}, + {.name = "z", .offset = 8, .datatype = PointField::Datatype::kFloat32, .count = 1}, + {.name = "intensity", .offset = 12, .datatype = PointField::Datatype::kFloat32, .count = 1}, + }; + std::vector payload(48, 0xAB); + in.data = Span(payload.data(), payload.size()); + + const auto bytes = serializePointCloud(in); + auto out = deserializePointCloud(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_EQ(out->width, in.width); + EXPECT_EQ(out->height, in.height); + EXPECT_EQ(out->point_step, in.point_step); + EXPECT_EQ(out->row_step, in.row_step); + EXPECT_FALSE(out->is_bigendian); + EXPECT_TRUE(out->is_dense); + EXPECT_EQ(out->frame_id, in.frame_id); + ASSERT_EQ(out->fields.size(), 4u); + for (size_t i = 0; i < in.fields.size(); ++i) { + EXPECT_EQ(out->fields[i].name, in.fields[i].name); + EXPECT_EQ(out->fields[i].offset, in.fields[i].offset); + EXPECT_EQ(out->fields[i].datatype, in.fields[i].datatype); + EXPECT_EQ(out->fields[i].count, in.fields[i].count); + } + ASSERT_EQ(out->data.size(), payload.size()); + EXPECT_EQ(std::memcmp(out->data.data(), payload.data(), payload.size()), 0); +} + +TEST(PointCloudCodecTest, FrameIdAbsentRoundTrips) { + PointCloud in; + in.width = 1; + in.height = 1; + in.point_step = 12; + in.row_step = 12; + + const auto bytes = serializePointCloud(in); + auto out = deserializePointCloud(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_TRUE(out->frame_id.empty()); +} + +} // namespace +} // namespace PJ diff --git a/pj_base/tests/protobuf_wire_test_helpers.hpp b/pj_base/tests/protobuf_wire_test_helpers.hpp new file mode 100644 index 0000000..896460b --- /dev/null +++ b/pj_base/tests/protobuf_wire_test_helpers.hpp @@ -0,0 +1,133 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include +#include + +#include "pj_base/builtin/frame_transforms.hpp" +#include "pj_base/types.hpp" + +namespace PJ { +namespace test_pb { + +// Low-level protobuf wire builders. Mirror the encoding performed by the +// codecs themselves; tests use these to construct golden expected bytes +// independently of the codec under test. + +inline void appendVarint(std::vector& out, uint64_t v) { + while (v >= 0x80u) { + out.push_back(static_cast((v & 0x7Fu) | 0x80u)); + v >>= 7; + } + out.push_back(static_cast(v)); +} + +inline void appendTag(std::vector& out, uint32_t field, uint32_t wire) { + appendVarint(out, (static_cast(field) << 3) | wire); +} + +inline void appendFixed32(std::vector& out, uint32_t v) { + for (int i = 0; i < 4; ++i) { + out.push_back(static_cast((v >> (8 * i)) & 0xFFu)); + } +} + +inline void appendFixed64(std::vector& out, uint64_t v) { + for (int i = 0; i < 8; ++i) { + out.push_back(static_cast((v >> (8 * i)) & 0xFFu)); + } +} + +inline void appendFloat(std::vector& out, float v) { + uint32_t bits = 0; + std::memcpy(&bits, &v, sizeof(v)); + appendFixed32(out, bits); +} + +inline void appendDouble(std::vector& out, double v) { + uint64_t bits = 0; + std::memcpy(&bits, &v, sizeof(v)); + appendFixed64(out, bits); +} + +inline void appendLenDelim(std::vector& out, const std::vector& body) { + appendVarint(out, body.size()); + out.insert(out.end(), body.begin(), body.end()); +} + +inline void appendString(std::vector& out, std::string_view value) { + appendVarint(out, value.size()); + out.insert(out.end(), value.begin(), value.end()); +} + +inline void appendBytes(std::vector& out, const uint8_t* data, size_t size) { + appendVarint(out, size); + out.insert(out.end(), data, data + size); +} + +// Geometry encoders matching `pj_base/src/builtin/geometry_codec.hpp` — +// each builds the inner message body (sans length prefix) for the proto type. + +inline std::vector encodeTimestamp(Timestamp timestamp_ns) { + constexpr int64_t ns_per_second = 1'000'000'000LL; + int64_t seconds = timestamp_ns / ns_per_second; + int32_t nanos = static_cast(timestamp_ns % ns_per_second); + if (nanos < 0) { + --seconds; + nanos += static_cast(ns_per_second); + } + std::vector body; + appendTag(body, 1, 0); + appendVarint(body, static_cast(seconds)); + appendTag(body, 2, 0); + appendVarint(body, static_cast(nanos)); + return body; +} + +inline std::vector encodeVector3(double x, double y, double z) { + std::vector body; + appendTag(body, 1, 1); + appendDouble(body, x); + appendTag(body, 2, 1); + appendDouble(body, y); + appendTag(body, 3, 1); + appendDouble(body, z); + return body; +} + +inline std::vector encodeVector3(const sdk::Vector3& v) { + return encodeVector3(v.x, v.y, v.z); +} + +inline std::vector encodeQuaternion(double x, double y, double z, double w) { + std::vector body; + appendTag(body, 1, 1); + appendDouble(body, x); + appendTag(body, 2, 1); + appendDouble(body, y); + appendTag(body, 3, 1); + appendDouble(body, z); + appendTag(body, 4, 1); + appendDouble(body, w); + return body; +} + +inline std::vector encodeQuaternion(const sdk::Quaternion& q) { + return encodeQuaternion(q.x, q.y, q.z, q.w); +} + +inline std::vector encodePose(const sdk::Pose& pose) { + std::vector body; + appendTag(body, 1, 2); + appendLenDelim(body, encodeVector3(pose.position)); + appendTag(body, 2, 2); + appendLenDelim(body, encodeQuaternion(pose.orientation)); + return body; +} + +} // namespace test_pb +} // namespace PJ diff --git a/pj_base/tests/scene_entities_codec_test.cpp b/pj_base/tests/scene_entities_codec_test.cpp new file mode 100644 index 0000000..7660eb3 --- /dev/null +++ b/pj_base/tests/scene_entities_codec_test.cpp @@ -0,0 +1,201 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/scene_entities_codec.hpp" + +#include + +#include +#include + +#include "protobuf_wire_test_helpers.hpp" + +namespace PJ { +namespace { + +using sdk::ArrowPrimitive; +using sdk::AxesPrimitive; +using sdk::ColorRGBA; +using sdk::CubePrimitive; +using sdk::CylinderPrimitive; +using sdk::LinePrimitive; +using sdk::LineType; +using sdk::Point3; +using sdk::Pose; +using sdk::Quaternion; +using sdk::SceneEntities; +using sdk::SceneEntity; +using sdk::SpherePrimitive; +using sdk::TextPrimitive; +using sdk::TrianglePrimitive; +using sdk::Vector3; +namespace pb = ::PJ::test_pb; + +// Compare two ColorRGBA values allowing 1-LSB drift on each channel from the +// uint8 -> double in [0,1] -> uint8 round-trip in the codec. +::testing::AssertionResult ColorNear(const ColorRGBA& a, const ColorRGBA& b) { + auto near = [](uint8_t x, uint8_t y) { return x > y ? (x - y) <= 1 : (y - x) <= 1; }; + if (near(a.r, b.r) && near(a.g, b.g) && near(a.b, b.b) && near(a.a, b.a)) { + return ::testing::AssertionSuccess(); + } + return ::testing::AssertionFailure() << "Color mismatch: got {" << +b.r << "," << +b.g << "," << +b.b << "," << +b.a + << "}, expected {" << +a.r << "," << +a.g << "," << +a.b << "," << +a.a << "}"; +} + +Pose makePose(double tx, double ty, double tz) { + return Pose{.position = {.x = tx, .y = ty, .z = tz}, .orientation = {.x = 0.0, .y = 0.0, .z = 0.0, .w = 1.0}}; +} + +TEST(SceneEntitiesCodecTest, SchemaName) { + EXPECT_EQ(kSchemaSceneEntities, "PJ.SceneEntities"); +} + +TEST(SceneEntitiesCodecTest, EmptyBufferDecodesEmpty) { + // SceneEntities round-trips an empty batch as empty bytes (top-level + // is a `repeated SceneEntity entities = 1`). + SceneEntities empty; + const auto bytes = serializeSceneEntities(empty); + EXPECT_TRUE(bytes.empty()); +} + +TEST(SceneEntitiesCodecTest, EmptyBufferDeserializesAsError) { + EXPECT_FALSE(deserializeSceneEntities(nullptr, 0).has_value()); +} + +TEST(SceneEntitiesCodecTest, RoundTripOneEntityWithEachPrimitive) { + SceneEntities in; + SceneEntity e; + e.timestamp = 100'000'000LL; + e.frame_id = "world"; + e.id = "mixed_demo"; + e.lifetime_ns = 5'000'000'000LL; + e.frame_locked = true; + + e.arrows.push_back( + ArrowPrimitive{ + .pose = makePose(1.0, 0.0, 0.0), + .shaft_length = 0.5, + .shaft_diameter = 0.05, + .head_length = 0.1, + .head_diameter = 0.1, + .color = {255, 0, 0, 255}, + }); + e.cubes.push_back( + CubePrimitive{ + .pose = makePose(0.0, 1.0, 0.0), + .size = {.x = 0.2, .y = 0.2, .z = 0.2}, + .color = {0, 255, 0, 255}, + }); + e.spheres.push_back( + SpherePrimitive{ + .pose = makePose(0.0, 0.0, 1.0), + .size = {.x = 0.3, .y = 0.3, .z = 0.3}, + .color = {0, 0, 255, 255}, + }); + e.cylinders.push_back( + CylinderPrimitive{ + .pose = makePose(1.0, 1.0, 0.0), + .size = {.x = 0.2, .y = 0.2, .z = 0.5}, + .bottom_scale = 1.0, + .top_scale = 0.5, + .color = {255, 255, 0, 255}, + }); + + LinePrimitive line; + line.type = LineType::kLineStrip; + line.pose = makePose(0.0, 0.0, 0.0); + line.thickness = 0.02; + line.scale_invariant = false; + line.points = {{0.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 1.0, 0.0}}; + line.color = {255, 255, 255, 255}; + e.lines.push_back(std::move(line)); + + TrianglePrimitive tri; + tri.pose = makePose(0.0, 0.0, 2.0); + tri.points = {{0.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}}; + tri.color = {128, 128, 128, 255}; + e.triangles.push_back(std::move(tri)); + + e.texts.push_back( + TextPrimitive{ + .pose = makePose(0.0, 0.0, 3.0), + .billboard = true, + .font_size = 14.0, + .scale_invariant = true, + .color = {255, 255, 255, 255}, + .text = "label", + }); + + e.axes.push_back( + AxesPrimitive{ + .pose = makePose(2.0, 0.0, 0.0), + .length = 1.0, + .thickness = 0.02, + .scale_invariant = false, + }); + + in.entities.push_back(std::move(e)); + + const auto bytes = serializeSceneEntities(in); + auto out = deserializeSceneEntities(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + ASSERT_EQ(out->entities.size(), 1u); + const auto& src = in.entities.front(); + const auto& dst = out->entities.front(); + + EXPECT_EQ(dst.timestamp, src.timestamp); + EXPECT_EQ(dst.frame_id, src.frame_id); + EXPECT_EQ(dst.id, src.id); + EXPECT_EQ(dst.lifetime_ns, src.lifetime_ns); + EXPECT_EQ(dst.frame_locked, src.frame_locked); + + ASSERT_EQ(dst.arrows.size(), 1u); + ASSERT_EQ(dst.cubes.size(), 1u); + ASSERT_EQ(dst.spheres.size(), 1u); + ASSERT_EQ(dst.cylinders.size(), 1u); + ASSERT_EQ(dst.lines.size(), 1u); + ASSERT_EQ(dst.triangles.size(), 1u); + ASSERT_EQ(dst.texts.size(), 1u); + ASSERT_EQ(dst.axes.size(), 1u); + + EXPECT_EQ(dst.arrows.front().pose, src.arrows.front().pose); + EXPECT_TRUE(ColorNear(src.arrows.front().color, dst.arrows.front().color)); + EXPECT_EQ(dst.cubes.front().size, src.cubes.front().size); + EXPECT_DOUBLE_EQ(dst.cylinders.front().top_scale, src.cylinders.front().top_scale); + EXPECT_EQ(dst.lines.front().type, src.lines.front().type); + EXPECT_EQ(dst.lines.front().points, src.lines.front().points); + EXPECT_EQ(dst.triangles.front().points, src.triangles.front().points); + EXPECT_EQ(dst.texts.front().text, src.texts.front().text); + EXPECT_TRUE(dst.texts.front().billboard); + EXPECT_DOUBLE_EQ(dst.axes.front().length, src.axes.front().length); +} + +TEST(SceneEntitiesCodecTest, RoundTripLineWithPerVertexColorsAndIndices) { + SceneEntities in; + SceneEntity e; + e.frame_id = "world"; + e.id = "lines"; + LinePrimitive line; + line.type = LineType::kLineList; + line.pose = makePose(0.0, 0.0, 0.0); + line.thickness = 0.01; + line.points = {{0.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 1.0, 0.0}, {0.0, 1.0, 0.0}}; + line.colors = {{255, 0, 0, 255}, {0, 255, 0, 255}, {0, 0, 255, 255}, {255, 255, 0, 255}}; + line.indices = {0, 1, 2, 3}; + e.lines.push_back(std::move(line)); + in.entities.push_back(std::move(e)); + + const auto bytes = serializeSceneEntities(in); + auto out = deserializeSceneEntities(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + ASSERT_EQ(out->entities.size(), 1u); + ASSERT_EQ(out->entities.front().lines.size(), 1u); + const auto& dst_line = out->entities.front().lines.front(); + EXPECT_EQ(dst_line.type, LineType::kLineList); + EXPECT_EQ(dst_line.indices, std::vector({0, 1, 2, 3})); + ASSERT_EQ(dst_line.colors.size(), 4u); + EXPECT_TRUE(ColorNear(ColorRGBA{0, 255, 0, 255}, dst_line.colors[1])); +} + +} // namespace +} // namespace PJ diff --git a/pj_base/tests/video_frame_codec_test.cpp b/pj_base/tests/video_frame_codec_test.cpp new file mode 100644 index 0000000..f1ec4de --- /dev/null +++ b/pj_base/tests/video_frame_codec_test.cpp @@ -0,0 +1,47 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: MIT + +#include "pj_base/builtin/video_frame_codec.hpp" + +#include + +#include +#include +#include + +#include "protobuf_wire_test_helpers.hpp" + +namespace PJ { +namespace { + +using sdk::VideoFrame; +namespace pb = ::PJ::test_pb; + +TEST(VideoFrameCodecTest, SchemaName) { + EXPECT_EQ(kSchemaVideoFrame, "PJ.VideoFrame"); +} + +TEST(VideoFrameCodecTest, EmptyBufferProducesError) { + EXPECT_FALSE(deserializeVideoFrame(nullptr, 0).has_value()); +} + +TEST(VideoFrameCodecTest, RoundTripRealisticPayload) { + VideoFrame in; + in.timestamp_ns = 1'700'000'000'500'000'000LL; + in.frame_id = "camera_color_optical_frame"; + in.format = "h264"; + const std::vector payload = {0x00, 0x00, 0x00, 0x01, 0x67, 0x42, 0xC0, 0x1F}; + in.data = Span(payload.data(), payload.size()); + + const auto bytes = serializeVideoFrame(in); + auto out = deserializeVideoFrame(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_EQ(out->timestamp_ns, in.timestamp_ns); + EXPECT_EQ(out->frame_id, in.frame_id); + EXPECT_EQ(out->format, in.format); + ASSERT_EQ(out->data.size(), payload.size()); + EXPECT_EQ(std::memcmp(out->data.data(), payload.data(), payload.size()), 0); +} + +} // namespace +} // namespace PJ diff --git a/pj_plugins/include/pj_plugins/host/message_parser_handle.hpp b/pj_plugins/include/pj_plugins/host/message_parser_handle.hpp index 640aedb..e5ff275 100644 --- a/pj_plugins/include/pj_plugins/host/message_parser_handle.hpp +++ b/pj_plugins/include/pj_plugins/host/message_parser_handle.hpp @@ -12,7 +12,7 @@ #include #include -#include +#include #include #include #include diff --git a/pj_plugins/include/pj_plugins/sdk/message_parser_plugin_base.hpp b/pj_plugins/include/pj_plugins/sdk/message_parser_plugin_base.hpp index 3337b7c..f1c004b 100644 --- a/pj_plugins/include/pj_plugins/sdk/message_parser_plugin_base.hpp +++ b/pj_plugins/include/pj_plugins/sdk/message_parser_plugin_base.hpp @@ -23,7 +23,7 @@ #include #include -#include "pj_base/builtin/BuiltinObject.hpp" +#include "pj_base/builtin/builtin_object.hpp" #include "pj_base/expected.hpp" #include "pj_base/message_parser_protocol.h" #include "pj_base/plugin_abi_export.hpp" diff --git a/pj_plugins/include/pj_plugins/sdk/object_ingest_policy.hpp b/pj_plugins/include/pj_plugins/sdk/object_ingest_policy.hpp index 6d3a715..b7a449e 100644 --- a/pj_plugins/include/pj_plugins/sdk/object_ingest_policy.hpp +++ b/pj_plugins/include/pj_plugins/sdk/object_ingest_policy.hpp @@ -20,7 +20,7 @@ #include #include -#include "pj_base/builtin/BuiltinObject.hpp" +#include "pj_base/builtin/builtin_object.hpp" namespace PJ { namespace sdk {