diff --git a/.claude/skills/plotjuggler-plugin/references/builtin-objects.md b/.claude/skills/plotjuggler-plugin/references/builtin-objects.md index a1cdc6be..8c62eda4 100644 --- a/.claude/skills/plotjuggler-plugin/references/builtin-objects.md +++ b/.claude/skills/plotjuggler-plugin/references/builtin-objects.md @@ -30,7 +30,7 @@ object store ABI in `V4_STORE.md`. custom payload as a builtin. `BuiltinObjectType::kNone` means "scalar/unknown classification", not "storage forbidden". -The full list (17 concrete types) and their exact fields are in +The full list (18 concrete types) and their exact fields are in `docs/builtin_type.md` and one header each under `pj_base/include/pj_base/builtin/` (the `builtin_object.hpp` enum is the authoritative roster — trust it over prose that may lag behind newer additions). @@ -80,7 +80,7 @@ into the returned object so large payloads stay zero-copy. See ## Two storage families → two byte strategies - **Byte-backed** (Image, DepthImage, PointCloud, CompressedPointCloud, - OccupancyGrid(+Update), VoxelGrid, Mesh3D, VideoFrame): potentially megabytes. + OccupancyGrid(+Update), VoxelGrid, GridMap, Mesh3D, VideoFrame): potentially megabytes. Keep the payload a zero-copy `Span` anchored by a `BufferAnchor` (typically a `shared_ptr>`); only allocate new bytes if a conversion is unavoidable. diff --git a/CHANGELOG.md b/CHANGELOG.md index 97d7d09b..54127ce7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,6 +3,25 @@ All notable changes to `plotjuggler_sdk` are recorded here. Versioning policy is in [`CLAUDE.md`](./CLAUDE.md) → "Release Versioning". +## [0.26.0] + +### Feature: `GridMap` canonical builtin object (MINOR) + +`sdk::GridMap` (`pj_base/builtin/grid_map.hpp`, `BuiltinObjectType::kGridMap` = 20, +`PJ_BUILTIN_OBJECT_TYPE_GRID_MAP`) is a 2D grid whose cells carry named channels — the +layered, generic-valued sibling of `OccupancyGrid` for elevation maps and multi-layer +costmaps (`grid_map_msgs/GridMap`, `foxglove.Grid`). Row-major fixed-size cell records +described by the shared `PointField` channel model, the packed layout `foxglove.Grid` uses, +so producers with that layout hand `data` over zero-copy; a NaN in a float channel means +"no data". `PJ.GridMap` wire format + `grid_map_codec.hpp` (`serializeGridMap`, +`deserializeGridMap`, and `validateGridMap` for the full layout check once spliced bytes are +attached), an entry in the type-erased dispatcher, in the frozen splice table (`data` = +field 10) and in both host splice-attachment paths, a `GridMapBuilder` (`gridMap()`) in the +parser-module `ObjectWriter`, and `sdk::Vector2` in the geometry vocabulary (already present +as `PJ.Vector2` on the wire). The `PointField` wire helpers shared by the PointCloud, +VoxelGrid and GridMap codecs now live in one private header. Additive: no existing struct, +slot, or wire format changes. + ## [0.25.0] ### Feature: plugin-authoring CMake helpers ship with the SDK (MINOR) diff --git a/CLAUDE.md b/CLAUDE.md index e5e7e627..40b7347f 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -15,10 +15,10 @@ not in the PJ4 superproject. This file is the root navigation node for the whole ### Modules - **pj_base** — vocabulary types (`Timestamp`, `DatasetId`, `Expected`, `Span`, type trees), - the canonical builtin object vocabulary (`pj_base/builtin/`: 17 struct headers — Image, DepthImage, + the canonical builtin object vocabulary (`pj_base/builtin/`: 18 struct headers — Image, DepthImage, PointCloud, CompressedPointCloud, OccupancyGrid(+Update), Mesh3D, VideoFrame, SceneEntities, RobotDescription, CameraInfo, Log, ImageAnnotations, FrameTransforms, PosesInFrame, - VoxelGrid, PlotMarkers) and their canonical wire codecs, the C-ABI protocol headers for + VoxelGrid, PlotMarkers, GridMap) and their canonical wire codecs, the C-ABI protocol headers for DataSource/MessageParser/Toolbox + the C++ SDK base classes / host-view helpers built on them, the standalone C++17 functional parser-module authoring kit (`pj_base/parser_module/`), the host-side wasm parser-module manifest custom-section codec, and the test-only static WASI ABI auditor. The diff --git a/VERSION b/VERSION index d21d277b..4e8f395f 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -0.25.0 +0.26.0 diff --git a/docs/builtin_type.md b/docs/builtin_type.md index 5915f68b..2c10395b 100644 --- a/docs/builtin_type.md +++ b/docs/builtin_type.md @@ -31,6 +31,7 @@ The public headers live under: #include #include #include +#include #include // Codecs — one per type, all share the canonical PJ. wire format under pj_base/proto/pj/. #include @@ -50,6 +51,7 @@ The public headers live under: #include #include #include +#include #include ``` @@ -97,7 +99,7 @@ Builtin objects fall into two serialization families: | Family | Current types | Storage model | Codec policy | |--------|---------------|---------------|--------------| -| Byte-backed views | `Image`, `DepthImage`, `PointCloud`, `CompressedPointCloud`, `OccupancyGrid`, `OccupancyGridUpdate`, `VoxelGrid`, `Mesh3D`, `VideoFrame` | Header fields live in the SDK struct; payload bytes live behind `Span` plus `BufferAnchor`. | Preserve zero-copy views while in-process; use the canonical codec at storage or C ABI boundaries. If no source anchor exists, materialize before returning a long-lived view. | +| Byte-backed views | `Image`, `DepthImage`, `PointCloud`, `CompressedPointCloud`, `OccupancyGrid`, `OccupancyGridUpdate`, `VoxelGrid`, `GridMap`, `Mesh3D`, `VideoFrame` | Header fields live in the SDK struct; payload bytes live behind `Span` plus `BufferAnchor`. | Preserve zero-copy views while in-process; use the canonical codec at storage or C ABI boundaries. If no source anchor exists, materialize before returning a long-lived view. | | Owned values | `ImageAnnotations`, `FrameTransforms`, `SceneEntities`, `RobotDescription`, `CameraInfo`, `Log`, `PosesInFrame`, `PlotMarkers` | SDK structs own their vectors/strings/scalars directly. | Canonical codecs serialize the owned value to the protobuf-wire payload described by the `.proto` contract, using shared private wire primitives. `RobotDescription.text` remains raw URDF/SDF/MJCF source inside its small canonical envelope. | Canonical `.proto` files live under `pj_base/proto/pj` and act as the wire @@ -138,6 +140,7 @@ annotations, frame transforms, or no builtin object. | `kLog` | `PJ::sdk::Log` | Textual log message (severity level + text + originating name). | | `kPosesInFrame` | `PJ::sdk::PosesInFrame` | Array of poses in one frame (PoseArray / particle clouds); styling is viewer-side. | | `kVoxelGrid` | `PJ::sdk::VoxelGrid` | Dense 3D voxel grid (occupancy/cost/ESDF/semantic); the volumetric sibling of `OccupancyGrid`. | +| `kGridMap` | `PJ::sdk::GridMap` | 2D grid of per-cell channels (elevation maps, layered costmaps) in world coordinates. | | `kPlotMarkers` | `PJ::sdk::PlotMarkers` | Findings on a time-series plot (regions, events, bands, and labels). | ### Object-topic renderer metadata @@ -618,6 +621,55 @@ viewer-side, so one type serves occupancy/cost/ESDF/semantic grids. `pj_base/builtin/voxel_grid_codec.hpp` serializes and deserializes this type using the canonical `PJ.VoxelGrid` protobuf wire format. +## GridMap + +`GridMap` is a 2D grid whose cells carry named channels — the layered, +generic-valued sibling of `OccupancyGrid` — for elevation maps, multi-layer +costmaps, and terrain classification (e.g. `grid_map_msgs/GridMap`, +`foxglove.Grid`). + +It is a byte-backed view: `column_count * row_count` fixed-size cell records +live in `data` (a `Span` + `BufferAnchor`) in row-major order +(x / column fastest), `row_stride` bytes per row, and the channels inside one +record are described by `fields` — the same `PointField` model `PointCloud` +and `VoxelGrid` use. The packed cell layout is the one `foxglove.Grid` uses, +so a parser can hand that message's `data` over zero-copy; the header and +field descriptors are converted (own wire field numbers, an explicit +`row_count`, and a `PointField` datatype numbering that differs from +`PackedElementField`'s). A producer with another layout (grid_map's +column-major ring buffer) transcodes once at the boundary. + +Unlike `OccupancyGrid` (one fixed `-1`/`0..100` channel), the per-cell **value +is generic** and there can be many channels per cell. A NaN in a float channel +means "no data" for that cell; integer channels have no empty sentinel. Which +channel is height, which one drives color, and the colormap are viewer-side +choices. Channel-name conventions consumers may rely on: `elevation` is the +conventional height channel; `red`, `green`, `blue`, `alpha` are the RGBA color +channels, as in `foxglove.Grid`. + +| Field | Type | Notes | +|-------|------|-------| +| `timestamp_ns` | `Timestamp` | Time of the grid. `0` when the source had none. | +| `frame_id` | `std::string` | Source coordinate frame; 3D consumers TF-transform from it. | +| `origin` | `Pose` | Corner of cell `(0,0)` in `frame_id`; the grid lies in the pose's local xy-plane. | +| `cell_size` | `Vector2` | Metric cell size along local x (columns) and y (rows), meters. | +| `column_count` / `row_count` | `uint32_t` | Cells along x / y. | +| `cell_stride` / `row_stride` | `uint32_t` | Byte spacing of a cell record / a row (padding allowed). | +| `fields` | `std::vector` | Per-cell channel layout. | +| `data` | `Span` + `BufferAnchor` | Packed cell records in row-major order. | + +Cell `(c, r)` has its center at `origin ∘ ((c + .5)*cell_size.x, (r + .5)*cell_size.y, 0)`. + +`pj_base/builtin/grid_map_codec.hpp` serializes and deserializes this type +using the canonical `PJ.GridMap` protobuf wire format. Decoding rejects a +layout the cell math could not index safely (a field with an unknown datatype +or zero count, a field reaching past `cell_stride`, a zero stride with cells +declared, a row shorter than its columns, `data` shorter than +`row_count * row_stride`); a wire that carries no `data` is the functional-v2 +splice form and decodes with an empty span, so hosts that attach spliced bytes +and consumers that index cells run `validateGridMap()` once the bytes are in +place. + ## Conversion Examples | Source type | Canonical builtin type | Conversion intent | @@ -637,6 +689,8 @@ using the canonical `PJ.VoxelGrid` protobuf wire format. | ROS `sensor_msgs/CameraInfo` | `CameraInfo` | Map K / D / R / P plus dimensions; correlate to the image topic by name. Sub-window (binning / ROI) is dropped. | | ROS `map_msgs/OccupancyGridUpdate` | `OccupancyGridUpdate` | Forward the cell-space patch (`x`/`y`/`width`/`height` + bytes); the consumer pairs it with the base grid and supplies origin/resolution. | | `foxglove.VoxelGrid` / `costmap_2d/VoxelGrid` | `VoxelGrid` | Map counts/strides/`cell_size`/`origin` into the struct; keep voxel bytes zero-copy in Z-Y-X order. The draw predicate is viewer-side. | +| `foxglove.Grid` / `foxglove_msgs/Grid` | `GridMap` | Map `pose`/`cell_size`/strides into the struct and keep cell bytes zero-copy; `row_count` is `data.size / row_stride`; each `PackedElementField` becomes a `PointField` with the datatype renumbered (Foxglove: UINT8=1, INT8=2, UINT16=3, INT16=4, UINT32=5, INT32=6, FLOAT32=7, FLOAT64=8) and `count = 1`. | +| ROS `grid_map_msgs/GridMap` | `GridMap` | Transcode once: flip both axes (grid_map puts index (0,0) at the +x/+y corner), unwrap the ring-buffer start indices, interleave the column-major per-layer arrays into row-major cell records (one float32 channel per layer), and move the center pose to the corner of cell (0,0). | 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/CLAUDE.md b/pj_base/CLAUDE.md index b866ee84..dcc0d7e0 100644 --- a/pj_base/CLAUDE.md +++ b/pj_base/CLAUDE.md @@ -1,10 +1,10 @@ # pj_base — SDK vocabulary, builtin object schemas, and the C plugin ABI -pj_base is the **Level 0** foundation and the **SDK boundary** for plugin authors. It owns: the zero-dependency vocabulary types (`Timestamp`, `DatasetId`, `Range`, `Expected`, `Span`, `TypeTree`); the 17 canonical *builtin object* schemas (`sdk::Image`, `PointCloud`, `DepthImage`, `OccupancyGrid`, `VoxelGrid`, `FrameTransforms`, …) and all 17 wire codecs; and the **C ABI** primitives every plugin family speaks (`plugin_data_api.h` + the service registry) plus the C-ABI protocol headers for **three** families — `data_source_protocol.h`, `message_parser_protocol.h`, `toolbox_protocol.h` — and additive extension headers such as `parser_functional_protocol.h`. The **Dialog** protocol header is the exception: it lives in `pj_plugins/dialog_protocol/`, not here. It also ships the C++ SDK base classes for DataSource and Toolbox; the MessageParser and Dialog base classes live in `pj_plugins`. Builds as a STATIC lib with **zero public deps** — `fast_float` is a `BUILD_INTERFACE` private impl detail of `parseNumber`. Must NOT depend on `pj_datastore`, `pj_plugins`, Qt, or any Conan runtime lib. This is a read-only submodule subtree: change it only when explicitly working in `plotjuggler_sdk`. +pj_base is the **Level 0** foundation and the **SDK boundary** for plugin authors. It owns: the zero-dependency vocabulary types (`Timestamp`, `DatasetId`, `Range`, `Expected`, `Span`, `TypeTree`); the 18 canonical *builtin object* schemas (`sdk::Image`, `PointCloud`, `DepthImage`, `OccupancyGrid`, `VoxelGrid`, `FrameTransforms`, …) and all 18 wire codecs; and the **C ABI** primitives every plugin family speaks (`plugin_data_api.h` + the service registry) plus the C-ABI protocol headers for **three** families — `data_source_protocol.h`, `message_parser_protocol.h`, `toolbox_protocol.h` — and additive extension headers such as `parser_functional_protocol.h`. The **Dialog** protocol header is the exception: it lives in `pj_plugins/dialog_protocol/`, not here. It also ships the C++ SDK base classes for DataSource and Toolbox; the MessageParser and Dialog base classes live in `pj_plugins`. Builds as a STATIC lib with **zero public deps** — `fast_float` is a `BUILD_INTERFACE` private impl detail of `parseNumber`. Must NOT depend on `pj_datastore`, `pj_plugins`, Qt, or any Conan runtime lib. This is a read-only submodule subtree: change it only when explicitly working in `plotjuggler_sdk`. ## Layout - `include/pj_base/` — vocabulary primitives: `types.hpp`, `time.hpp` (absolute time spine: `Timepoint`/`Duration` + `fromRaw`/`toRaw`), `type_tree.hpp`, `dataset.hpp`, `expected.hpp`, `span.hpp`, `number_parse.hpp`, `assert.hpp`, `diagnostic_sink.hpp`, `buffer_anchor.hpp`. -- `include/pj_base/builtin/` — 17 builtin object struct headers (`*.hpp`; stable numeric tags with values 2 and 12 permanently reserved) + all 17 wire codecs (`*_codec.hpp`) + the `BuiltinObject` (`std::any`) type-erased holder and type-erased codec dispatcher. +- `include/pj_base/builtin/` — 18 builtin object struct headers (`*.hpp`; stable numeric tags with values 2 and 12 permanently reserved) + all 18 wire codecs (`*_codec.hpp`) + the `BuiltinObject` (`std::any`) type-erased holder and type-erased codec dispatcher. - `include/pj_base/sdk/` — C++ SDK over the ABI: DataSource + Toolbox `*_plugin_base.hpp`, `service_registry.hpp`/`service_traits.hpp`, host views, Arrow RAII holders, `testing/`. - `include/pj_base/*_protocol.h`, `plugin_data_api.h`, `builtin_object_abi.h`, `plugin_abi_export.hpp` — the stable C-ABI surface for DataSource/MessageParser/Toolbox (the Dialog protocol header lives in `pj_plugins/dialog_protocol/`). - `proto/pj/` — canonical `.proto` wire contracts for the builtin types (see its README). diff --git a/pj_base/CMakeLists.txt b/pj_base/CMakeLists.txt index 6603178f..b10c386a 100644 --- a/pj_base/CMakeLists.txt +++ b/pj_base/CMakeLists.txt @@ -28,6 +28,7 @@ add_library(pj_base STATIC src/builtin/scene_entities_codec.cpp src/builtin/video_frame_codec.cpp src/builtin/voxel_grid_codec.cpp + src/builtin/grid_map_codec.cpp src/number_parse.cpp src/parser_module_abi.cpp src/parser_module_manifest.cpp @@ -190,6 +191,7 @@ if(PJ_BUILD_TESTS) tests/time_spine_test.cpp tests/poses_in_frame_codec_test.cpp tests/voxel_grid_codec_test.cpp + tests/grid_map_codec_test.cpp tests/plot_markers_codec_test.cpp ) diff --git a/pj_base/include/pj_base/builtin/builtin_object.hpp b/pj_base/include/pj_base/builtin/builtin_object.hpp index a223925a..62388f27 100644 --- a/pj_base/include/pj_base/builtin/builtin_object.hpp +++ b/pj_base/include/pj_base/builtin/builtin_object.hpp @@ -30,6 +30,7 @@ #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/grid_map.hpp" #include "pj_base/builtin/image.hpp" #include "pj_base/builtin/image_annotations.hpp" #include "pj_base/builtin/log.hpp" @@ -67,6 +68,7 @@ enum class BuiltinObjectType : uint16_t { kPosesInFrame = 17, ///< sdk::PosesInFrame — array of poses in one reference frame. kVoxelGrid = 18, ///< sdk::VoxelGrid — dense 3D voxel grid (occupancy/cost/ESDF/semantic). kPlotMarkers = 19, ///< sdk::PlotMarkers — findings on a time-series plot (regions, events, bands, labels). + kGridMap = 20, ///< sdk::GridMap — 2D grid of per-cell channels (elevation maps, layered costmaps). }; /// A-priori classification of a schema. Currently carries only the type; @@ -115,6 +117,8 @@ struct SchemaClassification { return "kVoxelGrid"; case BuiltinObjectType::kPlotMarkers: return "kPlotMarkers"; + case BuiltinObjectType::kGridMap: + return "kGridMap"; } return "kNone"; } @@ -176,6 +180,9 @@ struct SchemaClassification { if (s == "kPlotMarkers") { return BuiltinObjectType::kPlotMarkers; } + if (s == "kGridMap") { + return BuiltinObjectType::kGridMap; + } return std::nullopt; } @@ -240,6 +247,9 @@ using BuiltinObject = std::any; if (t == typeid(PlotMarkers)) { return BuiltinObjectType::kPlotMarkers; } + if (t == typeid(GridMap)) { + return BuiltinObjectType::kGridMap; + } return BuiltinObjectType::kNone; } diff --git a/pj_base/include/pj_base/builtin/frame_transforms.hpp b/pj_base/include/pj_base/builtin/frame_transforms.hpp index d75b0b0e..1f42482e 100644 --- a/pj_base/include/pj_base/builtin/frame_transforms.hpp +++ b/pj_base/include/pj_base/builtin/frame_transforms.hpp @@ -18,6 +18,13 @@ namespace PJ { namespace sdk { +/// Vector in 2D space (a direction or an extent, e.g. a grid's cell size). +struct Vector2 { + double x = 0.0; + double y = 0.0; + bool operator==(const Vector2&) const = default; +}; + /// Translation vector in 3D space. struct Vector3 { double x = 0.0; diff --git a/pj_base/include/pj_base/builtin/grid_map.hpp b/pj_base/include/pj_base/builtin/grid_map.hpp new file mode 100644 index 00000000..ae88c784 --- /dev/null +++ b/pj_base/include/pj_base/builtin/grid_map.hpp @@ -0,0 +1,70 @@ +/** + * @file grid_map.hpp + * @brief 2D grid of per-cell channels (elevation maps, layered costmaps) placed in world coordinates. + */ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +#include "pj_base/buffer_anchor.hpp" +#include "pj_base/builtin/frame_transforms.hpp" // for Pose, Vector2 +#include "pj_base/builtin/point_cloud.hpp" // for PointField (shared channel descriptor) +#include "pj_base/span.hpp" +#include "pj_base/types.hpp" + +namespace PJ { +namespace sdk { + +/// 2D grid whose cells carry named channels — the layered, generic-valued +/// sibling of OccupancyGrid: elevation maps, multi-layer costmaps, terrain +/// classification, anything a `grid_map_msgs/GridMap` or `foxglove.Grid` holds. +/// +/// Cells are fixed-size records of `cell_stride` bytes stored densely in +/// row-major order (x / column varies fastest), `row_stride` bytes per row, so +/// the byte offset of cell (column c, row r) is `r*row_stride + c*cell_stride` +/// and `data.size()` must be at least `row_count * row_stride`. `fields` +/// describes the channels inside one record with the same `PointField` model +/// PointCloud and VoxelGrid use; a NaN in a float channel means "no data" for +/// that cell, integer channels have no empty sentinel. The packed cell layout +/// is the one `foxglove.Grid` uses (row-major records, x fastest), so a parser +/// can hand that message's `data` over as a zero-copy view; its header and +/// field descriptors still need conversion (different wire field numbers, a +/// `PackedElementField` datatype numbering that differs from `PointField`'s, +/// and `count` always 1). Producers with another layout (grid_map's +/// column-major ring buffer) transcode once at the boundary. +/// +/// Cell (c, r) has its **center** at, in `frame_id`: +/// origin ∘ ((c + .5)*cell_size.x, (r + .5)*cell_size.y, 0) +/// where `origin` is the corner of cell (0,0) and the grid lies in the +/// origin's local xy-plane. Which channel is height, which is color, and how +/// values map to a colormap are viewer-side choices; the type carries no +/// styling. +/// +/// Channel-name conventions consumers may rely on: `elevation` is the +/// conventional height channel; `red`, `green`, `blue`, `alpha` are the RGBA +/// color channels, as in `foxglove.Grid`. +/// +/// `anchor` keeps the underlying buffer alive — `data` may be a view into the +/// source payload or into a freshly allocated buffer; consumers don't need to +/// know which. +struct GridMap { + Timestamp timestamp_ns = 0; + std::string frame_id; ///< Source coordinate frame; required for 3D TF resolution. + Pose origin; ///< Corner of cell (0,0) in `frame_id`; the grid lies in its local xy-plane. + Vector2 cell_size; ///< Metric cell size along local x (columns) and y (rows), meters. + uint32_t column_count = 0; ///< Cells along x (fastest-varying). + uint32_t row_count = 0; ///< Cells along y. + uint32_t cell_stride = 0; ///< Bytes per cell record (>= the largest field end: offset + size * count). + uint32_t row_stride = 0; ///< Bytes per row (>= column_count * cell_stride). + std::vector fields; + Span data; + BufferAnchor anchor; +}; + +} // namespace sdk +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/grid_map_codec.hpp b/pj_base/include/pj_base/builtin/grid_map_codec.hpp new file mode 100644 index 00000000..1fbfe713 --- /dev/null +++ b/pj_base/include/pj_base/builtin/grid_map_codec.hpp @@ -0,0 +1,36 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include "pj_base/builtin/grid_map.hpp" +#include "pj_base/expected.hpp" + +namespace PJ { + +inline constexpr std::string_view kSchemaGridMap = "PJ.GridMap"; + +/// Serializes sdk::GridMap to canonical PJ.GridMap wire bytes +/// (see pj_base/proto/pj/GridMap.proto). The struct is written as-is; layout +/// consistency is checked on decode. +[[nodiscard]] std::vector serializeGridMap(const sdk::GridMap& grid); + +/// Decodes canonical PJ.GridMap wire bytes. The returned object owns its cell +/// bytes via `anchor`. Rejects a layout the cell math could not index safely +/// (see validateGridMap), except that a wire carrying no `data` decodes with an +/// empty span: that is the functional-v2 splice form, whose bytes the host +/// attaches afterwards, so the data length is left for validateGridMap. +[[nodiscard]] Expected deserializeGridMap(const uint8_t* data, size_t size); + +/// Full layout check for a grid whose bytes are in place: every field has a +/// known datatype, a non-zero count and ends within `cell_stride`; with cells +/// declared, both strides are non-zero, a row holds its columns, and `data` +/// covers `row_count * row_stride`. Consumers that index cells, and hosts that +/// attach spliced bytes, call this before trusting the layout. +[[nodiscard]] Expected validateGridMap(const sdk::GridMap& grid); + +} // 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 af263f8f..ad80f7e3 100644 --- a/pj_base/include/pj_base/builtin_object_abi.h +++ b/pj_base/include/pj_base/builtin_object_abi.h @@ -57,6 +57,7 @@ typedef enum PJ_builtin_object_type_t { PJ_BUILTIN_OBJECT_TYPE_POSES_IN_FRAME = 17, PJ_BUILTIN_OBJECT_TYPE_VOXEL_GRID = 18, PJ_BUILTIN_OBJECT_TYPE_PLOT_MARKERS = 19, + PJ_BUILTIN_OBJECT_TYPE_GRID_MAP = 20, /* 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). */ @@ -87,7 +88,7 @@ typedef struct PJ_builtin_object_splice_field_v1_t { uint32_t field_number; } PJ_builtin_object_splice_field_v1_t; -#define PJ_BUILTIN_OBJECT_SPLICE_FIELDS_V1_COUNT UINT32_C(9) +#define PJ_BUILTIN_OBJECT_SPLICE_FIELDS_V1_COUNT UINT32_C(10) /** Return the frozen splice-eligible table and optionally its entry count. */ static inline const PJ_builtin_object_splice_field_v1_t* pj_builtin_object_splice_fields_v1(uint32_t* out_count) { @@ -101,6 +102,7 @@ static inline const PJ_builtin_object_splice_field_v1_t* pj_builtin_object_splic {PJ_BUILTIN_OBJECT_TYPE_VIDEO_FRAME, 0, 3}, {PJ_BUILTIN_OBJECT_TYPE_OCCUPANCY_GRID_UPDATE, 0, 7}, {PJ_BUILTIN_OBJECT_TYPE_VOXEL_GRID, 0, 12}, + {PJ_BUILTIN_OBJECT_TYPE_GRID_MAP, 0, 10}, }; if (out_count != NULL) { *out_count = PJ_BUILTIN_OBJECT_SPLICE_FIELDS_V1_COUNT; diff --git a/pj_base/include/pj_base/parser_module/object_writer.hpp b/pj_base/include/pj_base/parser_module/object_writer.hpp index 36b24e41..0c958271 100644 --- a/pj_base/include/pj_base/parser_module/object_writer.hpp +++ b/pj_base/include/pj_base/parser_module/object_writer.hpp @@ -155,6 +155,7 @@ class ObjectWriter { static constexpr uint16_t kVideoFrameObjectType = 10; static constexpr uint16_t kOccupancyGridUpdateObjectType = 15; static constexpr uint16_t kVoxelGridObjectType = 18; + static constexpr uint16_t kGridMapObjectType = 20; static constexpr uint32_t kImageDataField = 7; static constexpr uint32_t kPointCloudDataField = 9; static constexpr uint32_t kDepthImageDataField = 5; @@ -164,6 +165,7 @@ class ObjectWriter { static constexpr uint32_t kVideoFrameDataField = 3; static constexpr uint32_t kOccupancyGridUpdateDataField = 7; static constexpr uint32_t kVoxelGridDataField = 12; + static constexpr uint32_t kGridMapDataField = 10; enum class PointFieldDatatype : uint32_t { kUnknown = 0, @@ -186,6 +188,7 @@ class ObjectWriter { class VideoFrameBuilder; class OccupancyGridUpdateBuilder; class VoxelGridBuilder; + class GridMapBuilder; explicit ObjectWriter(PayloadView input_payload = {}) : input_payload_(input_payload) {} @@ -198,6 +201,7 @@ class ObjectWriter { [[nodiscard]] VideoFrameBuilder videoFrame(); [[nodiscard]] OccupancyGridUpdateBuilder occupancyGridUpdate(); [[nodiscard]] VoxelGridBuilder voxelGrid(); + [[nodiscard]] GridMapBuilder gridMap(); [[nodiscard]] const Status& status() const noexcept { return status_; @@ -266,6 +270,12 @@ class ObjectWriter { kVideoFrame, kOccupancyGridUpdate, kVoxelGrid, + kGridMap, + }; + + struct Vector2State { + double x = 0; + double y = 0; }; struct Vector3State { @@ -401,6 +411,19 @@ class ObjectWriter { Blob data; }; + struct GridMapState { + int64_t timestamp_ns = 0; + std::string frame_id; + PoseState origin; + Vector2State cell_size; + uint32_t column_count = 0; + uint32_t row_count = 0; + uint32_t cell_stride = 0; + uint32_t row_stride = 0; + std::vector fields; + Blob data; + }; + struct SpliceState { bool has_value = false; InputSpanRef reference; @@ -504,6 +527,15 @@ class ObjectWriter { return status.isOk() ? writer.messageField(1, nested) : status; } + [[nodiscard]] static Status writeVector2(WireWriter& writer, uint32_t field, const Vector2State& value) { + WireWriter nested; + Status status = nested.doubleField(1, value.x); + if (status.isOk()) { + status = nested.doubleField(2, value.y); + } + return status.isOk() ? writer.messageField(field, nested) : status; + } + [[nodiscard]] static Status writeVector3(WireWriter& writer, uint32_t field, const Vector3State& value) { WireWriter nested; Status status = nested.doubleField(1, value.x); @@ -600,6 +632,8 @@ class ObjectWriter { return writeOccupancyGridUpdate(); case Kind::kVoxelGrid: return writeVoxelGrid(); + case Kind::kGridMap: + return writeGridMap(); case Kind::kNone: break; } @@ -626,6 +660,8 @@ class ObjectWriter { return kOccupancyGridUpdateObjectType; case Kind::kVoxelGrid: return kVoxelGridObjectType; + case Kind::kGridMap: + return kGridMapObjectType; case Kind::kNone: return 0; } @@ -652,6 +688,8 @@ class ObjectWriter { return kOccupancyGridUpdateDataField; case Kind::kVoxelGrid: return kVoxelGridDataField; + case Kind::kGridMap: + return kGridMapDataField; case Kind::kNone: return 0; } @@ -882,6 +920,41 @@ class ObjectWriter { return status.isOk() ? Expected(writer.take()) : Expected(status); } + [[nodiscard]] Expected writeGridMap() { + WireWriter writer; + Status status = writeTimestamp(writer, grid_map_.timestamp_ns); + if (status.isOk()) { + status = writer.stringField(2, grid_map_.frame_id); + } + if (status.isOk()) { + status = writePose(writer, 3, grid_map_.origin); + } + if (status.isOk()) { + status = writeVector2(writer, 4, grid_map_.cell_size); + } + if (status.isOk()) { + status = writer.varintField(5, grid_map_.column_count); + } + if (status.isOk()) { + status = writer.varintField(6, grid_map_.row_count); + } + if (status.isOk()) { + status = writer.varintField(7, grid_map_.cell_stride); + } + if (status.isOk()) { + status = writer.varintField(8, grid_map_.row_stride); + } + for (const auto& field : grid_map_.fields) { + if (status.isOk()) { + status = writePointField(writer, 9, field); + } + } + if (status.isOk() && !splice_.has_value) { + status = writer.lengthDelimited(10, grid_map_.data.view()); + } + return status.isOk() ? Expected(writer.take()) : Expected(status); + } + [[nodiscard]] Expected writeVoxelGrid() { WireWriter writer; Status status = writeTimestamp(writer, voxel_grid_.timestamp_ns); @@ -936,6 +1009,7 @@ class ObjectWriter { VideoFrameState video_frame_; OccupancyGridUpdateState occupancy_grid_update_; VoxelGridState voxel_grid_; + GridMapState grid_map_; SpliceState splice_; friend class PointCloudBuilder; @@ -947,6 +1021,7 @@ class ObjectWriter { friend class VideoFrameBuilder; friend class OccupancyGridUpdateBuilder; friend class VoxelGridBuilder; + friend class GridMapBuilder; friend class ScalarWriter; }; @@ -1418,6 +1493,79 @@ inline ObjectWriter::VoxelGridBuilder ObjectWriter::voxelGrid() { return VoxelGridBuilder(*this); } +class ObjectWriter::GridMapBuilder { + public: + explicit GridMapBuilder(ObjectWriter& owner) : owner_(&owner) { + (void)owner_->select(Kind::kGridMap); + } + [[nodiscard]] Status setTimestamp(int64_t value) { + owner_->grid_map_.timestamp_ns = value; + return owner_->status_; + } + [[nodiscard]] Status setFrameId(std::string_view value) { + return owner_->setString(owner_->grid_map_.frame_id, value); + } + [[nodiscard]] Status setOrigin( + double px, double py, double pz, double qx = 0, double qy = 0, double qz = 0, double qw = 1) { + owner_->grid_map_.origin = {{px, py, pz}, {qx, qy, qz, qw}}; + return owner_->status_; + } + [[nodiscard]] Status setCellSize(double x, double y) { + owner_->grid_map_.cell_size = {x, y}; + return owner_->status_; + } + [[nodiscard]] Status setColumnCount(uint32_t value) { + owner_->grid_map_.column_count = value; + return owner_->status_; + } + [[nodiscard]] Status setRowCount(uint32_t value) { + owner_->grid_map_.row_count = value; + return owner_->status_; + } + [[nodiscard]] Status setCellStride(uint32_t value) { + owner_->grid_map_.cell_stride = value; + return owner_->status_; + } + [[nodiscard]] Status setRowStride(uint32_t value) { + owner_->grid_map_.row_stride = value; + return owner_->status_; + } + [[nodiscard]] Status addField( + std::string_view name, uint32_t offset, PointFieldDatatype datatype, uint32_t count = 1) { + if (!owner_->status_.isOk()) { + return owner_->status_; + } + PJ_PARSER_MODULE_TRY { + PointFieldState field; + field.name.assign(name.data(), name.size()); + field.offset = offset; + field.datatype = datatype; + field.count = count; + owner_->grid_map_.fields.push_back(std::move(field)); + return Status::ok(); + } + PJ_PARSER_MODULE_CATCH_BAD_ALLOC { + return owner_->fail("ObjectWriter grid-map-field allocation failed"); + } + PJ_PARSER_MODULE_CATCH_ALL { + return owner_->fail("ObjectWriter grid-map-field creation failed"); + } + } + [[nodiscard]] Status setData(PayloadView value) { + return owner_->setData(owner_->grid_map_.data, value); + } + [[nodiscard]] Status setDataFromInput(InputSpanRef value) { + return owner_->setDataFromInput(value); + } + + private: + ObjectWriter* owner_; +}; + +inline ObjectWriter::GridMapBuilder ObjectWriter::gridMap() { + return GridMapBuilder(*this); +} + class ScalarWriter { public: [[nodiscard]] Status setTimestamp(int64_t timestamp_ns) { diff --git a/pj_base/proto/pj/GridMap.proto b/pj_base/proto/pj/GridMap.proto new file mode 100644 index 00000000..1b67d5e5 --- /dev/null +++ b/pj_base/proto/pj/GridMap.proto @@ -0,0 +1,68 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: Apache-2.0 + +// PlotJuggler canonical 2D grid-map protobuf schema. +// Wire-level contract for the byte-backed `GridMap` builtin +// (SDK struct lives in pj_base/include/pj_base/builtin/grid_map.hpp). + +syntax = "proto3"; + +import "pj/Geometry.proto"; +import "pj/PointCloud.proto"; +import "google/protobuf/timestamp.proto"; + +package PJ; + +// A 2D grid whose cells carry named channels — the layered, generic-valued +// sibling of OccupancyGrid: elevation maps, multi-layer costmaps, terrain +// classification. The packed cell layout is the one foxglove.Grid uses, so a +// parser can expose that message's `data` as a zero-copy view; the header and +// field descriptors are converted (this message has its own field numbers and +// an explicit row_count, and PJ.PointField's datatype numbering differs from +// PackedElementField's). +// +// Cells are fixed-size records of `cell_stride` bytes in row-major order +// (x / column varies fastest), `row_stride` bytes per row; the byte offset of +// cell (c, r) is `r*row_stride + c*cell_stride` and `data.size()` must be at +// least `row_count * row_stride`. `fields` describes the channels inside one +// record; a NaN in a float channel means "no data" for that cell. +// +// Cell (c, r) center is at `origin ∘ ((c+.5)*cell_size.x, (r+.5)*cell_size.y, 0)` +// in `frame_id`, where `origin` is the corner of cell (0,0) and the grid lies +// in the origin's local xy-plane. Which channel is height, which is color and +// the colormap are viewer-side. Conventional channel names: `elevation` for +// height; `red`, `green`, `blue`, `alpha` for RGBA color. +// +// On the SDK side `data` is exposed as `Span` plus a +// `BufferAnchor`; the anchor is a C++ lifetime concept with no wire equivalent. +message GridMap { + // Timestamp of the grid + google.protobuf.Timestamp timestamp = 1; + + // Source coordinate frame for the grid (3D consumers TF-transform from this frame to a fixed render frame) + string frame_id = 2; + + // Pose of the corner of cell (0,0) in `frame_id`; the grid lies in the pose's local xy-plane + PJ.Pose origin = 3; + + // Metric cell size along the local x (columns) and y (rows) axes, meters + PJ.Vector2 cell_size = 4; + + // Number of cells along the local x axis (fastest-varying) + uint32 column_count = 5; + + // Number of cells along the local y axis + uint32 row_count = 6; + + // Bytes per cell record (>= the largest field end: offset + element size * count) + uint32 cell_stride = 7; + + // Bytes per row (>= column_count * cell_stride) + uint32 row_stride = 8; + + // Channel layout for each cell record in `data` + repeated PJ.PointField fields = 9; + + // Packed cell records in row-major order + bytes data = 10; +} diff --git a/pj_base/proto/pj/README.md b/pj_base/proto/pj/README.md index b5a878bc..6c9d7d15 100644 --- a/pj_base/proto/pj/README.md +++ b/pj_base/proto/pj/README.md @@ -64,6 +64,8 @@ rationale. - `PosesInFrame` - **`VoxelGrid.proto`** — dense 3D voxel grid (the volumetric sibling of `OccupancyGrid`); reuses `PointField` for the per-voxel channel layout and mirrors `foxglove.VoxelGrid`'s Z-Y-X byte layout so `data` stays a zero-copy view. Serves occupancy/cost/ESDF/semantic grids; the draw predicate is viewer-side. - `VoxelGrid` +- **`GridMap.proto`** — 2D grid of per-cell channels (the layered sibling of `OccupancyGrid`: elevation maps, multi-layer costmaps); reuses `PointField` for the per-cell channel layout and mirrors `foxglove.Grid`'s row-major record layout so `data` stays a zero-copy view. Which channel is height/color is viewer-side. + - `GridMap` ### 2D image annotations (vector overlays) diff --git a/pj_base/src/builtin/builtin_object_codec.cpp b/pj_base/src/builtin/builtin_object_codec.cpp index 5851b847..9878ea87 100644 --- a/pj_base/src/builtin/builtin_object_codec.cpp +++ b/pj_base/src/builtin/builtin_object_codec.cpp @@ -13,6 +13,7 @@ #include "pj_base/builtin/compressed_point_cloud_codec.hpp" #include "pj_base/builtin/depth_image_codec.hpp" #include "pj_base/builtin/frame_transforms_codec.hpp" +#include "pj_base/builtin/grid_map_codec.hpp" #include "pj_base/builtin/image_annotations_codec.hpp" #include "pj_base/builtin/image_codec.hpp" #include "pj_base/builtin/log_codec.hpp" @@ -98,6 +99,8 @@ Expected> serializeBuiltinObject(const sdk::BuiltinObject& return serializeAs(object, type, serializeVoxelGrid); case sdk::BuiltinObjectType::kPlotMarkers: return serializeAs(object, type, serializePlotMarkers); + case sdk::BuiltinObjectType::kGridMap: + return serializeAs(object, type, serializeGridMap); case sdk::BuiltinObjectType::kNone: return unexpected(std::string("cannot serialize builtin object with type kNone")); } @@ -140,6 +143,8 @@ Expected deserializeBuiltinObject(sdk::BuiltinObjectType typ return deserializeAs(data, size, deserializeVoxelGrid); case sdk::BuiltinObjectType::kPlotMarkers: return deserializeAs(data, size, deserializePlotMarkers); + case sdk::BuiltinObjectType::kGridMap: + return deserializeAs(data, size, deserializeGridMap); case sdk::BuiltinObjectType::kNone: return unexpected(std::string("cannot deserialize builtin object with type kNone")); } diff --git a/pj_base/src/builtin/geometry_codec.hpp b/pj_base/src/builtin/geometry_codec.hpp index b9ad69b6..eeeae1f4 100644 --- a/pj_base/src/builtin/geometry_codec.hpp +++ b/pj_base/src/builtin/geometry_codec.hpp @@ -4,7 +4,7 @@ // // 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 +// (Vector2, 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]). // @@ -14,7 +14,7 @@ #include #include -#include "pj_base/builtin/frame_transforms.hpp" // Vector3, Quaternion, Pose +#include "pj_base/builtin/frame_transforms.hpp" // Vector2, Vector3, Quaternion, Pose #include "pj_base/builtin/image_annotations.hpp" // ColorRGBA #include "pj_base/builtin/scene_entities.hpp" // Point3 #include "pj_base/types.hpp" @@ -92,6 +92,34 @@ inline bool readTimestampMessage(Reader& reader, Timestamp& out) { return reader.readMessage(nested) && decodeTimestamp(nested, out); } +// ---------- Vector2 ---------- + +inline void writeVector2(Writer& writer, const sdk::Vector2& v) { + writer.doubleField(1, v.x); + writer.doubleField(2, v.y); +} + +inline bool decodeVector2(Reader& reader, sdk::Vector2& 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); + default: + return false; + } + }); +} + +inline bool readVector2Message(Reader& reader, sdk::Vector2& out) { + Reader nested; + return reader.readMessage(nested) && decodeVector2(nested, out); +} + // ---------- Vector3 ---------- inline void writeVector3(Writer& writer, const sdk::Vector3& v) { diff --git a/pj_base/src/builtin/grid_map_codec.cpp b/pj_base/src/builtin/grid_map_codec.cpp new file mode 100644 index 00000000..c5aaea7b --- /dev/null +++ b/pj_base/src/builtin/grid_map_codec.cpp @@ -0,0 +1,157 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: Apache-2.0 + +#include "pj_base/builtin/grid_map_codec.hpp" + +#include +#include +#include +#include +#include + +#include "geometry_codec.hpp" +#include "point_field_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::GridMap; +using sdk::PointField; + +bool readVarintInto(Reader& reader, WireType type, uint32_t& out) { + if (type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!reader.readVarint(v)) { + return false; + } + out = static_cast(v); + return true; +} + +bool readBytesIntoGrid(Reader& reader, GridMap& 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; +} + +// `check_data` is off for a header-only wire (the functional-v2 splice form, +// whose bytes the host attaches afterwards); everything else is checked +// regardless of whether the grid has cells. +Expected validateLayout(const GridMap& grid, bool check_data) { + for (const auto& field : grid.fields) { + if (field.datatype == PointField::Datatype::kUnknown) { + return unexpected(std::string("GridMap: field '") + field.name + "' has an unknown datatype"); + } + if (field.count == 0) { + return unexpected(std::string("GridMap: field '") + field.name + "' has a zero count"); + } + const uint64_t end = static_cast(field.offset) + + (static_cast(sdk::bytesPerElement(field.datatype)) * field.count); + if (end > grid.cell_stride) { + return unexpected(std::string("GridMap: field '") + field.name + "' reaches past cell_stride"); + } + } + const bool has_cells = grid.column_count > 0 && grid.row_count > 0; + if (!has_cells) { + return {}; + } + if (grid.cell_stride == 0 || grid.row_stride == 0) { + return unexpected(std::string("GridMap: zero stride with cells declared")); + } + if (static_cast(grid.row_stride) < static_cast(grid.column_count) * grid.cell_stride) { + return unexpected(std::string("GridMap: row_stride shorter than column_count * cell_stride")); + } + if (check_data && static_cast(grid.data.size()) < static_cast(grid.row_count) * grid.row_stride) { + return unexpected(std::string("GridMap: data shorter than row_count * row_stride")); + } + return {}; +} + +} // namespace + +Expected validateGridMap(const sdk::GridMap& grid) { + return validateLayout(grid, /*check_data=*/true); +} + +std::vector serializeGridMap(const GridMap& 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.message(4, [&](Writer& nested) { builtin_wire::writeVector2(nested, grid.cell_size); }); + writer.varint(5, grid.column_count); + writer.varint(6, grid.row_count); + writer.varint(7, grid.cell_stride); + writer.varint(8, grid.row_stride); + for (const auto& field : grid.fields) { + writer.message(9, [&](Writer& nested) { builtin_wire::writePointField(nested, field); }); + } + writer.bytes(10, grid.data.data(), grid.data.size()); + + return out; +} + +Expected deserializeGridMap(const uint8_t* data, size_t size) { + if (data == nullptr || size == 0) { + return unexpected(std::string("GridMap wire: empty buffer")); + } + + Reader reader(data, size); + sdk::GridMap grid; + + const bool ok = parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + return tag.type == WireType::kLengthDelimited && builtin_wire::readTimestampMessage(r, grid.timestamp_ns); + case 2: + return tag.type == WireType::kLengthDelimited && r.readString(grid.frame_id); + case 3: + return tag.type == WireType::kLengthDelimited && builtin_wire::readPoseMessage(r, grid.origin); + case 4: + return tag.type == WireType::kLengthDelimited && builtin_wire::readVector2Message(r, grid.cell_size); + case 5: + return readVarintInto(r, tag.type, grid.column_count); + case 6: + return readVarintInto(r, tag.type, grid.row_count); + case 7: + return readVarintInto(r, tag.type, grid.cell_stride); + case 8: + return readVarintInto(r, tag.type, grid.row_stride); + case 9: + return tag.type == WireType::kLengthDelimited && builtin_wire::readPointFieldIntoVector(r, grid.fields); + case 10: + return tag.type == WireType::kLengthDelimited && readBytesIntoGrid(r, grid); + default: + return false; + } + }); + + if (!ok) { + return unexpected(std::string("GridMap wire: decode failed")); + } + // A wire without `data` is the spliced form: the bytes are attached later, + // so their length is validated then (validateGridMap), not here. + if (auto valid = validateLayout(grid, /*check_data=*/!grid.data.empty()); !valid) { + return unexpected(std::move(valid).error()); + } + + return grid; +} + +} // namespace PJ diff --git a/pj_base/src/builtin/point_cloud_codec.cpp b/pj_base/src/builtin/point_cloud_codec.cpp index 4b5fd1bd..557fe632 100644 --- a/pj_base/src/builtin/point_cloud_codec.cpp +++ b/pj_base/src/builtin/point_cloud_codec.cpp @@ -10,6 +10,7 @@ #include #include "geometry_codec.hpp" +#include "point_field_codec.hpp" #include "protobuf_wire.hpp" namespace PJ { @@ -17,112 +18,14 @@ namespace { using builtin_wire::parseFields; using builtin_wire::Reader; +using builtin_wire::readPointFieldIntoVector; using builtin_wire::Tag; using builtin_wire::WireType; +using builtin_wire::writePointField; 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) { diff --git a/pj_base/src/builtin/point_field_codec.hpp b/pj_base/src/builtin/point_field_codec.hpp new file mode 100644 index 00000000..cab4bcc4 --- /dev/null +++ b/pj_base/src/builtin/point_field_codec.hpp @@ -0,0 +1,102 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: Apache-2.0 +// +// Internal wire helpers for the PJ.PointField message (name=1, offset=2, +// datatype=3, count=4), shared by every codec that carries the per-record +// channel model: PointCloud, VoxelGrid, GridMap. +// +// Inline-only; not exposed through the public include path. + +#include +#include +#include + +#include "pj_base/builtin/point_cloud.hpp" +#include "protobuf_wire.hpp" + +namespace PJ::builtin_wire { + +// The proto enum (UNKNOWN=0, INT8=1, UINT8=2, INT16=3, UINT16=4, INT32=5, +// UINT32=6, FLOAT32=7, FLOAT64=8) is numerically identical to +// sdk::PointField::Datatype, so encoding is a cast; decoding still funnels +// unknown numbers to kUnknown instead of trusting the wire. + +inline uint32_t pointFieldDatatypeToWire(sdk::PointField::Datatype dt) { + return static_cast(dt); +} + +inline sdk::PointField::Datatype pointFieldDatatypeFromWire(uint64_t value) { + using Datatype = sdk::PointField::Datatype; + switch (value) { + case 1: + return Datatype::kInt8; + case 2: + return Datatype::kUint8; + case 3: + return Datatype::kInt16; + case 4: + return Datatype::kUint16; + case 5: + return Datatype::kInt32; + case 6: + return Datatype::kUint32; + case 7: + return Datatype::kFloat32; + case 8: + return Datatype::kFloat64; + case 0: + default: + return Datatype::kUnknown; + } +} + +inline void writePointField(Writer& writer, const sdk::PointField& field) { + writer.string(1, field.name); + writer.varint(2, field.offset); + writer.varint(3, pointFieldDatatypeToWire(field.datatype)); + writer.varint(4, field.count); +} + +inline bool decodePointField(Reader& reader, sdk::PointField& out) { + return parseFields(reader, [&](Tag tag, Reader& r) { + if (tag.field == 1) { + return tag.type == WireType::kLengthDelimited && r.readString(out.name); + } + if (tag.type != WireType::kVarint) { + return false; + } + uint64_t v = 0; + if (!r.readVarint(v)) { + return false; + } + switch (tag.field) { + case 2: + out.offset = static_cast(v); + return true; + case 3: + out.datatype = pointFieldDatatypeFromWire(v); + return true; + case 4: + out.count = static_cast(v); + return true; + default: + return false; + } + }); +} + +inline bool readPointFieldIntoVector(Reader& reader, std::vector& out) { + Reader nested; + if (!reader.readMessage(nested)) { + return false; + } + sdk::PointField field; + if (!decodePointField(nested, field)) { + return false; + } + out.push_back(std::move(field)); + return true; +} + +} // namespace PJ::builtin_wire diff --git a/pj_base/src/builtin/voxel_grid_codec.cpp b/pj_base/src/builtin/voxel_grid_codec.cpp index c5b9ea7d..65cf4783 100644 --- a/pj_base/src/builtin/voxel_grid_codec.cpp +++ b/pj_base/src/builtin/voxel_grid_codec.cpp @@ -10,6 +10,7 @@ #include #include "geometry_codec.hpp" +#include "point_field_codec.hpp" #include "protobuf_wire.hpp" namespace PJ { @@ -17,110 +18,14 @@ namespace { using builtin_wire::parseFields; using builtin_wire::Reader; +using builtin_wire::readPointFieldIntoVector; using builtin_wire::Tag; using builtin_wire::WireType; +using builtin_wire::writePointField; using builtin_wire::Writer; using sdk::PointField; using sdk::VoxelGrid; -// ---------- PointField enum mapping ---------- -// Numerically identical to the proto enum (UNKNOWN=0 .. FLOAT64=8); see -// point_cloud_codec.cpp. The PointField wire helpers below intentionally mirror -// that codec's local helpers — extracting a shared point_field_codec.hpp is a -// candidate follow-up once a third consumer appears (Rule of Three). - -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; - } -} - -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; -} - bool readBytesIntoGrid(Reader& reader, VoxelGrid& out) { const uint8_t* data = nullptr; size_t size = 0; diff --git a/pj_base/tests/abi_layout_sentinels_test.cpp b/pj_base/tests/abi_layout_sentinels_test.cpp index 845f61f7..77da1820 100644 --- a/pj_base/tests/abi_layout_sentinels_test.cpp +++ b/pj_base/tests/abi_layout_sentinels_test.cpp @@ -129,6 +129,7 @@ static_assert(PJ_BUILTIN_OBJECT_TYPE_LOG == 16, "Log type id pinned"); static_assert(PJ_BUILTIN_OBJECT_TYPE_POSES_IN_FRAME == 17, "PosesInFrame type id pinned"); static_assert(PJ_BUILTIN_OBJECT_TYPE_VOXEL_GRID == 18, "VoxelGrid type id pinned"); static_assert(PJ_BUILTIN_OBJECT_TYPE_PLOT_MARKERS == 19, "PlotMarkers type id pinned"); +static_assert(PJ_BUILTIN_OBJECT_TYPE_GRID_MAP == 20, "GridMap 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/builtin_object_codec_test.cpp b/pj_base/tests/builtin_object_codec_test.cpp index 51874be9..1768fccb 100644 --- a/pj_base/tests/builtin_object_codec_test.cpp +++ b/pj_base/tests/builtin_object_codec_test.cpp @@ -12,7 +12,7 @@ namespace { TEST(BuiltinObjectCodec, DispatchesEveryStableBuiltinType) { - const std::array objects{ + const std::array objects{ PJ::sdk::Image{}, PJ::sdk::PointCloud{}, PJ::sdk::DepthImage{}, @@ -41,6 +41,7 @@ TEST(BuiltinObjectCodec, DispatchesEveryStableBuiltinType) { PJ::sdk::Log{}, PJ::sdk::PosesInFrame{}, PJ::sdk::VoxelGrid{}, + PJ::sdk::GridMap{}, [] { PJ::sdk::PlotMarkers markers; markers.markers.emplace_back(); @@ -74,7 +75,7 @@ TEST(BuiltinObjectCodec, RejectsEmptyAndUnknownTypeTags) { } TEST(BuiltinObjectCodec, RoundTripsDefaultObjectsIncludingZeroByteProtoMessages) { - const std::array defaults{ + const std::array defaults{ PJ::sdk::Image{}, PJ::sdk::PointCloud{}, PJ::sdk::DepthImage{}, @@ -91,6 +92,7 @@ TEST(BuiltinObjectCodec, RoundTripsDefaultObjectsIncludingZeroByteProtoMessages) PJ::sdk::Log{}, PJ::sdk::PosesInFrame{}, PJ::sdk::VoxelGrid{}, + PJ::sdk::GridMap{}, PJ::sdk::PlotMarkers{}, }; diff --git a/pj_base/tests/builtin_object_test.cpp b/pj_base/tests/builtin_object_test.cpp index 8341068a..0c7f9946 100644 --- a/pj_base/tests/builtin_object_test.cpp +++ b/pj_base/tests/builtin_object_test.cpp @@ -11,6 +11,7 @@ using PJ::sdk::CameraInfo; using PJ::sdk::CompressedPointCloud; using PJ::sdk::DepthImage; using PJ::sdk::FrameTransforms; +using PJ::sdk::GridMap; using PJ::sdk::Image; using PJ::sdk::ImageAnnotations; using PJ::sdk::Log; @@ -46,6 +47,7 @@ TEST(BuiltinObjectTest, TypeOfRecognizesKnownBuiltinTypes) { EXPECT_EQ(typeOf(BuiltinObject{Log{}}), BuiltinObjectType::kLog); EXPECT_EQ(typeOf(BuiltinObject{PosesInFrame{}}), BuiltinObjectType::kPosesInFrame); EXPECT_EQ(typeOf(BuiltinObject{VoxelGrid{}}), BuiltinObjectType::kVoxelGrid); + EXPECT_EQ(typeOf(BuiltinObject{GridMap{}}), BuiltinObjectType::kGridMap); EXPECT_EQ(typeOf(BuiltinObject{PlotMarkers{}}), BuiltinObjectType::kPlotMarkers); } @@ -69,6 +71,7 @@ TEST(BuiltinObjectTest, NameAndParseRoundTripForEveryEnumEntry) { BuiltinObjectType::kPosesInFrame, BuiltinObjectType::kVoxelGrid, BuiltinObjectType::kPlotMarkers, + BuiltinObjectType::kGridMap, }) { const auto parsed = parseBuiltinObjectType(name(t)); ASSERT_TRUE(parsed.has_value()) << "parseBuiltinObjectType failed for " << name(t); diff --git a/pj_base/tests/grid_map_codec_test.cpp b/pj_base/tests/grid_map_codec_test.cpp new file mode 100644 index 00000000..894e3252 --- /dev/null +++ b/pj_base/tests/grid_map_codec_test.cpp @@ -0,0 +1,194 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: Apache-2.0 + +#include "pj_base/builtin/grid_map_codec.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace PJ { +namespace { + +using sdk::GridMap; +using sdk::PointField; + +constexpr float kNaN = std::numeric_limits::quiet_NaN(); + +// 3 columns x 2 rows, two float32 channels per cell (elevation, cost), row-major. +// Cell (2,1) has a NaN elevation: the "no data" sentinel must survive the wire. +GridMap makeGrid(std::vector& storage) { + GridMap grid; + grid.timestamp_ns = 42'000'000'000LL; + grid.frame_id = "odom"; + grid.origin.position = {.x = 1.0, .y = -2.0, .z = 0.5}; + grid.origin.orientation = {.x = 0.0, .y = 0.0, .z = 0.0, .w = 1.0}; + grid.cell_size = {.x = 0.02, .y = 0.05}; + grid.column_count = 3; + grid.row_count = 2; + grid.cell_stride = 8; + grid.row_stride = 24; + grid.fields.push_back({.name = "elevation", .offset = 0, .datatype = PointField::Datatype::kFloat32, .count = 1}); + grid.fields.push_back({.name = "cost", .offset = 4, .datatype = PointField::Datatype::kFloat32, .count = 1}); + const std::vector values = { + 0.10f, 1.0f, 0.20f, 2.0f, 0.30f, 3.0f, // row 0: (elevation, cost) x 3 columns + 0.40f, 4.0f, 0.50f, 5.0f, kNaN, 6.0f, // row 1 + }; + storage.resize(values.size() * sizeof(float)); + std::memcpy(storage.data(), values.data(), storage.size()); + grid.data = Span(storage.data(), storage.size()); + return grid; +} + +float cellValue(const GridMap& grid, uint32_t column, uint32_t row, uint32_t field_offset) { + float value = 0.0f; + std::memcpy( + &value, grid.data.data() + (row * grid.row_stride) + (column * grid.cell_stride) + field_offset, sizeof(float)); + return value; +} + +TEST(GridMapCodecTest, SchemaName) { + EXPECT_EQ(kSchemaGridMap, "PJ.GridMap"); +} + +TEST(GridMapCodecTest, EmptyBufferProducesError) { + EXPECT_FALSE(deserializeGridMap(nullptr, 0).has_value()); +} + +TEST(GridMapCodecTest, RoundTrip3x2TwoChannels) { + std::vector storage; + const GridMap in = makeGrid(storage); + + const auto bytes = serializeGridMap(in); + auto out = deserializeGridMap(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()) << out.error(); + 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->cell_size.x, in.cell_size.x); + EXPECT_DOUBLE_EQ(out->cell_size.y, in.cell_size.y); + EXPECT_EQ(out->column_count, in.column_count); + EXPECT_EQ(out->row_count, in.row_count); + EXPECT_EQ(out->cell_stride, in.cell_stride); + EXPECT_EQ(out->row_stride, in.row_stride); + ASSERT_EQ(out->fields.size(), 2u); + EXPECT_EQ(out->fields[0].name, "elevation"); + EXPECT_EQ(out->fields[0].offset, 0u); + EXPECT_EQ(out->fields[0].datatype, PointField::Datatype::kFloat32); + EXPECT_EQ(out->fields[0].count, 1u); + EXPECT_EQ(out->fields[1].name, "cost"); + EXPECT_EQ(out->fields[1].offset, 4u); + ASSERT_EQ(out->data.size(), storage.size()); + EXPECT_EQ(std::memcmp(out->data.data(), storage.data(), storage.size()), 0); + + // Decoded bytes are owned by the result, not by the wire buffer or `storage`. + EXPECT_NE(out->data.data(), storage.data()); + EXPECT_TRUE(static_cast(out->anchor)); + + EXPECT_FLOAT_EQ(cellValue(*out, 1, 0, 0), 0.20f); + EXPECT_FLOAT_EQ(cellValue(*out, 1, 1, 4), 5.0f); + EXPECT_TRUE(std::isnan(cellValue(*out, 2, 1, 0))); +} + +TEST(GridMapCodecTest, RoundTripEmptyGridNoFields) { + GridMap in; + in.frame_id = "map"; + const auto bytes = serializeGridMap(in); + auto out = deserializeGridMap(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()) << out.error(); + EXPECT_EQ(out->frame_id, "map"); + EXPECT_TRUE(out->fields.empty()); + EXPECT_EQ(out->data.size(), 0u); + EXPECT_EQ(out->column_count, 0u); + EXPECT_EQ(out->row_count, 0u); +} + +TEST(GridMapCodecTest, RejectsDataShorterThanDeclaredRows) { + std::vector storage; + GridMap in = makeGrid(storage); + in.row_count = 3; // declares 72 bytes, only 48 present + const auto bytes = serializeGridMap(in); + EXPECT_FALSE(deserializeGridMap(bytes.data(), bytes.size()).has_value()); +} + +TEST(GridMapCodecTest, RejectsFieldPastCellStride) { + std::vector storage; + GridMap in = makeGrid(storage); + in.fields[1].offset = 8; // float32 at byte 8 of an 8-byte cell + const auto bytes = serializeGridMap(in); + EXPECT_FALSE(deserializeGridMap(bytes.data(), bytes.size()).has_value()); +} + +TEST(GridMapCodecTest, RejectsZeroStrideWithCells) { + std::vector storage; + GridMap in = makeGrid(storage); + in.cell_stride = 0; + const auto bytes = serializeGridMap(in); + EXPECT_FALSE(deserializeGridMap(bytes.data(), bytes.size()).has_value()); +} + +TEST(GridMapCodecTest, RejectsRowStrideShorterThanColumns) { + std::vector storage; + GridMap in = makeGrid(storage); + in.row_stride = 16; // 3 columns x 8 bytes need 24 + const auto bytes = serializeGridMap(in); + EXPECT_FALSE(deserializeGridMap(bytes.data(), bytes.size()).has_value()); +} + +TEST(GridMapCodecTest, RejectsUnknownDatatype) { + std::vector storage; + GridMap in = makeGrid(storage); + in.fields[1].datatype = PointField::Datatype::kUnknown; + const auto bytes = serializeGridMap(in); + EXPECT_FALSE(deserializeGridMap(bytes.data(), bytes.size()).has_value()); +} + +TEST(GridMapCodecTest, RejectsZeroFieldCount) { + std::vector storage; + GridMap in = makeGrid(storage); + in.fields[1].count = 0; + const auto bytes = serializeGridMap(in); + EXPECT_FALSE(deserializeGridMap(bytes.data(), bytes.size()).has_value()); +} + +TEST(GridMapCodecTest, RejectsFieldOnEmptyGridWithZeroCellStride) { + GridMap in; // no cells, cell_stride 0, but a channel that needs 4 bytes + in.fields.push_back({.name = "elevation", .offset = 0, .datatype = PointField::Datatype::kFloat32, .count = 1}); + const auto bytes = serializeGridMap(in); + EXPECT_FALSE(deserializeGridMap(bytes.data(), bytes.size()).has_value()); +} + +// The functional-v2 splice contract: the canonical wire omits `data` (the host +// attaches the bytes afterwards), so a header-only wire must decode. The full +// check, including the data length, is validateGridMap(), run once the bytes +// are attached. +TEST(GridMapCodecTest, HeaderOnlyWireDecodesForSpliceAndValidateChecksAttachedData) { + std::vector storage; + GridMap in = makeGrid(storage); + in.data = Span{}; + const auto bytes = serializeGridMap(in); + + auto out = deserializeGridMap(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()) << out.error(); + EXPECT_EQ(out->column_count, 3u); + EXPECT_TRUE(out->data.empty()); + EXPECT_FALSE(validateGridMap(*out).has_value()); + + out->data = Span(storage.data(), 40); // one row short + EXPECT_FALSE(validateGridMap(*out).has_value()); + out->data = Span(storage.data(), storage.size()); + EXPECT_TRUE(validateGridMap(*out).has_value()); +} + +TEST(GridMapCodecTest, ValidateAcceptsFieldEndingExactlyAtCellStride) { + std::vector storage; + const GridMap in = makeGrid(storage); // cost: offset 4 + 4 bytes == cell_stride 8 + EXPECT_TRUE(validateGridMap(in).has_value()); +} + +} // namespace +} // namespace PJ diff --git a/pj_base/tests/object_topic_metadata_test.cpp b/pj_base/tests/object_topic_metadata_test.cpp index 594a213f..92ce68a2 100644 --- a/pj_base/tests/object_topic_metadata_test.cpp +++ b/pj_base/tests/object_topic_metadata_test.cpp @@ -60,6 +60,7 @@ constexpr std::array kBuiltinObjectTypes{ BuiltinObjectType::kPosesInFrame, BuiltinObjectType::kVoxelGrid, BuiltinObjectType::kPlotMarkers, + BuiltinObjectType::kGridMap, }; struct RegistrationRecorder { diff --git a/pj_base/tests/parser_module_abi_test.cpp b/pj_base/tests/parser_module_abi_test.cpp index 0855b6ef..6fe9d399 100644 --- a/pj_base/tests/parser_module_abi_test.cpp +++ b/pj_base/tests/parser_module_abi_test.cpp @@ -244,7 +244,7 @@ TEST(ParserModuleAbi, OutputReaderRejectsTruncationAndMalformedDescriptors) { } TEST(BuiltinObjectSpliceTable, ContainsOnlyUnambiguousTopLevelBulkByteFields) { - const std::array expected{ + const std::array expected{ PJ_builtin_object_splice_field_v1_t{PJ_BUILTIN_OBJECT_TYPE_IMAGE, 0, 7}, PJ_builtin_object_splice_field_v1_t{PJ_BUILTIN_OBJECT_TYPE_POINTCLOUD, 0, 9}, PJ_builtin_object_splice_field_v1_t{PJ_BUILTIN_OBJECT_TYPE_DEPTH_IMAGE, 0, 5}, @@ -254,6 +254,7 @@ TEST(BuiltinObjectSpliceTable, ContainsOnlyUnambiguousTopLevelBulkByteFields) { PJ_builtin_object_splice_field_v1_t{PJ_BUILTIN_OBJECT_TYPE_VIDEO_FRAME, 0, 3}, PJ_builtin_object_splice_field_v1_t{PJ_BUILTIN_OBJECT_TYPE_OCCUPANCY_GRID_UPDATE, 0, 7}, PJ_builtin_object_splice_field_v1_t{PJ_BUILTIN_OBJECT_TYPE_VOXEL_GRID, 0, 12}, + PJ_builtin_object_splice_field_v1_t{PJ_BUILTIN_OBJECT_TYPE_GRID_MAP, 0, 10}, }; uint32_t count = 0; const auto* table = pj_builtin_object_splice_fields_v1(&count); diff --git a/pj_base/tests/parser_module_object_writer_test.cpp b/pj_base/tests/parser_module_object_writer_test.cpp index 428f5ebe..b3e7042a 100644 --- a/pj_base/tests/parser_module_object_writer_test.cpp +++ b/pj_base/tests/parser_module_object_writer_test.cpp @@ -14,6 +14,7 @@ #include "pj_base/builtin/builtin_object_codec.hpp" #include "pj_base/builtin/compressed_point_cloud.hpp" #include "pj_base/builtin/depth_image.hpp" +#include "pj_base/builtin/grid_map.hpp" #include "pj_base/builtin/image.hpp" #include "pj_base/builtin/mesh3d.hpp" #include "pj_base/builtin/occupancy_grid.hpp" @@ -520,6 +521,37 @@ TEST(ParserModuleObjectWriter, AllAdditionalSpliceEligibleBuildersRoundTripFullW ASSERT_EQ(voxel_value->fields.size(), 1U); EXPECT_EQ(voxel_value->fields[0].name, "occupancy"); EXPECT_EQ(voxel_value->data.size(), 3U); + + pj::ObjectWriter grid_map_writer; + auto grid_map = grid_map_writer.gridMap(); + ASSERT_TRUE(grid_map.setTimestamp(7).isOk()); + ASSERT_TRUE(grid_map.setFrameId("odom").isOk()); + ASSERT_TRUE(grid_map.setOrigin(1, 2, 3).isOk()); + ASSERT_TRUE(grid_map.setCellSize(0.1, 0.2).isOk()); + ASSERT_TRUE(grid_map.setColumnCount(3).isOk()); + ASSERT_TRUE(grid_map.setRowCount(1).isOk()); + ASSERT_TRUE(grid_map.setCellStride(1).isOk()); + ASSERT_TRUE(grid_map.setRowStride(3).isOk()); + ASSERT_TRUE(grid_map.addField("cost", 0, pj::ObjectWriter::PointFieldDatatype::kUint8).isOk()); + ASSERT_TRUE(grid_map.setData({data.data(), data.size()}).isOk()); + auto grid_map_descriptor = grid_map_writer.finish(); + ASSERT_TRUE(grid_map_descriptor.hasValue()) << grid_map_descriptor.status().message(); + auto grid_map_output = + PJ::parser_module::readOutputDescriptorV1({grid_map_descriptor->data(), grid_map_descriptor->size()}); + ASSERT_TRUE(grid_map_output.has_value()) << grid_map_output.error(); + const auto& grid_map_wire = std::get(*grid_map_output).wire; + auto decoded_grid_map = + PJ::deserializeBuiltinObject(PJ::sdk::BuiltinObjectType::kGridMap, grid_map_wire.data(), grid_map_wire.size()); + ASSERT_TRUE(decoded_grid_map.has_value()) << decoded_grid_map.error(); + const auto* grid_map_value = std::any_cast(&*decoded_grid_map); + ASSERT_NE(grid_map_value, nullptr); + EXPECT_EQ(grid_map_value->frame_id, "odom"); + EXPECT_DOUBLE_EQ(grid_map_value->cell_size.x, 0.1); + EXPECT_DOUBLE_EQ(grid_map_value->cell_size.y, 0.2); + EXPECT_EQ(grid_map_value->column_count, 3U); + ASSERT_EQ(grid_map_value->fields.size(), 1U); + EXPECT_EQ(grid_map_value->fields[0].name, "cost"); + EXPECT_EQ(grid_map_value->data.size(), 3U); } TEST(ParserModuleObjectWriter, EveryEligibleBuilderSupportsValidatedSpliceOutput) { @@ -558,6 +590,9 @@ TEST(ParserModuleObjectWriter, EveryEligibleBuilderSupportsValidatedSpliceOutput pj::ObjectWriter voxel({payload.data(), payload.size()}); ASSERT_TRUE(voxel.voxelGrid().setDataFromInput({2, 3}).isOk()); expect_splice(voxel, 18, 12); + pj::ObjectWriter grid_map({payload.data(), payload.size()}); + ASSERT_TRUE(grid_map.gridMap().setDataFromInput({2, 3}).isOk()); + expect_splice(grid_map, 20, 10); } TEST(ParserModuleObjectWriter, RejectsConflictingOrOutOfBoundsBulkDataSelection) { 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 a492b447..1d737773 100644 --- a/pj_plugins/include/pj_plugins/host/message_parser_handle.hpp +++ b/pj_plugins/include/pj_plugins/host/message_parser_handle.hpp @@ -494,6 +494,9 @@ class MessageParserHandle { case sdk::BuiltinObjectType::kVoxelGrid: attached = attach.template operator()(); break; + case sdk::BuiltinObjectType::kGridMap: + attached = attach.template operator()(); + break; default: break; } diff --git a/pj_plugins/src/parser_module_runtime.cpp b/pj_plugins/src/parser_module_runtime.cpp index 79dc3fda..a9ac4585 100644 --- a/pj_plugins/src/parser_module_runtime.cpp +++ b/pj_plugins/src/parser_module_runtime.cpp @@ -137,6 +137,9 @@ Expected ownObjectOutput( case sdk::BuiltinObjectType::kVoxelGrid: attached = attach.template operator()(); break; + case sdk::BuiltinObjectType::kGridMap: + attached = attach.template operator()(); + break; default: break; } diff --git a/pj_plugins/tests/message_parser_functional_extension_test.cpp b/pj_plugins/tests/message_parser_functional_extension_test.cpp index ba6c638a..d3bd6987 100644 --- a/pj_plugins/tests/message_parser_functional_extension_test.cpp +++ b/pj_plugins/tests/message_parser_functional_extension_test.cpp @@ -13,6 +13,7 @@ #include #include +#include "pj_base/builtin/grid_map_codec.hpp" #include "pj_base/builtin/image.hpp" #include "pj_base/parser_functional_protocol.h" #include "pj_plugins/host/message_parser_handle.hpp" @@ -179,6 +180,17 @@ bool emitSplicedPointCloudV2( PJ_bytes_view_t{kPointCloudWire.data(), kPointCloudWire.size()}, 9, 1, 2, out_error); } +// Header-only PJ.GridMap wire: column_count=1, row_count=1, cell_stride=1, +// row_stride=1 (fields 5..8); `data` (field 10) arrives as the splice. +bool emitSplicedGridMapV2( + void*, int64_t, PJ_payload_t, const PJ_parser_object_sink_v2_t* sink, PJ_error_t* out_error) noexcept { + static constexpr std::array kGridMapWire{0x28, 0x01, 0x30, 0x01, 0x38, 0x01, 0x40, 0x01}; + return sink != nullptr && sink->accept_object_spliced != nullptr && + sink->accept_object_spliced( + sink->ctx, true, 89, PJ_BUILTIN_OBJECT_TYPE_GRID_MAP, + PJ_bytes_view_t{kGridMapWire.data(), kGridMapWire.size()}, 10, 1, 2, out_error); +} + bool emitMismatchedImageV2( void*, int64_t, PJ_payload_t, const PJ_parser_object_sink_v2_t* sink, PJ_error_t* out_error) noexcept { static constexpr std::array kImageWire{0x10, 0x01}; @@ -206,7 +218,7 @@ const PJ_message_parser_vtable_t* adversarialVtable() { return &vtable; } -template +template const PJ_message_parser_vtable_t* adversarialV2Vtable() { static const PJ_parser_functional_v2_t extension{ .struct_size = sizeof(PJ_parser_functional_v2_t), @@ -220,7 +232,7 @@ const PJ_message_parser_vtable_t* adversarialV2Vtable() { if (out == nullptr) { return false; } - out->object_type = PJ_BUILTIN_OBJECT_TYPE_POINTCLOUD; + out->object_type = ClassifiedType; out->reserved = 0; return true; }; @@ -445,6 +457,21 @@ TEST(MessageParserFunctionalExtension, HostV2PathReconstructsEligibleSplices) { EXPECT_EQ(cloud->data[1], 30U); } +TEST(MessageParserFunctionalExtension, HostV2PathReconstructsGridMapSplice) { + PJ::MessageParserHandle handle(adversarialV2Vtable()); + ASSERT_TRUE(handle.bindSchema("example/GridMap", {})); + const std::array payload{10, 20, 30, 40}; + auto record = handle.parseObjectFunctional(0, PJ::Span(payload)); + ASSERT_TRUE(record.has_value()) << record.error(); + EXPECT_EQ(record->ts, 89); + const auto* grid = std::any_cast(&record->object); + ASSERT_NE(grid, nullptr); + ASSERT_EQ(grid->data.size(), 2U); + EXPECT_EQ(grid->data[0], 20U); + EXPECT_EQ(grid->data[1], 30U); + EXPECT_TRUE(PJ::validateGridMap(*grid).has_value()); +} + TEST(MessageParserFunctionalExtension, HostRejectsObjectTypeThatDiffersFromBindingClassification) { PJ::MessageParserHandle handle(adversarialV2Vtable()); ASSERT_TRUE(handle.bindSchema("example/PointCloud", {})); diff --git a/pj_plugins/tests/native_parser_module_fixture.cpp b/pj_plugins/tests/native_parser_module_fixture.cpp index 612921dc..23389008 100644 --- a/pj_plugins/tests/native_parser_module_fixture.cpp +++ b/pj_plugins/tests/native_parser_module_fixture.cpp @@ -13,6 +13,7 @@ #include #include +#include "pj_base/builtin/grid_map_codec.hpp" #include "pj_base/builtin/point_cloud_codec.hpp" #include "pj_base/builtin_object_abi.h" #include "pj_base/parser_module_abi.h" @@ -45,7 +46,8 @@ constexpr char kManifest[] = R"({ {"claim_id":"splice-ineligible","encoding":"protobuf","type_name":"fixture.SpliceIneligible","routes":["object"],"object_type":"kPointCloud","priority":0}, {"claim_id":"bad-token","encoding":"protobuf","type_name":"fixture.BadToken","routes":["scalar"],"priority":0}, {"claim_id":"route-mismatch","encoding":"protobuf","type_name":"fixture.RouteMismatch","routes":["scalar"],"priority":0}, - {"claim_id":"type-mismatch","encoding":"protobuf","type_name":"fixture.TypeMismatch","routes":["object"],"object_type":"kPointCloud","priority":0} + {"claim_id":"type-mismatch","encoding":"protobuf","type_name":"fixture.TypeMismatch","routes":["object"],"object_type":"kPointCloud","priority":0}, + {"claim_id":"splice-grid-map","encoding":"protobuf","type_name":"fixture.SpliceGridMap","routes":["object"],"object_type":"kGridMap","priority":0} ] })"; @@ -226,6 +228,22 @@ PJ_FIXTURE_EXPORT int32_t pj_module_parse( .wire = {}, }; break; + case kSpliceGridMap: { + PJ::sdk::GridMap grid; // header only: the two cell bytes arrive as the splice + grid.column_count = 2; + grid.row_count = 1; + grid.cell_stride = 1; + grid.row_stride = 2; + grid.fields.push_back( + {.name = "cost", .offset = 0, .datatype = PJ::sdk::PointField::Datatype::kUint8, .count = 1}); + wire = PJ::serializeGridMap(grid); + descriptor = PJ::parser_module::ObjectOutputV1{ + .object_type = PJ_BUILTIN_OBJECT_TYPE_GRID_MAP, + .splice = PJ::parser_module::ObjectSpliceV1{.field_number = 10, .input_offset = 1, .input_length = 2}, + .wire = wire, + }; + break; + } default: { const std::array point_data{1, 2, 3, 4}; PJ::sdk::PointCloud cloud; diff --git a/pj_plugins/tests/native_parser_module_fixture.hpp b/pj_plugins/tests/native_parser_module_fixture.hpp index e12d168a..0827193b 100644 --- a/pj_plugins/tests/native_parser_module_fixture.hpp +++ b/pj_plugins/tests/native_parser_module_fixture.hpp @@ -22,7 +22,8 @@ enum ClaimIndex : uint32_t { kBadToken = 9, kRouteMismatch = 10, kTypeMismatch = 11, - kClaimCount = 12, + kSpliceGridMap = 12, + kClaimCount = 13, }; } // namespace pj_fixture diff --git a/pj_plugins/tests/parser_module_runtime_test.cpp b/pj_plugins/tests/parser_module_runtime_test.cpp index 2ec65341..2a01a779 100644 --- a/pj_plugins/tests/parser_module_runtime_test.cpp +++ b/pj_plugins/tests/parser_module_runtime_test.cpp @@ -12,6 +12,7 @@ #include #include "native_parser_module_fixture.hpp" +#include "pj_base/builtin/grid_map_codec.hpp" #include "pj_base/builtin/point_cloud.hpp" #include "pj_base/builtin_object_abi.h" #include "pj_base/span.hpp" @@ -181,6 +182,25 @@ TEST(ParserModuleRuntime, AcceptsEligibleSpliceAndRejectsInvalidReferences) { EXPECT_NE(ineligible_result->message.find("not eligible"), std::string::npos); } +TEST(ParserModuleRuntime, AttachesGridMapSpliceToTheDecodedHeader) { + auto module = loadFixture(); + auto bound = createBound(module, kSpliceGridMap, parser_module::Route::kObject, PJ_BUILTIN_OBJECT_TYPE_GRID_MAP); + auto result = bound.parse(input()); + ASSERT_TRUE(result.has_value()) << result.error(); + ASSERT_EQ(result->fault, ParserModuleFaultKind::kNone) << result->message; + const auto* object = std::get_if(&*result->output); + ASSERT_NE(object, nullptr); + ASSERT_TRUE(object->splice.has_value()); + EXPECT_EQ(object->splice->field_number, 10U); + const auto* grid = std::any_cast(&object->object); + ASSERT_NE(grid, nullptr); + EXPECT_EQ(grid->column_count, 2U); + ASSERT_EQ(grid->data.size(), 2U); + EXPECT_EQ(grid->data[0], 20U); + EXPECT_EQ(grid->data[1], 30U); + EXPECT_TRUE(validateGridMap(*grid).has_value()); +} + TEST(ParserModuleRuntime, StrikeTrackerQuarantinesReplaysAndThenDisables) { auto module = loadFixture(); const ParserModuleClaimKey key{"org.plotjuggler.test.native-module", "malformed"};