diff --git a/CLAUDE.md b/CLAUDE.md index c0764b2..e33b1eb 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -15,9 +15,9 @@ two modules below have no own CLAUDE.md. ### Modules - **pj_base** — vocabulary types (`Timestamp`, `DatasetId`, `Expected`, `Span`, type trees), - the canonical builtin object vocabulary (`pj_base/builtin/`: 15 struct headers — Image, DepthImage, + the canonical builtin object vocabulary (`pj_base/builtin/`: 16 struct headers — Image, DepthImage, PointCloud, CompressedPointCloud, OccupancyGrid(+Update), Mesh3D, VideoFrame, AssetVideo, - SceneEntities, RobotDescription, CameraInfo, Log, ImageAnnotations, FrameTransforms) and their 14 + SceneEntities, RobotDescription, CameraInfo, Log, ImageAnnotations, FrameTransforms, PosesInFrame) and their 15 wire codecs (RobotDescription carries source text as-is — no codec), the C-ABI protocol headers for DataSource/MessageParser/Toolbox + the C++ SDK base classes / host-view helpers built on them. - **pj_plugins** — host-side loaders + RAII handles + plugin discovery/catalog for four plugin diff --git a/CMakeLists.txt b/CMakeLists.txt index 684fde6..6b4238e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -111,7 +111,7 @@ endif() if(PJ_INSTALL_SDK) include(CMakePackageConfigHelpers) - set(PJ_PACKAGE_VERSION "0.7.0") + set(PJ_PACKAGE_VERSION "0.8.0") set(PJ_PACKAGE_CMAKE_DIR ${CMAKE_INSTALL_LIBDIR}/cmake/plotjuggler_sdk) install(EXPORT plotjuggler_sdkTargets diff --git a/conanfile.py b/conanfile.py index 5accb29..8299149 100644 --- a/conanfile.py +++ b/conanfile.py @@ -6,7 +6,7 @@ plugin_sdk — umbrella for plugin authors (base + dialog SDK + parser SDK) plugin_host — umbrella for host loaders (data_source/parser/toolbox/dialog) -A consuming Conan recipe declares e.g. `plotjuggler_sdk/0.7.0` and then: +A consuming Conan recipe declares e.g. `plotjuggler_sdk/0.8.0` and then: find_package(plotjuggler_sdk REQUIRED COMPONENTS plugin_sdk) target_link_libraries(my_plugin PRIVATE plotjuggler_sdk::plugin_sdk) @@ -30,7 +30,7 @@ class PlotjugglerSdkConan(ConanFile): name = "plotjuggler_sdk" - version = "0.7.0" + version = "0.8.0" # Apache-2.0 covers the whole SDK (pj_base + pj_plugins). See LICENSE. license = "Apache-2.0" url = "https://github.com/PlotJuggler/plotjuggler_sdk" diff --git a/docs/builtin_type.md b/docs/builtin_type.md index 7b89fe0..84ca45b 100644 --- a/docs/builtin_type.md +++ b/docs/builtin_type.md @@ -27,6 +27,7 @@ The public headers live under: #include #include #include +#include // Codecs — one per type, all share the canonical PJ. wire format under pj_base/proto/pj/. #include #include @@ -39,6 +40,7 @@ The public headers live under: #include #include #include +#include ``` ## Design Principles @@ -84,7 +86,7 @@ Builtin objects fall into two serialization families: | Family | Current types | Storage model | Codec policy | |--------|---------------|---------------|--------------| | Byte-backed views | `Image`, `DepthImage`, `PointCloud`, `CompressedPointCloud`, `OccupancyGrid`, `OccupancyGridUpdate`, `Mesh3D`, `VideoFrame` | Header fields live in the SDK struct; payload bytes live behind `Span` plus `BufferAnchor`. | No mandatory canonical codec; preserve zero-copy views over ROS, MCAP, compressed image, point-cloud, or plugin-owned payloads. If conversion is unavoidable, allocate a new payload and anchor it. | -| Owned values | `ImageAnnotations`, `FrameTransforms`, `SceneEntities`, `AssetVideo`, `RobotDescription`, `CameraInfo`; future marker types | SDK structs own their vectors/strings/scalars directly. | Add explicit codecs when canonical bytes are needed. Codecs serialize the owned value to the protobuf-wire payload described by the `.proto` contract, using shared private wire primitives. `RobotDescription` carries source-format text as-is (no canonical codec) — the format hint distinguishes URDF / SDF / MJCF. | +| Owned values | `ImageAnnotations`, `FrameTransforms`, `SceneEntities`, `AssetVideo`, `RobotDescription`, `CameraInfo`, `PosesInFrame`; future marker types | SDK structs own their vectors/strings/scalars directly. | Add explicit codecs when canonical bytes are needed. Codecs serialize the owned value to the protobuf-wire payload described by the `.proto` contract, using shared private wire primitives. `RobotDescription` carries source-format text as-is (no canonical codec) — the format hint distinguishes URDF / SDF / MJCF. | Canonical `.proto` files live under `pj_base/proto/pj` and act as the wire format contract. One file per top-level message, each named after its message @@ -123,6 +125,7 @@ annotations, frame transforms, or no builtin object. | `kCameraInfo` | `PJ::sdk::CameraInfo` | Pinhole camera calibration (intrinsics K, distortion D, rectification R, projection P). | | `kOccupancyGridUpdate` | `PJ::sdk::OccupancyGridUpdate` | Incremental sub-rectangle patch for a previously-published `OccupancyGrid`. | | `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. | `BuiltinObject` is `std::any`. Producers store a concrete builtin value in it; consumers recover the concrete type with `std::any_cast(&object)` or ask @@ -569,6 +572,27 @@ Foxglove's source-location fields (`file`, `line`) are intentionally omitted. `pj_base/builtin/log_codec.hpp` serializes and deserializes this type using the canonical `PJ.Log` protobuf wire format. +## PosesInFrame + +`PosesInFrame` is an array of poses expressed in a single reference frame at one +instant — ROS `geometry_msgs/PoseArray`, `foxglove.PosesInFrame` (which it +mirrors field-for-field), AMCL particle clouds, candidate grasp sets, sampled +trajectories. + +It deliberately carries **no styling**: how the poses are drawn (arrows vs +triads, scale, color) is a viewer-side decision, exactly as the source messages +carry none. It is an owned value (strings and scalar geometry, no byte blob), +so no `BufferAnchor` is needed. + +| Field | Type | Notes | +|-------|------|-------| +| `timestamp_ns` | `Timestamp` | Time of the poses. `0` when the source had no timestamp. | +| `frame_id` | `std::string` | Frame all poses are expressed in. | +| `poses` | `std::vector` | The poses. Order is meaningful only to the producer. | + +`pj_base/builtin/poses_in_frame_codec.hpp` serializes and deserializes this +type using the canonical `PJ.PosesInFrame` protobuf wire format. + ## Conversion Examples | Source type | Canonical builtin type | Conversion intent | @@ -584,6 +608,7 @@ canonical `PJ.Log` protobuf wire format. | MP4 / MKV / AV1 dataset file | `AssetVideo` | Push once per topic with the file path and metadata; consumers seek into the file by tracker time. | | Detection or tracking message | `ImageAnnotations` | Convert boxes, points, circles, and labels into pixel-space primitives. | | ROS `tf2_msgs/TFMessage` | `FrameTransforms` | Convert transform batches into named parent/child frame relationships. | +| ROS `geometry_msgs/PoseArray` | `PosesInFrame` | Forward timestamp, frame, and the pose array; the viewer chooses how to draw them. | | ROS `std_msgs/String` on `/robot_description` (or matching name) carrying URDF XML | `RobotDescription` | Validate root element matches `format`, then carry the raw text + format hint. No mesh resolution at parse time. | | ROS `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. | diff --git a/pj_base/CLAUDE.md b/pj_base/CLAUDE.md index ddd93e1..1b25bd7 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 canonical *builtin object* schemas (`sdk::Image`, `PointCloud`, `DepthImage`, `OccupancyGrid`, `FrameTransforms`, … — 15 types) **and 14 of their wire codecs** (RobotDescription has none); 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`. 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 canonical *builtin object* schemas (`sdk::Image`, `PointCloud`, `DepthImage`, `OccupancyGrid`, `FrameTransforms`, … — 16 types) **and 15 of their wire codecs** (RobotDescription has none); 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`. 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`, `type_tree.hpp`, `dataset.hpp`, `expected.hpp`, `span.hpp`, `number_parse.hpp`, `assert.hpp`, `diagnostic_sink.hpp`, `buffer_anchor.hpp`. -- `include/pj_base/builtin/` — the 15 builtin object struct headers (`*.hpp`; 16 enum values in `BuiltinObjectType`, value 2 reserved) + their 14 wire codecs (`*_codec.hpp`; RobotDescription has none) + the `BuiltinObject` (`std::any`) type-erased holder. +- `include/pj_base/builtin/` — the 16 builtin object struct headers (`*.hpp`; 17 enum values in `BuiltinObjectType`, value 2 reserved) + their 15 wire codecs (`*_codec.hpp`; RobotDescription has none) + the `BuiltinObject` (`std::any`) type-erased holder. - `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 d07808a..9c94b72 100644 --- a/pj_base/CMakeLists.txt +++ b/pj_base/CMakeLists.txt @@ -15,6 +15,7 @@ add_library(pj_base STATIC src/builtin/occupancy_grid_codec.cpp src/builtin/occupancy_grid_update_codec.cpp src/builtin/point_cloud_codec.cpp + src/builtin/poses_in_frame_codec.cpp src/builtin/scene_entities_codec.cpp src/builtin/video_frame_codec.cpp src/number_parse.cpp @@ -101,6 +102,7 @@ if(PJ_BUILD_TESTS) tests/video_frame_codec_test.cpp tests/scene_entities_codec_test.cpp tests/asset_video_codec_test.cpp + tests/poses_in_frame_codec_test.cpp ) foreach(test_src ${PJ_BASE_TESTS}) diff --git a/pj_base/include/pj_base/builtin/builtin_object.hpp b/pj_base/include/pj_base/builtin/builtin_object.hpp index 2c0ebda..b08822b 100644 --- a/pj_base/include/pj_base/builtin/builtin_object.hpp +++ b/pj_base/include/pj_base/builtin/builtin_object.hpp @@ -38,6 +38,7 @@ #include "pj_base/builtin/occupancy_grid.hpp" #include "pj_base/builtin/occupancy_grid_update.hpp" #include "pj_base/builtin/point_cloud.hpp" +#include "pj_base/builtin/poses_in_frame.hpp" #include "pj_base/builtin/robot_description.hpp" #include "pj_base/builtin/scene_entities.hpp" #include "pj_base/builtin/video_frame.hpp" @@ -62,6 +63,7 @@ enum class BuiltinObjectType : uint16_t { kCameraInfo = 14, ///< sdk::CameraInfo — pinhole camera calibration (K/D/R/P). kOccupancyGridUpdate = 15, ///< sdk::OccupancyGridUpdate — incremental sub-rectangle patch for an OccupancyGrid. kLog = 16, ///< sdk::Log — textual log message (level + text + name). + kPosesInFrame = 17, ///< sdk::PosesInFrame — array of poses in one reference frame. }; /// A-priori classification of a schema. Currently carries only the type; @@ -106,6 +108,8 @@ struct SchemaClassification { return "kOccupancyGridUpdate"; case BuiltinObjectType::kLog: return "kLog"; + case BuiltinObjectType::kPosesInFrame: + return "kPosesInFrame"; } return "kNone"; } @@ -161,6 +165,9 @@ struct SchemaClassification { if (s == "kLog") { return BuiltinObjectType::kLog; } + if (s == "kPosesInFrame") { + return BuiltinObjectType::kPosesInFrame; + } return std::nullopt; } @@ -219,6 +226,9 @@ using BuiltinObject = std::any; if (t == typeid(Log)) { return BuiltinObjectType::kLog; } + if (t == typeid(PosesInFrame)) { + return BuiltinObjectType::kPosesInFrame; + } return BuiltinObjectType::kNone; } diff --git a/pj_base/include/pj_base/builtin/poses_in_frame.hpp b/pj_base/include/pj_base/builtin/poses_in_frame.hpp new file mode 100644 index 0000000..88d2534 --- /dev/null +++ b/pj_base/include/pj_base/builtin/poses_in_frame.hpp @@ -0,0 +1,32 @@ +/** + * @file poses_in_frame.hpp + * @brief Array of poses in a single reference frame at one instant. + */ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include "pj_base/builtin/frame_transforms.hpp" // Pose / Vector3 / Quaternion +#include "pj_base/types.hpp" + +namespace PJ { +namespace sdk { + +/// An array of poses in a single reference frame at one instant +/// (geometry_msgs/PoseArray, foxglove.PosesInFrame). Pure data: rendering +/// style (arrow vs triad, size, color) is chosen by the viewer at draw time. +struct PosesInFrame { + /// Acquisition time in nanoseconds (0 when the source had no timestamp). + Timestamp timestamp_ns = 0; + /// Frame all poses are expressed in. + std::string frame_id; + /// The poses. Order is meaningful only to the producer. + std::vector poses; +}; + +} // namespace sdk +} // namespace PJ diff --git a/pj_base/include/pj_base/builtin/poses_in_frame_codec.hpp b/pj_base/include/pj_base/builtin/poses_in_frame_codec.hpp new file mode 100644 index 0000000..6c62116 --- /dev/null +++ b/pj_base/include/pj_base/builtin/poses_in_frame_codec.hpp @@ -0,0 +1,24 @@ +#pragma once +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include "pj_base/builtin/poses_in_frame.hpp" +#include "pj_base/expected.hpp" + +namespace PJ { + +inline constexpr std::string_view kSchemaPosesInFrame = "PJ.PosesInFrame"; + +/// Serializes sdk::PosesInFrame to canonical PJ.PosesInFrame wire bytes +/// (see pj_base/proto/pj/PosesInFrame.proto). +[[nodiscard]] std::vector serializePosesInFrame(const sdk::PosesInFrame& poses); + +/// Decodes canonical PJ.PosesInFrame wire bytes into sdk::PosesInFrame. +[[nodiscard]] Expected deserializePosesInFrame(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 35ec152..b9296ae 100644 --- a/pj_base/include/pj_base/builtin_object_abi.h +++ b/pj_base/include/pj_base/builtin_object_abi.h @@ -56,6 +56,7 @@ typedef enum PJ_builtin_object_type_t { PJ_BUILTIN_OBJECT_TYPE_CAMERA_INFO = 14, PJ_BUILTIN_OBJECT_TYPE_OCCUPANCY_GRID_UPDATE = 15, PJ_BUILTIN_OBJECT_TYPE_LOG = 16, + PJ_BUILTIN_OBJECT_TYPE_POSES_IN_FRAME = 17, /* 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). */ diff --git a/pj_base/proto/pj/PosesInFrame.proto b/pj_base/proto/pj/PosesInFrame.proto new file mode 100644 index 0000000..e9ee82a --- /dev/null +++ b/pj_base/proto/pj/PosesInFrame.proto @@ -0,0 +1,23 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: Apache-2.0 + +syntax = "proto3"; + +import "google/protobuf/timestamp.proto"; +import "pj/Geometry.proto"; + +package PJ; + +// An array of poses expressed in a single reference frame at one instant. +// Mirrors foxglove.PosesInFrame field-for-field. Carries NO styling: how the +// poses are drawn (arrows, triads, scale, color) is a viewer-side decision. +message PosesInFrame { + // Time of the poses. + google.protobuf.Timestamp timestamp = 1; + + // Frame of reference for all poses. + string frame_id = 2; + + // Poses in 3D space. + repeated PJ.Pose poses = 3; +} diff --git a/pj_base/proto/pj/README.md b/pj_base/proto/pj/README.md index 9058916..efddb04 100644 --- a/pj_base/proto/pj/README.md +++ b/pj_base/proto/pj/README.md @@ -60,6 +60,8 @@ rationale. - Batch: `SceneEntities` - **`Mesh3D.proto`** — 3D mesh asset delivered in its native binary format (GLTF/GLB/STL/PLY/OBJ/USD/DAE) for URDF-style or scene-mesh visualization. - `Mesh3D` +- **`PosesInFrame.proto`** — array of poses in a single reference frame at one instant (`geometry_msgs/PoseArray`, particle clouds); mirrors `foxglove.PosesInFrame` field-for-field and carries no styling — rendering is viewer-side. + - `PosesInFrame` ### 2D image annotations (vector overlays) diff --git a/pj_base/src/builtin/poses_in_frame_codec.cpp b/pj_base/src/builtin/poses_in_frame_codec.cpp new file mode 100644 index 0000000..520d6f1 --- /dev/null +++ b/pj_base/src/builtin/poses_in_frame_codec.cpp @@ -0,0 +1,79 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: Apache-2.0 + +#include "pj_base/builtin/poses_in_frame_codec.hpp" + +#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; + +} // namespace + +std::vector serializePosesInFrame(const sdk::PosesInFrame& poses) { + std::vector out; + Writer writer(out); + + writer.message(1, [&](Writer& nested) { builtin_wire::writeTimestamp(nested, poses.timestamp_ns); }); + writer.string(2, poses.frame_id); + for (const sdk::Pose& pose : poses.poses) { + writer.message(3, [&](Writer& nested) { builtin_wire::writePose(nested, pose); }); + } + + return out; +} + +Expected deserializePosesInFrame(const uint8_t* data, size_t size) { + if (data == nullptr || size == 0) { + return unexpected(std::string("PosesInFrame wire: empty buffer")); + } + + Reader reader(data, size); + sdk::PosesInFrame result; + + const bool ok = parseFields(reader, [&](Tag tag, Reader& r) { + switch (tag.field) { + case 1: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return builtin_wire::readTimestampMessage(r, result.timestamp_ns); + case 2: + if (tag.type != WireType::kLengthDelimited) { + return false; + } + return r.readString(result.frame_id); + case 3: { + if (tag.type != WireType::kLengthDelimited) { + return false; + } + sdk::Pose pose; + if (!builtin_wire::readPoseMessage(r, pose)) { + return false; + } + result.poses.push_back(pose); + return true; + } + default: + return false; + } + }); + + if (!ok) { + return unexpected(std::string("PosesInFrame wire: decode failed")); + } + + return result; +} + +} // namespace PJ diff --git a/pj_base/tests/abi_layout_sentinels_test.cpp b/pj_base/tests/abi_layout_sentinels_test.cpp index 9301cf6..c44353a 100644 --- a/pj_base/tests/abi_layout_sentinels_test.cpp +++ b/pj_base/tests/abi_layout_sentinels_test.cpp @@ -121,6 +121,7 @@ static_assert(PJ_BUILTIN_OBJECT_TYPE_ROBOT_DESCRIPTION == 13, "RobotDescription static_assert(PJ_BUILTIN_OBJECT_TYPE_CAMERA_INFO == 14, "CameraInfo type id pinned"); static_assert(PJ_BUILTIN_OBJECT_TYPE_OCCUPANCY_GRID_UPDATE == 15, "OccupancyGridUpdate type id pinned"); 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(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 424e870..916dc61 100644 --- a/pj_base/tests/builtin_object_test.cpp +++ b/pj_base/tests/builtin_object_test.cpp @@ -21,6 +21,7 @@ using PJ::sdk::OccupancyGrid; using PJ::sdk::OccupancyGridUpdate; using PJ::sdk::parseBuiltinObjectType; using PJ::sdk::PointCloud; +using PJ::sdk::PosesInFrame; using PJ::sdk::RobotDescription; using PJ::sdk::SceneEntities; using PJ::sdk::typeOf; @@ -43,6 +44,7 @@ TEST(BuiltinObjectTest, TypeOfRecognizesKnownBuiltinTypes) { EXPECT_EQ(typeOf(BuiltinObject{CameraInfo{}}), BuiltinObjectType::kCameraInfo); EXPECT_EQ(typeOf(BuiltinObject{OccupancyGridUpdate{}}), BuiltinObjectType::kOccupancyGridUpdate); EXPECT_EQ(typeOf(BuiltinObject{Log{}}), BuiltinObjectType::kLog); + EXPECT_EQ(typeOf(BuiltinObject{PosesInFrame{}}), BuiltinObjectType::kPosesInFrame); } TEST(BuiltinObjectTest, NameAndParseRoundTripForEveryEnumEntry) { @@ -63,6 +65,7 @@ TEST(BuiltinObjectTest, NameAndParseRoundTripForEveryEnumEntry) { BuiltinObjectType::kCameraInfo, BuiltinObjectType::kOccupancyGridUpdate, BuiltinObjectType::kLog, + BuiltinObjectType::kPosesInFrame, }) { const auto parsed = parseBuiltinObjectType(name(t)); ASSERT_TRUE(parsed.has_value()) << "parseBuiltinObjectType failed for " << name(t); diff --git a/pj_base/tests/poses_in_frame_codec_test.cpp b/pj_base/tests/poses_in_frame_codec_test.cpp new file mode 100644 index 0000000..b85b8ed --- /dev/null +++ b/pj_base/tests/poses_in_frame_codec_test.cpp @@ -0,0 +1,64 @@ +// Copyright 2026 Davide Faconti +// SPDX-License-Identifier: Apache-2.0 + +#include "pj_base/builtin/poses_in_frame_codec.hpp" + +#include + +#include +#include + +#include "protobuf_wire_test_helpers.hpp" + +namespace PJ { +namespace { + +namespace pb = ::PJ::test_pb; + +TEST(PosesInFrameCodecTest, SchemaName) { + EXPECT_EQ(kSchemaPosesInFrame, "PJ.PosesInFrame"); +} + +TEST(PosesInFrameCodecTest, EmptyBufferProducesError) { + EXPECT_FALSE(deserializePosesInFrame(nullptr, 0).has_value()); +} + +TEST(PosesInFrameCodecTest, RoundTrip) { + sdk::PosesInFrame in; + in.timestamp_ns = 1'781'069'110'760'016'524LL; + in.frame_id = "map"; + in.poses.push_back(sdk::Pose{{1.0, -2.0, 0.5}, {0.0, 0.0, 0.7071067811865476, 0.7071067811865476}}); + in.poses.push_back(sdk::Pose{}); // identity orientation (w=1), zero position + + const auto bytes = serializePosesInFrame(in); + const auto out = deserializePosesInFrame(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_EQ(out->timestamp_ns, in.timestamp_ns); + EXPECT_EQ(out->frame_id, in.frame_id); + ASSERT_EQ(out->poses.size(), 2u); + EXPECT_EQ(out->poses[0], in.poses[0]); + EXPECT_EQ(out->poses[1], in.poses[1]); +} + +TEST(PosesInFrameCodecTest, InvalidNestedMessageProducesError) { + std::vector bytes; + pb::appendTag(bytes, 3, 2); + pb::appendVarint(bytes, 10); + bytes.push_back(0x08); + + auto output = deserializePosesInFrame(bytes.data(), bytes.size()); + EXPECT_FALSE(output.has_value()); +} + +TEST(PosesInFrameCodecTest, EmptyPosesRoundTrip) { + sdk::PosesInFrame in; + in.frame_id = "odom"; + const auto bytes = serializePosesInFrame(in); + const auto out = deserializePosesInFrame(bytes.data(), bytes.size()); + ASSERT_TRUE(out.has_value()); + EXPECT_TRUE(out->poses.empty()); + EXPECT_EQ(out->frame_id, "odom"); +} + +} // namespace +} // namespace PJ