From 01df94678a438629e29d145cb12d0cef63265431 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 2 Sep 2026 21:38:59 +0200 Subject: [PATCH 1/2] feat(pj_base): add GridMap canonical builtin object (SDK 0.26.0) sdk::GridMap 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, mirroring foxglove.Grid plus an explicit row_count so producers with that layout hand `data` over zero-copy; a NaN in a float channel means "no data". - grid_map.hpp + PJ.GridMap wire format (data = field 10) + grid_map_codec: decode rejects a layout the cell math could not index safely (zero stride with cells, row shorter than its columns, data shorter than row_count * row_stride, field past cell_stride) - kGridMap = 20 / PJ_BUILTIN_OBJECT_TYPE_GRID_MAP, splice-table entry, type-erased dispatcher cases - sdk::Vector2 in the geometry vocabulary (PJ.Vector2 already on the wire) - docs: builtin_type.md section, proto README, builtin counts; CHANGELOG; VERSION 0.26.0 (MINOR: additions only) Co-Authored-By: Claude Fable 5.1 --- CHANGELOG.md | 16 ++ CLAUDE.md | 4 +- VERSION | 2 +- docs/builtin_type.md | 47 ++++ pj_base/CLAUDE.md | 4 +- pj_base/CMakeLists.txt | 2 + .../pj_base/builtin/builtin_object.hpp | 10 + .../pj_base/builtin/frame_transforms.hpp | 7 + pj_base/include/pj_base/builtin/grid_map.hpp | 67 +++++ .../pj_base/builtin/grid_map_codec.hpp | 28 ++ pj_base/include/pj_base/builtin_object_abi.h | 4 +- pj_base/proto/pj/GridMap.proto | 65 +++++ pj_base/proto/pj/README.md | 2 + pj_base/src/builtin/builtin_object_codec.cpp | 5 + pj_base/src/builtin/geometry_codec.hpp | 28 ++ pj_base/src/builtin/grid_map_codec.cpp | 241 ++++++++++++++++++ pj_base/tests/abi_layout_sentinels_test.cpp | 1 + pj_base/tests/builtin_object_test.cpp | 3 + pj_base/tests/grid_map_codec_test.cpp | 143 +++++++++++ pj_base/tests/object_topic_metadata_test.cpp | 1 + pj_base/tests/parser_module_abi_test.cpp | 3 +- 21 files changed, 676 insertions(+), 7 deletions(-) create mode 100644 pj_base/include/pj_base/builtin/grid_map.hpp create mode 100644 pj_base/include/pj_base/builtin/grid_map_codec.hpp create mode 100644 pj_base/proto/pj/GridMap.proto create mode 100644 pj_base/src/builtin/grid_map_codec.cpp create mode 100644 pj_base/tests/grid_map_codec_test.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 97d7d09b..64be840d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,6 +3,22 @@ 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, mirroring `foxglove.Grid` (plus an +explicit `row_count`) 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`, which rejects a layout the cell math could not +index safely), an entry in the type-erased dispatcher and in the frozen splice table +(`data` = field 10), and `sdk::Vector2` in the geometry vocabulary (already present as +`PJ.Vector2` on the wire). 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..2f29a7fc 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 ``` @@ -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,48 @@ 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 layout mirrors `foxglove.Grid` (plus an explicit +`row_count`) so a parser can hand out `data` zero-copy; 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 zero stride with cells +declared, a row shorter than its columns, `data` shorter than +`row_count * row_stride`, or a field reaching past `cell_stride`. + ## Conversion Examples | Source type | Canonical builtin type | Conversion intent | @@ -637,6 +682,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/`fields` into the struct; keep cell bytes zero-copy. `row_count` is `data.size / row_stride`. | +| 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..5ac2f582 --- /dev/null +++ b/pj_base/include/pj_base/builtin/grid_map.hpp @@ -0,0 +1,67 @@ +/** + * @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 layout mirrors +/// `foxglove.Grid` so a parser can hand out `data` as a zero-copy view; +/// 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 (>= sum of field element sizes). + 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..bc1cf8d5 --- /dev/null +++ b/pj_base/include/pj_base/builtin/grid_map_codec.hpp @@ -0,0 +1,28 @@ +#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: +/// a zero stride with cells declared, a row shorter than its columns, `data` +/// shorter than `row_count * row_stride`, or a field reaching past `cell_stride`. +[[nodiscard]] Expected deserializeGridMap(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 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/proto/pj/GridMap.proto b/pj_base/proto/pj/GridMap.proto new file mode 100644 index 00000000..f858c465 --- /dev/null +++ b/pj_base/proto/pj/GridMap.proto @@ -0,0 +1,65 @@ +// 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. Mirrors foxglove.Grid field for field (plus an explicit +// row_count) so a parser can expose `data` as a zero-copy view. +// +// 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 (>= sum of field element sizes) + 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..747909dc 100644 --- a/pj_base/src/builtin/geometry_codec.hpp +++ b/pj_base/src/builtin/geometry_codec.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..087bf8fa --- /dev/null +++ b/pj_base/src/builtin/grid_map_codec.cpp @@ -0,0 +1,241 @@ +// 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 "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; + +// ---------- PointField wire mapping ---------- +// Numerically identical to the proto enum (UNKNOWN=0 .. FLOAT64=8) and a +// mirror of the helpers in point_cloud_codec.cpp / voxel_grid_codec.cpp. + +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; + } +} + +uint64_t datatypeSize(PointField::Datatype dt) { + switch (dt) { + case PointField::Datatype::kInt8: + case PointField::Datatype::kUint8: + return 1; + case PointField::Datatype::kInt16: + case PointField::Datatype::kUint16: + return 2; + case PointField::Datatype::kInt32: + case PointField::Datatype::kUint32: + case PointField::Datatype::kFloat32: + return 4; + case PointField::Datatype::kFloat64: + return 8; + case PointField::Datatype::kUnknown: + default: + return 0; + } +} + +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 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 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: + return readVarintInto(r, tag.type, out.offset); + 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: + return readVarintInto(r, tag.type, out.count); + 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, 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; +} + +// A layout the cell math `r*row_stride + c*cell_stride + offset` could not +// index safely. Checked once here so every consumer can trust a decoded grid. +Expected validateLayout(const GridMap& grid) { + const bool has_cells = grid.column_count > 0 && grid.row_count > 0; + if (has_cells) { + if (grid.cell_stride == 0 || grid.row_stride == 0) { + return unexpected(std::string("GridMap wire: zero stride with cells declared")); + } + if (static_cast(grid.row_stride) < static_cast(grid.column_count) * grid.cell_stride) { + return unexpected(std::string("GridMap wire: row_stride shorter than column_count * cell_stride")); + } + if (static_cast(grid.data.size()) < static_cast(grid.row_count) * grid.row_stride) { + return unexpected(std::string("GridMap wire: data shorter than row_count * row_stride")); + } + } + if (grid.cell_stride > 0) { + for (const auto& field : grid.fields) { + const uint64_t elements = field.count == 0 ? 1 : field.count; + if (static_cast(field.offset) + (datatypeSize(field.datatype) * elements) > grid.cell_stride) { + return unexpected(std::string("GridMap wire: field '") + field.name + "' reaches past cell_stride"); + } + } + } + return {}; +} + +} // namespace + +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) { 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 && 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")); + } + if (auto valid = validateLayout(grid); !valid) { + return unexpected(std::move(valid).error()); + } + + return grid; +} + +} // namespace PJ 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_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..280af62d --- /dev/null +++ b/pj_base/tests/grid_map_codec_test.cpp @@ -0,0 +1,143 @@ +// 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()); +} + +} // 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); From f9f328896306386dc4401cadee40efb4e28d1d6c Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 2 Sep 2026 22:08:30 +0200 Subject: [PATCH 2/2] fix(pj_base): make GridMap decode splice-safe, add host attachment and ObjectWriter builder Review follow-up for the GridMap builtin: - The functional-v2 splice path decodes the canonical wire before the bulk bytes are attached, so a decode-time data-length check rejected every spliced GridMap. A wire without `data` now decodes with an empty span and the full layout check lives in validateGridMap(), run once bytes are in place. Field validation is unconditional and rejects unknown datatypes and zero counts (bytesPerElement instead of a local size table). - Both host splice-attachment switches gain the kGridMap case, and the parser-module ObjectWriter gains GridMapBuilder / gridMap() (object type 20, data field 10), with builder, splice and host-path tests for each. - PointField wire helpers shared by the PointCloud, VoxelGrid and GridMap codecs move to one private header (point_field_codec.hpp). - Docs: the packed cell layout matches foxglove.Grid, not the wire format or field descriptors (PackedElementField datatype numbering differs, count is always 1); cell_stride bound is the largest field end; pose composition wording; dispatcher invariant tests and the plugin guide count 18 builtins. Co-Authored-By: Claude Fable 5.1 --- .../references/builtin-objects.md | 4 +- CHANGELOG.md | 17 +- docs/builtin_type.md | 21 ++- pj_base/include/pj_base/builtin/grid_map.hpp | 13 +- .../pj_base/builtin/grid_map_codec.hpp | 14 +- .../pj_base/parser_module/object_writer.hpp | 148 +++++++++++++++++ pj_base/proto/pj/GridMap.proto | 11 +- pj_base/src/builtin/geometry_codec.hpp | 4 +- pj_base/src/builtin/grid_map_codec.cpp | 154 ++++-------------- pj_base/src/builtin/point_cloud_codec.cpp | 103 +----------- pj_base/src/builtin/point_field_codec.hpp | 102 ++++++++++++ pj_base/src/builtin/voxel_grid_codec.cpp | 101 +----------- pj_base/tests/builtin_object_codec_test.cpp | 6 +- pj_base/tests/grid_map_codec_test.cpp | 53 +++++- .../parser_module_object_writer_test.cpp | 35 ++++ .../pj_plugins/host/message_parser_handle.hpp | 3 + pj_plugins/src/parser_module_runtime.cpp | 3 + ...ssage_parser_functional_extension_test.cpp | 31 +++- .../tests/native_parser_module_fixture.cpp | 20 ++- .../tests/native_parser_module_fixture.hpp | 3 +- .../tests/parser_module_runtime_test.cpp | 20 +++ 21 files changed, 512 insertions(+), 354 deletions(-) create mode 100644 pj_base/src/builtin/point_field_codec.hpp 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 64be840d..54127ce7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,13 +11,16 @@ All notable changes to `plotjuggler_sdk` are recorded here. Versioning policy is `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, mirroring `foxglove.Grid` (plus an -explicit `row_count`) 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`, which rejects a layout the cell math could not -index safely), an entry in the type-erased dispatcher and in the frozen splice table -(`data` = field 10), and `sdk::Vector2` in the geometry vocabulary (already present as -`PJ.Vector2` on the wire). Additive: no existing struct, slot, or wire format changes. +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] diff --git a/docs/builtin_type.md b/docs/builtin_type.md index 2f29a7fc..2c10395b 100644 --- a/docs/builtin_type.md +++ b/docs/builtin_type.md @@ -99,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 @@ -632,9 +632,12 @@ 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 layout mirrors `foxglove.Grid` (plus an explicit -`row_count`) so a parser can hand out `data` zero-copy; a producer with another -layout (grid_map's column-major ring buffer) transcodes once at the boundary. +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 @@ -659,9 +662,13 @@ Cell `(c, r)` has its center at `origin ∘ ((c + .5)*cell_size.x, (r + .5)*cell `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 zero stride with cells +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`, or a field reaching past `cell_stride`. +`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 @@ -682,7 +689,7 @@ declared, a row shorter than its columns, `data` shorter than | 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/`fields` into the struct; keep cell bytes zero-copy. `row_count` is `data.size / row_stride`. | +| `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 diff --git a/pj_base/include/pj_base/builtin/grid_map.hpp b/pj_base/include/pj_base/builtin/grid_map.hpp index 5ac2f582..ae88c784 100644 --- a/pj_base/include/pj_base/builtin/grid_map.hpp +++ b/pj_base/include/pj_base/builtin/grid_map.hpp @@ -30,10 +30,13 @@ namespace sdk { /// 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 layout mirrors -/// `foxglove.Grid` so a parser can hand out `data` as a zero-copy view; -/// producers with another layout (grid_map's column-major ring buffer) -/// transcode once at the boundary. +/// 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) @@ -56,7 +59,7 @@ struct GridMap { 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 (>= sum of field element sizes). + 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; diff --git a/pj_base/include/pj_base/builtin/grid_map_codec.hpp b/pj_base/include/pj_base/builtin/grid_map_codec.hpp index bc1cf8d5..1fbfe713 100644 --- a/pj_base/include/pj_base/builtin/grid_map_codec.hpp +++ b/pj_base/include/pj_base/builtin/grid_map_codec.hpp @@ -20,9 +20,17 @@ inline constexpr std::string_view kSchemaGridMap = "PJ.GridMap"; [[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: -/// a zero stride with cells declared, a row shorter than its columns, `data` -/// shorter than `row_count * row_stride`, or a field reaching past `cell_stride`. +/// 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/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 index f858c465..1b67d5e5 100644 --- a/pj_base/proto/pj/GridMap.proto +++ b/pj_base/proto/pj/GridMap.proto @@ -15,8 +15,11 @@ package PJ; // A 2D grid whose cells carry named channels — the layered, generic-valued // sibling of OccupancyGrid: elevation maps, multi-layer costmaps, terrain -// classification. Mirrors foxglove.Grid field for field (plus an explicit -// row_count) so a parser can expose `data` as a zero-copy view. +// 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 @@ -24,7 +27,7 @@ package PJ; // 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)` +// 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 @@ -51,7 +54,7 @@ message GridMap { // Number of cells along the local y axis uint32 row_count = 6; - // Bytes per cell record (>= sum of field element sizes) + // Bytes per cell record (>= the largest field end: offset + element size * count) uint32 cell_stride = 7; // Bytes per row (>= column_count * cell_stride) diff --git a/pj_base/src/builtin/geometry_codec.hpp b/pj_base/src/builtin/geometry_codec.hpp index 747909dc..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" diff --git a/pj_base/src/builtin/grid_map_codec.cpp b/pj_base/src/builtin/grid_map_codec.cpp index 087bf8fa..c5aaea7b 100644 --- a/pj_base/src/builtin/grid_map_codec.cpp +++ b/pj_base/src/builtin/grid_map_codec.cpp @@ -10,6 +10,7 @@ #include #include "geometry_codec.hpp" +#include "point_field_codec.hpp" #include "protobuf_wire.hpp" namespace PJ { @@ -23,65 +24,6 @@ using builtin_wire::Writer; using sdk::GridMap; using sdk::PointField; -// ---------- PointField wire mapping ---------- -// Numerically identical to the proto enum (UNKNOWN=0 .. FLOAT64=8) and a -// mirror of the helpers in point_cloud_codec.cpp / voxel_grid_codec.cpp. - -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; - } -} - -uint64_t datatypeSize(PointField::Datatype dt) { - switch (dt) { - case PointField::Datatype::kInt8: - case PointField::Datatype::kUint8: - return 1; - case PointField::Datatype::kInt16: - case PointField::Datatype::kUint16: - return 2; - case PointField::Datatype::kInt32: - case PointField::Datatype::kUint32: - case PointField::Datatype::kFloat32: - return 4; - case PointField::Datatype::kFloat64: - return 8; - case PointField::Datatype::kUnknown: - default: - return 0; - } -} - -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 readVarintInto(Reader& reader, WireType type, uint32_t& out) { if (type != WireType::kVarint) { return false; @@ -94,45 +36,6 @@ bool readVarintInto(Reader& reader, WireType type, uint32_t& out) { return true; } -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: - return readVarintInto(r, tag.type, out.offset); - 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: - return readVarintInto(r, tag.type, out.count); - 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, GridMap& out) { const uint8_t* data = nullptr; size_t size = 0; @@ -145,34 +48,45 @@ bool readBytesIntoGrid(Reader& reader, GridMap& out) { return true; } -// A layout the cell math `r*row_stride + c*cell_stride + offset` could not -// index safely. Checked once here so every consumer can trust a decoded grid. -Expected validateLayout(const GridMap& grid) { - const bool has_cells = grid.column_count > 0 && grid.row_count > 0; - if (has_cells) { - if (grid.cell_stride == 0 || grid.row_stride == 0) { - return unexpected(std::string("GridMap wire: zero stride with cells declared")); +// `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 (static_cast(grid.row_stride) < static_cast(grid.column_count) * grid.cell_stride) { - return unexpected(std::string("GridMap wire: row_stride shorter than column_count * cell_stride")); + if (field.count == 0) { + return unexpected(std::string("GridMap: field '") + field.name + "' has a zero count"); } - if (static_cast(grid.data.size()) < static_cast(grid.row_count) * grid.row_stride) { - return unexpected(std::string("GridMap wire: data shorter than row_count * row_stride")); + 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"); } } - if (grid.cell_stride > 0) { - for (const auto& field : grid.fields) { - const uint64_t elements = field.count == 0 ? 1 : field.count; - if (static_cast(field.offset) + (datatypeSize(field.datatype) * elements) > grid.cell_stride) { - return unexpected(std::string("GridMap wire: 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); @@ -186,7 +100,7 @@ std::vector serializeGridMap(const GridMap& grid) { writer.varint(7, grid.cell_stride); writer.varint(8, grid.row_stride); for (const auto& field : grid.fields) { - writer.message(9, [&](Writer& nested) { writePointField(nested, field); }); + writer.message(9, [&](Writer& nested) { builtin_wire::writePointField(nested, field); }); } writer.bytes(10, grid.data.data(), grid.data.size()); @@ -220,7 +134,7 @@ Expected deserializeGridMap(const uint8_t* data, size_t size) { case 8: return readVarintInto(r, tag.type, grid.row_stride); case 9: - return tag.type == WireType::kLengthDelimited && readPointFieldIntoVector(r, grid.fields); + return tag.type == WireType::kLengthDelimited && builtin_wire::readPointFieldIntoVector(r, grid.fields); case 10: return tag.type == WireType::kLengthDelimited && readBytesIntoGrid(r, grid); default: @@ -231,7 +145,9 @@ Expected deserializeGridMap(const uint8_t* data, size_t size) { if (!ok) { return unexpected(std::string("GridMap wire: decode failed")); } - if (auto valid = validateLayout(grid); !valid) { + // 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()); } 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/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/grid_map_codec_test.cpp b/pj_base/tests/grid_map_codec_test.cpp index 280af62d..894e3252 100644 --- a/pj_base/tests/grid_map_codec_test.cpp +++ b/pj_base/tests/grid_map_codec_test.cpp @@ -47,7 +47,7 @@ GridMap makeGrid(std::vector& storage) { 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)); + &value, grid.data.data() + (row * grid.row_stride) + (column * grid.cell_stride) + field_offset, sizeof(float)); return value; } @@ -139,5 +139,56 @@ TEST(GridMapCodecTest, RejectsRowStrideShorterThanColumns) { 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/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"};