From cf54d9ccb7dd6e8b0daaeeaf19a64b9a2aa5d128 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Thu, 19 Oct 2023 21:41:47 -0700 Subject: [PATCH] [wpiutil, ntcore] Add structured data support (#5391) This adds support for two serialization formats for complex data types: - Protobuf for complex objects with variable length internals that need forward and backward wire compatibility (lower speed, more flexible) - Raw struct (ByteBuffer-style) for fixed-length objects (higher speed, less flexible) Deserialization can be done either by creating a new object (for immutable objects) or overwriting the contents of an existing object (for mutable objects). Implementing classes should provide inner classes that implement the Protobuf or Struct interface (in Java) or specialize the wpi::Protobuf or wpi::Struct struct (in C++). It is possible for classes to implement both. If the class itself does not implement serialization, it's possible for third parties/users to provide an implementation instead. Uses the Google protobuf implementation for C++ and the QuickBuffers alternative protobuf implementation for Java. --- .github/workflows/cmake.yml | 11 +- .github/workflows/sanitizers.yml | 8 +- .styleguide | 1 + CMakeLists.txt | 6 + README-CMAKE.md | 2 + build.gradle | 1 + docs/build.gradle | 2 + glass/.styleguide | 1 + glass/src/libnt/native/cpp/NetworkTables.cpp | 581 +++++++++++++-- .../glass/networktables/NetworkTables.h | 17 +- ntcore/CMakeLists.txt | 5 + .../java/NetworkTableInstance.java.jinja | 207 +++++- .../wpi/first/networktables/NetworkTable.java | 42 ++ .../first/networktables/ProtobufEntry.java | 17 + .../networktables/ProtobufEntryImpl.java | 209 ++++++ .../networktables/ProtobufPublisher.java | 52 ++ .../networktables/ProtobufSubscriber.java | 94 +++ .../first/networktables/ProtobufTopic.java | 178 +++++ .../first/networktables/StructArrayEntry.java | 17 + .../networktables/StructArrayEntryImpl.java | 197 +++++ .../networktables/StructArrayPublisher.java | 52 ++ .../networktables/StructArraySubscriber.java | 79 ++ .../first/networktables/StructArrayTopic.java | 178 +++++ .../wpi/first/networktables/StructEntry.java | 17 + .../first/networktables/StructEntryImpl.java | 207 ++++++ .../first/networktables/StructPublisher.java | 52 ++ .../first/networktables/StructSubscriber.java | 94 +++ .../wpi/first/networktables/StructTopic.java | 177 +++++ .../networktables/TimestampedObject.java | 33 + ntcore/src/main/native/cpp/LocalStorage.cpp | 36 + ntcore/src/main/native/cpp/LocalStorage.h | 10 + .../main/native/cpp/networktables/Topic.cpp | 5 + ntcore/src/main/native/cpp/ntcore_c.cpp | 9 + ntcore/src/main/native/cpp/ntcore_cpp.cpp | 15 + .../include/networktables/NetworkTable.h | 42 ++ .../networktables/NetworkTableInstance.h | 105 +++ .../networktables/NetworkTableInstance.inc | 50 ++ .../include/networktables/ProtobufTopic.h | 474 ++++++++++++ .../include/networktables/StructArrayTopic.h | 593 +++++++++++++++ .../include/networktables/StructTopic.h | 438 +++++++++++ .../native/include/networktables/Topic.inc | 5 - ntcore/src/main/native/include/ntcore_c.h | 38 + ntcore/src/main/native/include/ntcore_cpp.h | 60 ++ shared/java/javacommon.gradle | 25 + shared/javacpp/setupBuild.gradle | 9 +- shared/jni/setupBuild.gradle | 9 +- styleguide/checkstyle-suppressions.xml | 2 + styleguide/pmd-ruleset.xml | 1 + wpimath/CMakeLists.txt | 54 +- wpimath/build.gradle | 1 + .../edu/wpi/first/math/geometry/Pose2d.java | 84 +++ .../edu/wpi/first/math/geometry/Pose3d.java | 84 +++ .../wpi/first/math/geometry/Quaternion.java | 75 ++ .../wpi/first/math/geometry/Rotation2d.java | 68 ++ .../wpi/first/math/geometry/Rotation3d.java | 78 ++ .../wpi/first/math/geometry/Transform2d.java | 84 +++ .../wpi/first/math/geometry/Transform3d.java | 84 +++ .../first/math/geometry/Translation2d.java | 71 ++ .../first/math/geometry/Translation3d.java | 73 ++ .../edu/wpi/first/math/geometry/Twist2d.java | 73 ++ .../edu/wpi/first/math/geometry/Twist3d.java | 85 +++ .../src/main/native/cpp/geometry/Pose2d.cpp | 21 + .../src/main/native/cpp/geometry/Pose3d.cpp | 22 + .../main/native/cpp/geometry/Quaternion.cpp | 23 + .../main/native/cpp/geometry/Rotation2d.cpp | 19 + .../main/native/cpp/geometry/Rotation3d.cpp | 19 + .../main/native/cpp/geometry/Transform2d.cpp | 21 + .../main/native/cpp/geometry/Transform3d.cpp | 21 + .../native/cpp/geometry/Translation2d.cpp | 20 + .../native/cpp/geometry/Translation3d.cpp | 22 + .../src/main/native/cpp/geometry/Twist2d.cpp | 30 + .../src/main/native/cpp/geometry/Twist3d.cpp | 34 + .../main/native/include/frc/geometry/Pose2d.h | 37 + .../main/native/include/frc/geometry/Pose3d.h | 37 + .../native/include/frc/geometry/Quaternion.h | 31 + .../native/include/frc/geometry/Rotation2d.h | 23 + .../native/include/frc/geometry/Rotation3d.h | 30 + .../native/include/frc/geometry/Transform2d.h | 38 + .../native/include/frc/geometry/Transform3d.h | 38 + .../include/frc/geometry/Translation2d.h | 26 + .../include/frc/geometry/Translation3d.h | 28 + .../native/include/frc/geometry/Twist2d.h | 27 + .../native/include/frc/geometry/Twist3d.h | 34 + wpimath/src/main/proto/controller.proto | 37 + wpimath/src/main/proto/geometry2d.proto | 30 + wpimath/src/main/proto/geometry3d.proto | 41 ++ wpimath/src/main/proto/kinematics.proto | 64 ++ wpimath/src/main/proto/plant.proto | 16 + wpimath/src/main/proto/spline.proto | 19 + wpimath/src/main/proto/system.proto | 17 + wpimath/src/main/proto/trajectory.proto | 20 + wpimath/src/main/proto/wpimath.proto | 15 + wpiutil/.styleguide | 1 + wpiutil/CMakeLists.txt | 20 +- wpiutil/build.gradle | 1 + wpiutil/doc/struct.adoc | 311 ++++++++ .../edu/wpi/first/util/datalog/DataLog.java | 148 ++++ .../wpi/first/util/datalog/DataLogJNI.java | 5 + .../first/util/datalog/ProtobufLogEntry.java | 117 +++ .../util/datalog/StructArrayLogEntry.java | 140 ++++ .../first/util/datalog/StructLogEntry.java | 106 +++ .../edu/wpi/first/util/protobuf/Protobuf.java | 121 ++++ .../first/util/protobuf/ProtobufBuffer.java | 164 +++++ .../first/util/struct/BadSchemaException.java | 43 ++ .../wpi/first/util/struct/DynamicStruct.java | 632 ++++++++++++++++ .../edu/wpi/first/util/struct/Struct.java | 116 +++ .../wpi/first/util/struct/StructBuffer.java | 224 ++++++ .../first/util/struct/StructDescriptor.java | 156 ++++ .../util/struct/StructDescriptorDatabase.java | 148 ++++ .../util/struct/StructFieldDescriptor.java | 241 +++++++ .../first/util/struct/StructFieldType.java | 67 ++ .../wpi/first/util/struct/parser/Lexer.java | 132 ++++ .../util/struct/parser/ParseException.java | 33 + .../util/struct/parser/ParsedDeclaration.java | 25 + .../util/struct/parser/ParsedSchema.java | 14 + .../wpi/first/util/struct/parser/Parser.java | 157 ++++ .../first/util/struct/parser/TokenKind.java | 32 + wpiutil/src/main/native/cpp/DataLog.cpp | 33 + .../src/main/native/cpp/jni/DataLogJNI.cpp | 41 ++ .../src/main/native/cpp/protobuf/Protobuf.cpp | 194 +++++ .../cpp/protobuf/ProtobufMessageDatabase.cpp | 119 +++ .../main/native/cpp/struct/DynamicStruct.cpp | 444 ++++++++++++ .../main/native/cpp/struct/SchemaParser.cpp | 238 ++++++ wpiutil/src/main/native/include/wpi/DataLog.h | 221 ++++++ .../native/include/wpi/protobuf/Protobuf.h | 260 +++++++ .../wpi/protobuf/ProtobufMessageDatabase.h | 76 ++ .../native/include/wpi/struct/DynamicStruct.h | 682 ++++++++++++++++++ .../native/include/wpi/struct/SchemaParser.h | 186 +++++ .../main/native/include/wpi/struct/Struct.h | 533 ++++++++++++++ .../first/util/struct/DynamicStructTest.java | 390 ++++++++++ .../first/util/struct/parser/ParserTest.java | 212 ++++++ .../native/cpp/struct/DynamicStructTest.cpp | 358 +++++++++ .../native/cpp/struct/SchemaParserTest.cpp | 218 ++++++ 133 files changed, 13506 insertions(+), 90 deletions(-) create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntry.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntryImpl.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/ProtobufPublisher.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/ProtobufSubscriber.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/ProtobufTopic.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntry.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntryImpl.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/StructArrayPublisher.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/StructArraySubscriber.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/StructArrayTopic.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/StructEntry.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/StructEntryImpl.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/StructPublisher.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/StructSubscriber.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/StructTopic.java create mode 100644 ntcore/src/main/java/edu/wpi/first/networktables/TimestampedObject.java create mode 100644 ntcore/src/main/native/include/networktables/ProtobufTopic.h create mode 100644 ntcore/src/main/native/include/networktables/StructArrayTopic.h create mode 100644 ntcore/src/main/native/include/networktables/StructTopic.h create mode 100644 wpimath/src/main/native/cpp/geometry/Twist2d.cpp create mode 100644 wpimath/src/main/native/cpp/geometry/Twist3d.cpp create mode 100644 wpimath/src/main/proto/controller.proto create mode 100644 wpimath/src/main/proto/geometry2d.proto create mode 100644 wpimath/src/main/proto/geometry3d.proto create mode 100644 wpimath/src/main/proto/kinematics.proto create mode 100644 wpimath/src/main/proto/plant.proto create mode 100644 wpimath/src/main/proto/spline.proto create mode 100644 wpimath/src/main/proto/system.proto create mode 100644 wpimath/src/main/proto/trajectory.proto create mode 100644 wpimath/src/main/proto/wpimath.proto create mode 100644 wpiutil/doc/struct.adoc create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/datalog/ProtobufLogEntry.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/datalog/StructArrayLogEntry.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/datalog/StructLogEntry.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/protobuf/Protobuf.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/BadSchemaException.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/DynamicStruct.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/Struct.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/StructBuffer.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptor.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptorDatabase.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldDescriptor.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldType.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/parser/Lexer.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParseException.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedDeclaration.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedSchema.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/parser/Parser.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/struct/parser/TokenKind.java create mode 100644 wpiutil/src/main/native/cpp/protobuf/Protobuf.cpp create mode 100644 wpiutil/src/main/native/cpp/protobuf/ProtobufMessageDatabase.cpp create mode 100644 wpiutil/src/main/native/cpp/struct/DynamicStruct.cpp create mode 100644 wpiutil/src/main/native/cpp/struct/SchemaParser.cpp create mode 100644 wpiutil/src/main/native/include/wpi/protobuf/Protobuf.h create mode 100644 wpiutil/src/main/native/include/wpi/protobuf/ProtobufMessageDatabase.h create mode 100644 wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h create mode 100644 wpiutil/src/main/native/include/wpi/struct/SchemaParser.h create mode 100644 wpiutil/src/main/native/include/wpi/struct/Struct.h create mode 100644 wpiutil/src/test/java/edu/wpi/first/util/struct/DynamicStructTest.java create mode 100644 wpiutil/src/test/java/edu/wpi/first/util/struct/parser/ParserTest.java create mode 100644 wpiutil/src/test/native/cpp/struct/DynamicStructTest.cpp create mode 100644 wpiutil/src/test/native/cpp/struct/SchemaParserTest.cpp diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 459b237923..a11ff84131 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -19,7 +19,8 @@ jobs: - os: macOS-12 name: macOS container: "" - flags: "-DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON" + env: "PATH=\"/usr/local/opt/protobuf@3/bin:$PATH\"" + flags: "-DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DCMAKE_LIBRARY_PATH=/usr/local/opt/protobuf@3/lib -DProtobuf_INCLUDE_DIR=/usr/local/opt/protobuf@3/include -DProtobuf_PROTOC_EXECUTABLE=/usr/local/opt/protobuf@3/bin/protoc" name: "Build - ${{ matrix.name }}" runs-on: ${{ matrix.os }} @@ -27,10 +28,14 @@ jobs: steps: - name: Install dependencies (Linux) if: runner.os == 'Linux' - run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java python-is-python3 ninja-build + run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java python-is-python3 libprotobuf-dev protobuf-compiler ninja-build + + - name: Install QuickBuffers (Linux) + if: runner.os == 'Linux' + run: wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.2/protoc-gen-quickbuf_1.3.2_amd64.deb && sudo apt install ./protoc-gen-quickbuf_1.3.2_amd64.deb - name: Install opencv (macOS) - run: brew install opencv ninja + run: brew install opencv protobuf@3 ninja if: runner.os == 'macOS' - name: Set up Python 3.8 (macOS) diff --git a/.github/workflows/sanitizers.yml b/.github/workflows/sanitizers.yml index 0db875d1a1..1c9c935928 100644 --- a/.github/workflows/sanitizers.yml +++ b/.github/workflows/sanitizers.yml @@ -29,7 +29,11 @@ jobs: container: wpilib/roborio-cross-ubuntu:2024-22.04 steps: - name: Install Dependencies - run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java python-is-python3 clang-14 + run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java python-is-python3 clang-14 libprotobuf-dev protobuf-compiler ninja-build + + - name: Install QuickBuffers + if: runner.os == 'Linux' + run: wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.2/protoc-gen-quickbuf_1.3.2_amd64.deb && sudo apt install ./protoc-gen-quickbuf_1.3.2_amd64.deb - name: Run sccache-cache uses: mozilla-actions/sccache-action@v0.0.3 @@ -40,7 +44,7 @@ jobs: - uses: actions/checkout@v3 - name: configure - run: mkdir build && cd build && cmake -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-14 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-14 ${{ matrix.cmake-flags }} .. + run: mkdir build && cd build && cmake -G Ninja -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-14 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-14 ${{ matrix.cmake-flags }} .. env: SCCACHE_GHA_ENABLED: "true" diff --git a/.styleguide b/.styleguide index 02e7254bba..1ba4e1dcce 100644 --- a/.styleguide +++ b/.styleguide @@ -31,6 +31,7 @@ includeOtherLibs { ^cscore ^fmt/ ^gtest/ + ^google/ ^hal/ ^imgui ^implot diff --git a/CMakeLists.txt b/CMakeLists.txt index e617a756a5..a4a7544a48 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -176,6 +176,12 @@ endif() find_package(LIBSSH 0.7.1) +find_package(Protobuf REQUIRED) +find_program(Quickbuf_EXECUTABLE + NAMES protoc-gen-quickbuf + DOC "The Quickbuf protoc plugin" +) + if (WITH_FLAT_INSTALL) set(WPIUTIL_DEP_REPLACE "include($\{SELF_DIR\}/wpiutil-config.cmake)") set(WPINET_DEP_REPLACE "include($\{SELF_DIR\}/wpinet-config.cmake)") diff --git a/README-CMAKE.md b/README-CMAKE.md index 2718a56191..c85a089981 100644 --- a/README-CMAKE.md +++ b/README-CMAKE.md @@ -20,6 +20,8 @@ By default, all libraries except for the HAL and WPILib get built with a default The jinja2 pip package is needed to generate classes for NT4's pubsub. +The protobuf library and compiler are needed for protobuf generation. The QuickBuffers protoc-gen package is also required when Java is being built. + OpenCV needs to be findable by CMake. On systems like the Jetson, this is installed by default. Otherwise, you will need to build OpenCV from source and install it. If you want JNI and Java, you will need a JDK of at least version 11 installed. In addition, you need a `JAVA_HOME` environment variable set properly and set to the JDK directory. diff --git a/build.gradle b/build.gradle index f4b2ec50e0..b1c3ca2e6d 100644 --- a/build.gradle +++ b/build.gradle @@ -24,6 +24,7 @@ plugins { id 'com.github.johnrengelman.shadow' version '8.1.1' apply false id 'com.diffplug.spotless' version '6.20.0' apply false id 'com.github.spotbugs' version '5.1.3' apply false + id 'com.google.protobuf' version '0.9.3' apply false } wpilibVersioning.buildServerMode = project.hasProperty('buildServer') diff --git a/docs/build.gradle b/docs/build.gradle index d38f3ea508..3762cf4f9d 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -142,6 +142,8 @@ doxygen { //TODO: building memory docs causes search to break exclude 'wpi/memory/**' + exclude '*.pb.h' + aliases 'effects=\\par Effects:^^', 'notes=\\par Notes:^^', 'requires=\\par Requires:^^', diff --git a/glass/.styleguide b/glass/.styleguide index 974eaa954b..148c63451e 100644 --- a/glass/.styleguide +++ b/glass/.styleguide @@ -24,6 +24,7 @@ includeOtherLibs { ^fmt/ ^fields/ ^frc/ + ^google/ ^imgui ^networktables/ ^ntcore diff --git a/glass/src/libnt/native/cpp/NetworkTables.cpp b/glass/src/libnt/native/cpp/NetworkTables.cpp index c1e8a36184..c367798bc6 100644 --- a/glass/src/libnt/native/cpp/NetworkTables.cpp +++ b/glass/src/libnt/native/cpp/NetworkTables.cpp @@ -14,6 +14,8 @@ #include #include +#include +#include #include #include #include @@ -114,36 +116,37 @@ void NetworkTablesModel::Entry::UpdateInfo(nt::TopicInfo&& info_) { } } -static void UpdateMsgpackValueSource(NetworkTablesModel::ValueSource* out, +static void UpdateMsgpackValueSource(NetworkTablesModel& model, + NetworkTablesModel::ValueSource* out, mpack_reader_t& r, std::string_view name, int64_t time) { mpack_tag_t tag = mpack_read_tag(&r); switch (mpack_tag_type(&tag)) { case mpack::mpack_type_bool: - out->UpdateFromValue( - nt::Value::MakeBoolean(mpack_tag_bool_value(&tag), time), name, ""); + out->value = nt::Value::MakeBoolean(mpack_tag_bool_value(&tag), time); + out->UpdateFromValue(model, name, ""); break; case mpack::mpack_type_int: - out->UpdateFromValue( - nt::Value::MakeInteger(mpack_tag_int_value(&tag), time), name, ""); + out->value = nt::Value::MakeInteger(mpack_tag_int_value(&tag), time); + out->UpdateFromValue(model, name, ""); break; case mpack::mpack_type_uint: - out->UpdateFromValue( - nt::Value::MakeInteger(mpack_tag_uint_value(&tag), time), name, ""); + out->value = nt::Value::MakeInteger(mpack_tag_uint_value(&tag), time); + out->UpdateFromValue(model, name, ""); break; case mpack::mpack_type_float: - out->UpdateFromValue( - nt::Value::MakeFloat(mpack_tag_float_value(&tag), time), name, ""); + out->value = nt::Value::MakeFloat(mpack_tag_float_value(&tag), time); + out->UpdateFromValue(model, name, ""); break; case mpack::mpack_type_double: - out->UpdateFromValue( - nt::Value::MakeDouble(mpack_tag_double_value(&tag), time), name, ""); + out->value = nt::Value::MakeDouble(mpack_tag_double_value(&tag), time); + out->UpdateFromValue(model, name, ""); break; case mpack::mpack_type_str: { std::string str; mpack_read_str(&r, &tag, &str); - out->UpdateFromValue(nt::Value::MakeString(std::move(str), time), name, - ""); + out->value = nt::Value::MakeString(std::move(str), time); + out->UpdateFromValue(model, name, ""); break; } case mpack::mpack_type_bin: @@ -164,7 +167,8 @@ static void UpdateMsgpackValueSource(NetworkTablesModel::ValueSource* out, child.path = fmt::format("{}{}", name, child.name); } ++i; - UpdateMsgpackValueSource(&child, r, child.path, time); // recurse + UpdateMsgpackValueSource(model, &child, r, child.path, + time); // recurse } mpack_done_array(&r); break; @@ -186,7 +190,7 @@ static void UpdateMsgpackValueSource(NetworkTablesModel::ValueSource* out, auto it = elems.find(key); if (it != elems.end()) { auto& child = out->valueChildren[it->second]; - UpdateMsgpackValueSource(&child, r, child.path, time); + UpdateMsgpackValueSource(model, &child, r, child.path, time); elems.erase(it); } else { added = true; @@ -194,7 +198,7 @@ static void UpdateMsgpackValueSource(NetworkTablesModel::ValueSource* out, auto& child = out->valueChildren.back(); child.name = std::move(key); child.path = fmt::format("{}/{}", name, child.name); - UpdateMsgpackValueSource(&child, r, child.path, time); + UpdateMsgpackValueSource(model, &child, r, child.path, time); } } } @@ -219,7 +223,318 @@ static void UpdateMsgpackValueSource(NetworkTablesModel::ValueSource* out, } } -static void UpdateJsonValueSource(NetworkTablesModel::ValueSource* out, +static void UpdateStructValueSource(NetworkTablesModel& model, + NetworkTablesModel::ValueSource* out, + const wpi::DynamicStruct& s, + std::string_view name, int64_t time) { + auto desc = s.GetDescriptor(); + out->typeStr = "struct:" + desc->GetName(); + auto& fields = desc->GetFields(); + if (!out->valueChildrenMap || fields.size() != out->valueChildren.size()) { + out->valueChildren.clear(); + out->valueChildrenMap = true; + out->valueChildren.reserve(fields.size()); + for (auto&& field : fields) { + out->valueChildren.emplace_back(); + auto& child = out->valueChildren.back(); + child.name = field.GetName(); + child.path = fmt::format("{}/{}", name, child.name); + } + } + auto outIt = out->valueChildren.begin(); + for (auto&& field : fields) { + auto& child = *outIt++; + switch (field.GetType()) { + case wpi::StructFieldType::kBool: + if (field.IsArray()) { + std::vector v; + v.reserve(field.GetArraySize()); + for (size_t i = 0; i < field.GetArraySize(); ++i) { + v.emplace_back(s.GetBoolField(&field, i)); + } + child.value = nt::Value::MakeBooleanArray(std::move(v), time); + } else { + child.value = nt::Value::MakeBoolean(s.GetBoolField(&field), time); + } + child.UpdateFromValue(model, child.path, ""); + break; + case wpi::StructFieldType::kChar: + child.value = nt::Value::MakeString(s.GetStringField(&field), time); + child.UpdateFromValue(model, child.path, ""); + break; + case wpi::StructFieldType::kInt8: + case wpi::StructFieldType::kInt16: + case wpi::StructFieldType::kInt32: + case wpi::StructFieldType::kInt64: + case wpi::StructFieldType::kUint8: + case wpi::StructFieldType::kUint16: + case wpi::StructFieldType::kUint32: + case wpi::StructFieldType::kUint64: { + bool isUint = field.IsUint(); + if (field.IsArray()) { + std::vector v; + v.reserve(field.GetArraySize()); + for (size_t i = 0; i < field.GetArraySize(); ++i) { + if (isUint) { + v.emplace_back(s.GetUintField(&field, i)); + } else { + v.emplace_back(s.GetIntField(&field, i)); + } + } + child.value = nt::Value::MakeIntegerArray(std::move(v), time); + } else { + if (isUint) { + child.value = nt::Value::MakeInteger(s.GetUintField(&field), time); + } else { + child.value = nt::Value::MakeInteger(s.GetIntField(&field), time); + } + } + child.UpdateFromValue(model, child.path, ""); + break; + } + case wpi::StructFieldType::kFloat: + if (field.IsArray()) { + std::vector v; + v.reserve(field.GetArraySize()); + for (size_t i = 0; i < field.GetArraySize(); ++i) { + v.emplace_back(s.GetFloatField(&field, i)); + } + child.value = nt::Value::MakeFloatArray(std::move(v), time); + } else { + child.value = nt::Value::MakeFloat(s.GetFloatField(&field), time); + } + child.UpdateFromValue(model, child.path, ""); + break; + case wpi::StructFieldType::kDouble: + if (field.IsArray()) { + std::vector v; + v.reserve(field.GetArraySize()); + for (size_t i = 0; i < field.GetArraySize(); ++i) { + v.emplace_back(s.GetDoubleField(&field, i)); + } + child.value = nt::Value::MakeDoubleArray(std::move(v), time); + } else { + child.value = nt::Value::MakeDouble(s.GetDoubleField(&field), time); + } + child.UpdateFromValue(model, child.path, ""); + break; + case wpi::StructFieldType::kStruct: + if (field.IsArray()) { + if (child.valueChildrenMap) { + child.valueChildren.clear(); + child.valueChildrenMap = false; + } + child.valueChildren.resize(field.GetArraySize()); + unsigned int i = 0; + for (auto&& child2 : child.valueChildren) { + if (child2.name.empty()) { + child2.name = fmt::format("[{}]", i); + child2.path = fmt::format("{}{}", name, child.name); + } + UpdateStructValueSource(model, &child2, s.GetStructField(&field, i), + child2.path, time); // recurse + ++i; + } + } else { + UpdateStructValueSource(model, &child, s.GetStructField(&field), + child.path, time); // recurse + } + break; + } + } +} + +static void UpdateProtobufValueSource(NetworkTablesModel& model, + NetworkTablesModel::ValueSource* out, + const google::protobuf::Message& msg, + std::string_view name, int64_t time) { + auto desc = msg.GetDescriptor(); + out->typeStr = "proto:" + desc->full_name(); + if (!out->valueChildrenMap || + desc->field_count() != static_cast(out->valueChildren.size())) { + out->valueChildren.clear(); + out->valueChildrenMap = true; + out->valueChildren.reserve(desc->field_count()); + for (int i = 0, end = desc->field_count(); i < end; ++i) { + out->valueChildren.emplace_back(); + auto& child = out->valueChildren.back(); + child.name = desc->field(i)->name(); + child.path = fmt::format("{}/{}", name, child.name); + } + } + auto refl = msg.GetReflection(); + auto outIt = out->valueChildren.begin(); + for (int fieldNum = 0, end = desc->field_count(); fieldNum < end; + ++fieldNum) { + auto field = desc->field(fieldNum); + auto& child = *outIt++; + switch (field->cpp_type()) { + case google::protobuf::FieldDescriptor::CPPTYPE_BOOL: + if (field->is_repeated()) { + size_t size = refl->FieldSize(msg, field); + std::vector v; + v.reserve(size); + for (size_t i = 0; i < size; ++i) { + v.emplace_back(refl->GetRepeatedBool(msg, field, i)); + } + child.value = nt::Value::MakeBooleanArray(std::move(v), time); + } else { + child.value = nt::Value::MakeBoolean(refl->GetBool(msg, field), time); + } + child.UpdateFromValue(model, child.path, ""); + break; + case google::protobuf::FieldDescriptor::CPPTYPE_STRING: + if (field->is_repeated()) { + size_t size = refl->FieldSize(msg, field); + std::vector v; + v.reserve(size); + for (size_t i = 0; i < size; ++i) { + v.emplace_back(refl->GetRepeatedString(msg, field, i)); + } + child.value = nt::Value::MakeStringArray(std::move(v), time); + } else { + child.value = + nt::Value::MakeString(refl->GetString(msg, field), time); + child.UpdateFromValue(model, child.path, ""); + } + break; + case google::protobuf::FieldDescriptor::CPPTYPE_INT32: + if (field->is_repeated()) { + size_t size = refl->FieldSize(msg, field); + std::vector v; + v.reserve(size); + for (size_t i = 0; i < size; ++i) { + v.emplace_back(refl->GetRepeatedInt32(msg, field, i)); + } + child.value = nt::Value::MakeIntegerArray(std::move(v), time); + } else { + child.value = + nt::Value::MakeInteger(refl->GetInt32(msg, field), time); + } + child.UpdateFromValue(model, child.path, ""); + break; + case google::protobuf::FieldDescriptor::CPPTYPE_INT64: + if (field->is_repeated()) { + size_t size = refl->FieldSize(msg, field); + std::vector v; + v.reserve(size); + for (size_t i = 0; i < size; ++i) { + v.emplace_back(refl->GetRepeatedInt64(msg, field, i)); + } + child.value = nt::Value::MakeIntegerArray(std::move(v), time); + } else { + child.value = + nt::Value::MakeInteger(refl->GetInt64(msg, field), time); + } + child.UpdateFromValue(model, child.path, ""); + break; + case google::protobuf::FieldDescriptor::CPPTYPE_UINT32: + if (field->is_repeated()) { + size_t size = refl->FieldSize(msg, field); + std::vector v; + v.reserve(size); + for (size_t i = 0; i < size; ++i) { + v.emplace_back(refl->GetRepeatedUInt32(msg, field, i)); + } + child.value = nt::Value::MakeIntegerArray(std::move(v), time); + } else { + child.value = + nt::Value::MakeInteger(refl->GetUInt32(msg, field), time); + } + child.UpdateFromValue(model, child.path, ""); + break; + case google::protobuf::FieldDescriptor::CPPTYPE_UINT64: + if (field->is_repeated()) { + size_t size = refl->FieldSize(msg, field); + std::vector v; + v.reserve(size); + for (size_t i = 0; i < size; ++i) { + v.emplace_back(refl->GetRepeatedUInt64(msg, field, i)); + } + child.value = nt::Value::MakeIntegerArray(std::move(v), time); + } else { + child.value = + nt::Value::MakeInteger(refl->GetUInt64(msg, field), time); + } + child.UpdateFromValue(model, child.path, ""); + break; + case google::protobuf::FieldDescriptor::CPPTYPE_FLOAT: + if (field->is_repeated()) { + size_t size = refl->FieldSize(msg, field); + std::vector v; + v.reserve(size); + for (size_t i = 0; i < size; ++i) { + v.emplace_back(refl->GetRepeatedFloat(msg, field, i)); + } + child.value = nt::Value::MakeFloatArray(std::move(v), time); + } else { + child.value = nt::Value::MakeFloat(refl->GetFloat(msg, field), time); + } + child.UpdateFromValue(model, child.path, ""); + break; + case google::protobuf::FieldDescriptor::CPPTYPE_DOUBLE: + if (field->is_repeated()) { + size_t size = refl->FieldSize(msg, field); + std::vector v; + v.reserve(size); + for (size_t i = 0; i < size; ++i) { + v.emplace_back(refl->GetRepeatedDouble(msg, field, i)); + } + child.value = nt::Value::MakeDoubleArray(std::move(v), time); + } else { + child.value = + nt::Value::MakeDouble(refl->GetDouble(msg, field), time); + } + child.UpdateFromValue(model, child.path, ""); + break; + case google::protobuf::FieldDescriptor::CPPTYPE_ENUM: + if (field->is_repeated()) { + size_t size = refl->FieldSize(msg, field); + std::vector v; + v.reserve(size); + for (size_t i = 0; i < size; ++i) { + v.emplace_back(refl->GetRepeatedEnum(msg, field, i)->name()); + } + child.value = nt::Value::MakeStringArray(std::move(v), time); + } else { + child.value = + nt::Value::MakeString(refl->GetEnum(msg, field)->name(), time); + } + child.UpdateFromValue(model, child.path, ""); + break; + case google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE: + if (field->is_repeated()) { + if (child.valueChildrenMap) { + child.valueChildren.clear(); + child.valueChildrenMap = false; + } + size_t size = refl->FieldSize(msg, field); + child.valueChildren.resize(size); + unsigned int i = 0; + for (auto&& child2 : child.valueChildren) { + if (child2.name.empty()) { + child2.name = fmt::format("[{}]", i); + child2.path = fmt::format("{}{}", name, child.name); + } + UpdateProtobufValueSource(model, &child2, + refl->GetRepeatedMessage(msg, field, i), + child2.path, time); // recurse + ++i; + } + } else { + UpdateProtobufValueSource( + model, &child, + refl->GetMessage(msg, field, + model.GetProtobufDatabase().GetMessageFactory()), + child.path, time); // recurse + } + break; + } + } +} + +static void UpdateJsonValueSource(NetworkTablesModel& model, + NetworkTablesModel::ValueSource* out, const wpi::json& j, std::string_view name, int64_t time) { switch (j.type()) { @@ -237,7 +552,7 @@ static void UpdateJsonValueSource(NetworkTablesModel::ValueSource* out, auto it = elems.find(kv.key()); if (it != elems.end()) { auto& child = out->valueChildren[it->second]; - UpdateJsonValueSource(&child, kv.value(), child.path, time); + UpdateJsonValueSource(model, &child, kv.value(), child.path, time); elems.erase(it); } else { added = true; @@ -245,7 +560,7 @@ static void UpdateJsonValueSource(NetworkTablesModel::ValueSource* out, auto& child = out->valueChildren.back(); child.name = kv.key(); child.path = fmt::format("{}/{}", name, child.name); - UpdateJsonValueSource(&child, kv.value(), child.path, time); + UpdateJsonValueSource(model, &child, kv.value(), child.path, time); } } // erase unmatched keys @@ -273,30 +588,30 @@ static void UpdateJsonValueSource(NetworkTablesModel::ValueSource* out, child.name = fmt::format("[{}]", i); child.path = fmt::format("{}{}", name, child.name); } - UpdateJsonValueSource(&child, j[i++], child.path, time); // recurse + // recurse + UpdateJsonValueSource(model, &child, j[i++], child.path, time); } break; } case wpi::json::value_t::string: - out->UpdateFromValue( - nt::Value::MakeString(j.get_ref(), time), name, - ""); + out->value = nt::Value::MakeString(j.get_ref(), time); + out->UpdateFromValue(model, name, ""); break; case wpi::json::value_t::boolean: - out->UpdateFromValue(nt::Value::MakeBoolean(j.get(), time), name, - ""); + out->value = nt::Value::MakeBoolean(j.get(), time); + out->UpdateFromValue(model, name, ""); break; case wpi::json::value_t::number_integer: - out->UpdateFromValue(nt::Value::MakeInteger(j.get(), time), name, - ""); + out->value = nt::Value::MakeInteger(j.get(), time); + out->UpdateFromValue(model, name, ""); break; case wpi::json::value_t::number_unsigned: - out->UpdateFromValue(nt::Value::MakeInteger(j.get(), time), - name, ""); + out->value = nt::Value::MakeInteger(j.get(), time); + out->UpdateFromValue(model, name, ""); break; case wpi::json::value_t::number_float: - out->UpdateFromValue(nt::Value::MakeDouble(j.get(), time), name, - ""); + out->value = nt::Value::MakeDouble(j.get(), time); + out->UpdateFromValue(model, name, ""); break; default: out->value = {}; @@ -336,8 +651,8 @@ void NetworkTablesModel::ValueSource::UpdateDiscreteArray( } void NetworkTablesModel::ValueSource::UpdateFromValue( - nt::Value&& v, std::string_view name, std::string_view typeStr) { - value = v; + NetworkTablesModel& model, std::string_view name, + std::string_view typeStr) { switch (value.type()) { case NT_BOOLEAN: UpdateDiscreteSource(name, value.GetBoolean() ? 1 : 0, value.time(), @@ -381,15 +696,16 @@ void NetworkTablesModel::ValueSource::UpdateFromValue( child.name = fmt::format("[{}]", i); child.path = fmt::format("{}{}", name, child.name); } - child.UpdateFromValue(nt::Value::MakeString(arr[i++], value.time()), - child.path, ""); + child.value = nt::Value::MakeString(arr[i++], value.time()); + child.UpdateFromValue(model, child.path, ""); } break; } case NT_STRING: if (typeStr == "json") { try { - UpdateJsonValueSource(this, wpi::json::parse(value.GetString()), name, + UpdateJsonValueSource(model, this, + wpi::json::parse(value.GetString()), name, value.last_change()); } catch (wpi::json::exception&) { // ignore @@ -407,9 +723,57 @@ void NetworkTablesModel::ValueSource::UpdateFromValue( if (typeStr == "msgpack") { mpack_reader_t r; mpack_reader_init_data(&r, value.GetRaw()); - UpdateMsgpackValueSource(this, r, name, value.last_change()); - + UpdateMsgpackValueSource(model, this, r, name, value.last_change()); mpack_reader_destroy(&r); + } else if (wpi::starts_with(typeStr, "struct:")) { + auto structName = wpi::drop_front(typeStr, 7); + bool isArray = structName.ends_with("[]"); + if (isArray) { + structName = wpi::drop_back(structName, 2); + } + auto desc = model.m_structDb.Find(structName); + if (desc && desc->IsValid()) { + if (isArray) { + // array of struct at top level + if (valueChildrenMap) { + valueChildren.clear(); + valueChildrenMap = false; + } + auto raw = value.GetRaw(); + valueChildren.resize(raw.size() / desc->GetSize()); + unsigned int i = 0; + for (auto&& child : valueChildren) { + if (child.name.empty()) { + child.name = fmt::format("[{}]", i); + child.path = fmt::format("{}{}", name, child.name); + } + wpi::DynamicStruct s{desc, raw}; + UpdateStructValueSource(model, &child, s, child.path, + value.last_change()); + ++i; + raw = wpi::drop_front(raw, desc->GetSize()); + } + } else { + wpi::DynamicStruct s{desc, value.GetRaw()}; + UpdateStructValueSource(model, this, s, name, value.last_change()); + } + } else { + valueChildren.clear(); + } + } else if (wpi::starts_with(typeStr, "proto:")) { + auto msg = model.m_protoDb.Find(wpi::drop_front(typeStr, 6)); + if (msg) { + msg->Clear(); + auto raw = value.GetRaw(); + if (msg->ParseFromArray(raw.data(), raw.size())) { + UpdateProtobufValueSource(model, this, *msg, name, + value.last_change()); + } else { + valueChildren.clear(); + } + } else { + valueChildren.clear(); + } } else { valueChildren.clear(); } @@ -473,8 +837,8 @@ void NetworkTablesModel::Update() { } else if (auto valueData = event.GetValueEventData()) { auto& entry = m_entries[valueData->topic]; if (entry) { - entry->UpdateFromValue(std::move(valueData->value), entry->info.name, - entry->info.type_str); + entry->value = std::move(valueData->value); + entry->UpdateFromValue(*this); if (wpi::starts_with(entry->info.name, '$') && entry->value.IsRaw() && entry->info.type_str == "msgpack") { // meta topic handling @@ -499,6 +863,46 @@ void NetworkTablesModel::Update() { it->second.UpdateSubscribers(entry->value.GetRaw()); } } + } else if (entry->value.IsRaw() && + wpi::starts_with(entry->info.name, "/.schema/struct:") && + entry->info.type_str == "structschema") { + // struct schema handling + auto typeStr = wpi::drop_front(entry->info.name, 16); + std::string_view schema{ + reinterpret_cast(entry->value.GetRaw().data()), + entry->value.GetRaw().size()}; + std::string err; + auto desc = m_structDb.Add(typeStr, schema, &err); + if (!desc) { + fmt::print("could not decode struct '{}' schema '{}': {}\n", + entry->info.name, schema, err); + } else if (desc->IsValid()) { + // loop over all entries with this type and update + for (auto&& entryPair : m_entries) { + auto ts = entryPair.second->info.type_str; + if (!wpi::starts_with(ts, "struct:")) { + continue; + } + ts = wpi::drop_front(ts, 7); + if (ts == typeStr || (wpi::ends_with(ts, "[]") && + wpi::drop_back(ts, 2) == typeStr)) { + entryPair.second->UpdateFromValue(*this); + } + } + } + } else if (entry->value.IsRaw() && + wpi::starts_with(entry->info.name, "/.schema/proto:") && + entry->info.type_str == "proto:FileDescriptorProto") { + // protobuf descriptor handling + auto filename = wpi::drop_front(entry->info.name, 15); + m_protoDb.Add(filename, entry->value.GetRaw()); + // loop over all protobuf entries and update (conservatively) + for (auto&& entryPair : m_entries) { + auto ts = entryPair.second->info.type_str; + if (wpi::starts_with(ts, "proto:")) { + entryPair.second->UpdateFromValue(*this); + } + } } } } @@ -663,6 +1067,24 @@ void NetworkTablesModel::UpdateClients(std::span data) { m_clients = std::move(newClients); } +static bool SimplifyTypeString(std::string_view* ts) { + if (wpi::starts_with(*ts, "proto:")) { + *ts = wpi::drop_front(*ts, 6); + auto lastdot = ts->rfind('.'); + if (lastdot != std::string_view::npos) { + *ts = wpi::substr(*ts, lastdot + 1); + } + if (wpi::starts_with(*ts, "Protobuf")) { + *ts = wpi::drop_front(*ts, 8); + } + return true; + } else if (wpi::starts_with(*ts, "struct:")) { + *ts = wpi::drop_front(*ts, 7); + return true; + } + return false; +} + static void EmitEntryValueReadonly(const NetworkTablesModel::ValueSource& entry, const char* typeStr, NetworkTablesFlags flags) { @@ -717,9 +1139,20 @@ static void EmitEntryValueReadonly(const NetworkTablesModel::ValueSource& entry, case NT_STRING_ARRAY: ImGui::LabelText(typeStr ? typeStr : "string[]", "[]"); break; - case NT_RAW: - ImGui::LabelText(typeStr ? typeStr : "raw", "[...]"); + case NT_RAW: { + std::string_view ts = typeStr ? typeStr : "raw"; + bool partial = SimplifyTypeString(&ts); + ImGui::LabelText(val.GetRaw().empty() ? "[]" : "[...]", "%s", ts.data()); + if (ImGui::IsItemHovered()) { + ImGui::BeginTooltip(); + if (partial) { + ImGui::TextUnformatted(typeStr); + } + ImGui::Text("%u bytes", static_cast(val.GetRaw().size())); + ImGui::EndTooltip(); + } break; + } default: ImGui::LabelText(typeStr ? typeStr : "other", "?"); break; @@ -1022,10 +1455,20 @@ static void EmitEntryValueEditable(NetworkTablesModel* model, break; } break; - case NT_RAW: - ImGui::LabelText(typeStr ? typeStr : "raw", - val.GetRaw().empty() ? "[]" : "[...]"); + case NT_RAW: { + std::string_view ts = typeStr ? typeStr : "raw"; + bool partial = SimplifyTypeString(&ts); + ImGui::LabelText(val.GetRaw().empty() ? "[]" : "[...]", "%s", ts.data()); + if (ImGui::IsItemHovered()) { + ImGui::BeginTooltip(); + if (partial) { + ImGui::TextUnformatted(typeStr); + } + ImGui::Text("%u bytes", static_cast(val.GetRaw().size())); + ImGui::EndTooltip(); + } break; + } case NT_RPC: ImGui::LabelText(typeStr ? typeStr : "rpc", "[...]"); break; @@ -1164,15 +1607,25 @@ static void EmitValueTree( ImGui::TableNextColumn(); if (!child.valueChildren.empty()) { - char label[64]; - if (child.valueChildrenMap) { - wpi::format_to_n_c_str(label, sizeof(label), "{{...}}##v_{}", - child.name); - } else { - wpi::format_to_n_c_str(label, sizeof(label), "[...]##v_{}", child.name); + auto pos = ImGui::GetCursorPos(); + char label[128]; + std::string_view ts = child.typeStr; + bool havePopup = SimplifyTypeString(&ts); + wpi::format_to_n_c_str(label, sizeof(label), "{}##v_{}", ts.data(), + child.name.c_str()); + bool valueChildrenOpen = + TreeNodeEx(label, ImGuiTreeNodeFlags_SpanFullWidth); + if (havePopup) { + if (ImGui::IsItemHovered()) { + ImGui::BeginTooltip(); + ImGui::TextUnformatted(child.typeStr.c_str()); + ImGui::EndTooltip(); + } } - - if (TreeNodeEx(label, ImGuiTreeNodeFlags_SpanFullWidth)) { + // make it look like a normal label w/type + ImGui::SetCursorPos(pos); + ImGui::LabelText(child.valueChildrenMap ? "{...}" : "[...]", "%s", ""); + if (valueChildrenOpen) { EmitValueTree(child.valueChildren, flags); TreePop(); } @@ -1197,19 +1650,21 @@ static void EmitEntry(NetworkTablesModel* model, ImGui::TableNextColumn(); if (!entry.valueChildren.empty()) { auto pos = ImGui::GetCursorPos(); - - char label[64]; - if (entry.valueChildrenMap) { - wpi::format_to_n_c_str(label, sizeof(label), "{{...}}##v_{}", - entry.info.name.c_str()); - } else { - wpi::format_to_n_c_str(label, sizeof(label), "[...]##v_{}", - entry.info.name.c_str()); - } - + char label[128]; + std::string_view ts = entry.info.type_str; + bool havePopup = SimplifyTypeString(&ts); + wpi::format_to_n_c_str(label, sizeof(label), "{}##v_{}", ts.data(), + entry.info.name.c_str()); valueChildrenOpen = TreeNodeEx(label, ImGuiTreeNodeFlags_SpanFullWidth | ImGuiTreeNodeFlags_AllowItemOverlap); + if (havePopup) { + if (ImGui::IsItemHovered()) { + ImGui::BeginTooltip(); + ImGui::TextUnformatted(entry.info.type_str.c_str()); + ImGui::EndTooltip(); + } + } // make it look like a normal label w/type ImGui::SetCursorPos(pos); ImGui::LabelText(entry.info.type_str.c_str(), "%s", ""); diff --git a/glass/src/libnt/native/include/glass/networktables/NetworkTables.h b/glass/src/libnt/native/include/glass/networktables/NetworkTables.h index da8f9b5d49..8416cf9dfd 100644 --- a/glass/src/libnt/native/include/glass/networktables/NetworkTables.h +++ b/glass/src/libnt/native/include/glass/networktables/NetworkTables.h @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include "glass/Model.h" #include "glass/View.h" @@ -31,7 +33,7 @@ class NetworkTablesModel : public Model { struct EntryValueTreeNode; struct ValueSource { - void UpdateFromValue(nt::Value&& v, std::string_view name, + void UpdateFromValue(NetworkTablesModel& model, std::string_view name, std::string_view typeStr); /** The latest value. */ @@ -40,6 +42,9 @@ class NetworkTablesModel : public Model { /** String representation of the value (for arrays / complex values). */ std::string valueStr; + /** Data type */ + std::string typeStr; + /** Data source (for numeric values). */ std::unique_ptr source; @@ -73,6 +78,10 @@ class NetworkTablesModel : public Model { Entry& operator=(const Entry&) = delete; ~Entry(); + void UpdateFromValue(NetworkTablesModel& model) { + ValueSource::UpdateFromValue(model, info.name, info.type_str); + } + void UpdateTopic(nt::Event&& event) { if (std::holds_alternative(event.data)) { UpdateInfo(std::get(std::move(event.data))); @@ -158,6 +167,9 @@ class NetworkTablesModel : public Model { Entry* GetEntry(std::string_view name); Entry* AddEntry(NT_Topic topic); + wpi::StructDescriptorDatabase& GetStructDatabase() { return m_structDb; } + wpi::ProtobufMessageDatabase& GetProtobufDatabase() { return m_protoDb; } + private: void RebuildTree(); void RebuildTreeImpl(std::vector* tree, int category); @@ -177,6 +189,9 @@ class NetworkTablesModel : public Model { std::map> m_clients; Client m_server; + + wpi::StructDescriptorDatabase m_structDb; + wpi::ProtobufMessageDatabase m_protoDb; }; using NetworkTablesFlags = int; diff --git a/ntcore/CMakeLists.txt b/ntcore/CMakeLists.txt index f7198f7a82..049d6100c1 100644 --- a/ntcore/CMakeLists.txt +++ b/ntcore/CMakeLists.txt @@ -54,6 +54,11 @@ if (WITH_JAVA) include(UseJava) set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") + file(GLOB QUICKBUF_JAR + ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/*.jar) + + set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${QUICKBUF_JAR}) + file(GLOB ntcore_jni_src src/main/native/cpp/jni/*.cpp ${WPILIB_BINARY_DIR}/ntcore/generated/main/native/cpp/jni/*.cpp) diff --git a/ntcore/src/generate/java/NetworkTableInstance.java.jinja b/ntcore/src/generate/java/NetworkTableInstance.java.jinja index 7dabd1070c..9df129d803 100644 --- a/ntcore/src/generate/java/NetworkTableInstance.java.jinja +++ b/ntcore/src/generate/java/NetworkTableInstance.java.jinja @@ -7,16 +7,22 @@ package edu.wpi.first.networktables; import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.util.concurrent.Event; import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.charset.StandardCharsets; import java.util.EnumSet; import java.util.HashMap; +import java.util.HashSet; import java.util.Map; import java.util.OptionalLong; +import java.util.Set; import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ConcurrentMap; import java.util.concurrent.TimeUnit; import java.util.concurrent.locks.Condition; import java.util.concurrent.locks.ReentrantLock; import java.util.function.Consumer; +import us.hebi.quickbuf.ProtoMessage; /** * NetworkTables Instance. @@ -86,6 +92,7 @@ public final class NetworkTableInstance implements AutoCloseable { public synchronized void close() { if (m_owned && m_handle != 0) { m_listeners.close(); + m_schemas.forEach((k, v) -> v.close()); NetworkTablesJNI.destroyInstance(m_handle); m_handle = 0; } @@ -176,15 +183,119 @@ public final class NetworkTableInstance implements AutoCloseable { handle = topic.getHandle(); } - topic = new {{ t.TypeName }}Topic(this, handle); - m_topics.put(name, topic); + {{ t.TypeName }}Topic wrapTopic = new {{ t.TypeName }}Topic(this, handle); + m_topics.put(name, wrapTopic); // also cache by handle - m_topicsByHandle.put(handle, topic); + m_topicsByHandle.put(handle, wrapTopic); - return ({{ t.TypeName }}Topic) topic; + return wrapTopic; } {% endfor %} + + /** + * Get protobuf-encoded value topic. + * + * @param value class (inferred from proto) + * @param protobuf message type (inferred from proto) + * @param name topic name + * @param proto protobuf serialization implementation + * @return ProtobufTopic + */ + public > + ProtobufTopic getProtobufTopic(String name, Protobuf proto) { + Topic topic = m_topics.get(name); + if (topic instanceof ProtobufTopic + && ((ProtobufTopic) topic).getProto().equals(proto)) { + @SuppressWarnings("unchecked") + ProtobufTopic wrapTopic = (ProtobufTopic) topic; + return wrapTopic; + } + + int handle; + if (topic == null) { + handle = NetworkTablesJNI.getTopic(m_handle, name); + } else { + handle = topic.getHandle(); + } + + ProtobufTopic wrapTopic = ProtobufTopic.wrap(this, handle, proto); + m_topics.put(name, wrapTopic); + + // also cache by handle + m_topicsByHandle.put(handle, wrapTopic); + + return wrapTopic; + } + + /** + * Get struct-encoded value topic. + * + * @param value class (inferred from struct) + * @param name topic name + * @param struct struct serialization implementation + * @return StructTopic + */ + public + StructTopic getStructTopic(String name, Struct struct) { + Topic topic = m_topics.get(name); + if (topic instanceof StructTopic + && ((StructTopic) topic).getStruct().equals(struct)) { + @SuppressWarnings("unchecked") + StructTopic wrapTopic = (StructTopic) topic; + return wrapTopic; + } + + int handle; + if (topic == null) { + handle = NetworkTablesJNI.getTopic(m_handle, name); + } else { + handle = topic.getHandle(); + } + + StructTopic wrapTopic = StructTopic.wrap(this, handle, struct); + m_topics.put(name, wrapTopic); + + // also cache by handle + m_topicsByHandle.put(handle, wrapTopic); + + return wrapTopic; + } + + /** + * Get struct-encoded value array topic. + * + * @param value class (inferred from struct) + * @param name topic name + * @param struct struct serialization implementation + * @return StructArrayTopic + */ + public + StructArrayTopic getStructArrayTopic(String name, Struct struct) { + Topic topic = m_topics.get(name); + if (topic instanceof StructArrayTopic + && ((StructArrayTopic) topic).getStruct().equals(struct)) { + @SuppressWarnings("unchecked") + StructArrayTopic wrapTopic = (StructArrayTopic) topic; + return wrapTopic; + } + + int handle; + if (topic == null) { + handle = NetworkTablesJNI.getTopic(m_handle, name); + } else { + handle = topic.getHandle(); + } + + StructArrayTopic wrapTopic = StructArrayTopic.wrap(this, handle, struct); + m_topics.put(name, wrapTopic); + + // also cache by handle + m_topicsByHandle.put(handle, wrapTopic); + + return wrapTopic; + } + private Topic[] topicHandlesToTopics(int[] handles) { Topic[] topics = new Topic[handles.length]; for (int i = 0; i < handles.length; i++) { @@ -1050,6 +1161,78 @@ public final class NetworkTableInstance implements AutoCloseable { return m_listeners.addLogger(minLevel, maxLevel, func); } + /** + * Returns whether there is a data schema already registered with the given name that this + * instance has published. This does NOT perform a check as to whether the schema has already + * been published by another node on the network. + * + * @param name Name (the string passed as the data type for topics using this schema) + * @return True if schema already registered + */ + public boolean hasSchema(String name) { + return m_schemas.containsKey("/.schema/" + name); + } + + /** + * Registers a data schema. Data schemas provide information for how a certain data type string + * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g. + * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In NetworkTables, schemas + * are published just like normal topics, with the name being generated from the provided name: + * "/.schema/name". Duplicate calls to this function with the same name are silently ignored. + * + * @param name Name (the string passed as the data type for topics using this schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + */ + public void addSchema(String name, String type, byte[] schema) { + m_schemas.computeIfAbsent("/.schema/" + name, k -> { + RawPublisher pub = getRawTopic(k).publishEx(type, "{\"retained\":true}"); + pub.setDefault(schema); + return pub; + }); + } + + /** + * Registers a data schema. Data schemas provide information for how a certain data type string + * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g. + * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In NetworkTables, schemas + * are published just like normal topics, with the name being generated from the provided name: + * "/.schema/name". Duplicate calls to this function with the same name are silently ignored. + * + * @param name Name (the string passed as the data type for topics using this schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + */ + public void addSchema(String name, String type, String schema) { + m_schemas.computeIfAbsent("/.schema/" + name, k -> { + RawPublisher pub = getRawTopic(k).publishEx(type, "{\"retained\":true}"); + pub.setDefault(StandardCharsets.UTF_8.encode(schema)); + return pub; + }); + } + + /** + * Registers a protobuf schema. Duplicate calls to this function with the same name are silently + * ignored. + * + * @param proto protobuf serialization object + */ + public void addSchema(Protobuf proto) { + proto.forEachDescriptor( + this::hasSchema, + (typeString, schema) -> addSchema(typeString, "proto:FileDescriptorProto", schema)); + } + + /** + * Registers a struct schema. Duplicate calls to this function with the same name are silently + * ignored. + * + * @param struct struct serialization object + */ + public void addSchema(Struct struct) { + addSchemaImpl(struct, new HashSet<>()); + } + @Override public boolean equals(Object other) { if (other == this) { @@ -1067,6 +1250,22 @@ public final class NetworkTableInstance implements AutoCloseable { return m_handle; } + private void addSchemaImpl(Struct struct, Set seen) { + String typeString = struct.getTypeString(); + if (hasSchema(typeString)) { + return; + } + if (!seen.add(typeString)) { + throw new UnsupportedOperationException(typeString + ": circular reference with " + seen); + } + addSchema(typeString, "structschema", struct.getSchema()); + for (Struct inner : struct.getNested()) { + addSchemaImpl(inner, seen); + } + seen.remove(typeString); + } + private boolean m_owned; private int m_handle; + private final ConcurrentMap m_schemas = new ConcurrentHashMap<>(); } diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java b/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java index cc38239228..3c6593809b 100644 --- a/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java +++ b/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java @@ -4,6 +4,8 @@ package edu.wpi.first.networktables; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; import java.util.ArrayList; import java.util.EnumSet; import java.util.HashSet; @@ -13,8 +15,10 @@ import java.util.Set; import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ConcurrentMap; import java.util.function.Consumer; +import us.hebi.quickbuf.ProtoMessage; /** A network table that knows its subtable path. */ +@SuppressWarnings("PMD.CouplingBetweenObjects") public final class NetworkTable { /** The path separator for sub-tables and keys. */ public static final char PATH_SEPARATOR = '/'; @@ -250,6 +254,44 @@ public final class NetworkTable { return m_inst.getStringArrayTopic(m_pathWithSep + name); } + /** + * Get protobuf-encoded value topic. + * + * @param value class (inferred from proto) + * @param protobuf message type (inferred from proto) + * @param name topic name + * @param proto protobuf serialization implementation + * @return ProtobufTopic + */ + public > ProtobufTopic getProtobufTopic( + String name, Protobuf proto) { + return m_inst.getProtobufTopic(m_pathWithSep + name, proto); + } + + /** + * Get struct-encoded value topic. + * + * @param value class (inferred from struct) + * @param name topic name + * @param struct struct serialization implementation + * @return StructTopic + */ + public StructTopic getStructTopic(String name, Struct struct) { + return m_inst.getStructTopic(m_pathWithSep + name, struct); + } + + /** + * Get struct-encoded value array topic. + * + * @param value class (inferred from struct) + * @param name topic name + * @param struct struct serialization implementation + * @return StructTopic + */ + public StructArrayTopic getStructArrayTopic(String name, Struct struct) { + return m_inst.getStructArrayTopic(m_pathWithSep + name, struct); + } + private final ConcurrentMap m_entries = new ConcurrentHashMap<>(); /** diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntry.java b/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntry.java new file mode 100644 index 0000000000..3f82c7ace4 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntry.java @@ -0,0 +1,17 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +/** + * NetworkTables protobuf-encoded value entry. + * + *

Unlike NetworkTableEntry, the entry goes away when close() is called. + * + * @param value class + */ +public interface ProtobufEntry extends ProtobufSubscriber, ProtobufPublisher { + /** Stops publishing the entry if it's published. */ + void unpublish(); +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntryImpl.java b/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntryImpl.java new file mode 100644 index 0000000000..b4359eaf03 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufEntryImpl.java @@ -0,0 +1,209 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import edu.wpi.first.util.protobuf.ProtobufBuffer; +import java.io.IOException; +import java.lang.reflect.Array; +import java.nio.ByteBuffer; + +/** + * NetworkTables protobuf-encoded value implementation. + * + * @param value class + */ +final class ProtobufEntryImpl extends EntryBase implements ProtobufEntry { + /** + * Constructor. + * + * @param topic Topic + * @param handle Native handle + * @param defaultValue Default value for get() + */ + ProtobufEntryImpl( + ProtobufTopic topic, + ProtobufBuffer buf, + int handle, + T defaultValue, + boolean schemaPublished) { + super(handle); + m_topic = topic; + m_defaultValue = defaultValue; + m_buf = buf; + m_schemaPublished = schemaPublished; + } + + @Override + public ProtobufTopic getTopic() { + return m_topic; + } + + @Override + public T get() { + return fromRaw(NetworkTablesJNI.getRaw(m_handle, m_emptyRaw), m_defaultValue); + } + + @Override + public T get(T defaultValue) { + return fromRaw(NetworkTablesJNI.getRaw(m_handle, m_emptyRaw), defaultValue); + } + + @Override + public boolean getInto(T out) { + byte[] raw = NetworkTablesJNI.getRaw(m_handle, m_emptyRaw); + if (raw.length == 0) { + return false; + } + try { + synchronized (m_buf) { + m_buf.readInto(out, raw); + return true; + } + } catch (IOException e) { + // ignored + } + return false; + } + + @Override + public TimestampedObject getAtomic() { + return fromRaw(NetworkTablesJNI.getAtomicRaw(m_handle, m_emptyRaw), m_defaultValue); + } + + @Override + public TimestampedObject getAtomic(T defaultValue) { + return fromRaw(NetworkTablesJNI.getAtomicRaw(m_handle, m_emptyRaw), defaultValue); + } + + @Override + public TimestampedObject[] readQueue() { + TimestampedRaw[] raw = NetworkTablesJNI.readQueueRaw(m_handle); + @SuppressWarnings("unchecked") + TimestampedObject[] arr = (TimestampedObject[]) new TimestampedObject[raw.length]; + int err = 0; + for (int i = 0; i < raw.length; i++) { + arr[i] = fromRaw(raw[i], null); + if (arr[i].value == null) { + err++; + } + } + + // discard bad values + if (err > 0) { + @SuppressWarnings("unchecked") + TimestampedObject[] newArr = + (TimestampedObject[]) new TimestampedObject[raw.length - err]; + int i = 0; + for (TimestampedObject e : arr) { + if (e.value != null) { + arr[i] = e; + i++; + } + } + arr = newArr; + } + + return arr; + } + + @Override + public T[] readQueueValues() { + byte[][] raw = NetworkTablesJNI.readQueueValuesRaw(m_handle); + @SuppressWarnings("unchecked") + T[] arr = (T[]) Array.newInstance(m_topic.getProto().getTypeClass(), raw.length); + int err = 0; + for (int i = 0; i < raw.length; i++) { + arr[i] = fromRaw(raw[i], null); + if (arr[i] == null) { + err++; + } + } + + // discard bad values + if (err > 0) { + @SuppressWarnings("unchecked") + T[] newArr = (T[]) Array.newInstance(m_topic.getProto().getTypeClass(), raw.length - err); + int i = 0; + for (T e : arr) { + if (e != null) { + arr[i] = e; + i++; + } + } + arr = newArr; + } + + return arr; + } + + @Override + public void set(T value, long time) { + try { + synchronized (m_buf) { + if (!m_schemaPublished) { + m_schemaPublished = true; + m_topic.getInstance().addSchema(m_buf.getProto()); + } + ByteBuffer bb = m_buf.write(value); + NetworkTablesJNI.setRaw(m_handle, time, bb, 0, bb.position()); + } + } catch (IOException e) { + // ignore + } + } + + @Override + public void setDefault(T value) { + try { + synchronized (m_buf) { + if (!m_schemaPublished) { + m_schemaPublished = true; + m_topic.getInstance().addSchema(m_buf.getProto()); + } + ByteBuffer bb = m_buf.write(value); + NetworkTablesJNI.setDefaultRaw(m_handle, 0, bb, 0, bb.position()); + } + } catch (IOException e) { + // ignore + } + } + + @Override + public void unpublish() { + NetworkTablesJNI.unpublish(m_handle); + } + + private T fromRaw(byte[] raw, T defaultValue) { + if (raw.length == 0) { + return defaultValue; + } + try { + synchronized (m_buf) { + return m_buf.read(raw); + } + } catch (IOException e) { + return defaultValue; + } + } + + private TimestampedObject fromRaw(TimestampedRaw raw, T defaultValue) { + if (raw.value.length == 0) { + return new TimestampedObject(0, 0, defaultValue); + } + try { + synchronized (m_buf) { + return new TimestampedObject(raw.timestamp, raw.serverTime, m_buf.read(raw.value)); + } + } catch (IOException e) { + return new TimestampedObject(0, 0, defaultValue); + } + } + + private final ProtobufTopic m_topic; + private final T m_defaultValue; + private final ProtobufBuffer m_buf; + private boolean m_schemaPublished; + private static final byte[] m_emptyRaw = new byte[] {}; +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufPublisher.java b/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufPublisher.java new file mode 100644 index 0000000000..ab22b86d13 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufPublisher.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import java.util.function.Consumer; + +/** + * NetworkTables protobuf-encoded value publisher. + * + * @param value class + */ +public interface ProtobufPublisher extends Publisher, Consumer { + /** + * Get the corresponding topic. + * + * @return Topic + */ + @Override + ProtobufTopic getTopic(); + + /** + * Publish a new value using current NT time. + * + * @param value value to publish + */ + default void set(T value) { + set(value, 0); + } + + /** + * Publish a new value. + * + * @param value value to publish + * @param time timestamp; 0 indicates current NT time should be used + */ + void set(T value, long time); + + /** + * Publish a default value. On reconnect, a default value will never be used in preference to a + * published value. + * + * @param value value + */ + void setDefault(T value); + + @Override + default void accept(T value) { + set(value); + } +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufSubscriber.java b/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufSubscriber.java new file mode 100644 index 0000000000..2236ce9a32 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufSubscriber.java @@ -0,0 +1,94 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import java.util.function.Supplier; + +/** + * NetworkTables protobuf-encoded value subscriber. + * + * @param value class + */ +@SuppressWarnings("PMD.MissingOverride") +public interface ProtobufSubscriber extends Subscriber, Supplier { + /** + * Get the corresponding topic. + * + * @return Topic + */ + @Override + ProtobufTopic getTopic(); + + /** + * Get the last published value. If no value has been published or the value cannot be unpacked, + * returns the stored default value. + * + * @return value + */ + T get(); + + /** + * Get the last published value. If no value has been published or the value cannot be unpacked, + * returns the passed defaultValue. + * + * @param defaultValue default value to return if no value has been published + * @return value + */ + T get(T defaultValue); + + /** + * Get the last published value, replacing the contents in place of an existing object. If no + * value has been published or the value cannot be unpacked, does not replace the contents and + * returns false. This function will not work (will throw UnsupportedOperationException) unless T + * is mutable (and the implementation of Struct implements unpackInto). + * + *

Note: due to Java language limitations, it's not possible to validate at compile time that + * the out parameter is mutable. + * + * @param out object to replace contents of; must be mutable + * @return true if successful + * @throws UnsupportedOperationException if T is immutable + */ + boolean getInto(T out); + + /** + * Get the last published value along with its timestamp. If no value has been published or the + * value cannot be unpacked, returns the stored default value and a timestamp of 0. + * + * @return timestamped value + */ + TimestampedObject getAtomic(); + + /** + * Get the last published value along with its timestamp. If no value has been published or the + * value cannot be unpacked, returns the passed defaultValue and a timestamp of 0. + * + * @param defaultValue default value to return if no value has been published + * @return timestamped value + */ + TimestampedObject getAtomic(T defaultValue); + + /** + * Get an array of all valid value changes since the last call to readQueue. Also provides a + * timestamp for each value. Values that cannot be unpacked are dropped. + * + *

The "poll storage" subscribe option can be used to set the queue depth. + * + * @return Array of timestamped values; empty array if no valid new changes have been published + * since the previous call. + */ + TimestampedObject[] readQueue(); + + /** + * Get an array of all valid value changes since the last call to readQueue. Values that cannot be + * unpacked are dropped. + * + *

The "poll storage" subscribe option can be used to set the queue depth. + * + * @return Array of values; empty array if no valid new changes have been published since the + * previous call. + */ + T[] readQueueValues(); +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufTopic.java b/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufTopic.java new file mode 100644 index 0000000000..c3dad133a5 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/ProtobufTopic.java @@ -0,0 +1,178 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.protobuf.ProtobufBuffer; + +/** + * NetworkTables protobuf-encoded value topic. + * + * @param value class + */ +public final class ProtobufTopic extends Topic { + private ProtobufTopic(Topic topic, Protobuf proto) { + super(topic.m_inst, topic.m_handle); + m_proto = proto; + } + + private ProtobufTopic(NetworkTableInstance inst, int handle, Protobuf proto) { + super(inst, handle); + m_proto = proto; + } + + /** + * Create a ProtobufTopic from a generic topic. + * + * @param value class (inferred from proto) + * @param topic generic topic + * @param proto protobuf serialization implementation + * @return ProtobufTopic for value class + */ + public static ProtobufTopic wrap(Topic topic, Protobuf proto) { + return new ProtobufTopic(topic, proto); + } + + /** + * Create a ProtobufTopic from a native handle; generally NetworkTableInstance.getProtobufTopic() + * should be used instead. + * + * @param value class (inferred from proto) + * @param inst Instance + * @param handle Native handle + * @param proto protobuf serialization implementation + * @return ProtobufTopic for value class + */ + public static ProtobufTopic wrap( + NetworkTableInstance inst, int handle, Protobuf proto) { + return new ProtobufTopic(inst, handle, proto); + } + + /** + * Create a new subscriber to the topic. + * + *

The subscriber is only active as long as the returned object is not closed. + * + *

Subscribers that do not match the published data type do not return any values. To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param defaultValue default value used when a default is not provided to a getter function + * @param options subscribe options + * @return subscriber + */ + public ProtobufSubscriber subscribe(T defaultValue, PubSubOption... options) { + return new ProtobufEntryImpl( + this, + ProtobufBuffer.create(m_proto), + NetworkTablesJNI.subscribe( + m_handle, NetworkTableType.kRaw.getValue(), m_proto.getTypeString(), options), + defaultValue, + false); + } + + /** + * Create a new publisher to the topic. + * + *

The publisher is only active as long as the returned object is not closed. + * + *

It is not possible to publish two different data types to the same topic. Conflicts between + * publishers are typically resolved by the server on a first-come, first-served basis. Any + * published values that do not match the topic's data type are dropped (ignored). To determine if + * the data type matches, use the appropriate Topic functions. + * + * @param options publish options + * @return publisher + */ + public ProtobufPublisher publish(PubSubOption... options) { + m_inst.addSchema(m_proto); + return new ProtobufEntryImpl( + this, + ProtobufBuffer.create(m_proto), + NetworkTablesJNI.publish( + m_handle, NetworkTableType.kRaw.getValue(), m_proto.getTypeString(), options), + null, + true); + } + + /** + * Create a new publisher to the topic, with type string and initial properties. + * + *

The publisher is only active as long as the returned object is not closed. + * + *

It is not possible to publish two different data types to the same topic. Conflicts between + * publishers are typically resolved by the server on a first-come, first-served basis. Any + * published values that do not match the topic's data type are dropped (ignored). To determine if + * the data type matches, use the appropriate Topic functions. + * + * @param properties JSON properties + * @param options publish options + * @return publisher + * @throws IllegalArgumentException if properties is not a JSON object + */ + public ProtobufPublisher publishEx(String properties, PubSubOption... options) { + m_inst.addSchema(m_proto); + return new ProtobufEntryImpl( + this, + ProtobufBuffer.create(m_proto), + NetworkTablesJNI.publishEx( + m_handle, + NetworkTableType.kRaw.getValue(), + m_proto.getTypeString(), + properties, + options), + null, + true); + } + + /** + * Create a new entry for the topic. + * + *

Entries act as a combination of a subscriber and a weak publisher. The subscriber is active + * as long as the entry is not closed. The publisher is created when the entry is first written + * to, and remains active until either unpublish() is called or the entry is closed. + * + *

It is not possible to use two different data types with the same topic. Conflicts between + * publishers are typically resolved by the server on a first-come, first-served basis. Any + * published values that do not match the topic's data type are dropped (ignored), and the entry + * will show no new values if the data type does not match. To determine if the data type matches, + * use the appropriate Topic functions. + * + * @param defaultValue default value used when a default is not provided to a getter function + * @param options publish and/or subscribe options + * @return entry + */ + public ProtobufEntry getEntry(T defaultValue, PubSubOption... options) { + return new ProtobufEntryImpl( + this, + ProtobufBuffer.create(m_proto), + NetworkTablesJNI.getEntry( + m_handle, NetworkTableType.kRaw.getValue(), m_proto.getTypeString(), options), + defaultValue, + false); + } + + public Protobuf getProto() { + return m_proto; + } + + @Override + public boolean equals(Object other) { + if (other == this) { + return true; + } + if (!(other instanceof ProtobufTopic)) { + return false; + } + + return super.equals(other) && m_proto == ((ProtobufTopic) other).m_proto; + } + + @Override + public int hashCode() { + return super.hashCode() ^ m_proto.hashCode(); + } + + private final Protobuf m_proto; +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntry.java b/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntry.java new file mode 100644 index 0000000000..4cc9e8593d --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntry.java @@ -0,0 +1,17 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +/** + * NetworkTables struct-encoded array value entry. + * + *

Unlike NetworkTableEntry, the entry goes away when close() is called. + * + * @param value class + */ +public interface StructArrayEntry extends StructArraySubscriber, StructArrayPublisher { + /** Stops publishing the entry if it's published. */ + void unpublish(); +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntryImpl.java b/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntryImpl.java new file mode 100644 index 0000000000..4e8a4a0495 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayEntryImpl.java @@ -0,0 +1,197 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import edu.wpi.first.util.struct.StructBuffer; +import java.lang.reflect.Array; +import java.nio.ByteBuffer; + +/** + * NetworkTables struct-encoded value implementation. + * + * @param value class + */ +@SuppressWarnings("PMD.ArrayIsStoredDirectly") +final class StructArrayEntryImpl extends EntryBase implements StructArrayEntry { + /** + * Constructor. + * + * @param topic Topic + * @param handle Native handle + * @param defaultValue Default value for get() + */ + StructArrayEntryImpl( + StructArrayTopic topic, + StructBuffer buf, + int handle, + T[] defaultValue, + boolean schemaPublished) { + super(handle); + m_topic = topic; + m_defaultValue = defaultValue; + m_buf = buf; + m_schemaPublished = schemaPublished; + } + + @Override + public StructArrayTopic getTopic() { + return m_topic; + } + + @Override + public T[] get() { + return fromRaw(NetworkTablesJNI.getRaw(m_handle, m_emptyRaw), m_defaultValue); + } + + @Override + public T[] get(T[] defaultValue) { + return fromRaw(NetworkTablesJNI.getRaw(m_handle, m_emptyRaw), defaultValue); + } + + @Override + public TimestampedObject getAtomic() { + return fromRaw(NetworkTablesJNI.getAtomicRaw(m_handle, m_emptyRaw), m_defaultValue); + } + + @Override + public TimestampedObject getAtomic(T[] defaultValue) { + return fromRaw(NetworkTablesJNI.getAtomicRaw(m_handle, m_emptyRaw), defaultValue); + } + + @Override + public TimestampedObject[] readQueue() { + TimestampedRaw[] raw = NetworkTablesJNI.readQueueRaw(m_handle); + @SuppressWarnings("unchecked") + TimestampedObject[] arr = (TimestampedObject[]) new TimestampedObject[raw.length]; + int err = 0; + for (int i = 0; i < raw.length; i++) { + arr[i] = fromRaw(raw[i], null); + if (arr[i].value == null) { + err++; + } + } + + // discard bad values + if (err > 0) { + @SuppressWarnings("unchecked") + TimestampedObject[] newArr = + (TimestampedObject[]) new TimestampedObject[raw.length - err]; + int i = 0; + for (TimestampedObject e : arr) { + if (e.value != null) { + arr[i] = e; + i++; + } + } + arr = newArr; + } + + return arr; + } + + @Override + public T[][] readQueueValues() { + byte[][] raw = NetworkTablesJNI.readQueueValuesRaw(m_handle); + @SuppressWarnings("unchecked") + T[][] arr = (T[][]) Array.newInstance(Array.class, raw.length); + int err = 0; + for (int i = 0; i < raw.length; i++) { + arr[i] = fromRaw(raw[i], null); + if (arr[i] == null) { + err++; + } + } + + // discard bad values + if (err > 0) { + @SuppressWarnings("unchecked") + T[][] newArr = (T[][]) Array.newInstance(Array.class, raw.length - err); + int i = 0; + for (T[] e : arr) { + if (e != null) { + arr[i] = e; + i++; + } + } + arr = newArr; + } + + return arr; + } + + @SuppressWarnings("PMD.AvoidCatchingGenericException") + @Override + public void set(T[] value, long time) { + try { + synchronized (m_buf) { + if (!m_schemaPublished) { + m_schemaPublished = true; + m_topic.getInstance().addSchema(m_buf.getStruct()); + } + ByteBuffer bb = m_buf.writeArray(value); + NetworkTablesJNI.setRaw(m_handle, time, bb, 0, bb.position()); + } + } catch (RuntimeException e) { + // ignore + } + } + + @SuppressWarnings("PMD.AvoidCatchingGenericException") + @Override + public void setDefault(T[] value) { + try { + synchronized (m_buf) { + if (!m_schemaPublished) { + m_schemaPublished = true; + m_topic.getInstance().addSchema(m_buf.getStruct()); + } + ByteBuffer bb = m_buf.writeArray(value); + NetworkTablesJNI.setDefaultRaw(m_handle, 0, bb, 0, bb.position()); + } + } catch (RuntimeException e) { + // ignore + } + } + + @Override + public void unpublish() { + NetworkTablesJNI.unpublish(m_handle); + } + + @SuppressWarnings("PMD.AvoidCatchingGenericException") + private T[] fromRaw(byte[] raw, T[] defaultValue) { + if (raw.length == 0) { + return defaultValue; + } + try { + synchronized (m_buf) { + return m_buf.readArray(raw); + } + } catch (RuntimeException e) { + return defaultValue; + } + } + + @SuppressWarnings("PMD.AvoidCatchingGenericException") + private TimestampedObject fromRaw(TimestampedRaw raw, T[] defaultValue) { + if (raw.value.length == 0) { + return new TimestampedObject(0, 0, defaultValue); + } + try { + synchronized (m_buf) { + return new TimestampedObject( + raw.timestamp, raw.serverTime, m_buf.readArray(raw.value)); + } + } catch (RuntimeException e) { + return new TimestampedObject(0, 0, defaultValue); + } + } + + private final StructArrayTopic m_topic; + private final T[] m_defaultValue; + private final StructBuffer m_buf; + private boolean m_schemaPublished; + private static final byte[] m_emptyRaw = new byte[] {}; +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayPublisher.java b/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayPublisher.java new file mode 100644 index 0000000000..aa291e9d49 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayPublisher.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import java.util.function.Consumer; + +/** + * NetworkTables struct-encoded array value publisher. + * + * @param value class + */ +public interface StructArrayPublisher extends Publisher, Consumer { + /** + * Get the corresponding topic. + * + * @return Topic + */ + @Override + StructArrayTopic getTopic(); + + /** + * Publish a new value using current NT time. + * + * @param value value to publish + */ + default void set(T[] value) { + set(value, 0); + } + + /** + * Publish a new value. + * + * @param value value to publish + * @param time timestamp; 0 indicates current NT time should be used + */ + void set(T[] value, long time); + + /** + * Publish a default value. On reconnect, a default value will never be used in preference to a + * published value. + * + * @param value value + */ + void setDefault(T[] value); + + @Override + default void accept(T[] value) { + set(value); + } +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/StructArraySubscriber.java b/ntcore/src/main/java/edu/wpi/first/networktables/StructArraySubscriber.java new file mode 100644 index 0000000000..b4755e6d1c --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/StructArraySubscriber.java @@ -0,0 +1,79 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import java.util.function.Supplier; + +/** + * NetworkTables struct-encoded array value subscriber. + * + * @param value class + */ +@SuppressWarnings("PMD.MissingOverride") +public interface StructArraySubscriber extends Subscriber, Supplier { + /** + * Get the corresponding topic. + * + * @return Topic + */ + @Override + StructArrayTopic getTopic(); + + /** + * Get the last published value. If no value has been published or the value cannot be unpacked, + * returns the stored default value. + * + * @return value + */ + T[] get(); + + /** + * Get the last published value. If no value has been published or the value cannot be unpacked, + * returns the passed defaultValue. + * + * @param defaultValue default value to return if no value has been published + * @return value + */ + T[] get(T[] defaultValue); + + /** + * Get the last published value along with its timestamp. If no value has been published or the + * value cannot be unpacked, returns the stored default value and a timestamp of 0. + * + * @return timestamped value + */ + TimestampedObject getAtomic(); + + /** + * Get the last published value along with its timestamp. If no value has been published or the + * value cannot be unpacked, returns the passed defaultValue and a timestamp of 0. + * + * @param defaultValue default value to return if no value has been published + * @return timestamped value + */ + TimestampedObject getAtomic(T[] defaultValue); + + /** + * Get an array of all valid value changes since the last call to readQueue. Also provides a + * timestamp for each value. Values that cannot be unpacked are dropped. + * + *

The "poll storage" subscribe option can be used to set the queue depth. + * + * @return Array of timestamped values; empty array if no valid new changes have been published + * since the previous call. + */ + TimestampedObject[] readQueue(); + + /** + * Get an array of all valid value changes since the last call to readQueue. Values that cannot be + * unpacked are dropped. + * + *

The "poll storage" subscribe option can be used to set the queue depth. + * + * @return Array of values; empty array if no valid new changes have been published since the + * previous call. + */ + T[][] readQueueValues(); +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayTopic.java b/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayTopic.java new file mode 100644 index 0000000000..247501bbfa --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/StructArrayTopic.java @@ -0,0 +1,178 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructBuffer; + +/** + * NetworkTables struct-encoded array value topic. + * + * @param value class + */ +public final class StructArrayTopic extends Topic { + private StructArrayTopic(Topic topic, Struct struct) { + super(topic.m_inst, topic.m_handle); + m_struct = struct; + } + + private StructArrayTopic(NetworkTableInstance inst, int handle, Struct struct) { + super(inst, handle); + m_struct = struct; + } + + /** + * Create a StructArrayTopic from a generic topic. + * + * @param value class (inferred from struct) + * @param topic generic topic + * @param struct struct serialization implementation + * @return StructArrayTopic for value class + */ + public static StructArrayTopic wrap(Topic topic, Struct struct) { + return new StructArrayTopic(topic, struct); + } + + /** + * Create a StructArrayTopic from a native handle; generally + * NetworkTableInstance.getStructArrayTopic() should be used instead. + * + * @param value class (inferred from struct) + * @param inst Instance + * @param handle Native handle + * @param struct struct serialization implementation + * @return StructArrayTopic for value class + */ + public static StructArrayTopic wrap( + NetworkTableInstance inst, int handle, Struct struct) { + return new StructArrayTopic(inst, handle, struct); + } + + /** + * Create a new subscriber to the topic. + * + *

The subscriber is only active as long as the returned object is not closed. + * + *

Subscribers that do not match the published data type do not return any values. To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param defaultValue default value used when a default is not provided to a getter function + * @param options subscribe options + * @return subscriber + */ + public StructArraySubscriber subscribe(T[] defaultValue, PubSubOption... options) { + return new StructArrayEntryImpl( + this, + StructBuffer.create(m_struct), + NetworkTablesJNI.subscribe( + m_handle, NetworkTableType.kRaw.getValue(), m_struct.getTypeString() + "[]", options), + defaultValue, + false); + } + + /** + * Create a new publisher to the topic. + * + *

The publisher is only active as long as the returned object is not closed. + * + *

It is not possible to publish two different data types to the same topic. Conflicts between + * publishers are typically resolved by the server on a first-come, first-served basis. Any + * published values that do not match the topic's data type are dropped (ignored). To determine if + * the data type matches, use the appropriate Topic functions. + * + * @param options publish options + * @return publisher + */ + public StructArrayPublisher publish(PubSubOption... options) { + m_inst.addSchema(m_struct); + return new StructArrayEntryImpl( + this, + StructBuffer.create(m_struct), + NetworkTablesJNI.publish( + m_handle, NetworkTableType.kRaw.getValue(), m_struct.getTypeString() + "[]", options), + null, + true); + } + + /** + * Create a new publisher to the topic, with type string and initial properties. + * + *

The publisher is only active as long as the returned object is not closed. + * + *

It is not possible to publish two different data types to the same topic. Conflicts between + * publishers are typically resolved by the server on a first-come, first-served basis. Any + * published values that do not match the topic's data type are dropped (ignored). To determine if + * the data type matches, use the appropriate Topic functions. + * + * @param properties JSON properties + * @param options publish options + * @return publisher + * @throws IllegalArgumentException if properties is not a JSON object + */ + public StructArrayPublisher publishEx(String properties, PubSubOption... options) { + m_inst.addSchema(m_struct); + return new StructArrayEntryImpl( + this, + StructBuffer.create(m_struct), + NetworkTablesJNI.publishEx( + m_handle, + NetworkTableType.kRaw.getValue(), + m_struct.getTypeString() + "[]", + properties, + options), + null, + true); + } + + /** + * Create a new entry for the topic. + * + *

Entries act as a combination of a subscriber and a weak publisher. The subscriber is active + * as long as the entry is not closed. The publisher is created when the entry is first written + * to, and remains active until either unpublish() is called or the entry is closed. + * + *

It is not possible to use two different data types with the same topic. Conflicts between + * publishers are typically resolved by the server on a first-come, first-served basis. Any + * published values that do not match the topic's data type are dropped (ignored), and the entry + * will show no new values if the data type does not match. To determine if the data type matches, + * use the appropriate Topic functions. + * + * @param defaultValue default value used when a default is not provided to a getter function + * @param options publish and/or subscribe options + * @return entry + */ + public StructArrayEntry getEntry(T[] defaultValue, PubSubOption... options) { + return new StructArrayEntryImpl( + this, + StructBuffer.create(m_struct), + NetworkTablesJNI.getEntry( + m_handle, NetworkTableType.kRaw.getValue(), m_struct.getTypeString() + "[]", options), + defaultValue, + false); + } + + public Struct getStruct() { + return m_struct; + } + + @Override + public boolean equals(Object other) { + if (other == this) { + return true; + } + if (!(other instanceof StructArrayTopic)) { + return false; + } + + return super.equals(other) && m_struct == ((StructArrayTopic) other).m_struct; + } + + @Override + public int hashCode() { + return super.hashCode() ^ m_struct.hashCode(); + } + + private final Struct m_struct; +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/StructEntry.java b/ntcore/src/main/java/edu/wpi/first/networktables/StructEntry.java new file mode 100644 index 0000000000..e687fdb452 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/StructEntry.java @@ -0,0 +1,17 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +/** + * NetworkTables struct-encoded value entry. + * + *

Unlike NetworkTableEntry, the entry goes away when close() is called. + * + * @param value class + */ +public interface StructEntry extends StructSubscriber, StructPublisher { + /** Stops publishing the entry if it's published. */ + void unpublish(); +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/StructEntryImpl.java b/ntcore/src/main/java/edu/wpi/first/networktables/StructEntryImpl.java new file mode 100644 index 0000000000..bd02d27fe5 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/StructEntryImpl.java @@ -0,0 +1,207 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import edu.wpi.first.util.struct.StructBuffer; +import java.lang.reflect.Array; +import java.nio.ByteBuffer; + +/** + * NetworkTables struct-encoded value implementation. + * + * @param value class + */ +final class StructEntryImpl extends EntryBase implements StructEntry { + /** + * Constructor. + * + * @param topic Topic + * @param handle Native handle + * @param defaultValue Default value for get() + */ + StructEntryImpl( + StructTopic topic, + StructBuffer buf, + int handle, + T defaultValue, + boolean schemaPublished) { + super(handle); + m_topic = topic; + m_defaultValue = defaultValue; + m_buf = buf; + m_schemaPublished = schemaPublished; + } + + @Override + public StructTopic getTopic() { + return m_topic; + } + + @Override + public T get() { + return fromRaw(NetworkTablesJNI.getRaw(m_handle, m_emptyRaw), m_defaultValue); + } + + @Override + public T get(T defaultValue) { + return fromRaw(NetworkTablesJNI.getRaw(m_handle, m_emptyRaw), defaultValue); + } + + @Override + public boolean getInto(T out) { + byte[] raw = NetworkTablesJNI.getRaw(m_handle, m_emptyRaw); + if (raw.length == 0) { + return false; + } + synchronized (m_buf) { + m_buf.readInto(out, raw); + return true; + } + } + + @Override + public TimestampedObject getAtomic() { + return fromRaw(NetworkTablesJNI.getAtomicRaw(m_handle, m_emptyRaw), m_defaultValue); + } + + @Override + public TimestampedObject getAtomic(T defaultValue) { + return fromRaw(NetworkTablesJNI.getAtomicRaw(m_handle, m_emptyRaw), defaultValue); + } + + @Override + public TimestampedObject[] readQueue() { + TimestampedRaw[] raw = NetworkTablesJNI.readQueueRaw(m_handle); + @SuppressWarnings("unchecked") + TimestampedObject[] arr = (TimestampedObject[]) new TimestampedObject[raw.length]; + int err = 0; + for (int i = 0; i < raw.length; i++) { + arr[i] = fromRaw(raw[i], null); + if (arr[i].value == null) { + err++; + } + } + + // discard bad values + if (err > 0) { + @SuppressWarnings("unchecked") + TimestampedObject[] newArr = + (TimestampedObject[]) new TimestampedObject[raw.length - err]; + int i = 0; + for (TimestampedObject e : arr) { + if (e.value != null) { + arr[i] = e; + i++; + } + } + arr = newArr; + } + + return arr; + } + + @Override + public T[] readQueueValues() { + byte[][] raw = NetworkTablesJNI.readQueueValuesRaw(m_handle); + @SuppressWarnings("unchecked") + T[] arr = (T[]) Array.newInstance(m_topic.getStruct().getTypeClass(), raw.length); + int err = 0; + for (int i = 0; i < raw.length; i++) { + arr[i] = fromRaw(raw[i], null); + if (arr[i] == null) { + err++; + } + } + + // discard bad values + if (err > 0) { + @SuppressWarnings("unchecked") + T[] newArr = (T[]) Array.newInstance(m_topic.getStruct().getTypeClass(), raw.length - err); + int i = 0; + for (T e : arr) { + if (e != null) { + arr[i] = e; + i++; + } + } + arr = newArr; + } + + return arr; + } + + @SuppressWarnings("PMD.AvoidCatchingGenericException") + @Override + public void set(T value, long time) { + try { + synchronized (m_buf) { + if (!m_schemaPublished) { + m_schemaPublished = true; + m_topic.getInstance().addSchema(m_buf.getStruct()); + } + ByteBuffer bb = m_buf.write(value); + NetworkTablesJNI.setRaw(m_handle, time, bb, 0, bb.position()); + } + } catch (RuntimeException e) { + // ignore + } + } + + @SuppressWarnings("PMD.AvoidCatchingGenericException") + @Override + public void setDefault(T value) { + try { + synchronized (m_buf) { + if (!m_schemaPublished) { + m_schemaPublished = true; + m_topic.getInstance().addSchema(m_buf.getStruct()); + } + ByteBuffer bb = m_buf.write(value); + NetworkTablesJNI.setDefaultRaw(m_handle, 0, bb, 0, bb.position()); + } + } catch (RuntimeException e) { + // ignore + } + } + + @Override + public void unpublish() { + NetworkTablesJNI.unpublish(m_handle); + } + + @SuppressWarnings("PMD.AvoidCatchingGenericException") + private T fromRaw(byte[] raw, T defaultValue) { + if (raw.length == 0) { + return defaultValue; + } + try { + synchronized (m_buf) { + return m_buf.read(raw); + } + } catch (RuntimeException e) { + return defaultValue; + } + } + + @SuppressWarnings("PMD.AvoidCatchingGenericException") + private TimestampedObject fromRaw(TimestampedRaw raw, T defaultValue) { + if (raw.value.length == 0) { + return new TimestampedObject(0, 0, defaultValue); + } + try { + synchronized (m_buf) { + return new TimestampedObject(raw.timestamp, raw.serverTime, m_buf.read(raw.value)); + } + } catch (RuntimeException e) { + return new TimestampedObject(0, 0, defaultValue); + } + } + + private final StructTopic m_topic; + private final T m_defaultValue; + private final StructBuffer m_buf; + private boolean m_schemaPublished; + private static final byte[] m_emptyRaw = new byte[] {}; +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/StructPublisher.java b/ntcore/src/main/java/edu/wpi/first/networktables/StructPublisher.java new file mode 100644 index 0000000000..ec766e4775 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/StructPublisher.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import java.util.function.Consumer; + +/** + * NetworkTables struct-encoded value publisher. + * + * @param value class + */ +public interface StructPublisher extends Publisher, Consumer { + /** + * Get the corresponding topic. + * + * @return Topic + */ + @Override + StructTopic getTopic(); + + /** + * Publish a new value using current NT time. + * + * @param value value to publish + */ + default void set(T value) { + set(value, 0); + } + + /** + * Publish a new value. + * + * @param value value to publish + * @param time timestamp; 0 indicates current NT time should be used + */ + void set(T value, long time); + + /** + * Publish a default value. On reconnect, a default value will never be used in preference to a + * published value. + * + * @param value value + */ + void setDefault(T value); + + @Override + default void accept(T value) { + set(value); + } +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/StructSubscriber.java b/ntcore/src/main/java/edu/wpi/first/networktables/StructSubscriber.java new file mode 100644 index 0000000000..b537b5cfc3 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/StructSubscriber.java @@ -0,0 +1,94 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import java.util.function.Supplier; + +/** + * NetworkTables struct-encoded value subscriber. + * + * @param value class + */ +@SuppressWarnings("PMD.MissingOverride") +public interface StructSubscriber extends Subscriber, Supplier { + /** + * Get the corresponding topic. + * + * @return Topic + */ + @Override + StructTopic getTopic(); + + /** + * Get the last published value. If no value has been published or the value cannot be unpacked, + * returns the stored default value. + * + * @return value + */ + T get(); + + /** + * Get the last published value. If no value has been published or the value cannot be unpacked, + * returns the passed defaultValue. + * + * @param defaultValue default value to return if no value has been published + * @return value + */ + T get(T defaultValue); + + /** + * Get the last published value, replacing the contents in place of an existing object. If no + * value has been published or the value cannot be unpacked, does not replace the contents and + * returns false. This function will not work (will throw UnsupportedOperationException) unless T + * is mutable (and the implementation of Struct implements unpackInto). + * + *

Note: due to Java language limitations, it's not possible to validate at compile time that + * the out parameter is mutable. + * + * @param out object to replace contents of; must be mutable + * @return true if successful, false if no value has been published + * @throws UnsupportedOperationException if T is immutable + */ + boolean getInto(T out); + + /** + * Get the last published value along with its timestamp. If no value has been published or the + * value cannot be unpacked, returns the stored default value and a timestamp of 0. + * + * @return timestamped value + */ + TimestampedObject getAtomic(); + + /** + * Get the last published value along with its timestamp If no value has been published or the + * value cannot be unpacked, returns the passed defaultValue and a timestamp of 0. + * + * @param defaultValue default value to return if no value has been published + * @return timestamped value + */ + TimestampedObject getAtomic(T defaultValue); + + /** + * Get an array of all valid value changes since the last call to readQueue. Also provides a + * timestamp for each value. Values that cannot be unpacked are dropped. + * + *

The "poll storage" subscribe option can be used to set the queue depth. + * + * @return Array of timestamped values; empty array if no valid new changes have been published + * since the previous call. + */ + TimestampedObject[] readQueue(); + + /** + * Get an array of all value changes since the last call to readQueue. Values that cannot be + * unpacked are dropped. + * + *

The "poll storage" subscribe option can be used to set the queue depth. + * + * @return Array of values; empty array if no valid new changes have been published since the + * previous call. + */ + T[] readQueueValues(); +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/StructTopic.java b/ntcore/src/main/java/edu/wpi/first/networktables/StructTopic.java new file mode 100644 index 0000000000..b1ff0264f3 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/StructTopic.java @@ -0,0 +1,177 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructBuffer; + +/** + * NetworkTables struct-encoded value topic. + * + * @param value class + */ +public final class StructTopic extends Topic { + private StructTopic(Topic topic, Struct struct) { + super(topic.m_inst, topic.m_handle); + m_struct = struct; + } + + private StructTopic(NetworkTableInstance inst, int handle, Struct struct) { + super(inst, handle); + m_struct = struct; + } + + /** + * Create a StructTopic from a generic topic. + * + * @param value class (inferred from struct) + * @param topic generic topic + * @param struct struct serialization implementation + * @return StructTopic for value class + */ + public static StructTopic wrap(Topic topic, Struct struct) { + return new StructTopic(topic, struct); + } + + /** + * Create a StructTopic from a native handle; generally NetworkTableInstance.getStructTopic() + * should be used instead. + * + * @param value class (inferred from struct) + * @param inst Instance + * @param handle Native handle + * @param struct struct serialization implementation + * @return StructTopic for value class + */ + public static StructTopic wrap(NetworkTableInstance inst, int handle, Struct struct) { + return new StructTopic(inst, handle, struct); + } + + /** + * Create a new subscriber to the topic. + * + *

The subscriber is only active as long as the returned object is not closed. + * + *

Subscribers that do not match the published data type do not return any values. To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param defaultValue default value used when a default is not provided to a getter function + * @param options subscribe options + * @return subscriber + */ + public StructSubscriber subscribe(T defaultValue, PubSubOption... options) { + return new StructEntryImpl( + this, + StructBuffer.create(m_struct), + NetworkTablesJNI.subscribe( + m_handle, NetworkTableType.kRaw.getValue(), m_struct.getTypeString(), options), + defaultValue, + false); + } + + /** + * Create a new publisher to the topic. + * + *

The publisher is only active as long as the returned object is not closed. + * + *

It is not possible to publish two different data types to the same topic. Conflicts between + * publishers are typically resolved by the server on a first-come, first-served basis. Any + * published values that do not match the topic's data type are dropped (ignored). To determine if + * the data type matches, use the appropriate Topic functions. + * + * @param options publish options + * @return publisher + */ + public StructPublisher publish(PubSubOption... options) { + m_inst.addSchema(m_struct); + return new StructEntryImpl( + this, + StructBuffer.create(m_struct), + NetworkTablesJNI.publish( + m_handle, NetworkTableType.kRaw.getValue(), m_struct.getTypeString(), options), + null, + true); + } + + /** + * Create a new publisher to the topic, with type string and initial properties. + * + *

The publisher is only active as long as the returned object is not closed. + * + *

It is not possible to publish two different data types to the same topic. Conflicts between + * publishers are typically resolved by the server on a first-come, first-served basis. Any + * published values that do not match the topic's data type are dropped (ignored). To determine if + * the data type matches, use the appropriate Topic functions. + * + * @param properties JSON properties + * @param options publish options + * @return publisher + * @throws IllegalArgumentException if properties is not a JSON object + */ + public StructPublisher publishEx(String properties, PubSubOption... options) { + m_inst.addSchema(m_struct); + return new StructEntryImpl( + this, + StructBuffer.create(m_struct), + NetworkTablesJNI.publishEx( + m_handle, + NetworkTableType.kRaw.getValue(), + m_struct.getTypeString(), + properties, + options), + null, + true); + } + + /** + * Create a new entry for the topic. + * + *

Entries act as a combination of a subscriber and a weak publisher. The subscriber is active + * as long as the entry is not closed. The publisher is created when the entry is first written + * to, and remains active until either unpublish() is called or the entry is closed. + * + *

It is not possible to use two different data types with the same topic. Conflicts between + * publishers are typically resolved by the server on a first-come, first-served basis. Any + * published values that do not match the topic's data type are dropped (ignored), and the entry + * will show no new values if the data type does not match. To determine if the data type matches, + * use the appropriate Topic functions. + * + * @param defaultValue default value used when a default is not provided to a getter function + * @param options publish and/or subscribe options + * @return entry + */ + public StructEntry getEntry(T defaultValue, PubSubOption... options) { + return new StructEntryImpl( + this, + StructBuffer.create(m_struct), + NetworkTablesJNI.getEntry( + m_handle, NetworkTableType.kRaw.getValue(), m_struct.getTypeString(), options), + defaultValue, + false); + } + + public Struct getStruct() { + return m_struct; + } + + @Override + public boolean equals(Object other) { + if (other == this) { + return true; + } + if (!(other instanceof StructTopic)) { + return false; + } + + return super.equals(other) && m_struct == ((StructTopic) other).m_struct; + } + + @Override + public int hashCode() { + return super.hashCode() ^ m_struct.hashCode(); + } + + private final Struct m_struct; +} diff --git a/ntcore/src/main/java/edu/wpi/first/networktables/TimestampedObject.java b/ntcore/src/main/java/edu/wpi/first/networktables/TimestampedObject.java new file mode 100644 index 0000000000..37c1544793 --- /dev/null +++ b/ntcore/src/main/java/edu/wpi/first/networktables/TimestampedObject.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.networktables; + +/** NetworkTables timestamped object. */ +public final class TimestampedObject { + /** + * Create a timestamped value. + * + * @param timestamp timestamp in local time base + * @param serverTime timestamp in server time base + * @param value value + */ + public TimestampedObject(long timestamp, long serverTime, T value) { + this.timestamp = timestamp; + this.serverTime = serverTime; + this.value = value; + } + + /** Timestamp in local time base. */ + @SuppressWarnings("MemberName") + public final long timestamp; + + /** Timestamp in server time base. May be 0 or 1 for locally set values. */ + @SuppressWarnings("MemberName") + public final long serverTime; + + /** Value. */ + @SuppressWarnings("MemberName") + public final T value; +} diff --git a/ntcore/src/main/native/cpp/LocalStorage.cpp b/ntcore/src/main/native/cpp/LocalStorage.cpp index 98ab73bc02..5c732e4253 100644 --- a/ntcore/src/main/native/cpp/LocalStorage.cpp +++ b/ntcore/src/main/native/cpp/LocalStorage.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -1481,6 +1482,41 @@ void LocalStorage::StopDataLog(NT_DataLogger logger) { } } +bool LocalStorage::HasSchema(std::string_view name) { + std::scoped_lock lock{m_mutex}; + wpi::SmallString<128> fullName{"/.schema/"}; + fullName += name; + auto it = m_impl.m_schemas.find(fullName); + return it != m_impl.m_schemas.end(); +} + +void LocalStorage::AddSchema(std::string_view name, std::string_view type, + std::span schema) { + std::scoped_lock lock{m_mutex}; + wpi::SmallString<128> fullName{"/.schema/"}; + fullName += name; + auto& pubHandle = m_impl.m_schemas[fullName]; + if (pubHandle != 0) { + return; + } + + auto topic = m_impl.GetOrCreateTopic(fullName); + + if (topic->localPublishers.size() >= kMaxPublishers) { + WPI_ERROR(m_impl.m_logger, + "reached maximum number of publishers to '{}', not publishing", + topic->name); + return; + } + + pubHandle = m_impl + .AddLocalPublisher(topic, {{"retained", true}}, + PubSubConfig{NT_RAW, type, {}}) + ->handle; + + m_impl.SetDefaultEntryValue(pubHandle, Value::MakeRaw(schema)); +} + void LocalStorage::Reset() { std::scoped_lock lock{m_mutex}; m_impl.m_network = nullptr; diff --git a/ntcore/src/main/native/cpp/LocalStorage.h b/ntcore/src/main/native/cpp/LocalStorage.h index 086c0574eb..af2b4debe0 100644 --- a/ntcore/src/main/native/cpp/LocalStorage.h +++ b/ntcore/src/main/native/cpp/LocalStorage.h @@ -321,6 +321,13 @@ class LocalStorage final : public net::ILocalStorage { std::string_view logPrefix); void StopDataLog(NT_DataLogger logger); + // + // Schema functions + // + bool HasSchema(std::string_view name); + void AddSchema(std::string_view name, std::string_view type, + std::span schema); + void Reset(); private: @@ -549,6 +556,9 @@ class LocalStorage final : public net::ILocalStorage { // string-based listeners VectorSet m_topicPrefixListeners; + // schema publishers + wpi::StringMap m_schemas; + // topic functions void NotifyTopic(TopicData* topic, unsigned int eventFlags); diff --git a/ntcore/src/main/native/cpp/networktables/Topic.cpp b/ntcore/src/main/native/cpp/networktables/Topic.cpp index e3f6ac02e8..1188667acd 100644 --- a/ntcore/src/main/native/cpp/networktables/Topic.cpp +++ b/ntcore/src/main/native/cpp/networktables/Topic.cpp @@ -7,9 +7,14 @@ #include #include "networktables/GenericEntry.h" +#include "networktables/NetworkTableInstance.h" using namespace nt; +NetworkTableInstance Topic::GetInstance() const { + return NetworkTableInstance{GetInstanceFromHandle(m_handle)}; +} + wpi::json Topic::GetProperty(std::string_view name) const { return ::nt::GetTopicProperty(m_handle, name); } diff --git a/ntcore/src/main/native/cpp/ntcore_c.cpp b/ntcore/src/main/native/cpp/ntcore_c.cpp index d6844d8e5b..b4326646fd 100644 --- a/ntcore/src/main/native/cpp/ntcore_c.cpp +++ b/ntcore/src/main/native/cpp/ntcore_c.cpp @@ -625,6 +625,15 @@ NT_Listener NT_AddPolledLogger(NT_ListenerPoller poller, unsigned int min_level, return nt::AddPolledLogger(poller, min_level, max_level); } +NT_Bool NT_HasSchema(NT_Inst inst, const char* name) { + return nt::HasSchema(inst, name); +} + +void NT_AddSchema(NT_Inst inst, const char* name, const char* type, + const uint8_t* schema, size_t schemaSize) { + nt::AddSchema(inst, name, type, {schema, schemaSize}); +} + void NT_DisposeValue(NT_Value* value) { switch (value->type) { case NT_UNASSIGNED: diff --git a/ntcore/src/main/native/cpp/ntcore_cpp.cpp b/ntcore/src/main/native/cpp/ntcore_cpp.cpp index 24f1f89991..4b48f050df 100644 --- a/ntcore/src/main/native/cpp/ntcore_cpp.cpp +++ b/ntcore/src/main/native/cpp/ntcore_cpp.cpp @@ -782,4 +782,19 @@ NT_Listener AddPolledLogger(NT_ListenerPoller poller, unsigned int minLevel, } } +bool HasSchema(NT_Inst inst, std::string_view name) { + if (auto ii = InstanceImpl::GetTyped(inst, Handle::kInstance)) { + return ii->localStorage.HasSchema(name); + } else { + return false; + } +} + +void AddSchema(NT_Inst inst, std::string_view name, std::string_view type, + std::span schema) { + if (auto ii = InstanceImpl::GetTyped(inst, Handle::kInstance)) { + ii->localStorage.AddSchema(name, type, schema); + } +} + } // namespace nt diff --git a/ntcore/src/main/native/include/networktables/NetworkTable.h b/ntcore/src/main/native/include/networktables/NetworkTable.h index d34f54bf2c..e03ea9e99a 100644 --- a/ntcore/src/main/native/include/networktables/NetworkTable.h +++ b/ntcore/src/main/native/include/networktables/NetworkTable.h @@ -14,8 +14,11 @@ #include #include +#include +#include #include "networktables/NetworkTableEntry.h" +#include "networktables/Topic.h" #include "ntcore_c.h" namespace nt { @@ -29,9 +32,15 @@ class FloatTopic; class IntegerArrayTopic; class IntegerTopic; class NetworkTableInstance; +template +class ProtobufTopic; class RawTopic; class StringArrayTopic; class StringTopic; +template +class StructArrayTopic; +template +class StructTopic; class Topic; /** @@ -220,6 +229,39 @@ class NetworkTable final { */ StringArrayTopic GetStringArrayTopic(std::string_view name) const; + /** + * Gets a protobuf serialized value topic. + * + * @param name topic name + * @return Topic + */ + template + ProtobufTopic GetProtobufTopic(std::string_view name) const { + return ProtobufTopic{GetTopic(name)}; + } + + /** + * Gets a raw struct serialized value topic. + * + * @param name topic name + * @return Topic + */ + template + StructTopic GetStructTopic(std::string_view name) const { + return StructTopic{GetTopic(name)}; + } + + /** + * Gets a raw struct serialized array topic. + * + * @param name topic name + * @return Topic + */ + template + StructArrayTopic GetStructArrayTopic(std::string_view name) const { + return StructArrayTopic{GetTopic(name)}; + } + /** * Returns the table at the specified key. If there is no table at the * specified key, it will create a new table diff --git a/ntcore/src/main/native/include/networktables/NetworkTableInstance.h b/ntcore/src/main/native/include/networktables/NetworkTableInstance.h index bcd30a9891..06e2cd6483 100644 --- a/ntcore/src/main/native/include/networktables/NetworkTableInstance.h +++ b/ntcore/src/main/native/include/networktables/NetworkTableInstance.h @@ -13,6 +13,9 @@ #include #include +#include +#include + #include "networktables/NetworkTable.h" #include "networktables/NetworkTableEntry.h" #include "ntcore_c.h" @@ -29,9 +32,15 @@ class FloatTopic; class IntegerArrayTopic; class IntegerTopic; class MultiSubscriber; +template +class ProtobufTopic; class RawTopic; class StringArrayTopic; class StringTopic; +template +class StructArrayTopic; +template +class StructTopic; class Subscriber; class Topic; @@ -238,6 +247,33 @@ class NetworkTableInstance final { */ StringArrayTopic GetStringArrayTopic(std::string_view name) const; + /** + * Gets a protobuf serialized value topic. + * + * @param name topic name + * @return Topic + */ + template + ProtobufTopic GetProtobufTopic(std::string_view name) const; + + /** + * Gets a raw struct serialized value topic. + * + * @param name topic name + * @return Topic + */ + template + StructTopic GetStructTopic(std::string_view name) const; + + /** + * Gets a raw struct serialized array topic. + * + * @param name topic name + * @return Topic + */ + template + StructArrayTopic GetStructArrayTopic(std::string_view name) const; + /** * Get Published Topics. * @@ -718,6 +754,75 @@ class NetworkTableInstance final { /** @} */ + /** + * @{ + * @name Schema Functions + */ + + /** + * Returns whether there is a data schema already registered with the given + * name. This does NOT perform a check as to whether the schema has already + * been published by another node on the network. + * + * @param name Name (the string passed as the data type for topics using this + * schema) + * @return True if schema already registered + */ + bool HasSchema(std::string_view name) const; + + /** + * Registers a data schema. Data schemas provide information for how a + * certain data type string can be decoded. The type string of a data schema + * indicates the type of the schema itself (e.g. "protobuf" for protobuf + * schemas, "struct" for struct schemas, etc). In NetworkTables, schemas are + * published just like normal topics, with the name being generated from the + * provided name: "/.schema/". Duplicate calls to this function with + * the same name are silently ignored. + * + * @param name Name (the string passed as the data type for topics using this + * schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + */ + void AddSchema(std::string_view name, std::string_view type, + std::span schema); + + /** + * Registers a data schema. Data schemas provide information for how a + * certain data type string can be decoded. The type string of a data schema + * indicates the type of the schema itself (e.g. "protobuf" for protobuf + * schemas, "struct" for struct schemas, etc). In NetworkTables, schemas are + * published just like normal topics, with the name being generated from the + * provided name: "/.schema/". Duplicate calls to this function with + * the same name are silently ignored. + * + * @param name Name (the string passed as the data type for topics using this + * schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + */ + void AddSchema(std::string_view name, std::string_view type, + std::string_view schema); + + /** + * Registers a protobuf schema. Duplicate calls to this function with the same + * name are silently ignored. + * + * @tparam T protobuf serializable type + * @param msg protobuf message + */ + template + void AddProtobufSchema(wpi::ProtobufMessage& msg); + + /** + * Registers a struct schema. Duplicate calls to this function with the same + * name are silently ignored. + * + * @param T struct serializable type + */ + template + void AddStructSchema(); + /** * Equality operator. Returns true if both instances refer to the same * native handle. diff --git a/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc b/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc index ee1f23c2b9..fdd517e201 100644 --- a/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc +++ b/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc @@ -38,6 +38,24 @@ inline NT_Inst NetworkTableInstance::GetHandle() const { return m_handle; } +template +inline ProtobufTopic NetworkTableInstance::GetProtobufTopic( + std::string_view name) const { + return ProtobufTopic{GetTopic(name)}; +} + +template +inline StructTopic NetworkTableInstance::GetStructTopic( + std::string_view name) const { + return StructTopic{GetTopic(name)}; +} + +template +inline StructArrayTopic NetworkTableInstance::GetStructArrayTopic( + std::string_view name) const { + return StructArrayTopic{GetTopic(name)}; +} + inline std::vector NetworkTableInstance::GetTopics() { auto handles = ::nt::GetTopics(m_handle, "", 0); return {handles.begin(), handles.end()}; @@ -223,4 +241,36 @@ inline NT_Listener NetworkTableInstance::AddLogger(unsigned int min_level, return ::nt::AddLogger(m_handle, min_level, max_level, std::move(func)); } +inline bool NetworkTableInstance::HasSchema(std::string_view name) const { + return ::nt::HasSchema(m_handle, name); +} + +inline void NetworkTableInstance::AddSchema(std::string_view name, + std::string_view type, + std::span schema) { + ::nt::AddSchema(m_handle, name, type, schema); +} + +inline void NetworkTableInstance::AddSchema(std::string_view name, + std::string_view type, + std::string_view schema) { + ::nt::AddSchema(m_handle, name, type, schema); +} + +template +void NetworkTableInstance::AddProtobufSchema(wpi::ProtobufMessage& msg) { + msg.ForEachProtobufDescriptor( + [this](auto typeString) { return HasSchema(typeString); }, + [this](auto typeString, auto schema) { + AddSchema(typeString, "proto:FileDescriptorProto", schema); + }); +} + +template +void NetworkTableInstance::AddStructSchema() { + wpi::ForEachStructSchema([this](auto typeString, auto schema) { + AddSchema(typeString, "structschema", schema); + }); +} + } // namespace nt diff --git a/ntcore/src/main/native/include/networktables/ProtobufTopic.h b/ntcore/src/main/native/include/networktables/ProtobufTopic.h new file mode 100644 index 0000000000..4c30bf7c4c --- /dev/null +++ b/ntcore/src/main/native/include/networktables/ProtobufTopic.h @@ -0,0 +1,474 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "networktables/NetworkTableInstance.h" +#include "networktables/Topic.h" +#include "ntcore_cpp.h" + +namespace wpi { +class json; +} // namespace wpi + +namespace nt { + +template +class ProtobufTopic; + +/** + * NetworkTables protobuf-encoded value subscriber. + */ +template +class ProtobufSubscriber : public Subscriber { + public: + using TopicType = ProtobufTopic; + using ValueType = T; + using ParamType = const T&; + using TimestampedValueType = Timestamped; + + ProtobufSubscriber() = default; + + /** + * Construct from a subscriber handle; recommended to use + * ProtobufTopic::Subscribe() instead. + * + * @param handle Native handle + * @param msg Protobuf message + * @param defaultValue Default value + */ + ProtobufSubscriber(NT_Subscriber handle, wpi::ProtobufMessage msg, + T defaultValue) + : Subscriber{handle}, + m_msg{std::move(msg)}, + m_defaultValue{std::move(defaultValue)} {} + + ProtobufSubscriber(const ProtobufSubscriber&) = delete; + ProtobufSubscriber& operator=(const ProtobufSubscriber&) = delete; + + ProtobufSubscriber(ProtobufSubscriber&& rhs) + : Subscriber{std::move(rhs)}, + m_msg{std::move(rhs.m_msg)}, + m_defaultValue{std::move(rhs.defaultValue)} {} + + ProtobufSubscriber& operator=(ProtobufSubscriber&& rhs) { + Subscriber::operator=(std::move(rhs)); + m_msg = std::move(rhs.m_msg); + m_defaultValue = std::move(rhs.defaultValue); + return *this; + } + + /** + * Get the last published value. + * If no value has been published or the value cannot be unpacked, returns the + * stored default value. + * + * @return value + */ + ValueType Get() const { return Get(m_defaultValue); } + + /** + * Get the last published value. + * If no value has been published or the value cannot be unpacked, returns the + * passed defaultValue. + * + * @param defaultValue default value to return if no value has been published + * @return value + */ + ValueType Get(const T& defaultValue) const { + return GetAtomic(defaultValue).value; + } + + /** + * Get the last published value, replacing the contents in place of an + * existing object. If no value has been published or the value cannot be + * unpacked, does not replace the contents and returns false. + * + * @param[out] out object to replace contents of + * @return true if successful + */ + bool GetInto(T* out) { + wpi::SmallVector buf; + TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {}); + if (view.value.empty()) { + return false; + } else { + std::scoped_lock lock{m_mutex}; + return m_msg.UnpackInto(out, view.value); + } + } + + /** + * Get the last published value along with its timestamp + * If no value has been published or the value cannot be unpacked, returns the + * stored default value and a timestamp of 0. + * + * @return timestamped value + */ + TimestampedValueType GetAtomic() const { return GetAtomic(m_defaultValue); } + + /** + * Get the last published value along with its timestamp. + * If no value has been published or the value cannot be unpacked, returns the + * passed defaultValue and a timestamp of 0. + * + * @param defaultValue default value to return if no value has been published + * @return timestamped value + */ + TimestampedValueType GetAtomic(const T& defaultValue) const { + wpi::SmallVector buf; + TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {}); + if (!view.value.empty()) { + std::scoped_lock lock{m_mutex}; + if (auto optval = m_msg.Unpack(view.value)) { + return {view.time, view.serverTime, *optval}; + } + } + return {0, 0, defaultValue}; + } + + /** + * Get an array of all valid value changes since the last call to ReadQueue. + * Also provides a timestamp for each value. Values that cannot be unpacked + * are dropped. + * + * @note The "poll storage" subscribe option can be used to set the queue + * depth. + * + * @return Array of timestamped values; empty array if no valid new changes + * have been published since the previous call. + */ + std::vector ReadQueue() { + auto raw = ::nt::ReadQueueRaw(m_subHandle); + std::vector rv; + rv.reserve(raw.size()); + std::scoped_lock lock{m_mutex}; + for (auto&& r : raw) { + if (auto optval = m_msg.Unpack(r.value)) { + rv.emplace_back(r.time, r.serverTime, *optval); + } + } + return rv; + } + + /** + * Get the corresponding topic. + * + * @return Topic + */ + TopicType GetTopic() const { + return ProtobufTopic{::nt::GetTopicFromHandle(m_subHandle)}; + } + + private: + wpi::mutex m_mutex; + wpi::ProtobufMessage m_msg; + ValueType m_defaultValue; +}; + +/** + * NetworkTables protobuf-encoded value publisher. + */ +template +class ProtobufPublisher : public Publisher { + public: + using TopicType = ProtobufTopic; + using ValueType = T; + using ParamType = const T&; + + using TimestampedValueType = Timestamped; + + ProtobufPublisher() = default; + + /** + * Construct from a publisher handle; recommended to use + * ProtobufTopic::Publish() instead. + * + * @param handle Native handle + * @param msg Protobuf message + */ + explicit ProtobufPublisher(NT_Publisher handle, wpi::ProtobufMessage msg) + : Publisher{handle}, m_msg{std::move(msg)} {} + + ProtobufPublisher(const ProtobufPublisher&) = delete; + ProtobufPublisher& operator=(const ProtobufPublisher&) = delete; + + ProtobufPublisher(ProtobufPublisher&& rhs) + : Publisher{std::move(rhs)}, + m_msg{std::move(rhs.m_msg)}, + m_schemaPublished{rhs.m_schemaPublished} {} + + ProtobufPublisher& operator=(ProtobufPublisher&& rhs) { + Publisher::operator=(std::move(rhs)); + m_msg = std::move(rhs.m_msg); + m_schemaPublished.clear(); + if (rhs.m_schemaPublished.test()) { + m_schemaPublished.test_and_set(); + } + return *this; + } + + /** + * Publish a new value. + * + * @param value value to publish + * @param time timestamp; 0 indicates current NT time should be used + */ + void Set(const T& value, int64_t time = 0) { + wpi::SmallVector buf; + { + std::scoped_lock lock{m_mutex}; + if (!m_schemaPublished.test_and_set()) { + GetTopic().GetInstance().template AddProtobufSchema(m_msg); + } + m_msg.Pack(buf, value); + } + ::nt::SetRaw(m_pubHandle, buf, time); + } + + /** + * Publish a default value. + * On reconnect, a default value will never be used in preference to a + * published value. + * + * @param value value + */ + void SetDefault(const T& value) { + wpi::SmallVector buf; + { + std::scoped_lock lock{m_mutex}; + if (!m_schemaPublished.test_and_set()) { + GetTopic().GetInstance().template AddProtobufSchema(m_msg); + } + m_msg.Pack(buf, value); + } + ::nt::SetDefaultRaw(m_pubHandle, buf); + } + + /** + * Get the corresponding topic. + * + * @return Topic + */ + TopicType GetTopic() const { + return ProtobufTopic{::nt::GetTopicFromHandle(m_pubHandle)}; + } + + private: + wpi::mutex m_mutex; + wpi::ProtobufMessage m_msg; + std::atomic_flag m_schemaPublished = ATOMIC_FLAG_INIT; +}; + +/** + * NetworkTables protobuf-encoded value entry. + * + * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed. + */ +template +class ProtobufEntry final : public ProtobufSubscriber, + public ProtobufPublisher { + public: + using SubscriberType = ProtobufSubscriber; + using PublisherType = ProtobufPublisher; + using TopicType = ProtobufTopic; + using ValueType = T; + using ParamType = const T&; + + using TimestampedValueType = Timestamped; + + ProtobufEntry() = default; + + /** + * Construct from an entry handle; recommended to use + * ProtobufTopic::GetEntry() instead. + * + * @param handle Native handle + * @param msg Protobuf message + * @param defaultValue Default value + */ + ProtobufEntry(NT_Entry handle, wpi::ProtobufMessage msg, T defaultValue) + : ProtobufSubscriber{handle, std::move(msg), std::move(defaultValue)}, + ProtobufPublisher{handle, {}} {} + + /** + * Determines if the native handle is valid. + * + * @return True if the native handle is valid, false otherwise. + */ + explicit operator bool() const { return this->m_subHandle != 0; } + + /** + * Gets the native handle for the entry. + * + * @return Native handle + */ + NT_Entry GetHandle() const { return this->m_subHandle; } + + /** + * Get the corresponding topic. + * + * @return Topic + */ + TopicType GetTopic() const { + return ProtobufTopic{::nt::GetTopicFromHandle(this->m_subHandle)}; + } + + /** + * Stops publishing the entry if it's published. + */ + void Unpublish() { ::nt::Unpublish(this->m_pubHandle); } +}; + +/** + * NetworkTables protobuf-encoded value topic. + */ +template +class ProtobufTopic final : public Topic { + public: + using SubscriberType = ProtobufSubscriber; + using PublisherType = ProtobufPublisher; + using EntryType = ProtobufEntry; + using ValueType = T; + using ParamType = const T&; + using TimestampedValueType = Timestamped; + + ProtobufTopic() = default; + + /** + * Construct from a topic handle; recommended to use + * NetworkTableInstance::GetProtobufTopic() instead. + * + * @param handle Native handle + */ + explicit ProtobufTopic(NT_Topic handle) : Topic{handle} {} + + /** + * Construct from a generic topic. + * + * @param topic Topic + */ + explicit ProtobufTopic(Topic topic) : Topic{topic} {} + + /** + * Create a new subscriber to the topic. + * + *

The subscriber is only active as long as the returned object + * is not destroyed. + * + * @note Subscribers that do not match the published data type do not return + * any values. To determine if the data type matches, use the appropriate + * Topic functions. + * + * @param defaultValue default value used when a default is not provided to a + * getter function + * @param options subscribe options + * @return subscriber + */ + [[nodiscard]] + SubscriberType Subscribe( + T defaultValue, const PubSubOptions& options = kDefaultPubSubOptions) { + wpi::ProtobufMessage msg; + auto typeString = msg.GetTypeString(); + return ProtobufSubscriber{ + ::nt::Subscribe(m_handle, NT_RAW, typeString, options), std::move(msg), + std::move(defaultValue)}; + } + + /** + * Create a new publisher to the topic. + * + * The publisher is only active as long as the returned object + * is not destroyed. + * + * @note It is not possible to publish two different data types to the same + * topic. Conflicts between publishers are typically resolved by the + * server on a first-come, first-served basis. Any published values that + * do not match the topic's data type are dropped (ignored). To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param options publish options + * @return publisher + */ + [[nodiscard]] + PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions) { + wpi::ProtobufMessage msg; + auto typeString = msg.GetTypeString(); + return ProtobufPublisher{ + ::nt::Publish(m_handle, NT_RAW, typeString, options), std::move(msg)}; + } + + /** + * Create a new publisher to the topic, with type string and initial + * properties. + * + * The publisher is only active as long as the returned object + * is not destroyed. + * + * @note It is not possible to publish two different data types to the same + * topic. Conflicts between publishers are typically resolved by the + * server on a first-come, first-served basis. Any published values that + * do not match the topic's data type are dropped (ignored). To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param properties JSON properties + * @param options publish options + * @return publisher + */ + [[nodiscard]] + PublisherType PublishEx( + const wpi::json& properties, + const PubSubOptions& options = kDefaultPubSubOptions) { + wpi::ProtobufMessage msg; + auto typeString = msg.GetTypeString(); + return ProtobufPublisher{ + ::nt::PublishEx(m_handle, NT_RAW, typeString, properties, options), + std::move(msg)}; + } + + /** + * Create a new entry for the topic. + * + * Entries act as a combination of a subscriber and a weak publisher. The + * subscriber is active as long as the entry is not destroyed. The publisher + * is created when the entry is first written to, and remains active until + * either Unpublish() is called or the entry is destroyed. + * + * @note It is not possible to use two different data types with the same + * topic. Conflicts between publishers are typically resolved by the + * server on a first-come, first-served basis. Any published values that + * do not match the topic's data type are dropped (ignored), and the entry + * will show no new values if the data type does not match. To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param defaultValue default value used when a default is not provided to a + * getter function + * @param options publish and/or subscribe options + * @return entry + */ + [[nodiscard]] + EntryType GetEntry(T defaultValue, + const PubSubOptions& options = kDefaultPubSubOptions) { + wpi::ProtobufMessage msg; + auto typeString = msg.GetTypeString(); + return ProtobufEntry{ + ::nt::GetEntry(m_handle, NT_RAW, typeString, options), std::move(msg), + std::move(defaultValue)}; + } +}; + +} // namespace nt diff --git a/ntcore/src/main/native/include/networktables/StructArrayTopic.h b/ntcore/src/main/native/include/networktables/StructArrayTopic.h new file mode 100644 index 0000000000..91f472147f --- /dev/null +++ b/ntcore/src/main/native/include/networktables/StructArrayTopic.h @@ -0,0 +1,593 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "networktables/NetworkTableInstance.h" +#include "networktables/Topic.h" +#include "ntcore_cpp.h" + +namespace wpi { +class json; +} // namespace wpi + +namespace nt { + +template +class StructArrayTopic; + +/** + * NetworkTables struct-encoded value array subscriber. + */ +template +class StructArraySubscriber : public Subscriber { + using S = wpi::Struct; + + public: + using TopicType = StructArrayTopic; + using ValueType = std::vector; + using ParamType = std::span; + using TimestampedValueType = Timestamped; + + StructArraySubscriber() = default; + + /** + * Construct from a subscriber handle; recommended to use + * StructTopic::Subscribe() instead. + * + * @param handle Native handle + * @param defaultValue Default value + */ + template +#if __cpp_lib_ranges >= 201911L + requires std::ranges::range && + std::convertible_to, T> +#endif + StructArraySubscriber(NT_Subscriber handle, U&& defaultValue) + : Subscriber{handle}, + m_defaultValue{defaultValue.begin(), defaultValue.end()} { + } + + /** + * Get the last published value. + * If no value has been published or the value cannot be unpacked, returns the + * stored default value. + * + * @return value + */ + ValueType Get() const { return Get(m_defaultValue); } + + /** + * Get the last published value. + * If no value has been published or the value cannot be unpacked, returns the + * passed defaultValue. + * + * @param defaultValue default value to return if no value has been published + * @return value + */ + template +#if __cpp_lib_ranges >= 201911L + requires std::ranges::range && + std::convertible_to, T> +#endif + ValueType Get(U&& defaultValue) const { + return GetAtomic(std::forward(defaultValue)).value; + } + + /** + * Get the last published value. + * If no value has been published or the value cannot be unpacked, returns the + * passed defaultValue. + * + * @param defaultValue default value to return if no value has been published + * @return value + */ + ValueType Get(std::span defaultValue) const { + return GetAtomic(defaultValue).value; + } + + /** + * Get the last published value along with its timestamp + * If no value has been published or the value cannot be unpacked, returns the + * stored default value and a timestamp of 0. + * + * @return timestamped value + */ + TimestampedValueType GetAtomic() const { return GetAtomic(m_defaultValue); } + + /** + * Get the last published value along with its timestamp. + * If no value has been published or the value cannot be unpacked, returns the + * passed defaultValue and a timestamp of 0. + * + * @param defaultValue default value to return if no value has been published + * @return timestamped value + */ + template +#if __cpp_lib_ranges >= 201911L + requires std::ranges::range && + std::convertible_to, T> +#endif + TimestampedValueType GetAtomic(U&& defaultValue) const { + wpi::SmallVector buf; + TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {}); + if (view.value.size() == 0 || (view.value.size() % S::kSize) != 0) { + return {0, 0, std::forward(defaultValue)}; + } + TimestampedValueType rv{view.time, view.serverTime, {}}; + rv.value.reserve(view.value.size() / S::kSize); + for (auto in = view.value.begin(), end = view.value.end(); in != end; + in += S::kSize) { + rv.value.emplace_back( + S::Unpack(std::span{in, in + S::kSize})); + } + return rv; + } + + /** + * Get the last published value along with its timestamp. + * If no value has been published or the value cannot be unpacked, returns the + * passed defaultValue and a timestamp of 0. + * + * @param defaultValue default value to return if no value has been published + * @return timestamped value + */ + TimestampedValueType GetAtomic(std::span defaultValue) const { + wpi::SmallVector buf; + TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {}); + if (view.value.size() == 0 || (view.value.size() % S::kSize) != 0) { + return {0, 0, {defaultValue.begin(), defaultValue.end()}}; + } + TimestampedValueType rv{view.time, view.serverTime, {}}; + rv.value.reserve(view.value.size() / S::kSize); + for (auto in = view.value.begin(), end = view.value.end(); in != end; + in += S::kSize) { + rv.value.emplace_back( + S::Unpack(std::span{in, in + S::kSize})); + } + return rv; + } + + /** + * Get an array of all valid value changes since the last call to ReadQueue. + * Also provides a timestamp for each value. Values that cannot be unpacked + * are dropped. + * + * @note The "poll storage" subscribe option can be used to set the queue + * depth. + * + * @return Array of timestamped values; empty array if no valid new changes + * have been published since the previous call. + */ + std::vector ReadQueue() { + auto raw = ::nt::ReadQueueRaw(m_subHandle); + std::vector rv; + rv.reserve(raw.size()); + for (auto&& r : raw) { + if (r.value.size() == 0 || (r.value.size() % S::kSize) != 0) { + continue; + } + std::vector values; + values.reserve(r.value.size() / S::kSize); + for (auto in = r.value.begin(), end = r.value.end(); in != end; + in += S::kSize) { + values.emplace_back( + S::Unpack(std::span{in, in + S::kSize})); + } + rv.emplace_back(r.time, r.serverTime, std::move(values)); + } + return rv; + } + + /** + * Get the corresponding topic. + * + * @return Topic + */ + TopicType GetTopic() const { + return StructArrayTopic{::nt::GetTopicFromHandle(m_subHandle)}; + } + + private: + ValueType m_defaultValue; +}; + +/** + * NetworkTables struct-encoded value array publisher. + */ +template +class StructArrayPublisher : public Publisher { + using S = wpi::Struct; + + public: + using TopicType = StructArrayTopic; + using ValueType = std::vector; + using ParamType = std::span; + + using TimestampedValueType = Timestamped; + + StructArrayPublisher() = default; + + /** + * Construct from a publisher handle; recommended to use + * StructTopic::Publish() instead. + * + * @param handle Native handle + */ + explicit StructArrayPublisher(NT_Publisher handle) : Publisher{handle} {} + + StructArrayPublisher(const StructArrayPublisher&) = delete; + StructArrayPublisher& operator=(const StructArrayPublisher&) = delete; + + StructArrayPublisher(StructArrayPublisher&& rhs) + : Publisher{std::move(rhs)}, + m_buf{std::move(rhs.m_buf)}, + m_schemaPublished{rhs.m_schemaPublished} {} + + StructArrayPublisher& operator=(StructArrayPublisher&& rhs) { + Publisher::operator=(std::move(rhs)); + m_buf = std::move(rhs.m_buf); + m_schemaPublished.clear(); + if (rhs.m_schemaPublished.test()) { + m_schemaPublished.test_and_set(); + } + return *this; + } + + /** + * Publish a new value. + * + * @param value value to publish + * @param time timestamp; 0 indicates current NT time should be used + */ + template +#if __cpp_lib_ranges >= 201911L + requires std::ranges::range && + std::convertible_to, T> +#endif + void Set(U&& value, int64_t time = 0) { + if (!m_schemaPublished.test_and_set()) { + GetTopic().GetInstance().template AddStructSchema(); + } + m_buf.Write(std::forward(value), + [&](auto bytes) { ::nt::SetRaw(m_pubHandle, bytes, time); }); + } + + /** + * Publish a new value. + * + * @param value value to publish + * @param time timestamp; 0 indicates current NT time should be used + */ + void Set(std::span value, int64_t time = 0) { + m_buf.Write(value, + [&](auto bytes) { ::nt::SetRaw(m_pubHandle, bytes, time); }); + } + + /** + * Publish a default value. + * On reconnect, a default value will never be used in preference to a + * published value. + * + * @param value value + */ + template +#if __cpp_lib_ranges >= 201911L + requires std::ranges::range && + std::convertible_to, T> +#endif + void SetDefault(U&& value) { + if (!m_schemaPublished.test_and_set()) { + GetTopic().GetInstance().template AddStructSchema(); + } + m_buf.Write(std::forward(value), + [&](auto bytes) { ::nt::SetDefaultRaw(m_pubHandle, bytes); }); + } + + /** + * Publish a default value. + * On reconnect, a default value will never be used in preference to a + * published value. + * + * @param value value + */ + void SetDefault(std::span value) { + m_buf.Write(value, + [&](auto bytes) { ::nt::SetDefaultRaw(m_pubHandle, bytes); }); + } + + /** + * Get the corresponding topic. + * + * @return Topic + */ + TopicType GetTopic() const { + return StructArrayTopic{::nt::GetTopicFromHandle(m_pubHandle)}; + } + + private: + wpi::StructArrayBuffer m_buf; + std::atomic_flag m_schemaPublished = ATOMIC_FLAG_INIT; +}; + +/** + * NetworkTables struct-encoded value array entry. + * + * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed. + */ +template +class StructArrayEntry final : public StructArraySubscriber, + public StructArrayPublisher { + public: + using SubscriberType = StructArraySubscriber; + using PublisherType = StructArrayPublisher; + using TopicType = StructArrayTopic; + using ValueType = std::vector; + using ParamType = std::span; + + using TimestampedValueType = Timestamped; + + StructArrayEntry() = default; + + /** + * Construct from an entry handle; recommended to use + * StructTopic::GetEntry() instead. + * + * @param handle Native handle + * @param defaultValue Default value + */ + template +#if __cpp_lib_ranges >= 201911L + requires std::ranges::range && + std::convertible_to, T> +#endif + StructArrayEntry(NT_Entry handle, U&& defaultValue) + : StructArraySubscriber{handle, defaultValue}, + StructArrayPublisher{handle} { + } + + /** + * Determines if the native handle is valid. + * + * @return True if the native handle is valid, false otherwise. + */ + explicit operator bool() const { return this->m_subHandle != 0; } + + /** + * Gets the native handle for the entry. + * + * @return Native handle + */ + NT_Entry GetHandle() const { return this->m_subHandle; } + + /** + * Get the corresponding topic. + * + * @return Topic + */ + TopicType GetTopic() const { + return StructArrayTopic{::nt::GetTopicFromHandle(this->m_subHandle)}; + } + + /** + * Stops publishing the entry if it's published. + */ + void Unpublish() { ::nt::Unpublish(this->m_pubHandle); } +}; + +/** + * NetworkTables struct-encoded value array topic. + */ +template +class StructArrayTopic final : public Topic { + public: + using SubscriberType = StructArraySubscriber; + using PublisherType = StructArrayPublisher; + using EntryType = StructArrayEntry; + using ValueType = std::vector; + using ParamType = std::span; + using TimestampedValueType = Timestamped; + + StructArrayTopic() = default; + + /** + * Construct from a topic handle; recommended to use + * NetworkTableInstance::GetStructTopic() instead. + * + * @param handle Native handle + */ + explicit StructArrayTopic(NT_Topic handle) : Topic{handle} {} + + /** + * Construct from a generic topic. + * + * @param topic Topic + */ + explicit StructArrayTopic(Topic topic) : Topic{topic} {} + + /** + * Create a new subscriber to the topic. + * + *

The subscriber is only active as long as the returned object + * is not destroyed. + * + * @note Subscribers that do not match the published data type do not return + * any values. To determine if the data type matches, use the appropriate + * Topic functions. + * + * @param defaultValue default value used when a default is not provided to a + * getter function + * @param options subscribe options + * @return subscriber + */ + template +#if __cpp_lib_ranges >= 201911L + requires std::ranges::range && + std::convertible_to, T> +#endif + [[nodiscard]] + SubscriberType Subscribe( + U&& defaultValue, const PubSubOptions& options = kDefaultPubSubOptions) { + return StructArraySubscriber{ + ::nt::Subscribe( + m_handle, NT_RAW, + wpi::MakeStructArrayTypeString(), options), + defaultValue}; + } + + /** + * Create a new subscriber to the topic. + * + *

The subscriber is only active as long as the returned object + * is not destroyed. + * + * @note Subscribers that do not match the published data type do not return + * any values. To determine if the data type matches, use the appropriate + * Topic functions. + * + * @param defaultValue default value used when a default is not provided to a + * getter function + * @param options subscribe options + * @return subscriber + */ + [[nodiscard]] + SubscriberType Subscribe( + std::span defaultValue, + const PubSubOptions& options = kDefaultPubSubOptions) { + return StructArraySubscriber{ + ::nt::Subscribe( + m_handle, NT_RAW, + wpi::MakeStructArrayTypeString(), options), + defaultValue}; + } + + /** + * Create a new publisher to the topic. + * + * The publisher is only active as long as the returned object + * is not destroyed. + * + * @note It is not possible to publish two different data types to the same + * topic. Conflicts between publishers are typically resolved by the + * server on a first-come, first-served basis. Any published values that + * do not match the topic's data type are dropped (ignored). To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param options publish options + * @return publisher + */ + [[nodiscard]] + PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions) { + return StructArrayPublisher{::nt::Publish( + m_handle, NT_RAW, + wpi::MakeStructArrayTypeString(), options)}; + } + + /** + * Create a new publisher to the topic, with type string and initial + * properties. + * + * The publisher is only active as long as the returned object + * is not destroyed. + * + * @note It is not possible to publish two different data types to the same + * topic. Conflicts between publishers are typically resolved by the + * server on a first-come, first-served basis. Any published values that + * do not match the topic's data type are dropped (ignored). To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param properties JSON properties + * @param options publish options + * @return publisher + */ + [[nodiscard]] + PublisherType PublishEx( + const wpi::json& properties, + const PubSubOptions& options = kDefaultPubSubOptions) { + return StructArrayPublisher{::nt::PublishEx( + m_handle, NT_RAW, + wpi::MakeStructArrayTypeString(), properties, + options)}; + } + + /** + * Create a new entry for the topic. + * + * Entries act as a combination of a subscriber and a weak publisher. The + * subscriber is active as long as the entry is not destroyed. The publisher + * is created when the entry is first written to, and remains active until + * either Unpublish() is called or the entry is destroyed. + * + * @note It is not possible to use two different data types with the same + * topic. Conflicts between publishers are typically resolved by the + * server on a first-come, first-served basis. Any published values that + * do not match the topic's data type are dropped (ignored), and the entry + * will show no new values if the data type does not match. To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param defaultValue default value used when a default is not provided to a + * getter function + * @param options publish and/or subscribe options + * @return entry + */ + template +#if __cpp_lib_ranges >= 201911L + requires std::ranges::range && + std::convertible_to, T> +#endif + [[nodiscard]] + EntryType GetEntry(U&& defaultValue, + const PubSubOptions& options = kDefaultPubSubOptions) { + return StructArrayEntry{ + ::nt::GetEntry(m_handle, NT_RAW, + wpi::MakeStructArrayTypeString(), + options), + defaultValue}; + } + + /** + * Create a new entry for the topic. + * + * Entries act as a combination of a subscriber and a weak publisher. The + * subscriber is active as long as the entry is not destroyed. The publisher + * is created when the entry is first written to, and remains active until + * either Unpublish() is called or the entry is destroyed. + * + * @note It is not possible to use two different data types with the same + * topic. Conflicts between publishers are typically resolved by the + * server on a first-come, first-served basis. Any published values that + * do not match the topic's data type are dropped (ignored), and the entry + * will show no new values if the data type does not match. To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param defaultValue default value used when a default is not provided to a + * getter function + * @param options publish and/or subscribe options + * @return entry + */ + [[nodiscard]] + EntryType GetEntry(std::span defaultValue, + const PubSubOptions& options = kDefaultPubSubOptions) { + return StructArrayEntry{ + ::nt::GetEntry(m_handle, NT_RAW, + wpi::MakeStructArrayTypeString(), + options), + defaultValue}; + } +}; + +} // namespace nt diff --git a/ntcore/src/main/native/include/networktables/StructTopic.h b/ntcore/src/main/native/include/networktables/StructTopic.h new file mode 100644 index 0000000000..88da9f39fc --- /dev/null +++ b/ntcore/src/main/native/include/networktables/StructTopic.h @@ -0,0 +1,438 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "networktables/NetworkTableInstance.h" +#include "networktables/Topic.h" +#include "ntcore_cpp.h" + +namespace wpi { +class json; +} // namespace wpi + +namespace nt { + +template +class StructTopic; + +/** + * NetworkTables struct-encoded value subscriber. + */ +template +class StructSubscriber : public Subscriber { + using S = wpi::Struct; + + public: + using TopicType = StructTopic; + using ValueType = T; + using ParamType = const T&; + using TimestampedValueType = Timestamped; + + StructSubscriber() = default; + + /** + * Construct from a subscriber handle; recommended to use + * StructTopic::Subscribe() instead. + * + * @param handle Native handle + * @param defaultValue Default value + */ + StructSubscriber(NT_Subscriber handle, T defaultValue) + : Subscriber{handle}, m_defaultValue{std::move(defaultValue)} {} + + /** + * Get the last published value. + * If no value has been published or the value cannot be unpacked, returns the + * stored default value. + * + * @return value + */ + ValueType Get() const { return Get(m_defaultValue); } + + /** + * Get the last published value. + * If no value has been published or the value cannot be unpacked, returns the + * passed defaultValue. + * + * @param defaultValue default value to return if no value has been published + * @return value + */ + ValueType Get(const T& defaultValue) const { + return GetAtomic(defaultValue).value; + } + + /** + * Get the last published value, replacing the contents in place of an + * existing object. If no value has been published or the value cannot be + * unpacked, does not replace the contents and returns false. + * + * @param[out] out object to replace contents of + * @return true if successful + */ + bool GetInto(T* out) { + wpi::SmallVector buf; + TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {}); + if (view.value.size() < S::kSize) { + return false; + } else { + wpi::UnpackStructInto(out, view.value.subspan<0, S::kSize>()); + return true; + } + } + + /** + * Get the last published value along with its timestamp + * If no value has been published or the value cannot be unpacked, returns the + * stored default value and a timestamp of 0. + * + * @return timestamped value + */ + TimestampedValueType GetAtomic() const { return GetAtomic(m_defaultValue); } + + /** + * Get the last published value along with its timestamp. + * If no value has been published or the value cannot be unpacked, returns the + * passed defaultValue and a timestamp of 0. + * + * @param defaultValue default value to return if no value has been published + * @return timestamped value + */ + TimestampedValueType GetAtomic(const T& defaultValue) const { + wpi::SmallVector buf; + TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {}); + if (view.value.size() < S::kSize) { + return {0, 0, defaultValue}; + } else { + return {view.time, view.serverTime, + S::Unpack(view.value.subspan<0, S::kSize>())}; + } + } + + /** + * Get an array of all valid value changes since the last call to ReadQueue. + * Also provides a timestamp for each value. Values that cannot be unpacked + * are dropped. + * + * @note The "poll storage" subscribe option can be used to set the queue + * depth. + * + * @return Array of timestamped values; empty array if no valid new changes + * have been published since the previous call. + */ + std::vector ReadQueue() { + auto raw = ::nt::ReadQueueRaw(m_subHandle); + std::vector rv; + rv.reserve(raw.size()); + for (auto&& r : raw) { + if (r.value.size() < S::kSize) { + continue; + } else { + rv.emplace_back( + r.time, r.serverTime, + S::Unpack( + std::span(r.value).subspan<0, S::kSize>())); + } + } + return rv; + } + + /** + * Get the corresponding topic. + * + * @return Topic + */ + TopicType GetTopic() const { + return StructTopic{::nt::GetTopicFromHandle(m_subHandle)}; + } + + private: + ValueType m_defaultValue; +}; + +/** + * NetworkTables struct-encoded value publisher. + */ +template +class StructPublisher : public Publisher { + using S = wpi::Struct; + + public: + using TopicType = StructTopic; + using ValueType = T; + using ParamType = const T&; + + using TimestampedValueType = Timestamped; + + StructPublisher() = default; + + StructPublisher(const StructPublisher&) = delete; + StructPublisher& operator=(const StructPublisher&) = delete; + + StructPublisher(StructPublisher&& rhs) + : Publisher{std::move(rhs)}, m_schemaPublished{rhs.m_schemaPublished} {} + + StructPublisher& operator=(StructPublisher&& rhs) { + Publisher::operator=(std::move(rhs)); + m_schemaPublished.clear(); + if (rhs.m_schemaPublished.test()) { + m_schemaPublished.test_and_set(); + } + return *this; + } + + /** + * Construct from a publisher handle; recommended to use + * StructTopic::Publish() instead. + * + * @param handle Native handle + */ + explicit StructPublisher(NT_Publisher handle) : Publisher{handle} {} + + /** + * Publish a new value. + * + * @param value value to publish + * @param time timestamp; 0 indicates current NT time should be used + */ + void Set(const T& value, int64_t time = 0) { + if (!m_schemaPublished.test_and_set()) { + GetTopic().GetInstance().template AddStructSchema(); + } + uint8_t buf[S::kSize]; + S::Pack(buf, value); + ::nt::SetRaw(m_pubHandle, buf, time); + } + + /** + * Publish a default value. + * On reconnect, a default value will never be used in preference to a + * published value. + * + * @param value value + */ + void SetDefault(const T& value) { + if (!m_schemaPublished.test_and_set()) { + GetTopic().GetInstance().template AddStructSchema(); + } + uint8_t buf[S::kSize]; + S::Pack(buf, value); + ::nt::SetDefaultRaw(m_pubHandle, buf); + } + + /** + * Get the corresponding topic. + * + * @return Topic + */ + TopicType GetTopic() const { + return StructTopic{::nt::GetTopicFromHandle(m_pubHandle)}; + } + + private: + std::atomic_flag m_schemaPublished = ATOMIC_FLAG_INIT; +}; + +/** + * NetworkTables struct-encoded value entry. + * + * @note Unlike NetworkTableEntry, the entry goes away when this is destroyed. + */ +template +class StructEntry final : public StructSubscriber, + public StructPublisher { + public: + using SubscriberType = StructSubscriber; + using PublisherType = StructPublisher; + using TopicType = StructTopic; + using ValueType = T; + using ParamType = const T&; + + using TimestampedValueType = Timestamped; + + StructEntry() = default; + + /** + * Construct from an entry handle; recommended to use + * StructTopic::GetEntry() instead. + * + * @param handle Native handle + * @param defaultValue Default value + */ + StructEntry(NT_Entry handle, T defaultValue) + : StructSubscriber{handle, std::move(defaultValue)}, + StructPublisher{handle} {} + + /** + * Determines if the native handle is valid. + * + * @return True if the native handle is valid, false otherwise. + */ + explicit operator bool() const { return this->m_subHandle != 0; } + + /** + * Gets the native handle for the entry. + * + * @return Native handle + */ + NT_Entry GetHandle() const { return this->m_subHandle; } + + /** + * Get the corresponding topic. + * + * @return Topic + */ + TopicType GetTopic() const { + return StructTopic{::nt::GetTopicFromHandle(this->m_subHandle)}; + } + + /** + * Stops publishing the entry if it's published. + */ + void Unpublish() { ::nt::Unpublish(this->m_pubHandle); } +}; + +/** + * NetworkTables struct-encoded value topic. + */ +template +class StructTopic final : public Topic { + public: + using SubscriberType = StructSubscriber; + using PublisherType = StructPublisher; + using EntryType = StructEntry; + using ValueType = T; + using ParamType = const T&; + using TimestampedValueType = Timestamped; + + StructTopic() = default; + + /** + * Construct from a topic handle; recommended to use + * NetworkTableInstance::GetStructTopic() instead. + * + * @param handle Native handle + */ + explicit StructTopic(NT_Topic handle) : Topic{handle} {} + + /** + * Construct from a generic topic. + * + * @param topic Topic + */ + explicit StructTopic(Topic topic) : Topic{topic} {} + + /** + * Create a new subscriber to the topic. + * + *

The subscriber is only active as long as the returned object + * is not destroyed. + * + * @note Subscribers that do not match the published data type do not return + * any values. To determine if the data type matches, use the appropriate + * Topic functions. + * + * @param defaultValue default value used when a default is not provided to a + * getter function + * @param options subscribe options + * @return subscriber + */ + [[nodiscard]] + SubscriberType Subscribe( + T defaultValue, const PubSubOptions& options = kDefaultPubSubOptions) { + return StructSubscriber{ + ::nt::Subscribe(m_handle, NT_RAW, wpi::GetStructTypeString(), + options), + std::move(defaultValue)}; + } + + /** + * Create a new publisher to the topic. + * + * The publisher is only active as long as the returned object + * is not destroyed. + * + * @note It is not possible to publish two different data types to the same + * topic. Conflicts between publishers are typically resolved by the + * server on a first-come, first-served basis. Any published values that + * do not match the topic's data type are dropped (ignored). To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param options publish options + * @return publisher + */ + [[nodiscard]] + PublisherType Publish(const PubSubOptions& options = kDefaultPubSubOptions) { + return StructPublisher{::nt::Publish( + m_handle, NT_RAW, wpi::GetStructTypeString(), options)}; + } + + /** + * Create a new publisher to the topic, with type string and initial + * properties. + * + * The publisher is only active as long as the returned object + * is not destroyed. + * + * @note It is not possible to publish two different data types to the same + * topic. Conflicts between publishers are typically resolved by the + * server on a first-come, first-served basis. Any published values that + * do not match the topic's data type are dropped (ignored). To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param properties JSON properties + * @param options publish options + * @return publisher + */ + [[nodiscard]] + PublisherType PublishEx( + const wpi::json& properties, + const PubSubOptions& options = kDefaultPubSubOptions) { + return StructPublisher{::nt::PublishEx( + m_handle, NT_RAW, wpi::GetStructTypeString(), properties, options)}; + } + + /** + * Create a new entry for the topic. + * + * Entries act as a combination of a subscriber and a weak publisher. The + * subscriber is active as long as the entry is not destroyed. The publisher + * is created when the entry is first written to, and remains active until + * either Unpublish() is called or the entry is destroyed. + * + * @note It is not possible to use two different data types with the same + * topic. Conflicts between publishers are typically resolved by the + * server on a first-come, first-served basis. Any published values that + * do not match the topic's data type are dropped (ignored), and the entry + * will show no new values if the data type does not match. To determine + * if the data type matches, use the appropriate Topic functions. + * + * @param defaultValue default value used when a default is not provided to a + * getter function + * @param options publish and/or subscribe options + * @return entry + */ + [[nodiscard]] + EntryType GetEntry(T defaultValue, + const PubSubOptions& options = kDefaultPubSubOptions) { + return StructEntry{ + ::nt::GetEntry(m_handle, NT_RAW, wpi::GetStructTypeString(), + options), + std::move(defaultValue)}; + } +}; + +} // namespace nt diff --git a/ntcore/src/main/native/include/networktables/Topic.inc b/ntcore/src/main/native/include/networktables/Topic.inc index 642e49eb2b..166dc6d7a1 100644 --- a/ntcore/src/main/native/include/networktables/Topic.inc +++ b/ntcore/src/main/native/include/networktables/Topic.inc @@ -6,7 +6,6 @@ #include -#include "networktables/NetworkTableInstance.h" #include "networktables/NetworkTableType.h" #include "networktables/Topic.h" #include "ntcore_c.h" @@ -14,10 +13,6 @@ namespace nt { -inline NetworkTableInstance Topic::GetInstance() const { - return NetworkTableInstance{GetInstanceFromHandle(m_handle)}; -} - inline std::string Topic::GetName() const { return ::nt::GetTopicName(m_handle); } diff --git a/ntcore/src/main/native/include/ntcore_c.h b/ntcore/src/main/native/include/ntcore_c.h index 56426fe818..1af0e6623b 100644 --- a/ntcore/src/main/native/include/ntcore_c.h +++ b/ntcore/src/main/native/include/ntcore_c.h @@ -1435,6 +1435,44 @@ NT_Listener NT_AddPolledLogger(NT_ListenerPoller poller, unsigned int min_level, /** @} */ +/** + * @defgroup ntcore_schema_cfunc Schema Functions + * @{ + */ + +/** + * Returns whether there is a data schema already registered with the given + * name. This does NOT perform a check as to whether the schema has already + * been published by another node on the network. + * + * @param inst instance + * @param name Name (the string passed as the data type for topics using this + * schema) + * @return True if schema already registered + */ +NT_Bool NT_HasSchema(NT_Inst inst, const char* name); + +/** + * Registers a data schema. Data schemas provide information for how a + * certain data type string can be decoded. The type string of a data schema + * indicates the type of the schema itself (e.g. "protobuf" for protobuf + * schemas, "struct" for struct schemas, etc). In NetworkTables, schemas are + * published just like normal topics, with the name being generated from the + * provided name: "/.schema/". Duplicate calls to this function with + * the same name are silently ignored. + * + * @param inst instance + * @param name Name (the string passed as the data type for topics using this + * schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + * @param schemaSize Size of schema data + */ +void NT_AddSchema(NT_Inst inst, const char* name, const char* type, + const uint8_t* schema, size_t schemaSize); + +/** @} */ + /** * @defgroup ntcore_interop_cfunc Interop Utility Functions * @{ diff --git a/ntcore/src/main/native/include/ntcore_cpp.h b/ntcore/src/main/native/include/ntcore_cpp.h index 7676b53416..482d1e3a9c 100644 --- a/ntcore/src/main/native/include/ntcore_cpp.h +++ b/ntcore/src/main/native/include/ntcore_cpp.h @@ -1300,6 +1300,66 @@ NT_Listener AddLogger(NT_Inst inst, unsigned int min_level, NT_Listener AddPolledLogger(NT_ListenerPoller poller, unsigned int min_level, unsigned int max_level); +/** @} */ + +/** + * @defgroup ntcore_schema_func Schema Functions + * @{ + */ + +/** + * Returns whether there is a data schema already registered with the given + * name. This does NOT perform a check as to whether the schema has already + * been published by another node on the network. + * + * @param inst instance + * @param name Name (the string passed as the data type for topics using this + * schema) + * @return True if schema already registered + */ +bool HasSchema(NT_Inst inst, std::string_view name); + +/** + * Registers a data schema. Data schemas provide information for how a + * certain data type string can be decoded. The type string of a data schema + * indicates the type of the schema itself (e.g. "protobuf" for protobuf + * schemas, "struct" for struct schemas, etc). In NetworkTables, schemas are + * published just like normal topics, with the name being generated from the + * provided name: "/.schema/". Duplicate calls to this function with + * the same name are silently ignored. + * + * @param inst instance + * @param name Name (the string passed as the data type for topics using this + * schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + */ +void AddSchema(NT_Inst inst, std::string_view name, std::string_view type, + std::span schema); + +/** + * Registers a data schema. Data schemas provide information for how a + * certain data type string can be decoded. The type string of a data schema + * indicates the type of the schema itself (e.g. "protobuf" for protobuf + * schemas, "struct" for struct schemas, etc). In NetworkTables, schemas are + * published just like normal topics, with the name being generated from the + * provided name: "/.schema/". Duplicate calls to this function with + * the same name are silently ignored. + * + * @param inst instance + * @param name Name (the string passed as the data type for topics using this + * schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + */ +inline void AddSchema(NT_Inst inst, std::string_view name, + std::string_view type, std::string_view schema) { + AddSchema( + inst, name, type, + std::span{reinterpret_cast(schema.data()), + schema.size()}); +} + /** @} */ /** @} */ diff --git a/shared/java/javacommon.gradle b/shared/java/javacommon.gradle index 57a864f471..32078f8f10 100644 --- a/shared/java/javacommon.gradle +++ b/shared/java/javacommon.gradle @@ -1,6 +1,7 @@ apply plugin: 'maven-publish' apply plugin: 'java-library' apply plugin: 'jacoco' +apply plugin: 'com.google.protobuf' def baseArtifactId = project.baseId def artifactGroupId = project.groupId @@ -141,3 +142,27 @@ jacocoTestReport { html.required = true } } + +protobuf { + protoc { + artifact = 'com.google.protobuf:protoc:3.21.12' + } + plugins { + quickbuf { + artifact = 'us.hebi.quickbuf:protoc-gen-quickbuf:1.3.2' + } + } + generateProtoTasks { + all().configureEach { task -> + task.builtins { + cpp {} + remove java + } + task.plugins { + quickbuf { + option "gen_descriptors=true" + } + } + } + } +} diff --git a/shared/javacpp/setupBuild.gradle b/shared/javacpp/setupBuild.gradle index 6ea7d6f151..f2d88c4a82 100644 --- a/shared/javacpp/setupBuild.gradle +++ b/shared/javacpp/setupBuild.gradle @@ -30,11 +30,11 @@ model { sources { cpp { source { - srcDirs 'src/main/native/cpp' - include '**/*.cpp' + srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp" + include '**/*.cpp', '**/*.cc' } exportedHeaders { - srcDirs 'src/main/native/include' + srcDirs 'src/main/native/include', "$buildDir/generated/source/proto/main/cpp" } } } @@ -43,6 +43,9 @@ model { it.buildable = false return } + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } if (project.hasProperty('extraSetup')) { extraSetup(it) } diff --git a/shared/jni/setupBuild.gradle b/shared/jni/setupBuild.gradle index 8c2f4e929d..6f4345ad57 100644 --- a/shared/jni/setupBuild.gradle +++ b/shared/jni/setupBuild.gradle @@ -41,15 +41,15 @@ model { sources { cpp { source { - srcDirs 'src/main/native/cpp' + srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp" if (project.hasProperty('generatedSources')) { srcDir generatedSources } - include '**/*.cpp' + include '**/*.cpp', '**/*.cc' exclude '**/jni/**/*.cpp' } exportedHeaders { - srcDir 'src/main/native/include' + srcDirs 'src/main/native/include', "$buildDir/generated/source/proto/main/cpp" if (project.hasProperty('generatedHeaders')) { srcDir generatedHeaders } @@ -67,6 +67,9 @@ model { if (!project.hasProperty('noWpiutil')) { lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' } + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } if (project.hasProperty('splitSetup')) { splitSetup(it) } diff --git a/styleguide/checkstyle-suppressions.xml b/styleguide/checkstyle-suppressions.xml index 9a66026c06..7d5fb7f76c 100644 --- a/styleguide/checkstyle-suppressions.xml +++ b/styleguide/checkstyle-suppressions.xml @@ -10,4 +10,6 @@ suppressions PUBLIC "-//Puppy Crawl//DTD Suppressions 1.1//EN" checks="(LocalVariableName|MemberName|MethodName|MethodTypeParameterName|ParameterName)" /> + diff --git a/styleguide/pmd-ruleset.xml b/styleguide/pmd-ruleset.xml index 3f0683a55a..f2ef2bc8db 100644 --- a/styleguide/pmd-ruleset.xml +++ b/styleguide/pmd-ruleset.xml @@ -8,6 +8,7 @@ .*/*JNI.* .*/*IntegrationTests.* + .*/quickbuf/.* diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt index 93d07f4617..6aa77d5595 100644 --- a/wpimath/CMakeLists.txt +++ b/wpimath/CMakeLists.txt @@ -5,6 +5,49 @@ include(CompileWarnings) include(AddTest) include(DownloadAndCheck) +file(GLOB wpimath_proto_src src/main/proto/*.proto) +protobuf_generate_cpp(WPIMATH_PROTO_SRCS WPIMATH_PROTO_HDRS PROTOC_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/protobuf" PROTOS ${wpimath_proto_src}) + +function(quickbuf_generate SRCS JAVA_PACKAGE) + if(NOT ARGN) + message(SEND_ERROR "Error: PROTOBUF_GENERATE_QUICKBUF() called without any proto files") + return() + endif() + + set(_generated_srcs_all) + foreach(_proto ${ARGN}) + get_filename_component(_abs_file ${_proto} ABSOLUTE) + get_filename_component(_abs_dir ${_abs_file} DIRECTORY) + get_filename_component(_basename ${_proto} NAME_WLE) + file(RELATIVE_PATH _rel_dir ${CMAKE_CURRENT_SOURCE_DIR} ${_abs_dir}) + + # convert to QuickBuffers Java case (geometry2d -> Geometry2D) + string(REGEX MATCHALL "[A-Za-z_]+|[0-9]+" _name_components ${_basename}) + set(_name_components_out) + foreach(_part ${_name_components}) + string(SUBSTRING ${_part} 0 1 _first_letter) + string(TOUPPER ${_first_letter} _first_letter) + string(REGEX REPLACE "^.(.*)" "${_first_letter}\\1" _part_out "${_part}") + list(APPEND _name_components_out ${_part_out}) + endforeach() + list(JOIN _name_components_out "" _basename_title) + + set(_generated_src "${CMAKE_CURRENT_BINARY_DIR}/quickbuf/${JAVA_PACKAGE}/${_basename_title}.java") + + list(APPEND _generated_srcs_all ${_generated_src}) + + add_custom_command( + OUTPUT ${_generated_src} + COMMAND protobuf::protoc + ARGS --plugin=protoc-gen-quickbuf=${Quickbuf_EXECUTABLE} --quickbuf_out=gen_descriptors=true:${CMAKE_CURRENT_BINARY_DIR}/quickbuf -I${_abs_dir} ${_abs_file} + DEPENDS ${_abs_file} protobuf::protoc + COMMENT "Running quickbuf protocol buffer compiler on ${_proto}" + VERBATIM ) + endforeach() + + set(${SRCS} ${_generated_srcs_all} PARENT_SCOPE) +endfunction() + file(GLOB wpimath_jni_src src/main/native/cpp/jni/WPIMathJNI_DARE.cpp src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp @@ -19,6 +62,8 @@ if (WITH_JAVA) include(UseJava) set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked") + quickbuf_generate(WPIMATH_QUICKBUF_SRCS "edu/wpi/first/math/proto" ${wpimath_proto_src}) + if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.43.1.jar") set(BASE_URL "https://search.maven.org/remotecontent?filepath=") set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml") @@ -45,8 +90,10 @@ if (WITH_JAVA) file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar") file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar") + file(GLOB QUICKBUF_JAR + ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/*.jar) - set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${EJML_JARS} ${JACKSON_JARS}) + set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${EJML_JARS} ${JACKSON_JARS} ${QUICKBUF_JAR}) execute_process(COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/generate_numbers.py ${WPILIB_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult) if(NOT (generateResult EQUAL "0")) @@ -61,7 +108,7 @@ if (WITH_JAVA) file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${WPILIB_BINARY_DIR}/wpimath/generated/*.java) - add_jar(wpimath_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath GENERATE_NATIVE_HEADERS wpimath_jni_headers) + add_jar(wpimath_jar ${JAVA_SOURCES} ${WPIMATH_QUICKBUF_SRCS} INCLUDE_JARS ${EJML_JARS} wpiutil_jar OUTPUT_NAME wpimath GENERATE_NATIVE_HEADERS wpimath_jni_headers) get_property(WPIMATH_JAR_FILE TARGET wpimath_jar PROPERTY JAR_FILE) install(FILES ${WPIMATH_JAR_FILE} DESTINATION "${java_lib_dest}") @@ -85,7 +132,7 @@ file(GLOB_RECURSE wpimath_native_src src/main/native/cpp/*.cpp) list(REMOVE_ITEM wpimath_native_src ${wpimath_jni_src}) set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS FALSE) -add_library(wpimath ${wpimath_native_src}) +add_library(wpimath ${wpimath_native_src} ${WPIMATH_PROTO_SRCS}) set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS TRUE) set_target_properties(wpimath PROPERTIES DEBUG_POSTFIX "d") @@ -116,6 +163,7 @@ target_include_directories(wpimath SYSTEM PUBLIC install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpimath") target_include_directories(wpimath PUBLIC $ + $ $) install(TARGETS wpimath EXPORT wpimath) diff --git a/wpimath/build.gradle b/wpimath/build.gradle index eaef6144c2..09df80c99a 100644 --- a/wpimath/build.gradle +++ b/wpimath/build.gradle @@ -41,6 +41,7 @@ dependencies { api "com.fasterxml.jackson.core:jackson-annotations:2.15.2" api "com.fasterxml.jackson.core:jackson-core:2.15.2" api "com.fasterxml.jackson.core:jackson-databind:2.15.2" + api "us.hebi.quickbuf:quickbuf-runtime:1.3.2" } def wpilibNumberFileInput = file("src/generate/GenericNumber.java.jinja") diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 43985865dd..bc7f536c25 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -9,10 +9,15 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; import java.util.Collections; import java.util.Comparator; import java.util.List; import java.util.Objects; +import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a 2D pose containing translational and rotational elements. */ @JsonIgnoreProperties(ignoreUnknown = true) @@ -305,4 +310,83 @@ public class Pose2d implements Interpolatable { return this.exp(scaledTwist); } } + + public static final class AStruct implements Struct { + @Override + public Class getTypeClass() { + return Pose2d.class; + } + + @Override + public String getTypeString() { + return "struct:Pose2d"; + } + + @Override + public int getSize() { + return Translation2d.struct.getSize() + Rotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation2d translation;Rotation2d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation2d.struct, Rotation2d.struct}; + } + + @Override + public Pose2d unpack(ByteBuffer bb) { + Translation2d translation = Translation2d.struct.unpack(bb); + Rotation2d rotation = Rotation2d.struct.unpack(bb); + return new Pose2d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Pose2d value) { + Translation2d.struct.pack(bb, value.m_translation); + Rotation2d.struct.pack(bb, value.m_rotation); + } + } + + public static final AStruct struct = new AStruct(); + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return Pose2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPose2d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation2d.proto, Rotation2d.proto}; + } + + @Override + public ProtobufPose2d createMessage() { + return ProtobufPose2d.newInstance(); + } + + @Override + public Pose2d unpack(ProtobufPose2d msg) { + return new Pose2d( + Translation2d.proto.unpack(msg.getTranslation()), + Rotation2d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufPose2d msg, Pose2d value) { + Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation); + Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation); + } + } + + public static final AProto proto = new AProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 0d44c095e9..2ea709439a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -10,7 +10,12 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; import java.util.Objects; +import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a 3D pose containing translational and rotational elements. */ @JsonIgnoreProperties(ignoreUnknown = true) @@ -318,4 +323,83 @@ public class Pose3d implements Interpolatable { return this.exp(scaledTwist); } } + + public static final class AStruct implements Struct { + @Override + public Class getTypeClass() { + return Pose3d.class; + } + + @Override + public String getTypeString() { + return "struct:Pose3d"; + } + + @Override + public int getSize() { + return Translation3d.struct.getSize() + Rotation3d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation3d translation;Rotation3d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation3d.struct, Rotation3d.struct}; + } + + @Override + public Pose3d unpack(ByteBuffer bb) { + Translation3d translation = Translation3d.struct.unpack(bb); + Rotation3d rotation = Rotation3d.struct.unpack(bb); + return new Pose3d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Pose3d value) { + Translation3d.struct.pack(bb, value.m_translation); + Rotation3d.struct.pack(bb, value.m_rotation); + } + } + + public static final AStruct struct = new AStruct(); + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return Pose3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPose3d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation3d.proto, Rotation3d.proto}; + } + + @Override + public ProtobufPose3d createMessage() { + return ProtobufPose3d.newInstance(); + } + + @Override + public Pose3d unpack(ProtobufPose3d msg) { + return new Pose3d( + Translation3d.proto.unpack(msg.getTranslation()), + Rotation3d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufPose3d msg, Pose3d value) { + Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation); + Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation); + } + } + + public static final AProto proto = new AProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java index b3ec5eaa86..23fd26b6c0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java @@ -11,7 +11,12 @@ import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; import java.util.Objects; +import us.hebi.quickbuf.Descriptors.Descriptor; @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) @@ -400,4 +405,74 @@ public class Quaternion { return VecBuilder.fill(coeff * getX(), coeff * getY(), coeff * getZ()); } + + public static final class AStruct implements Struct { + @Override + public Class getTypeClass() { + return Quaternion.class; + } + + @Override + public String getTypeString() { + return "struct:Quaternion"; + } + + @Override + public int getSize() { + return kSizeDouble * 4; + } + + @Override + public String getSchema() { + return "double w;double x;double y;double z"; + } + + @Override + public Quaternion unpack(ByteBuffer bb) { + double w = bb.getDouble(); + double x = bb.getDouble(); + double y = bb.getDouble(); + double z = bb.getDouble(); + return new Quaternion(w, x, y, z); + } + + @Override + public void pack(ByteBuffer bb, Quaternion value) { + bb.putDouble(value.getW()); + bb.putDouble(value.getX()); + bb.putDouble(value.getY()); + bb.putDouble(value.getZ()); + } + } + + public static final AStruct struct = new AStruct(); + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return Quaternion.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufQuaternion.getDescriptor(); + } + + @Override + public ProtobufQuaternion createMessage() { + return ProtobufQuaternion.newInstance(); + } + + @Override + public Quaternion unpack(ProtobufQuaternion msg) { + return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ()); + } + + @Override + public void pack(ProtobufQuaternion msg, Quaternion value) { + msg.setW(value.getW()).setX(value.getX()).setY(value.getY()).setZ(value.getZ()); + } + } + + public static final AProto proto = new AProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 5be61561e4..0556669d7d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -10,8 +10,13 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; import java.util.Objects; +import us.hebi.quickbuf.Descriptors.Descriptor; /** * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine). @@ -256,4 +261,67 @@ public class Rotation2d implements Interpolatable { public Rotation2d interpolate(Rotation2d endValue, double t) { return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); } + + public static final class AStruct implements Struct { + @Override + public Class getTypeClass() { + return Rotation2d.class; + } + + @Override + public String getTypeString() { + return "struct:Rotation2d"; + } + + @Override + public int getSize() { + return kSizeDouble; + } + + @Override + public String getSchema() { + return "double value"; + } + + @Override + public Rotation2d unpack(ByteBuffer bb) { + return new Rotation2d(bb.getDouble()); + } + + @Override + public void pack(ByteBuffer bb, Rotation2d value) { + bb.putDouble(value.m_value); + } + } + + public static final AStruct struct = new AStruct(); + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return Rotation2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufRotation2d.getDescriptor(); + } + + @Override + public ProtobufRotation2d createMessage() { + return ProtobufRotation2d.newInstance(); + } + + @Override + public Rotation2d unpack(ProtobufRotation2d msg) { + return new Rotation2d(msg.getValue()); + } + + @Override + public void pack(ProtobufRotation2d msg, Rotation2d value) { + msg.setValue(value.m_value); + } + } + + public static final AProto proto = new AProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 467ec1ca24..b434523687 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -17,8 +17,13 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; import java.util.Objects; import org.ejml.dense.row.factory.DecompositionFactory_DDRM; +import us.hebi.quickbuf.Descriptors.Descriptor; /** A rotation in a 3D coordinate frame represented by a quaternion. */ @JsonIgnoreProperties(ignoreUnknown = true) @@ -434,4 +439,77 @@ public class Rotation3d implements Interpolatable { public Rotation3d interpolate(Rotation3d endValue, double t) { return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); } + + public static final class AStruct implements Struct { + @Override + public Class getTypeClass() { + return Rotation3d.class; + } + + @Override + public String getTypeString() { + return "struct:Rotation3d"; + } + + @Override + public int getSize() { + return Quaternion.struct.getSize(); + } + + @Override + public String getSchema() { + return "Quaternion q"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Quaternion.struct}; + } + + @Override + public Rotation3d unpack(ByteBuffer bb) { + return new Rotation3d(Quaternion.struct.unpack(bb)); + } + + @Override + public void pack(ByteBuffer bb, Rotation3d value) { + Quaternion.struct.pack(bb, value.m_q); + } + } + + public static final AStruct struct = new AStruct(); + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return Rotation3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufRotation3d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Quaternion.proto}; + } + + @Override + public ProtobufRotation3d createMessage() { + return ProtobufRotation3d.newInstance(); + } + + @Override + public Rotation3d unpack(ProtobufRotation3d msg) { + return new Rotation3d(Quaternion.proto.unpack(msg.getQ())); + } + + @Override + public void pack(ProtobufRotation3d msg, Rotation3d value) { + Quaternion.proto.pack(msg.getMutableQ(), value.m_q); + } + } + + public static final AProto proto = new AProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index 04119b954d..c7959ba953 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -4,7 +4,12 @@ package edu.wpi.first.math.geometry; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; import java.util.Objects; +import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a transformation for a Pose2d in the pose's frame. */ public class Transform2d { @@ -163,4 +168,83 @@ public class Transform2d { public int hashCode() { return Objects.hash(m_translation, m_rotation); } + + public static final class AStruct implements Struct { + @Override + public Class getTypeClass() { + return Transform2d.class; + } + + @Override + public String getTypeString() { + return "struct:Transform2d"; + } + + @Override + public int getSize() { + return Translation2d.struct.getSize() + Rotation2d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation2d translation;Rotation2d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation2d.struct, Rotation2d.struct}; + } + + @Override + public Transform2d unpack(ByteBuffer bb) { + Translation2d translation = Translation2d.struct.unpack(bb); + Rotation2d rotation = Rotation2d.struct.unpack(bb); + return new Transform2d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Transform2d value) { + Translation2d.struct.pack(bb, value.m_translation); + Rotation2d.struct.pack(bb, value.m_rotation); + } + } + + public static final AStruct struct = new AStruct(); + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return Transform2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTransform2d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation2d.proto, Rotation2d.proto}; + } + + @Override + public ProtobufTransform2d createMessage() { + return ProtobufTransform2d.newInstance(); + } + + @Override + public Transform2d unpack(ProtobufTransform2d msg) { + return new Transform2d( + Translation2d.proto.unpack(msg.getTranslation()), + Rotation2d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufTransform2d msg, Transform2d value) { + Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation); + Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation); + } + } + + public static final AProto proto = new AProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java index 3e56a0a4d3..223a14b18f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -4,7 +4,12 @@ package edu.wpi.first.math.geometry; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; import java.util.Objects; +import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a transformation for a Pose3d in the pose's frame. */ public class Transform3d { @@ -173,4 +178,83 @@ public class Transform3d { public int hashCode() { return Objects.hash(m_translation, m_rotation); } + + public static final class AStruct implements Struct { + @Override + public Class getTypeClass() { + return Transform3d.class; + } + + @Override + public String getTypeString() { + return "struct:Transform3d"; + } + + @Override + public int getSize() { + return Translation3d.struct.getSize() + Rotation3d.struct.getSize(); + } + + @Override + public String getSchema() { + return "Translation3d translation;Rotation3d rotation"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Translation3d.struct, Rotation3d.struct}; + } + + @Override + public Transform3d unpack(ByteBuffer bb) { + Translation3d translation = Translation3d.struct.unpack(bb); + Rotation3d rotation = Rotation3d.struct.unpack(bb); + return new Transform3d(translation, rotation); + } + + @Override + public void pack(ByteBuffer bb, Transform3d value) { + Translation3d.struct.pack(bb, value.m_translation); + Rotation3d.struct.pack(bb, value.m_rotation); + } + } + + public static final AStruct struct = new AStruct(); + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return Transform3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTransform3d.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Translation3d.proto, Rotation3d.proto}; + } + + @Override + public ProtobufTransform3d createMessage() { + return ProtobufTransform3d.newInstance(); + } + + @Override + public Transform3d unpack(ProtobufTransform3d msg) { + return new Transform3d( + Translation3d.proto.unpack(msg.getTranslation()), + Rotation3d.proto.unpack(msg.getRotation())); + } + + @Override + public void pack(ProtobufTransform3d msg, Transform3d value) { + Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation); + Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation); + } + } + + public static final AProto proto = new AProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index 406a192b63..96e0001f50 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -10,10 +10,15 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; import java.util.Collections; import java.util.Comparator; import java.util.List; import java.util.Objects; +import us.hebi.quickbuf.Descriptors.Descriptor; /** * Represents a translation in 2D space. This object can be used to represent a point or a vector. @@ -229,4 +234,70 @@ public class Translation2d implements Interpolatable { MathUtil.interpolate(this.getX(), endValue.getX(), t), MathUtil.interpolate(this.getY(), endValue.getY(), t)); } + + public static final class AStruct implements Struct { + @Override + public Class getTypeClass() { + return Translation2d.class; + } + + @Override + public String getTypeString() { + return "struct:Translation2d"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double x;double y"; + } + + @Override + public Translation2d unpack(ByteBuffer bb) { + double x = bb.getDouble(); + double y = bb.getDouble(); + return new Translation2d(x, y); + } + + @Override + public void pack(ByteBuffer bb, Translation2d value) { + bb.putDouble(value.m_x); + bb.putDouble(value.m_y); + } + } + + public static final AStruct struct = new AStruct(); + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return Translation2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTranslation2d.getDescriptor(); + } + + @Override + public ProtobufTranslation2d createMessage() { + return ProtobufTranslation2d.newInstance(); + } + + @Override + public Translation2d unpack(ProtobufTranslation2d msg) { + return new Translation2d(msg.getX(), msg.getY()); + } + + @Override + public void pack(ProtobufTranslation2d msg, Translation2d value) { + msg.setX(value.m_x).setY(value.m_y); + } + } + + public static final AProto proto = new AProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 810f56cc0b..bc55f650c8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -10,7 +10,12 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; import java.util.Objects; +import us.hebi.quickbuf.Descriptors.Descriptor; /** * Represents a translation in 3D space. This object can be used to represent a point or a vector. @@ -231,4 +236,72 @@ public class Translation3d implements Interpolatable { MathUtil.interpolate(this.getY(), endValue.getY(), t), MathUtil.interpolate(this.getZ(), endValue.getZ(), t)); } + + public static final class AStruct implements Struct { + @Override + public Class getTypeClass() { + return Translation3d.class; + } + + @Override + public String getTypeString() { + return "struct:Translation3d"; + } + + @Override + public int getSize() { + return kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "double x;double y;double z"; + } + + @Override + public Translation3d unpack(ByteBuffer bb) { + double x = bb.getDouble(); + double y = bb.getDouble(); + double z = bb.getDouble(); + return new Translation3d(x, y, z); + } + + @Override + public void pack(ByteBuffer bb, Translation3d value) { + bb.putDouble(value.m_x); + bb.putDouble(value.m_y); + bb.putDouble(value.m_z); + } + } + + public static final AStruct struct = new AStruct(); + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return Translation3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTranslation3d.getDescriptor(); + } + + @Override + public ProtobufTranslation3d createMessage() { + return ProtobufTranslation3d.newInstance(); + } + + @Override + public Translation3d unpack(ProtobufTranslation3d msg) { + return new Translation3d(msg.getX(), msg.getY(), msg.getZ()); + } + + @Override + public void pack(ProtobufTranslation3d msg, Translation3d value) { + msg.setX(value.m_x).setY(value.m_y).setZ(value.m_z); + } + } + + public static final AProto proto = new AProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java index 1cfe7af2ad..a4ae9f83ac 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java @@ -4,7 +4,12 @@ package edu.wpi.first.math.geometry; +import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; import java.util.Objects; +import us.hebi.quickbuf.Descriptors.Descriptor; /** * A change in distance along a 2D arc since the last pose update. We can use ideas from @@ -62,4 +67,72 @@ public class Twist2d { public int hashCode() { return Objects.hash(dx, dy, dtheta); } + + public static final class AStruct implements Struct { + @Override + public Class getTypeClass() { + return Twist2d.class; + } + + @Override + public String getTypeString() { + return "struct:Twist2d"; + } + + @Override + public int getSize() { + return kSizeDouble * 3; + } + + @Override + public String getSchema() { + return "double dx;double dy;double dtheta"; + } + + @Override + public Twist2d unpack(ByteBuffer bb) { + double dx = bb.getDouble(); + double dy = bb.getDouble(); + double dtheta = bb.getDouble(); + return new Twist2d(dx, dy, dtheta); + } + + @Override + public void pack(ByteBuffer bb, Twist2d value) { + bb.putDouble(value.dx); + bb.putDouble(value.dy); + bb.putDouble(value.dtheta); + } + } + + public static final AStruct struct = new AStruct(); + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return Twist2d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTwist2d.getDescriptor(); + } + + @Override + public ProtobufTwist2d createMessage() { + return ProtobufTwist2d.newInstance(); + } + + @Override + public Twist2d unpack(ProtobufTwist2d msg) { + return new Twist2d(msg.getDx(), msg.getDy(), msg.getDtheta()); + } + + @Override + public void pack(ProtobufTwist2d msg, Twist2d value) { + msg.setDx(value.dx).setDy(value.dy).setDtheta(value.dtheta); + } + } + + public static final AProto proto = new AProto(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java index 454716711a..d08d5cf158 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java @@ -4,7 +4,12 @@ package edu.wpi.first.math.geometry; +import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; import java.util.Objects; +import us.hebi.quickbuf.Descriptors.Descriptor; /** * A change in distance along a 3D arc since the last pose update. We can use ideas from @@ -82,4 +87,84 @@ public class Twist3d { public int hashCode() { return Objects.hash(dx, dy, dz, rx, ry, rz); } + + public static final class AStruct implements Struct { + @Override + public Class getTypeClass() { + return Twist3d.class; + } + + @Override + public String getTypeString() { + return "struct:Twist3d"; + } + + @Override + public int getSize() { + return kSizeDouble * 6; + } + + @Override + public String getSchema() { + return "double dx;double dy;double dz;double rx;double ry;double rz"; + } + + @Override + public Twist3d unpack(ByteBuffer bb) { + double dx = bb.getDouble(); + double dy = bb.getDouble(); + double dz = bb.getDouble(); + double rx = bb.getDouble(); + double ry = bb.getDouble(); + double rz = bb.getDouble(); + return new Twist3d(dx, dy, dz, rx, ry, rz); + } + + @Override + public void pack(ByteBuffer bb, Twist3d value) { + bb.putDouble(value.dx); + bb.putDouble(value.dy); + bb.putDouble(value.dz); + bb.putDouble(value.rx); + bb.putDouble(value.ry); + bb.putDouble(value.rz); + } + } + + public static final AStruct struct = new AStruct(); + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return Twist3d.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTwist3d.getDescriptor(); + } + + @Override + public ProtobufTwist3d createMessage() { + return ProtobufTwist3d.newInstance(); + } + + @Override + public Twist3d unpack(ProtobufTwist3d msg) { + return new Twist3d( + msg.getDx(), msg.getDy(), msg.getDz(), msg.getRx(), msg.getRy(), msg.getRz()); + } + + @Override + public void pack(ProtobufTwist3d msg, Twist3d value) { + msg.setDx(value.dx) + .setDy(value.dy) + .setDz(value.dz) + .setRx(value.rx) + .setRy(value.ry) + .setRz(value.rz); + } + } + + public static final AProto proto = new AProto(); } diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp index 53779d05c8..2e15216527 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp @@ -9,6 +9,7 @@ #include #include "frc/MathUtil.h" +#include "geometry2d.pb.h" using namespace frc; @@ -108,3 +109,23 @@ void frc::from_json(const wpi::json& json, Pose2d& pose) { pose = Pose2d{json.at("translation").get(), json.at("rotation").get()}; } + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Pose2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return Pose2d{wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation())}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Pose2d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index f479b0d705..ffbaecbff4 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -9,6 +9,8 @@ #include #include +#include "geometry3d.pb.h" + using namespace frc; namespace { @@ -187,3 +189,23 @@ void frc::from_json(const wpi::json& json, Pose3d& pose) { pose = Pose3d{json.at("translation").get(), json.at("rotation").get()}; } + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Pose3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return Pose3d{wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation())}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Pose3d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp index ea9b179ca8..37afbb88a3 100644 --- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp +++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp @@ -8,6 +8,8 @@ #include +#include "geometry3d.pb.h" + using namespace frc; Quaternion::Quaternion(double w, double x, double y, double z) @@ -230,3 +232,24 @@ void frc::from_json(const wpi::json& json, Quaternion& quaternion) { Quaternion{json.at("W").get(), json.at("X").get(), json.at("Y").get(), json.at("Z").get()}; } + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Quaternion wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Quaternion{m->w(), m->x(), m->y(), m->z()}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Quaternion& value) { + auto m = static_cast(msg); + m->set_w(value.W()); + m->set_x(value.X()); + m->set_y(value.Y()); + m->set_z(value.Z()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp index 921e1f81a0..05a644ea2b 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp @@ -8,6 +8,7 @@ #include +#include "geometry2d.pb.h" #include "units/math.h" using namespace frc; @@ -19,3 +20,21 @@ void frc::to_json(wpi::json& json, const Rotation2d& rotation) { void frc::from_json(const wpi::json& json, Rotation2d& rotation) { rotation = Rotation2d{units::radian_t{json.at("radians").get()}}; } + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Rotation2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Rotation2d{units::radian_t{m->value()}}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Rotation2d& value) { + auto m = static_cast(msg); + m->set_value(value.Radians().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index b4dce35a5d..3a68870045 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -13,6 +13,7 @@ #include #include "frc/fmt/Eigen.h" +#include "geometry3d.pb.h" #include "units/math.h" #include "wpimath/MathShared.h" @@ -261,3 +262,21 @@ void frc::to_json(wpi::json& json, const Rotation3d& rotation) { void frc::from_json(const wpi::json& json, Rotation3d& rotation) { rotation = Rotation3d{json.at("quaternion").get()}; } + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Rotation3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return Rotation3d{wpi::UnpackProtobuf(m->q())}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Rotation3d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp index 25b05907ed..77f3ceed2d 100644 --- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp @@ -5,6 +5,7 @@ #include "frc/geometry/Transform2d.h" #include "frc/geometry/Pose2d.h" +#include "geometry2d.pb.h" using namespace frc; @@ -21,3 +22,23 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) { Transform2d Transform2d::operator+(const Transform2d& other) const { return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)}; } + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTransform2d>(arena); +} + +frc::Transform2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return Transform2d{wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation())}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Transform2d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp index 556a3027d0..de6c2533c9 100644 --- a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp @@ -5,6 +5,7 @@ #include "frc/geometry/Transform3d.h" #include "frc/geometry/Pose3d.h" +#include "geometry3d.pb.h" using namespace frc; @@ -35,3 +36,23 @@ Transform3d Transform3d::Inverse() const { Transform3d Transform3d::operator+(const Transform3d& other) const { return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)}; } + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTransform3d>(arena); +} + +frc::Transform3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return Transform3d{wpi::UnpackProtobuf(m->translation()), + wpi::UnpackProtobuf(m->rotation())}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Transform3d& value) { + auto m = static_cast(msg); + wpi::PackProtobuf(m->mutable_translation(), value.Translation()); + wpi::PackProtobuf(m->mutable_rotation(), value.Rotation()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index c8897d9101..6d5f3153ae 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp @@ -6,6 +6,7 @@ #include +#include "geometry2d.pb.h" #include "units/math.h" using namespace frc; @@ -48,3 +49,22 @@ void frc::from_json(const wpi::json& json, Translation2d& translation) { translation = Translation2d{units::meter_t{json.at("x").get()}, units::meter_t{json.at("y").get()}}; } + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTranslation2d>(arena); +} + +frc::Translation2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Translation2d{units::meter_t{m->x()}, units::meter_t{m->y()}}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Translation2d& value) { + auto m = static_cast(msg); + m->set_x(value.X().value()); + m->set_y(value.Y().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp index 2c53791bfd..90e94ae825 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp @@ -6,6 +6,7 @@ #include +#include "geometry3d.pb.h" #include "units/length.h" #include "units/math.h" @@ -52,3 +53,24 @@ void frc::from_json(const wpi::json& json, Translation3d& translation) { units::meter_t{json.at("y").get()}, units::meter_t{json.at("z").get()}}; } + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufTranslation3d>(arena); +} + +frc::Translation3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Translation3d{units::meter_t{m->x()}, units::meter_t{m->y()}, + units::meter_t{m->z()}}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Translation3d& value) { + auto m = static_cast(msg); + m->set_x(value.X().value()); + m->set_y(value.Y().value()); + m->set_z(value.Z().value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Twist2d.cpp b/wpimath/src/main/native/cpp/geometry/Twist2d.cpp new file mode 100644 index 0000000000..6c106eb436 --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/Twist2d.cpp @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/geometry/Twist2d.h" + +#include "geometry2d.pb.h" + +using namespace frc; + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Twist2d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Twist2d{units::meter_t{m->dx()}, units::meter_t{m->dy()}, + units::radian_t{m->dtheta()}}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Twist2d& value) { + auto m = static_cast(msg); + m->set_dx(value.dx.value()); + m->set_dy(value.dy.value()); + m->set_dtheta(value.dtheta.value()); +} diff --git a/wpimath/src/main/native/cpp/geometry/Twist3d.cpp b/wpimath/src/main/native/cpp/geometry/Twist3d.cpp new file mode 100644 index 0000000000..4f4ce86b4b --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/Twist3d.cpp @@ -0,0 +1,34 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/geometry/Twist3d.h" + +#include "geometry3d.pb.h" + +using namespace frc; + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage( + arena); +} + +frc::Twist3d wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + return frc::Twist3d{units::meter_t{m->dx()}, units::meter_t{m->dy()}, + units::meter_t{m->dz()}, units::radian_t{m->rx()}, + units::radian_t{m->ry()}, units::radian_t{m->rz()}}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const frc::Twist3d& value) { + auto m = static_cast(msg); + m->set_dx(value.dx.value()); + m->set_dy(value.dy.value()); + m->set_dz(value.dz.value()); + m->set_rx(value.rx.value()); + m->set_ry(value.ry.value()); + m->set_rz(value.rz.value()); +} diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 304149d209..970f792961 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -9,7 +9,10 @@ #include #include +#include +#include +#include "frc/geometry/Rotation2d.h" #include "frc/geometry/Transform2d.h" #include "frc/geometry/Translation2d.h" #include "frc/geometry/Twist2d.h" @@ -212,4 +215,38 @@ void from_json(const wpi::json& json, Pose2d& pose); } // namespace frc +template <> +struct wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Pose2d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation2d translation;Rotation2d rotation"; + static frc::Pose2d Unpack(std::span data) { + return {wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}; + } + static void Pack(std::span data, const frc::Pose2d& value) { + wpi::PackStruct<0>(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); + } + static void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + } + + private: + static constexpr size_t kRotationOff = wpi::Struct::kSize; +}; + +static_assert(wpi::HasNestedStruct); + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Pose2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Pose2d& value); +}; + #include "frc/geometry/Pose2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 7369563631..b5a0e031ba 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -6,8 +6,11 @@ #include #include +#include +#include #include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation3d.h" #include "frc/geometry/Transform3d.h" #include "frc/geometry/Translation3d.h" #include "frc/geometry/Twist3d.h" @@ -213,3 +216,37 @@ WPILIB_DLLEXPORT void from_json(const wpi::json& json, Pose3d& pose); } // namespace frc + +template <> +struct wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Pose3d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation3d translation;Rotation3d rotation"; + static frc::Pose3d Unpack(std::span data) { + return {wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}; + } + static void Pack(std::span data, const frc::Pose3d& value) { + wpi::PackStruct<0>(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); + } + static void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + } + + private: + static constexpr size_t kRotationOff = wpi::Struct::kSize; +}; + +static_assert(wpi::HasNestedStruct); + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Pose3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Pose3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h index 874928135c..63a3af2784 100644 --- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h +++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h @@ -7,6 +7,8 @@ #include #include #include +#include +#include namespace frc { @@ -187,3 +189,32 @@ WPILIB_DLLEXPORT void from_json(const wpi::json& json, Quaternion& quaternion); } // namespace frc + +template <> +struct wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Quaternion"; + static constexpr size_t kSize = 32; + static constexpr std::string_view kSchema = + "double w;double x;double y;double z"; + static frc::Quaternion Unpack(std::span data) { + return {wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}; + } + static void Pack(std::span data, const frc::Quaternion& value) { + wpi::PackStruct<0>(data, value.W()); + wpi::PackStruct<8>(data, value.X()); + wpi::PackStruct<16>(data, value.Y()); + wpi::PackStruct<24>(data, value.Z()); + } +}; + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static constexpr std::string_view kTypeString = "proto:Quaternion"; + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Quaternion Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Quaternion& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index 39416c615a..96041a4979 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -6,6 +6,8 @@ #include #include +#include +#include #include "units/angle.h" @@ -176,4 +178,25 @@ void from_json(const wpi::json& json, Rotation2d& rotation); } // namespace frc +template <> +struct wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Rotation2d"; + static constexpr size_t kSize = 8; + static constexpr std::string_view kSchema = "double value"; + static frc::Rotation2d Unpack(std::span data) { + return units::radian_t{wpi::UnpackStruct(data)}; + } + static void Pack(std::span data, const frc::Rotation2d& value) { + wpi::PackStruct(data, value.Radians().value()); + } +}; + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Rotation2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Rotation2d& value); +}; + #include "frc/geometry/Rotation2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 6755205572..6d04715316 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -7,6 +7,8 @@ #include #include #include +#include +#include #include "frc/geometry/Quaternion.h" #include "frc/geometry/Rotation2d.h" @@ -194,3 +196,31 @@ WPILIB_DLLEXPORT void from_json(const wpi::json& json, Rotation3d& rotation); } // namespace frc + +template <> +struct wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Rotation3d"; + static constexpr size_t kSize = wpi::Struct::kSize; + static constexpr std::string_view kSchema = "Quaternion q"; + static frc::Rotation3d Unpack(std::span data) { + return frc::Rotation3d{wpi::UnpackStruct(data)}; + } + static void Pack(std::span data, + const frc::Rotation3d& value) { + wpi::PackStruct<0>(data, value.GetQuaternion()); + } + static void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + } +}; + +static_assert(wpi::HasNestedStruct); + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Rotation3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Rotation3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index f91fbc7cdf..593dce03ab 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -5,6 +5,8 @@ #pragma once #include +#include +#include #include "frc/geometry/Translation2d.h" @@ -124,4 +126,40 @@ class WPILIB_DLLEXPORT Transform2d { }; } // namespace frc +template <> +struct wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Transform2d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation2d translation;Rotation2d rotation"; + static frc::Transform2d Unpack(std::span data) { + return {wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}; + } + static void Pack(std::span data, + const frc::Transform2d& value) { + wpi::PackStruct<0>(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); + } + static void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + } + + private: + static constexpr size_t kRotationOff = wpi::Struct::kSize; +}; + +static_assert(wpi::HasNestedStruct); + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Transform2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Transform2d& value); +}; + #include "frc/geometry/Transform2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index 9e4683fa1d..d5af97d473 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -5,6 +5,8 @@ #pragma once #include +#include +#include #include "frc/geometry/Translation3d.h" @@ -129,3 +131,39 @@ class WPILIB_DLLEXPORT Transform3d { Rotation3d m_rotation; }; } // namespace frc + +template <> +struct wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Transform3d"; + static constexpr size_t kSize = wpi::Struct::kSize + + wpi::Struct::kSize; + static constexpr std::string_view kSchema = + "Translation3d translation;Rotation3d rotation"; + static frc::Transform3d Unpack(std::span data) { + return {wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}; + } + static void Pack(std::span data, + const frc::Transform3d& value) { + wpi::PackStruct<0>(data, value.Translation()); + wpi::PackStruct(data, value.Rotation()); + } + static void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + } + + private: + static constexpr size_t kRotationOff = wpi::Struct::kSize; +}; + +static_assert(wpi::HasNestedStruct); + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Transform3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Transform3d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index 851e7c7275..1568586163 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -9,6 +9,8 @@ #include #include +#include +#include #include "frc/geometry/Rotation2d.h" #include "units/length.h" @@ -198,4 +200,28 @@ void from_json(const wpi::json& json, Translation2d& state); } // namespace frc +template <> +struct wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Translation2d"; + static constexpr size_t kSize = 16; + static constexpr std::string_view kSchema = "double x;double y"; + static frc::Translation2d Unpack(std::span data) { + return {units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}}; + } + static void Pack(std::span data, + const frc::Translation2d& value) { + wpi::PackStruct<0>(data, value.X().value()); + wpi::PackStruct<8>(data, value.Y().value()); + } +}; + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Translation2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Translation2d& value); +}; + #include "frc/geometry/Translation2d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index fb126af23a..47ba1f6bad 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -6,6 +6,8 @@ #include #include +#include +#include #include "frc/geometry/Rotation3d.h" #include "frc/geometry/Translation2d.h" @@ -183,4 +185,30 @@ void from_json(const wpi::json& json, Translation3d& state); } // namespace frc +template <> +struct wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Translation3d"; + static constexpr size_t kSize = 24; + static constexpr std::string_view kSchema = "double x;double y;double z"; + static frc::Translation3d Unpack(std::span data) { + return {units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}}; + } + static void Pack(std::span data, + const frc::Translation3d& value) { + wpi::PackStruct<0>(data, value.X().value()); + wpi::PackStruct<8>(data, value.Y().value()); + wpi::PackStruct<16>(data, value.Z().value()); + } +}; + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Translation3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::Translation3d& value); +}; + #include "frc/geometry/Translation3d.inc" diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h index 82722f9841..257f1b65e7 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h @@ -5,6 +5,8 @@ #pragma once #include +#include +#include #include "units/angle.h" #include "units/length.h" @@ -57,3 +59,28 @@ struct WPILIB_DLLEXPORT Twist2d { } }; } // namespace frc + +template <> +struct wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Twist2d"; + static constexpr size_t kSize = 24; + static constexpr std::string_view kSchema = + "double dx;double dy;double dtheta"; + static frc::Twist2d Unpack(std::span data) { + return {units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}}; + } + static void Pack(std::span data, const frc::Twist2d& value) { + wpi::PackStruct<0>(data, value.dx.value()); + wpi::PackStruct<8>(data, value.dy.value()); + wpi::PackStruct<16>(data, value.dtheta.value()); + } +}; + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Twist2d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Twist2d& value); +}; diff --git a/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/wpimath/src/main/native/include/frc/geometry/Twist3d.h index 48d3c5d653..4d902dfcbb 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist3d.h @@ -5,6 +5,8 @@ #pragma once #include +#include +#include #include "frc/geometry/Rotation3d.h" #include "units/angle.h" @@ -77,3 +79,35 @@ struct WPILIB_DLLEXPORT Twist3d { } }; } // namespace frc + +template <> +struct wpi::Struct { + static constexpr std::string_view kTypeString = "struct:Twist3d"; + static constexpr size_t kSize = 48; + static constexpr std::string_view kSchema = + "double dx;double dy;double dz;double rx;double ry;double rz"; + static frc::Twist3d Unpack(std::span data) { + return {units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}, + units::radian_t{wpi::UnpackStruct(data)}}; + } + static void Pack(std::span data, const frc::Twist3d& value) { + wpi::PackStruct<0>(data, value.dx.value()); + wpi::PackStruct<8>(data, value.dy.value()); + wpi::PackStruct<16>(data, value.dz.value()); + wpi::PackStruct<24>(data, value.rx.value()); + wpi::PackStruct<32>(data, value.ry.value()); + wpi::PackStruct<40>(data, value.rz.value()); + } +}; + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static constexpr std::string_view kTypeString = "proto:Twist3d"; + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::Twist3d Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, const frc::Twist3d& value); +}; diff --git a/wpimath/src/main/proto/controller.proto b/wpimath/src/main/proto/controller.proto new file mode 100644 index 0000000000..0d0d3fb6ff --- /dev/null +++ b/wpimath/src/main/proto/controller.proto @@ -0,0 +1,37 @@ +syntax = "proto3"; + +package wpi.proto; + +option java_package = "edu.wpi.first.math.proto"; + +message ProtobufArmFeedforward { + double ks = 1; + double kg = 2; + double kv = 3; + double ka = 4; +} + +message ProtobufDifferentialDriveFeedforward { + double kv_linear = 1; + double ka_linear = 2; + double kv_angular = 3; + double ka_angular = 4; +} + +message ProtobufElevatorFeedforward { + double ks = 1; + double kg = 2; + double kv = 3; + double ka = 4; +} + +message ProtobufSimpleMotorFeedforward { + double ks = 1; + double kv = 2; + double ka = 3; +} + +message ProtobufDifferentialDriveWheelVoltages { + double left = 1; + double right = 2; +} diff --git a/wpimath/src/main/proto/geometry2d.proto b/wpimath/src/main/proto/geometry2d.proto new file mode 100644 index 0000000000..d52da4585a --- /dev/null +++ b/wpimath/src/main/proto/geometry2d.proto @@ -0,0 +1,30 @@ +syntax = "proto3"; + +package wpi.proto; + +option java_package = "edu.wpi.first.math.proto"; + +message ProtobufTranslation2d { + double x = 1; + double y = 2; +} + +message ProtobufRotation2d { + double value = 1; +} + +message ProtobufPose2d { + ProtobufTranslation2d translation = 1; + ProtobufRotation2d rotation = 2; +} + +message ProtobufTransform2d { + ProtobufTranslation2d translation = 1; + ProtobufRotation2d rotation = 2; +} + +message ProtobufTwist2d { + double dx = 1; + double dy = 2; + double dtheta = 3; +} diff --git a/wpimath/src/main/proto/geometry3d.proto b/wpimath/src/main/proto/geometry3d.proto new file mode 100644 index 0000000000..23b4d6489c --- /dev/null +++ b/wpimath/src/main/proto/geometry3d.proto @@ -0,0 +1,41 @@ +syntax = "proto3"; + +package wpi.proto; + +option java_package = "edu.wpi.first.math.proto"; + +message ProtobufTranslation3d { + double x = 1; + double y = 2; + double z = 3; +} + +message ProtobufQuaternion { + double w = 1; + double x = 2; + double y = 3; + double z = 4; +} + +message ProtobufRotation3d { + ProtobufQuaternion q = 1; +} + +message ProtobufPose3d { + ProtobufTranslation3d translation = 1; + ProtobufRotation3d rotation = 2; +} + +message ProtobufTransform3d { + ProtobufTranslation3d translation = 1; + ProtobufRotation3d rotation = 2; +} + +message ProtobufTwist3d { + double dx = 1; + double dy = 2; + double dz = 3; + double rx = 4; + double ry = 5; + double rz = 6; +} diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto new file mode 100644 index 0000000000..8fdbf2d8b0 --- /dev/null +++ b/wpimath/src/main/proto/kinematics.proto @@ -0,0 +1,64 @@ +syntax = "proto3"; + +package wpi.proto; + +import "geometry2d.proto"; + +option java_package = "edu.wpi.first.math.proto"; + +message ProtobufChassisSpeeds { + double vx = 1; + double vy = 2; + double omega = 3; +} + +message ProtobufDifferentialDriveKinematics { + double track_width = 1; +} + +message ProtobufDifferentialDriveWheelSpeeds { + double left = 1; + double right = 2; +} + +message ProtobufMecanumDriveKinematics { + ProtobufTranslation2d front_left = 1; + ProtobufTranslation2d front_right = 2; + ProtobufTranslation2d rear_left = 3; + ProtobufTranslation2d rear_right = 4; +} + +message ProtobufMecanumDriveMotorVoltages { + double front_left = 1; + double front_right = 2; + double rear_left = 3; + double rear_right = 4; +} + +message ProtobufMecanumDriveWheelPositions { + double front_left = 1; + double front_right = 2; + double rear_left = 3; + double rear_right = 4; +} + +message ProtobufMecanumDriveWheelSpeeds { + double front_left = 1; + double front_right = 2; + double rear_left = 3; + double rear_right = 4; +} + +message ProtobufSwerveDriveKinematics { + repeated ProtobufTranslation2d modules = 1; +} + +message ProtobufSwerveModulePosition { + double distance = 1; + ProtobufRotation2d angle = 2; +} + +message ProtobufSwerveModuleState { + double speed = 1; + ProtobufRotation2d angle = 2; +} diff --git a/wpimath/src/main/proto/plant.proto b/wpimath/src/main/proto/plant.proto new file mode 100644 index 0000000000..d0d9eabc8a --- /dev/null +++ b/wpimath/src/main/proto/plant.proto @@ -0,0 +1,16 @@ +syntax = "proto3"; + +package wpi.proto; + +option java_package = "edu.wpi.first.math.proto"; + +message ProtobufDCMotor { + double nominal_voltage = 1; + double stall_torque = 2; + double stall_current = 3; + double free_current = 4; + double free_speed = 5; + double r = 6; + double kv = 7; + double kt = 8; +} diff --git a/wpimath/src/main/proto/spline.proto b/wpimath/src/main/proto/spline.proto new file mode 100644 index 0000000000..ef0a3c94da --- /dev/null +++ b/wpimath/src/main/proto/spline.proto @@ -0,0 +1,19 @@ +syntax = "proto3"; + +package wpi.proto; + +option java_package = "edu.wpi.first.math.proto"; + +message ProtobufCubicHermiteSpline { + repeated double x_initial = 1; + repeated double x_final = 2; + repeated double y_initial = 3; + repeated double y_final = 4; +} + +message ProtobufQuinticHermiteSpline { + repeated double x_initial = 1; + repeated double x_final = 2; + repeated double y_initial = 3; + repeated double y_final = 4; +} diff --git a/wpimath/src/main/proto/system.proto b/wpimath/src/main/proto/system.proto new file mode 100644 index 0000000000..4818c198a3 --- /dev/null +++ b/wpimath/src/main/proto/system.proto @@ -0,0 +1,17 @@ +syntax = "proto3"; + +package wpi.proto; + +import "wpimath.proto"; + +option java_package = "edu.wpi.first.math.proto"; + +message ProtobufLinearSystem { + uint32 num_states = 1; + uint32 num_inputs = 2; + uint32 num_outputs = 3; + ProtobufMatrix a = 4; + ProtobufMatrix b = 5; + ProtobufMatrix c = 6; + ProtobufMatrix d = 7; +} diff --git a/wpimath/src/main/proto/trajectory.proto b/wpimath/src/main/proto/trajectory.proto new file mode 100644 index 0000000000..a37a501d0d --- /dev/null +++ b/wpimath/src/main/proto/trajectory.proto @@ -0,0 +1,20 @@ +syntax = "proto3"; + +package wpi.proto; + +import "geometry2d.proto"; + +option java_package = "edu.wpi.first.math.proto"; + +message ProtobufTrajectoryState { + double time = 1; + double velocity = 2; + double acceleration = 3; + ProtobufPose2d pose = 4; + double curvature = 5; +} + +message ProtobufTrajectory { + double total_time = 1; + repeated ProtobufTrajectoryState states = 2; +} diff --git a/wpimath/src/main/proto/wpimath.proto b/wpimath/src/main/proto/wpimath.proto new file mode 100644 index 0000000000..06b993aceb --- /dev/null +++ b/wpimath/src/main/proto/wpimath.proto @@ -0,0 +1,15 @@ +syntax = "proto3"; + +package wpi.proto; + +option java_package = "edu.wpi.first.math.proto"; + +message ProtobufMatrix { + uint32 num_rows = 1; + uint32 num_cols = 2; + repeated double data = 3; +} + +message ProtobufVector { + repeated double rows = 1; +} diff --git a/wpiutil/.styleguide b/wpiutil/.styleguide index 882d9588aa..4932e16e05 100644 --- a/wpiutil/.styleguide +++ b/wpiutil/.styleguide @@ -41,6 +41,7 @@ repoRootNameOverride { includeOtherLibs { ^fmt/ + ^google/ ^gmock/ ^gtest/ } diff --git a/wpiutil/CMakeLists.txt b/wpiutil/CMakeLists.txt index 8e35001b57..a85045d7e8 100644 --- a/wpiutil/CMakeLists.txt +++ b/wpiutil/CMakeLists.txt @@ -34,13 +34,27 @@ if (WITH_JAVA) file(GLOB JACKSON_JARS ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar) - set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${JACKSON_JARS}) + if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/quickbuf-runtime-1.3.2.jar") + set(BASE_URL "https://search.maven.org/remotecontent?filepath=") + set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf") + + message(STATUS "Downloading Quickbuf jarfile...") + file(DOWNLOAD "${BASE_URL}us/hebi/quickbuf/quickbuf-runtime/1.3.2/quickbuf-runtime-1.3.2.jar" + "${JAR_ROOT}/quickbuf-runtime-1.3.2.jar") + + message(STATUS "Downloaded.") + endif() + + file(GLOB QUICKBUF_JAR + ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/*.jar) + + set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${JACKSON_JARS} ${QUICKBUF_JAR}) set(CMAKE_JNI_TARGET true) file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) - add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${JACKSON_JARS} OUTPUT_NAME wpiutil GENERATE_NATIVE_HEADERS wpiutil_jni_headers) + add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${JACKSON_JARS} ${QUICKBUF_JAR} OUTPUT_NAME wpiutil GENERATE_NATIVE_HEADERS wpiutil_jni_headers) get_property(WPIUTIL_JAR_FILE TARGET wpiutil_jar PROPERTY JAR_FILE) install(FILES ${WPIUTIL_JAR_FILE} DESTINATION "${java_lib_dest}") @@ -98,7 +112,7 @@ if (MSVC) target_compile_definitions(wpiutil PRIVATE -D_CRT_SECURE_NO_WARNINGS) endif() wpilib_target_warnings(wpiutil) -target_link_libraries(wpiutil Threads::Threads ${CMAKE_DL_LIBS}) +target_link_libraries(wpiutil protobuf::libprotobuf Threads::Threads ${CMAKE_DL_LIBS}) if (ATOMIC) target_link_libraries(wpiutil ${ATOMIC}) diff --git a/wpiutil/build.gradle b/wpiutil/build.gradle index a25e183cb0..ab13092a70 100644 --- a/wpiutil/build.gradle +++ b/wpiutil/build.gradle @@ -294,6 +294,7 @@ dependencies { api "com.fasterxml.jackson.core:jackson-annotations:2.15.2" api "com.fasterxml.jackson.core:jackson-core:2.15.2" api "com.fasterxml.jackson.core:jackson-databind:2.15.2" + api 'us.hebi.quickbuf:quickbuf-runtime:1.3.2' printlogImplementation sourceSets.main.output } diff --git a/wpiutil/doc/struct.adoc b/wpiutil/doc/struct.adoc new file mode 100644 index 0000000000..cac4d4a967 --- /dev/null +++ b/wpiutil/doc/struct.adoc @@ -0,0 +1,311 @@ += WPILib Packed Struct Serialization Specification, Version 1.0 +WPILib Developers +Revision 1.0 (0x0100), 6/8/2023 +:toc: +:toc-placement: preamble +:sectanchors: + +A simple format and schema for serialization of packed fixed size structured data. + +[[motivation]] +== Motivation + +Schema-based serialization formats such as Protobuf and Flatbuffers are extremely flexible and can handle data type evolution, complex nested data structures, variable size / repeated data, optional fields, etc. However, this flexibility comes at a cost in both serialized data size and processing overhead. Many simple data structures, such as screen coordinates or robot poses, are fixed in size and can be stored much more compactly and serialized/deserialized much more quickly, especially on embedded or real-time platforms. + +Simply storing a C-style packed structure is very compact and fast, but information about the layout of the structure and the meaning of each member must be separately communicated for introspection by other tools such as interactive dashboards for data analysis of individual structure members. The motivation for this standard layout and schema is to provide a standardized means to communicate this information and enable dynamic decoding. + +Python's struct module uses a character-based approach to describe data layout of structures, but has no provisions for naming each member to communicate intent/meaning. + +[[references]] +== References + +[[c-struct-declaration]] +* Struct declaration, https://en.cppreference.com/w/c/language/struct + +[[definitions]] +== Definitions + +[[schema]] +== Schema + +The schema is a text-based format with similar syntax to the list of variable declarations in a C structure. The C syntax is flexible, easy to parse, and matches the intent of specifying a fixed size structure. + +Each member of the struct is defined by a single declaration. Each declaration is either a standard declaration or a bit-field declaration. Declarations are separated by semicolons. The last declaration may optionally have a trailing semicolon. Empty declarations (e.g. two semicolons back-to-back or separated by only whitespace) are allowed but are ignored. Unlike C structures, every declaration must be separated by a semicolon; commas cannot be used to declare multiple members with the same type. Declarations may also start and end with whitespace. + +[[variable]] +=== Standard Declaration + +Standard declarations declare a member of a certain type or a fixed-size array of that type. The structure of a standard declaration is: + +* optional enum specification (integer data types only) +* optional whitespace +* type name +* whitespace +* identifier name +* optional array size, consisting of: + * optional whitespace + * `[` + * optional whitespace + * size of array + * optional whitespace + * `]` + +The type name may be one of these: + +[cols="1,1,3", options="header"] +|=== +|Type Name|Description|Payload Data Contents +|`bool`|boolean|single byte (0=false, 1=true) +|`char`|character|single byte (assumed UTF-8) +|`int8`|integer|1-byte (8-bit) signed value +|`int16`|integer|2-byte (16-bit) signed value +|`int32`|integer|4-byte (32-bit) signed value +|`int64`|integer|8-byte (64-bit) signed value +|`uint8`|unsigned integer|1-byte (8-bit) unsigned value +|`uint16`|unsigned integer|2-byte (16-bit) unsigned value +|`uint32`|unsigned integer|4-byte (32-bit) unsigned value +|`uint64`|unsigned integer|8-byte (64-bit) unsigned value +|`float` or `float32`|float|4-byte (32-bit) IEEE-754 value +|`double` or `float64`|double|8-byte (64-bit) IEEE-754 value +|=== + +If it is not one of the above, the type name must be the name of another struct. + +Examples of valid standard declarations: + +* `bool value` (boolean value, 1 byte) +* `double arr[4]` (array of 4 doubles, 32 bytes total) +* `enum {a=1, b=2} int8 val` (enumerated value, 1 byte) + +[[enum]] +==== Enum Specification + +Integer declarations may have an enum specification to provide meaning to specific values. Values that are not specified may be communicated, but have no specific defined meaning. The structure of an enum specification is: + +* optional `enum` +* optional whitespace +* `{` +* zero or more enum values, consisting of: + * optional whitespace + * identifier + * optional whitespace + * `=` + * optional whitespace + * integer value + * optional whitespace + * comma (optional for last value) +* optional whitespace +* `}` + +Examples of valid enum specifications: + +* `enum{}` +* `enum { a = 1 }` +* `enum{a=1,b=2,}` +* `{a=1}` + +Examples of invalid enum specifications: + +* `enum` (no `{}`) +* `enum{=2}` (missing identifier) +* `enum{a=1,b,c}` (missing values) + +[[]] +=== Bit-field Declaration + +Bit-field declarations declare a member with an explicit width in bits. The structure of a bit-field declaration is: + +* optional enum specification (integer data types only) +* optional whitespace +* type name; must be boolean or one of the integer data types +* whitespace +* identifier name +* optional whitespace +* colon (`:`) +* optional whitespace +* integer number of bits; minimum 1; maximum 1 for boolean types; for integer types, maximum is the width of the type (e.g. 32 for int32) + +As with non-bit-field integer variable declarations, an enum can be specified for integer bit-fields (e.g. `enum {a=1, b=2} uint32 value : 2`). + +It is not possible to have an array of bit-fields. + +Examples of valid bit-field declarations: + +* `bool value : 1` +* `enum{a=1,b=2}int8 value:2` + +Examples of invalid bit-field declarations: + +* `double val:2` (must be integer or boolean) +* `int32 val[2]:2` (cannot be array) +* `bool val:3` (bool must be 1 bit) +* `int16 val:17` (bit field larger than storage size) + +[[layout]] +== Data Layout + +Members are stored in the same order they appear in the schema. Individual members are stored in little-endian order. Members are not aligned to any particular boundary; no byte-level padding is present in the data. + +[source] +---- +bool b; +int16 i; +---- + +results in a 3-byte encoding: + +`bbbbbbbb iiiiiiii iiiiiiii` + +where the first `iiiiiiii` is the least significant byte of `i`. + +[[layout-array]] +=== Array Data Layout + +For array members, the individual items of the array are stored consecutively with no padding between each item. + +[source] +---- +int16 i[2]; +---- + +results in a 4-byte encoding: + +`i0i0i0i0 i0i0i0i0 i1i1i1i1 i1i1i1i1` + +where `i0` is the first element of the array, `i1` is the second element. + +[[layout-nested-structure]] + +Nested structures also have no surrounding padding. + +Given the Inner schema + +[source] +---- +int16 i; +int8 x; +---- + +and an outer schema of + +[source] +---- +char c; +Inner s; +bool b; +---- + +results in a 5-byte encoding: + +`cccccccc iiiiiiii iiiiiiii xxxxxxxx bbbbbbbb` + +[[layout-bit-field]] +=== Bit-Field Data Layout + +Multiple adjacant bit-fields of the same integer type width are packed together to fit in the minimum number of multiples of that type. The bit-fields are packed, starting from the least significant bit, in the order they appear in the schema. Individual bit-fields must not span across multiple underlying types; if a bit-field is larger than the remaining space in the data type, a new element of that type is started and the bit-field starts from the least significant bit of the new element. Unused bits should be set to 0 during serialization and must be ignored during deserialization. + +Boolean bit-fields are always a single bit wide. The underlying data type is by default uint8, but if a boolean bit-field immediately follows a bit-field of another integer type (and fits), it is packed into that type. + +[source] +---- +int8 a:4; +int16 b:4; +---- + +results in a 3-byte encoding: + +`0000aaaa 0000bbbb 00000000` + +as the integer type widths are different, even though the bits would fit. + +[source] +---- +int16 a:4; +uint16 b:5; +bool c:1; +int16 d:7; +---- + +results in a 4-byte encoding: + +`bbbbaaaa 000000cb 0ddddddd 00000000` + +As `c` is packed into the preceding int16, and `d` is too large to fit in the remaining bits of the first type. + +[source] +---- +uint8 a:4; +int8 b:2; +bool c:1; +int16 d:1; +---- + +results in a 3-byte encoding: + +`0cbbaaaa 0000000d 00000000` + +as `d` is int16, versus the `int8` of the previous values. + +[source] +---- +bool a:1; +bool b:1; +int8 c:2; +---- + +results in a 1-byte encoding: + +`0000ccba` + +as `c` is an int8. + +[source] +---- +bool a:1; +bool b:1; +int16 c:2; +---- + +results in a 3-byte encoding: + +`000000ba 000000cc 00000000` + +as `c` is an int16. + +Bit-fields do not "look inside" of nested structures. Given Inner + +[source] +---- +int8 a:1; +---- + +and outer + +[source] +---- +int8 b:1; +Outer s; +int8 c:1; +---- + +the result is a 3-byte encoding: + +`0000000b 0000000a 0000000c` + +[[layout-character-arrays]] +=== Character Array (String) Data Layout + +Character arrays, as with other arrays, must be fixed length. The text they contain should be UTF-8. If a string is shorter than the length of the character array, the string starts at the first byte of the array, and any unused bytes at the end of the array must be filled with 0. + +[source] +---- +char s[4]; +---- + +with a string of "a" results in: + +`01100001 00000000 00000000 00000000` + +with a string of "abcd" results in: + +`01100001 01100010 01100011 01100100` diff --git a/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java b/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java index 1003f80a51..eef2338521 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLog.java @@ -4,7 +4,14 @@ package edu.wpi.first.util.datalog; +import edu.wpi.first.util.WPIUtilJNI; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.struct.Struct; import java.nio.ByteBuffer; +import java.util.HashSet; +import java.util.Set; +import java.util.concurrent.ConcurrentHashMap; +import java.util.concurrent.ConcurrentMap; /** * A data log. The log file is created immediately upon construction with a temporary filename. The @@ -104,6 +111,130 @@ public final class DataLog implements AutoCloseable { DataLogJNI.resume(m_impl); } + /** + * Returns whether there is a data schema already registered with the given name. + * + * @param name Name (the string passed as the data type for records using this schema) + * @return True if schema already registered + */ + public boolean hasSchema(String name) { + return m_schemaSet.contains(name); + } + + /** + * Registers a data schema. Data schemas provide information for how a certain data type string + * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g. + * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In the data log, schemas + * are saved just like normal records, with the name being generated from the provided name: + * "/.schema/name". Duplicate calls to this function with the same name are silently ignored. + * + * @param name Name (the string passed as the data type for records using this schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + * @param timestamp Time stamp (may be 0 to indicate now) + */ + public void addSchema(String name, String type, byte[] schema, long timestamp) { + if (!m_schemaSet.add(name)) { + return; + } + DataLogJNI.addSchema(m_impl, name, type, schema, timestamp); + } + + /** + * Registers a data schema. Data schemas provide information for how a certain data type string + * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g. + * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In the data log, schemas + * are saved just like normal records, with the name being generated from the provided name: + * "/.schema/name". Duplicate calls to this function with the same name are silently ignored. + * + * @param name Name (the string passed as the data type for records using this schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + */ + public void addSchema(String name, String type, byte[] schema) { + addSchema(name, type, schema, 0); + } + + /** + * Registers a data schema. Data schemas provide information for how a certain data type string + * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g. + * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In the data log, schemas + * are saved just like normal records, with the name being generated from the provided name: + * "/.schema/name". Duplicate calls to this function with the same name are silently ignored. + * + * @param name Name (the string passed as the data type for records using this schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + * @param timestamp Time stamp (may be 0 to indicate now) + */ + public void addSchema(String name, String type, String schema, long timestamp) { + if (!m_schemaSet.add(name)) { + return; + } + DataLogJNI.addSchemaString(m_impl, name, type, schema, timestamp); + } + + /** + * Registers a data schema. Data schemas provide information for how a certain data type string + * can be decoded. The type string of a data schema indicates the type of the schema itself (e.g. + * "protobuf" for protobuf schemas, "struct" for struct schemas, etc). In the data log, schemas + * are saved just like normal records, with the name being generated from the provided name: + * "/.schema/name". Duplicate calls to this function with the same name are silently ignored. + * + * @param name Name (the string passed as the data type for records using this schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + */ + public void addSchema(String name, String type, String schema) { + addSchema(name, type, schema, 0); + } + + /** + * Registers a protobuf schema. Duplicate calls to this function with the same name are silently + * ignored. + * + * @param proto protobuf serialization object + * @param timestamp Time stamp (0 to indicate now) + */ + public void addSchema(Protobuf proto, long timestamp) { + final long actualTimestamp = timestamp == 0 ? WPIUtilJNI.now() : timestamp; + proto.forEachDescriptor( + this::hasSchema, + (typeString, schema) -> + addSchema(typeString, "proto:FileDescriptorProto", schema, actualTimestamp)); + } + + /** + * Registers a protobuf schema. Duplicate calls to this function with the same name are silently + * ignored. + * + * @param proto protobuf serialization object + */ + public void addSchema(Protobuf proto) { + addSchema(proto, 0); + } + + /** + * Registers a struct schema. Duplicate calls to this function with the same name are silently + * ignored. + * + * @param struct struct serialization object + * @param timestamp Time stamp (0 to indicate now) + */ + public void addSchema(Struct struct, long timestamp) { + addSchemaImpl(struct, timestamp == 0 ? WPIUtilJNI.now() : timestamp, new HashSet<>()); + } + + /** + * Registers a struct schema. Duplicate calls to this function with the same name are silently + * ignored. + * + * @param struct struct serialization object + */ + public void addSchema(Struct struct) { + addSchema(struct, 0); + } + /** * Start an entry. Duplicate names are allowed (with the same type), and result in the same index * being returned (start/finish are reference counted). A duplicate name with a different type @@ -358,5 +489,22 @@ public final class DataLog implements AutoCloseable { return m_impl; } + private void addSchemaImpl(Struct struct, long timestamp, Set seen) { + String typeString = struct.getTypeString(); + if (hasSchema(typeString)) { + return; + } + if (!seen.add(typeString)) { + throw new UnsupportedOperationException(typeString + ": circular reference with " + seen); + } + addSchema(typeString, "structschema", struct.getSchema(), timestamp); + for (Struct inner : struct.getNested()) { + addSchemaImpl(inner, timestamp, seen); + } + seen.remove(typeString); + } + private long m_impl; + private final ConcurrentMap m_schemaMap = new ConcurrentHashMap<>(); + private final Set m_schemaSet = m_schemaMap.keySet(); } diff --git a/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogJNI.java b/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogJNI.java index 85c85251b1..08b108c083 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogJNI.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/datalog/DataLogJNI.java @@ -18,6 +18,11 @@ public class DataLogJNI extends WPIUtilJNI { static native void resume(long impl); + static native void addSchema(long impl, String name, String type, byte[] schema, long timestamp); + + static native void addSchemaString( + long impl, String name, String type, String schema, long timestamp); + static native int start(long impl, String name, String type, String metadata, long timestamp); static native void finish(long impl, int entry, long timestamp); diff --git a/wpiutil/src/main/java/edu/wpi/first/util/datalog/ProtobufLogEntry.java b/wpiutil/src/main/java/edu/wpi/first/util/datalog/ProtobufLogEntry.java new file mode 100644 index 0000000000..1db2647b58 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/datalog/ProtobufLogEntry.java @@ -0,0 +1,117 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.datalog; + +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.protobuf.ProtobufBuffer; +import java.io.IOException; +import java.nio.ByteBuffer; +import us.hebi.quickbuf.ProtoMessage; + +/** + * Log protobuf-encoded values. + * + * @param value class + */ +public final class ProtobufLogEntry extends DataLogEntry { + private ProtobufLogEntry( + DataLog log, String name, Protobuf proto, String metadata, long timestamp) { + super(log, name, proto.getTypeString(), metadata, timestamp); + m_buf = ProtobufBuffer.create(proto); + log.addSchema(proto, timestamp); + } + + /** + * Creates a protobuf-encoded log entry. + * + * @param value class (inferred from proto) + * @param protobuf message type (inferred from proto) + * @param log datalog + * @param name name of the entry + * @param proto protobuf serialization implementation + * @param metadata metadata + * @param timestamp entry creation timestamp (0=now) + * @return ProtobufLogEntry + */ + public static > ProtobufLogEntry create( + DataLog log, String name, Protobuf proto, String metadata, long timestamp) { + return new ProtobufLogEntry(log, name, proto, metadata, timestamp); + } + + /** + * Creates a protobuf-encoded log entry. + * + * @param value class (inferred from proto) + * @param protobuf message type (inferred from proto) + * @param log datalog + * @param name name of the entry + * @param proto protobuf serialization implementation + * @param metadata metadata + * @return ProtobufLogEntry + */ + public static > ProtobufLogEntry create( + DataLog log, String name, Protobuf proto, String metadata) { + return create(log, name, proto, metadata, 0); + } + + /** + * Creates a protobuf-encoded log entry. + * + * @param value class (inferred from proto) + * @param protobuf message type (inferred from proto) + * @param log datalog + * @param name name of the entry + * @param proto protobuf serialization implementation + * @param timestamp entry creation timestamp (0=now) + * @return ProtobufLogEntry + */ + public static > ProtobufLogEntry create( + DataLog log, String name, Protobuf proto, long timestamp) { + return create(log, name, proto, "", timestamp); + } + + /** + * Creates a protobuf-encoded log entry. + * + * @param value class (inferred from proto) + * @param protobuf message type (inferred from proto) + * @param log datalog + * @param name name of the entry + * @param proto protobuf serialization implementation + * @return ProtobufLogEntry + */ + public static > ProtobufLogEntry create( + DataLog log, String name, Protobuf proto) { + return create(log, name, proto, 0); + } + + /** + * Appends a record to the log. + * + * @param value Value to record + * @param timestamp Time stamp (0 to indicate now) + */ + public void append(T value, long timestamp) { + try { + synchronized (m_buf) { + ByteBuffer bb = m_buf.write(value); + m_log.appendRaw(m_entry, bb, 0, bb.position(), timestamp); + } + } catch (IOException e) { + // ignore + } + } + + /** + * Appends a record to the log. + * + * @param value Value to record + */ + public void append(T value) { + append(value, 0); + } + + private final ProtobufBuffer m_buf; +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructArrayLogEntry.java b/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructArrayLogEntry.java new file mode 100644 index 0000000000..0f6cb2e7d0 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructArrayLogEntry.java @@ -0,0 +1,140 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.datalog; + +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructBuffer; +import java.nio.ByteBuffer; +import java.util.Collection; + +/** + * Log struct-encoded array values. + * + * @param value class + */ +public final class StructArrayLogEntry extends DataLogEntry { + private StructArrayLogEntry( + DataLog log, String name, Struct struct, String metadata, long timestamp) { + super(log, name, struct.getTypeString() + "[]", metadata, timestamp); + m_buf = StructBuffer.create(struct); + log.addSchema(struct, timestamp); + } + + /** + * Creates a struct-encoded array log entry. + * + * @param value class (inferred from struct) + * @param log datalog + * @param name name of the entry + * @param struct struct serialization implementation + * @param metadata metadata + * @param timestamp entry creation timestamp (0=now) + * @return StructArrayLogEntry + */ + public static StructArrayLogEntry create( + DataLog log, String name, Struct struct, String metadata, long timestamp) { + return new StructArrayLogEntry(log, name, struct, metadata, timestamp); + } + + /** + * Creates a struct-encoded array log entry. + * + * @param value class (inferred from struct) + * @param log datalog + * @param name name of the entry + * @param struct struct serialization implementation + * @param metadata metadata + * @return StructArrayLogEntry + */ + public static StructArrayLogEntry create( + DataLog log, String name, Struct struct, String metadata) { + return create(log, name, struct, metadata, 0); + } + + /** + * Creates a struct-encoded array log entry. + * + * @param value class (inferred from struct) + * @param log datalog + * @param name name of the entry + * @param struct struct serialization implementation + * @param timestamp entry creation timestamp (0=now) + * @return StructArrayLogEntry + */ + public static StructArrayLogEntry create( + DataLog log, String name, Struct struct, long timestamp) { + return create(log, name, struct, "", timestamp); + } + + /** + * Creates a struct-encoded array log entry. + * + * @param value class (inferred from struct) + * @param log datalog + * @param name name of the entry + * @param struct struct serialization implementation + * @return StructArrayLogEntry + */ + public static StructArrayLogEntry create(DataLog log, String name, Struct struct) { + return create(log, name, struct, 0); + } + + /** + * Ensures sufficient buffer space is available for the given number of elements. + * + * @param nelem number of elements + */ + public void reserve(int nelem) { + synchronized (m_buf) { + m_buf.reserve(nelem); + } + } + + /** + * Appends a record to the log. + * + * @param value Value to record + * @param timestamp Time stamp (0 to indicate now) + */ + public void append(T[] value, long timestamp) { + synchronized (this) { + ByteBuffer bb = m_buf.writeArray(value); + m_log.appendRaw(m_entry, bb, 0, bb.position(), timestamp); + } + } + + /** + * Appends a record to the log. + * + * @param value Value to record + */ + public void append(T[] value) { + append(value, 0); + } + + /** + * Appends a record to the log. + * + * @param value Value to record + * @param timestamp Time stamp (0 to indicate now) + */ + public void append(Collection value, long timestamp) { + synchronized (m_buf) { + ByteBuffer bb = m_buf.writeArray(value); + m_log.appendRaw(m_entry, bb, 0, bb.position(), timestamp); + } + } + + /** + * Appends a record to the log. + * + * @param value Value to record + */ + public void append(Collection value) { + append(value, 0); + } + + private final StructBuffer m_buf; +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructLogEntry.java b/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructLogEntry.java new file mode 100644 index 0000000000..a227c32d8b --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/datalog/StructLogEntry.java @@ -0,0 +1,106 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.datalog; + +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructBuffer; +import java.nio.ByteBuffer; + +/** + * Log struct-encoded values. + * + * @param value class + */ +public final class StructLogEntry extends DataLogEntry { + private StructLogEntry( + DataLog log, String name, Struct struct, String metadata, long timestamp) { + super(log, name, struct.getTypeString(), metadata, timestamp); + m_buf = StructBuffer.create(struct); + log.addSchema(struct, timestamp); + } + + /** + * Creates a struct-encoded log entry. + * + * @param value class (inferred from struct) + * @param log datalog + * @param name name of the entry + * @param struct struct serialization implementation + * @param metadata metadata + * @param timestamp entry creation timestamp (0=now) + * @return StructLogEntry + */ + public static StructLogEntry create( + DataLog log, String name, Struct struct, String metadata, long timestamp) { + return new StructLogEntry(log, name, struct, metadata, timestamp); + } + + /** + * Creates a struct-encoded log entry. + * + * @param value class (inferred from struct) + * @param log datalog + * @param name name of the entry + * @param struct struct serialization implementation + * @param metadata metadata + * @return StructLogEntry + */ + public static StructLogEntry create( + DataLog log, String name, Struct struct, String metadata) { + return create(log, name, struct, metadata, 0); + } + + /** + * Creates a struct-encoded log entry. + * + * @param value class (inferred from struct) + * @param log datalog + * @param name name of the entry + * @param struct struct serialization implementation + * @param timestamp entry creation timestamp (0=now) + * @return StructLogEntry + */ + public static StructLogEntry create( + DataLog log, String name, Struct struct, long timestamp) { + return create(log, name, struct, "", timestamp); + } + + /** + * Creates a struct-encoded log entry. + * + * @param value class (inferred from struct) + * @param log datalog + * @param name name of the entry + * @param struct struct serialization implementation + * @return StructLogEntry + */ + public static StructLogEntry create(DataLog log, String name, Struct struct) { + return create(log, name, struct, 0); + } + + /** + * Appends a record to the log. + * + * @param value Value to record + * @param timestamp Time stamp (0 to indicate now) + */ + public void append(T value, long timestamp) { + synchronized (m_buf) { + ByteBuffer bb = m_buf.write(value); + m_log.appendRaw(m_entry, bb, 0, bb.position(), timestamp); + } + } + + /** + * Appends a record to the log. + * + * @param value Value to record + */ + public void append(T value) { + append(value, 0); + } + + private final StructBuffer m_buf; +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/protobuf/Protobuf.java b/wpiutil/src/main/java/edu/wpi/first/util/protobuf/Protobuf.java new file mode 100644 index 0000000000..b77db7acae --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/protobuf/Protobuf.java @@ -0,0 +1,121 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.protobuf; + +import java.util.function.BiConsumer; +import java.util.function.Predicate; +import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.Descriptors.FileDescriptor; +import us.hebi.quickbuf.ProtoMessage; + +/** + * Interface for Protobuf serialization. + * + *

This is designed for serialization of more complex data structures including forward/backwards + * compatibility and repeated/nested/variable length members, etc. Serialization and deserialization + * code is auto-generated from .proto interface descriptions (the MessageType generic parameter). + * + *

Idiomatically, classes that support protobuf serialization should provide a static final + * member named "proto" that provides an instance of an implementation of this interface. + * + * @param object type + * @param protobuf message type + */ +public interface Protobuf> { + /** + * Gets the Class object for the stored value. + * + * @return Class + */ + Class getTypeClass(); + + /** + * Gets the type string (e.g. for NetworkTables). This should be globally unique and start with + * "proto:". + * + * @return type string + */ + default String getTypeString() { + return "proto:" + getDescriptor().getFullName(); + } + + /** + * Gets the protobuf descriptor. + * + * @return descriptor + */ + Descriptor getDescriptor(); + + /** + * Gets the list of protobuf types referenced by this protobuf. + * + * @return list of protobuf types + */ + default Protobuf[] getNested() { + return new Protobuf[] {}; + } + + /** + * Creates protobuf message. + * + * @return protobuf message + */ + MessageType createMessage(); + + /** + * Deserializes an object from a protobuf message. + * + * @param msg protobuf message + * @return New object + */ + T unpack(MessageType msg); + + /** + * Copies the object contents into a protobuf message. Implementations should call either + * msg.setMember(member) or member.copyToProto(msg.getMutableMember()) for each member. + * + * @param msg protobuf message + * @param value object to serialize + */ + void pack(MessageType msg, T value); + + /** + * Updates the object contents from a protobuf message. Implementations should call + * msg.getMember(member), MemberClass.makeFromProto(msg.getMember()), or + * member.updateFromProto(msg.getMember()) for each member. + * + *

Immutable classes cannot and should not implement this function. The default implementation + * throws UnsupportedOperationException. + * + * @param out object to update + * @param msg protobuf message + * @throws UnsupportedOperationException if the object is immutable + */ + default void unpackInto(T out, MessageType msg) { + throw new UnsupportedOperationException("object does not support unpackInto"); + } + + /** + * Loops over all protobuf descriptors including nested/referenced descriptors. + * + * @param exists function that returns false if fn should be called for the given type string + * @param fn function to call for each descriptor + */ + default void forEachDescriptor(Predicate exists, BiConsumer fn) { + forEachDescriptorImpl(getDescriptor().getFile(), exists, fn); + } + + private static void forEachDescriptorImpl( + FileDescriptor desc, Predicate exists, BiConsumer fn) { + String name = "proto:" + desc.getFullName(); + if (exists.test(name)) { + return; + } + for (FileDescriptor dep : desc.getDependencies()) { + forEachDescriptorImpl(dep, exists, fn); + } + fn.accept(name, desc.toProtoBytes()); + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java b/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java new file mode 100644 index 0000000000..af3e466411 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/protobuf/ProtobufBuffer.java @@ -0,0 +1,164 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.protobuf; + +import java.io.IOException; +import java.nio.ByteBuffer; +import us.hebi.quickbuf.ProtoMessage; +import us.hebi.quickbuf.ProtoSink; +import us.hebi.quickbuf.ProtoSource; + +/** + * Reusable buffer for serialization/deserialization to/from a protobuf. + * + * @param object type + * @param protobuf message type + */ +public final class ProtobufBuffer> { + private ProtobufBuffer(Protobuf proto) { + m_buf = ByteBuffer.allocateDirect(1024); + m_sink = ProtoSink.newDirectSink(); + m_sink.setOutput(m_buf); + m_source = ProtoSource.newDirectSource(); + m_msg = proto.createMessage(); + m_proto = proto; + } + + public static > ProtobufBuffer create( + Protobuf proto) { + return new ProtobufBuffer(proto); + } + + /** + * Gets the protobuf object of the stored type. + * + * @return protobuf object + */ + public Protobuf getProto() { + return m_proto; + } + + /** + * Gets the type string. + * + * @return type string + */ + public String getTypeString() { + return m_proto.getTypeString(); + } + + /** + * Serializes a value to a ByteBuffer. The returned ByteBuffer is a direct byte buffer with the + * position set to the end of the serialized data. + * + * @param value value + * @return byte buffer + * @throws IOException if serialization failed + */ + public ByteBuffer write(T value) throws IOException { + m_msg.clearQuick(); + m_proto.pack(m_msg, value); + int size = m_msg.getSerializedSize(); + if (size < m_buf.capacity()) { + m_buf = ByteBuffer.allocateDirect(size * 2); + m_sink.setOutput(m_buf); + } + m_sink.reset(); + m_msg.writeTo(m_sink); + m_buf.position(m_sink.getTotalBytesWritten()); + return m_buf; + } + + /** + * Deserializes a value from a byte array, creating a new object. + * + * @param buf byte array + * @param start starting location within byte array + * @param len length of serialized data + * @return new object + * @throws IOException if deserialization failed + */ + public T read(byte[] buf, int start, int len) throws IOException { + m_msg.clearQuick(); + m_source.setInput(buf, start, len); + m_msg.mergeFrom(m_source); + return m_proto.unpack(m_msg); + } + + /** + * Deserializes a value from a byte array, creating a new object. + * + * @param buf byte array + * @return new object + * @throws IOException if deserialization failed + */ + public T read(byte[] buf) throws IOException { + return read(buf, 0, buf.length); + } + + /** + * Deserializes a value from a ByteBuffer, creating a new object. + * + * @param buf byte buffer + * @return new object + * @throws IOException if deserialization failed + */ + public T read(ByteBuffer buf) throws IOException { + m_msg.clearQuick(); + m_source.setInput(buf); + m_msg.mergeFrom(m_source); + return m_proto.unpack(m_msg); + } + + /** + * Deserializes a value from a byte array into a mutable object. + * + * @param out object (will be updated with deserialized contents) + * @param buf byte array + * @param start starting location within byte array + * @param len length of serialized data + * @throws IOException if deserialization failed + * @throws UnsupportedOperationException if the object is immutable + */ + public void readInto(T out, byte[] buf, int start, int len) throws IOException { + m_msg.clearQuick(); + m_source.setInput(buf, start, len); + m_msg.mergeFrom(m_source); + m_proto.unpackInto(out, m_msg); + } + + /** + * Deserializes a value from a byte array into a mutable object. + * + * @param out object (will be updated with deserialized contents) + * @param buf byte array + * @throws IOException if deserialization failed + * @throws UnsupportedOperationException if the object is immutable + */ + public void readInto(T out, byte[] buf) throws IOException { + readInto(out, buf, 0, buf.length); + } + + /** + * Deserializes a value from a ByteBuffer into a mutable object. + * + * @param out object (will be updated with deserialized contents) + * @param buf byte buffer + * @throws IOException if deserialization failed + * @throws UnsupportedOperationException if the object is immutable + */ + public void readInto(T out, ByteBuffer buf) throws IOException { + m_msg.clearQuick(); + m_source.setInput(buf); + m_msg.mergeFrom(m_source); + m_proto.unpackInto(out, m_msg); + } + + private ByteBuffer m_buf; + private final ProtoSink m_sink; + private final ProtoSource m_source; + private final MessageType m_msg; + private final Protobuf m_proto; +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/BadSchemaException.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/BadSchemaException.java new file mode 100644 index 0000000000..6ff2236961 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/BadSchemaException.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct; + +public class BadSchemaException extends Exception { + private final String m_field; + + public BadSchemaException(String s) { + super(s); + m_field = ""; + } + + public BadSchemaException(String message, Throwable cause) { + super(message, cause); + m_field = ""; + } + + public BadSchemaException(Throwable cause) { + super(cause); + m_field = ""; + } + + public BadSchemaException(String field, String s) { + super(s); + m_field = field; + } + + public BadSchemaException(String field, String message, Throwable cause) { + super(message, cause); + m_field = field; + } + + public String getField() { + return m_field; + } + + @Override + public String toString() { + return m_field.isEmpty() ? getMessage() : "field " + m_field + ": " + getMessage(); + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/DynamicStruct.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/DynamicStruct.java new file mode 100644 index 0000000000..165f7db1cd --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/DynamicStruct.java @@ -0,0 +1,632 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct; + +import java.nio.BufferUnderflowException; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.nio.ReadOnlyBufferException; +import java.nio.charset.StandardCharsets; + +/** Dynamic (run-time) access to a serialized raw struct. */ +public final class DynamicStruct { + private DynamicStruct(StructDescriptor desc, ByteBuffer data) { + m_desc = desc; + m_data = data.order(ByteOrder.LITTLE_ENDIAN); + } + + /** + * Constructs a new dynamic struct object with internal storage. The descriptor must be valid. The + * internal storage is allocated using ByteBuffer.allocate(). + * + * @param desc struct descriptor + * @return dynamic struct object + * @throws IllegalStateException if struct descriptor is invalid + */ + public static DynamicStruct allocate(StructDescriptor desc) { + return new DynamicStruct(desc, ByteBuffer.allocate(desc.getSize())); + } + + /** + * Constructs a new dynamic struct object with internal storage. The descriptor must be valid. The + * internal storage is allocated using ByteBuffer.allocateDirect(). + * + * @param desc struct descriptor + * @return dynamic struct object + * @throws IllegalStateException if struct descriptor is invalid + */ + public static DynamicStruct allocateDirect(StructDescriptor desc) { + return new DynamicStruct(desc, ByteBuffer.allocateDirect(desc.getSize())); + } + + /** + * Constructs a new dynamic struct object. Note: the passed data buffer is not copied. + * Modifications to the passed buffer will be reflected in the struct and vice-versa. + * + * @param desc struct descriptor + * @param data byte buffer containing serialized data starting at current position + * @return dynamic struct object + */ + public static DynamicStruct wrap(StructDescriptor desc, ByteBuffer data) { + return new DynamicStruct(desc, data.slice()); + } + + /** + * Gets the struct descriptor. + * + * @return struct descriptor + */ + public StructDescriptor getDescriptor() { + return m_desc; + } + + /** + * Gets the serialized backing data buffer. + * + * @return data buffer + */ + public ByteBuffer getBuffer() { + return m_data.duplicate().position(0); + } + + /** + * Overwrites the entire serialized struct by copying data from a byte array. + * + * @param data replacement data for the struct + * @throws BufferUnderflowException if data is smaller than the struct size + * @throws ReadOnlyBufferException if the underlying buffer is read-only + * @throws IllegalStateException if struct descriptor is invalid + */ + public void setData(byte[] data) { + if (data.length < m_desc.getSize()) { + throw new BufferUnderflowException(); + } + m_data.position(0).put(data); + } + + /** + * Overwrites the entire serialized struct by copying data from a byte buffer. + * + * @param data replacement data for the struct; copy starts from current position + * @throws BufferUnderflowException if remaining data is smaller than the struct size + * @throws ReadOnlyBufferException if the underlying buffer is read-only + * @throws IllegalStateException if struct descriptor is invalid + */ + public void setData(ByteBuffer data) { + if (data.remaining() < m_desc.getSize()) { + throw new BufferUnderflowException(); + } + int oldLimit = data.limit(); + m_data.position(0).put(data.limit(m_desc.getSize())); + data.limit(oldLimit); + } + + /** + * Gets a struct field descriptor by name. + * + * @param name field name + * @return field descriptor, or null if no field with that name exists + */ + public StructFieldDescriptor findField(String name) { + return m_desc.findFieldByName(name); + } + + /** + * Gets the value of a boolean field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return boolean field value + * @throws UnsupportedOperationException if field is not bool type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ArrayIndexOutOfBoundsException if array index is out of bounds + */ + public boolean getBoolField(StructFieldDescriptor field, int arrIndex) { + if (field.getType() != StructFieldType.kBool) { + throw new UnsupportedOperationException("field is not bool type"); + } + return getFieldImpl(field, arrIndex) != 0; + } + + /** + * Gets the value of a boolean field. + * + * @param field field descriptor + * @return boolean field value + * @throws UnsupportedOperationException if field is not bool type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + */ + public boolean getBoolField(StructFieldDescriptor field) { + return getBoolField(field, 0); + } + + /** + * Sets the value of a boolean field. + * + * @param field field descriptor + * @param value boolean value + * @param arrIndex array index (must be less than field array size) + * @throws UnsupportedOperationException if field is not bool type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ArrayIndexOutOfBoundsException if array index is out of bounds + * @throws ReadOnlyBufferException if the underlying buffer is read-only + */ + public void setBoolField(StructFieldDescriptor field, boolean value, int arrIndex) { + if (field.getType() != StructFieldType.kBool) { + throw new UnsupportedOperationException("field is not bool type"); + } + setFieldImpl(field, value ? 1 : 0, arrIndex); + } + + /** + * Sets the value of a boolean field. + * + * @param field field descriptor + * @param value boolean value + * @throws UnsupportedOperationException if field is not bool type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ReadOnlyBufferException if the underlying buffer is read-only + */ + public void setBoolField(StructFieldDescriptor field, boolean value) { + setBoolField(field, value, 0); + } + + /** + * Gets the value of an integer field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return integer field value + * @throws UnsupportedOperationException if field is not integer type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ArrayIndexOutOfBoundsException if array index is out of bounds + */ + public long getIntField(StructFieldDescriptor field, int arrIndex) { + if (!field.isInt() && !field.isUint()) { + throw new UnsupportedOperationException("field is not integer type"); + } + return getFieldImpl(field, arrIndex); + } + + /** + * Gets the value of an integer field. + * + * @param field field descriptor + * @return integer field value + * @throws UnsupportedOperationException if field is not integer type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + */ + public long getIntField(StructFieldDescriptor field) { + return getIntField(field, 0); + } + + /** + * Sets the value of an integer field. + * + * @param field field descriptor + * @param value integer value + * @param arrIndex array index (must be less than field array size) + * @throws UnsupportedOperationException if field is not integer type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ArrayIndexOutOfBoundsException if array index is out of bounds + * @throws ReadOnlyBufferException if the underlying buffer is read-only + */ + public void setIntField(StructFieldDescriptor field, long value, int arrIndex) { + if (!field.isInt() && !field.isUint()) { + throw new UnsupportedOperationException("field is not integer type"); + } + setFieldImpl(field, value, arrIndex); + } + + /** + * Sets the value of an integer field. + * + * @param field field descriptor + * @param value integer value + * @throws UnsupportedOperationException if field is not integer type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ReadOnlyBufferException if the underlying buffer is read-only + */ + public void setIntField(StructFieldDescriptor field, long value) { + setIntField(field, value, 0); + } + + /** + * Gets the value of a float field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return float field value + * @throws UnsupportedOperationException if field is not float type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ArrayIndexOutOfBoundsException if array index is out of bounds + */ + public float getFloatField(StructFieldDescriptor field, int arrIndex) { + if (field.getType() != StructFieldType.kFloat) { + throw new UnsupportedOperationException("field is not float type"); + } + return Float.intBitsToFloat((int) getFieldImpl(field, arrIndex)); + } + + /** + * Gets the value of a float field. + * + * @param field field descriptor + * @return float field value + * @throws UnsupportedOperationException if field is not float type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + */ + public float getFloatField(StructFieldDescriptor field) { + return getFloatField(field, 0); + } + + /** + * Sets the value of a float field. + * + * @param field field descriptor + * @param value float value + * @param arrIndex array index (must be less than field array size) + * @throws UnsupportedOperationException if field is not float type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ArrayIndexOutOfBoundsException if array index is out of bounds + * @throws ReadOnlyBufferException if the underlying buffer is read-only + */ + public void setFloatField(StructFieldDescriptor field, float value, int arrIndex) { + if (field.getType() != StructFieldType.kFloat) { + throw new UnsupportedOperationException("field is not float type"); + } + setFieldImpl(field, Float.floatToIntBits(value), arrIndex); + } + + /** + * Sets the value of a float field. + * + * @param field field descriptor + * @param value float value + * @throws UnsupportedOperationException if field is not float type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ReadOnlyBufferException if the underlying buffer is read-only + */ + public void setFloatField(StructFieldDescriptor field, float value) { + setFloatField(field, value, 0); + } + + /** + * Gets the value of a double field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return double field value + * @throws UnsupportedOperationException if field is not double type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ArrayIndexOutOfBoundsException if array index is out of bounds + */ + public double getDoubleField(StructFieldDescriptor field, int arrIndex) { + if (field.getType() != StructFieldType.kDouble) { + throw new UnsupportedOperationException("field is not double type"); + } + return Double.longBitsToDouble(getFieldImpl(field, arrIndex)); + } + + /** + * Gets the value of a double field. + * + * @param field field descriptor + * @return double field value + * @throws UnsupportedOperationException if field is not double type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + */ + public double getDoubleField(StructFieldDescriptor field) { + return getDoubleField(field, 0); + } + + /** + * Sets the value of a double field. + * + * @param field field descriptor + * @param value double value + * @param arrIndex array index (must be less than field array size) + * @throws UnsupportedOperationException if field is not double type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ArrayIndexOutOfBoundsException if array index is out of bounds + * @throws ReadOnlyBufferException if the underlying buffer is read-only + */ + public void setDoubleField(StructFieldDescriptor field, double value, int arrIndex) { + if (field.getType() != StructFieldType.kDouble) { + throw new UnsupportedOperationException("field is not double type"); + } + setFieldImpl(field, Double.doubleToLongBits(value), arrIndex); + } + + /** + * Sets the value of a double field. + * + * @param field field descriptor + * @param value double value + * @throws UnsupportedOperationException if field is not double type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ReadOnlyBufferException if the underlying buffer is read-only + */ + public void setDoubleField(StructFieldDescriptor field, double value) { + setDoubleField(field, value, 0); + } + + /** + * Gets the value of a character or character array field. + * + * @param field field descriptor + * @return field value + * @throws UnsupportedOperationException if field is not char type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + */ + public String getStringField(StructFieldDescriptor field) { + if (field.getType() != StructFieldType.kChar) { + throw new UnsupportedOperationException("field is not char type"); + } + if (!field.getParent().equals(m_desc)) { + throw new IllegalArgumentException("field is not part of this struct"); + } + if (!m_desc.isValid()) { + throw new IllegalStateException("struct descriptor is not valid"); + } + byte[] bytes = new byte[field.m_arraySize]; + m_data.position(field.m_offset).get(bytes, 0, field.m_arraySize); + return new String(bytes, StandardCharsets.UTF_8); + } + + /** + * Sets the value of a character or character array field. + * + * @param field field descriptor + * @param value field value + * @throws UnsupportedOperationException if field is not char type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + */ + public void setStringField(StructFieldDescriptor field, String value) { + if (field.getType() != StructFieldType.kChar) { + throw new UnsupportedOperationException("field is not char type"); + } + if (!field.getParent().equals(m_desc)) { + throw new IllegalArgumentException("field is not part of this struct"); + } + if (!m_desc.isValid()) { + throw new IllegalStateException("struct descriptor is not valid"); + } + ByteBuffer bb = StandardCharsets.UTF_8.encode(value); + int len = Math.min(bb.remaining(), field.m_arraySize); + m_data.position(field.m_offset).put(bb.limit(len)); + for (int i = len; i < field.m_arraySize; i++) { + m_data.put((byte) 0); + } + } + + /** + * Gets the value of a struct field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return field value + * @throws UnsupportedOperationException if field is not of struct type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ArrayIndexOutOfBoundsException if array index is out of bounds + */ + public DynamicStruct getStructField(StructFieldDescriptor field, int arrIndex) { + if (field.getType() != StructFieldType.kStruct) { + throw new UnsupportedOperationException("field is not struct type"); + } + if (!field.getParent().equals(m_desc)) { + throw new IllegalArgumentException("field is not part of this struct"); + } + if (!m_desc.isValid()) { + throw new IllegalStateException("struct descriptor is not valid"); + } + if (arrIndex < 0 || arrIndex >= field.m_arraySize) { + throw new ArrayIndexOutOfBoundsException( + "arrIndex (" + arrIndex + ") is larger than array size (" + field.m_arraySize + ")"); + } + StructDescriptor struct = field.getStruct(); + return wrap(struct, m_data.position(field.m_offset + arrIndex * struct.m_size)); + } + + /** + * Gets the value of a struct field. + * + * @param field field descriptor + * @return field value + * @throws UnsupportedOperationException if field is not of struct type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + */ + public DynamicStruct getStructField(StructFieldDescriptor field) { + return getStructField(field, 0); + } + + /** + * Sets the value of a struct field. + * + * @param field field descriptor + * @param value struct value + * @param arrIndex array index (must be less than field array size) + * @throws UnsupportedOperationException if field is not struct type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ArrayIndexOutOfBoundsException if array index is out of bounds + * @throws ReadOnlyBufferException if the underlying buffer is read-only + */ + public void setStructField(StructFieldDescriptor field, DynamicStruct value, int arrIndex) { + if (field.getType() != StructFieldType.kStruct) { + throw new UnsupportedOperationException("field is not struct type"); + } + if (!field.getParent().equals(m_desc)) { + throw new IllegalArgumentException("field is not part of this struct"); + } + if (!m_desc.isValid()) { + throw new IllegalStateException("struct descriptor is not valid"); + } + StructDescriptor struct = field.getStruct(); + if (!value.getDescriptor().equals(struct)) { + throw new IllegalArgumentException("value's struct type does not match field struct type"); + } + if (!value.getDescriptor().isValid()) { + throw new IllegalStateException("value's struct descriptor is not valid"); + } + if (arrIndex < 0 || arrIndex >= field.m_arraySize) { + throw new ArrayIndexOutOfBoundsException( + "arrIndex (" + arrIndex + ") is larger than array size (" + field.m_arraySize + ")"); + } + m_data + .position(field.m_offset + arrIndex * struct.m_size) + .put(value.m_data.position(0).limit(value.getDescriptor().getSize())); + } + + /** + * Sets the value of a struct field. + * + * @param field field descriptor + * @param value struct value + * @throws UnsupportedOperationException if field is not struct type + * @throws IllegalArgumentException if field is not a member of this struct + * @throws IllegalStateException if struct descriptor is invalid + * @throws ReadOnlyBufferException if the underlying buffer is read-only + */ + public void setStructField(StructFieldDescriptor field, DynamicStruct value) { + setStructField(field, value, 0); + } + + private long getFieldImpl(StructFieldDescriptor field, int arrIndex) { + if (!field.getParent().equals(m_desc)) { + throw new IllegalArgumentException("field is not part of this struct"); + } + if (!m_desc.isValid()) { + throw new IllegalStateException("struct descriptor is not valid"); + } + if (arrIndex < 0 || arrIndex >= field.m_arraySize) { + throw new ArrayIndexOutOfBoundsException( + "arrIndex (" + arrIndex + ") is larger than array size (" + field.m_arraySize + ")"); + } + + long val; + switch (field.m_size) { + case 1: + val = m_data.get(field.m_offset + arrIndex); + break; + case 2: + val = m_data.getShort(field.m_offset + arrIndex * 2); + break; + case 4: + val = m_data.getInt(field.m_offset + arrIndex * 4); + break; + case 8: + val = m_data.getLong(field.m_offset + arrIndex * 8); + break; + default: + throw new IllegalStateException("invalid field size"); + } + + if (field.isUint() || field.getType() == StructFieldType.kBool) { + // for unsigned fields, we can simply logical shift and mask + return (val >>> field.m_bitShift) & field.getBitMask(); + } else { + // to get sign extension, shift so the sign bit within the bitfield goes to the long's sign + // bit (also clearing all higher bits), then shift back down (also clearing all lower bits); + // since upper and lower bits are cleared with the shifts, the bitmask is unnecessary + return (val << (64 - field.m_bitShift - field.getBitWidth())) >> (64 - field.getBitWidth()); + } + } + + private void setFieldImpl(StructFieldDescriptor field, long value, int arrIndex) { + if (!field.getParent().equals(m_desc)) { + throw new IllegalArgumentException("field is not part of this struct"); + } + if (!m_desc.isValid()) { + throw new IllegalStateException("struct descriptor is not valid"); + } + if (arrIndex < 0 || arrIndex >= field.m_arraySize) { + throw new ArrayIndexOutOfBoundsException( + "arrIndex (" + arrIndex + ") is larger than array size (" + field.m_arraySize + ")"); + } + + // common case is no bit shift and no masking + if (!field.isBitField()) { + switch (field.m_size) { + case 1: + m_data.put(field.m_offset + arrIndex, (byte) value); + break; + case 2: + m_data.putShort(field.m_offset + arrIndex * 2, (short) value); + break; + case 4: + m_data.putInt(field.m_offset + arrIndex * 4, (int) value); + break; + case 8: + m_data.putLong(field.m_offset + arrIndex * 8, value); + break; + default: + throw new IllegalStateException("invalid field size"); + } + return; + } + + // handle bit shifting and masking into current value + switch (field.m_size) { + case 1: + { + byte val = m_data.get(field.m_offset + arrIndex); + val &= ~(field.getBitMask() << field.m_bitShift); + val |= (value & field.getBitMask()) << field.m_bitShift; + m_data.put(field.m_offset + arrIndex, val); + break; + } + case 2: + { + short val = m_data.getShort(field.m_offset + arrIndex * 2); + val &= ~(field.getBitMask() << field.m_bitShift); + val |= (value & field.getBitMask()) << field.m_bitShift; + m_data.putShort(field.m_offset + arrIndex * 2, val); + break; + } + case 4: + { + int val = m_data.getInt(field.m_offset + arrIndex * 4); + val &= ~(field.getBitMask() << field.m_bitShift); + val |= (value & field.getBitMask()) << field.m_bitShift; + m_data.putInt(field.m_offset + arrIndex * 4, val); + break; + } + case 8: + { + long val = m_data.getLong(field.m_offset + arrIndex * 8); + val &= ~(field.getBitMask() << field.m_bitShift); + val |= (value & field.getBitMask()) << field.m_bitShift; + m_data.putLong(field.m_offset + arrIndex * 8, val); + break; + } + default: + throw new IllegalStateException("invalid field size"); + } + } + + private final StructDescriptor m_desc; + private final ByteBuffer m_data; +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/Struct.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/Struct.java new file mode 100644 index 0000000000..630ef8c0c1 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/Struct.java @@ -0,0 +1,116 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct; + +import java.nio.ByteBuffer; + +/** + * Interface for raw struct serialization. + * + *

This is designed for serializing small fixed-size data structures in the fastest and most + * compact means possible. Serialization consists of making relative put() calls to a ByteBuffer and + * deserialization consists of making relative get() calls from a ByteBuffer. + * + *

Idiomatically, classes that support raw struct serialization should provide a static final + * member named "struct" that provides an instance of an implementation of this interface. + * + * @param object type + */ +public interface Struct { + /** Serialized size of a "bool" value. */ + int kSizeBool = 1; + + /** Serialized size of an "int8" or "uint8" value. */ + int kSizeInt8 = 1; + + /** Serialized size of an "int16" or "uint16" value. */ + int kSizeInt16 = 2; + + /** Serialized size of an "int32" or "uint32" value. */ + int kSizeInt32 = 4; + + /** Serialized size of an "int64" or "uint64" value. */ + int kSizeInt64 = 8; + + /** Serialized size of an "float" or "float32" value. */ + int kSizeFloat = 4; + + /** Serialized size of an "double" or "float64" value. */ + int kSizeDouble = 8; + + /** + * Gets the Class object for the stored value. + * + * @return Class + */ + Class getTypeClass(); + + /** + * Gets the type string (e.g. for NetworkTables). This should be globally unique and start with + * "struct:". + * + * @return type string + */ + String getTypeString(); + + /** + * Gets the serialized size (in bytes). This should always be a constant. + * + * @return serialized size + */ + int getSize(); + + /** + * Gets the schema. + * + * @return schema + */ + String getSchema(); + + /** + * Gets the list of struct types referenced by this struct. + * + * @return list of struct types + */ + default Struct[] getNested() { + return new Struct[] {}; + } + + /** + * Deserializes an object from a raw struct serialized ByteBuffer starting at the current + * position. Will increment the ByteBuffer position by getStructSize() bytes. Will not otherwise + * modify the ByteBuffer (e.g. byte order will not be changed). + * + * @param bb ByteBuffer + * @return New object + */ + T unpack(ByteBuffer bb); + + /** + * Puts object contents to a ByteBuffer starting at the current position. Will increment the + * ByteBuffer position by getStructSize() bytes. Will not otherwise modify the ByteBuffer (e.g. + * byte order will not be changed). + * + * @param bb ByteBuffer + * @param value object to serialize + */ + void pack(ByteBuffer bb, T value); + + /** + * Updates object contents from a raw struct serialized ByteBuffer starting at the current + * position. Will increment the ByteBuffer position by getStructSize() bytes. Will not otherwise + * modify the ByteBuffer (e.g. byte order will not be changed). + * + *

Immutable classes cannot and should not implement this function. The default implementation + * throws UnsupportedOperationException. + * + * @param out object to update + * @param bb ByteBuffer + * @throws UnsupportedOperationException if the object is immutable + */ + default void unpackInto(T out, ByteBuffer bb) { + throw new UnsupportedOperationException("object does not support unpackInto"); + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/StructBuffer.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructBuffer.java new file mode 100644 index 0000000000..9021861d56 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructBuffer.java @@ -0,0 +1,224 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct; + +import java.lang.reflect.Array; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.Collection; + +/** + * Reusable buffer for serialization/deserialization to/from a raw struct. + * + * @param object type + */ +public final class StructBuffer { + private StructBuffer(Struct struct) { + m_structSize = struct.getSize(); + m_buf = ByteBuffer.allocateDirect(m_structSize).order(ByteOrder.LITTLE_ENDIAN); + m_struct = struct; + } + + public static StructBuffer create(Struct struct) { + return new StructBuffer(struct); + } + + /** + * Gets the struct object of the stored type. + * + * @return struct object + */ + public Struct getStruct() { + return m_struct; + } + + /** + * Gets the type string. + * + * @return type string + */ + public String getTypeString() { + return m_struct.getTypeString(); + } + + /** + * Ensures sufficient buffer space is available for the given number of elements. + * + * @param nelem number of elements + */ + public void reserve(int nelem) { + if ((nelem * m_structSize) > m_buf.capacity()) { + m_buf = ByteBuffer.allocateDirect(nelem * m_structSize).order(ByteOrder.LITTLE_ENDIAN); + } + } + + /** + * Serializes a value to a ByteBuffer. The returned ByteBuffer is a direct byte buffer with the + * position set to the end of the serialized data. + * + * @param value value + * @return byte buffer + */ + public ByteBuffer write(T value) { + m_buf.position(0); + m_struct.pack(m_buf, value); + return m_buf; + } + + /** + * Deserializes a value from a byte array, creating a new object. + * + * @param buf byte array + * @param start starting location within byte array + * @param len length of serialized data + * @return new object + */ + public T read(byte[] buf, int start, int len) { + return read(ByteBuffer.wrap(buf, start, len)); + } + + /** + * Deserializes a value from a byte array, creating a new object. + * + * @param buf byte array + * @return new object + */ + public T read(byte[] buf) { + return read(buf, 0, buf.length); + } + + /** + * Deserializes a value from a ByteBuffer, creating a new object. + * + * @param buf byte buffer + * @return new object + */ + public T read(ByteBuffer buf) { + buf.order(ByteOrder.LITTLE_ENDIAN); + return m_struct.unpack(buf); + } + + /** + * Deserializes a value from a byte array into a mutable object. + * + * @param out object (will be updated with deserialized contents) + * @param buf byte array + * @param start starting location within byte array + * @param len length of serialized data + * @throws UnsupportedOperationException if T is immutable + */ + public void readInto(T out, byte[] buf, int start, int len) { + readInto(out, ByteBuffer.wrap(buf, start, len)); + } + + /** + * Deserializes a value from a byte array into a mutable object. + * + * @param out object (will be updated with deserialized contents) + * @param buf byte array + * @throws UnsupportedOperationException if T is immutable + */ + public void readInto(T out, byte[] buf) { + readInto(out, buf, 0, buf.length); + } + + /** + * Deserializes a value from a ByteBuffer into a mutable object. + * + * @param out object (will be updated with deserialized contents) + * @param buf byte buffer + * @throws UnsupportedOperationException if T is immutable + */ + public void readInto(T out, ByteBuffer buf) { + m_struct.unpackInto(out, buf); + } + + /** + * Serializes a collection of values to a ByteBuffer. The returned ByteBuffer is a direct byte + * buffer with the position set to the end of the serialized data. + * + * @param values values + * @return byte buffer + */ + public ByteBuffer writeArray(Collection values) { + m_buf.position(0); + if ((values.size() * m_structSize) > m_buf.capacity()) { + m_buf = + ByteBuffer.allocateDirect(values.size() * m_structSize * 2) + .order(ByteOrder.LITTLE_ENDIAN); + } + for (T v : values) { + m_struct.pack(m_buf, v); + } + return m_buf; + } + + /** + * Serializes an array of values to a ByteBuffer. The returned ByteBuffer is a direct byte buffer + * with the position set to the end of the serialized data. + * + * @param values values + * @return byte buffer + */ + public ByteBuffer writeArray(T[] values) { + m_buf.position(0); + if ((values.length * m_structSize) > m_buf.capacity()) { + m_buf = + ByteBuffer.allocateDirect(values.length * m_structSize * 2) + .order(ByteOrder.LITTLE_ENDIAN); + } + for (T v : values) { + m_struct.pack(m_buf, v); + } + return m_buf; + } + + /** + * Deserializes an array of values from a byte array, creating an array of new objects. + * + * @param buf byte array + * @param start starting location within byte array + * @param len length of serialized data + * @return new object array + */ + public T[] readArray(byte[] buf, int start, int len) { + return readArray(ByteBuffer.wrap(buf, start, len)); + } + + /** + * Deserializes an array of values from a byte array, creating an array of new objects. + * + * @param buf byte array + * @return new object array + */ + public T[] readArray(byte[] buf) { + return readArray(buf, 0, buf.length); + } + + /** + * Deserializes an array of values from a ByteBuffer, creating an array of new objects. + * + * @param buf byte buffer + * @return new object array + */ + public T[] readArray(ByteBuffer buf) { + buf.order(ByteOrder.LITTLE_ENDIAN); + int len = buf.limit() - buf.position(); + if ((len % m_structSize) != 0) { + throw new RuntimeException("buffer size not a multiple of struct size"); + } + int nelem = len / m_structSize; + @SuppressWarnings("unchecked") + T[] arr = (T[]) Array.newInstance(m_struct.getClass(), nelem); + for (int i = 0; i < nelem; i++) { + arr[i] = m_struct.unpack(buf); + } + return arr; + } + + private ByteBuffer m_buf; + private final Struct m_struct; + private final int m_structSize; +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptor.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptor.java new file mode 100644 index 0000000000..438db434c4 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptor.java @@ -0,0 +1,156 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Stack; + +/** Raw struct dynamic struct descriptor. */ +public class StructDescriptor { + StructDescriptor(String name) { + m_name = name; + } + + /** + * Gets the struct name. + * + * @return name + */ + public String getName() { + return m_name; + } + + /** + * Gets the struct schema. + * + * @return schema + */ + public String getSchema() { + return m_schema; + } + + /** + * Returns whether the struct is valid (e.g. the struct is fully defined and field offsets + * computed). + * + * @return true if valid + */ + public boolean isValid() { + return m_valid; + } + + /** + * Returns the struct size, in bytes. Not valid unless IsValid() is true. + * + * @return size in bytes + * @throws IllegalStateException if descriptor is invalid + */ + public int getSize() { + if (!m_valid) { + throw new IllegalStateException("descriptor is invalid"); + } + return m_size; + } + + /** + * Gets a field descriptor by name. Note the field cannot be accessed until the struct is valid. + * + * @param name field name + * @return field descriptor, or nullptr if not found + */ + public StructFieldDescriptor findFieldByName(String name) { + return m_fieldsByName.get(name); + } + + /** + * Gets all field descriptors. Note fields cannot be accessed until the struct is valid. + * + * @return field descriptors + */ + public List getFields() { + return m_fields; + } + + boolean checkCircular(Stack stack) { + stack.push(this); + for (StructDescriptor ref : m_references) { + if (stack.contains(ref)) { + return false; + } + if (!ref.checkCircular(stack)) { + return false; + } + } + stack.pop(); + return true; + } + + void calculateOffsets(Stack stack) { + int offset = 0; + int shift = 0; + int prevBitfieldSize = 0; + for (StructFieldDescriptor field : m_fields) { + if (!field.isBitField()) { + shift = 0; // reset shift on non-bitfield element + offset += prevBitfieldSize; // finish bitfield if active + prevBitfieldSize = 0; // previous is now not bitfield + field.m_offset = offset; + StructDescriptor struct = field.getStruct(); + if (struct != null) { + if (!struct.isValid()) { + m_valid = false; + return; + } + field.m_size = struct.m_size; + } + offset += field.m_size * field.m_arraySize; + } else { + int bitWidth = field.getBitWidth(); + if (field.getType() == StructFieldType.kBool + && prevBitfieldSize != 0 + && (shift + 1) <= (prevBitfieldSize * 8)) { + // bool takes on size of preceding bitfield type (if it fits) + field.m_size = prevBitfieldSize; + } else if (field.m_size != prevBitfieldSize || (shift + bitWidth) > (field.m_size * 8)) { + shift = 0; + offset += prevBitfieldSize; + } + prevBitfieldSize = field.m_size; + field.m_offset = offset; + field.m_bitShift = shift; + shift += bitWidth; + } + } + + // update struct size + m_size = offset + prevBitfieldSize; + m_valid = true; + + // now that we're valid, referring types may be too + stack.push(this); + for (StructDescriptor ref : m_references) { + if (stack.contains(ref)) { + throw new IllegalStateException( + "internal error (inconsistent data): circular struct reference between " + + m_name + + " and " + + ref.m_name); + } + ref.calculateOffsets(stack); + } + stack.pop(); + } + + private final String m_name; + String m_schema; + final List m_references = new ArrayList<>(); + final List m_fields = new ArrayList<>(); + final Map m_fieldsByName = new HashMap<>(); + int m_size; + boolean m_valid; +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptorDatabase.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptorDatabase.java new file mode 100644 index 0000000000..be7343fcfe --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructDescriptorDatabase.java @@ -0,0 +1,148 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct; + +import edu.wpi.first.util.struct.parser.ParseException; +import edu.wpi.first.util.struct.parser.ParsedDeclaration; +import edu.wpi.first.util.struct.parser.ParsedSchema; +import edu.wpi.first.util.struct.parser.Parser; +import java.util.HashMap; +import java.util.Map; +import java.util.Stack; + +/** Database of raw struct dynamic descriptors. */ +public class StructDescriptorDatabase { + /** + * Adds a structure schema to the database. If the struct references other structs that have not + * yet been added, it will not be valid until those structs are also added. + * + * @param name structure name + * @param schema structure schema + * @return Added struct dynamic descriptor + * @throws BadSchemaException if schema invalid + */ + public StructDescriptor add(String name, String schema) throws BadSchemaException { + Parser parser = new Parser(schema); + ParsedSchema parsed; + try { + parsed = parser.parse(); + } catch (ParseException e) { + throw new BadSchemaException("parse error", e); + } + + // turn parsed schema into descriptors + StructDescriptor theStruct = m_structs.computeIfAbsent(name, k -> new StructDescriptor(k)); + theStruct.m_schema = schema; + theStruct.m_fields.clear(); + boolean isValid = true; + for (ParsedDeclaration decl : parsed.declarations) { + StructFieldType type = StructFieldType.fromString(decl.typeString); + int size = type.size; + + // bitfield checks + if (decl.bitWidth != 0) { + // only integer or boolean types are allowed + if (!type.isInt && !type.isUint && type != StructFieldType.kBool) { + throw new BadSchemaException( + decl.name, "type " + decl.typeString + " cannot be bitfield"); + } + + // bit width cannot be larger than field size + if (decl.bitWidth > (size * 8)) { + throw new BadSchemaException( + decl.name, "bit width " + decl.bitWidth + " exceeds type size"); + } + + // bit width must be 1 for booleans + if (type == StructFieldType.kBool && decl.bitWidth != 1) { + throw new BadSchemaException(decl.name, "bit width must be 1 for bool type"); + } + + // cannot combine array and bitfield (shouldn't parse, but double-check) + if (decl.arraySize > 1) { + throw new BadSchemaException(decl.name, "cannot combine array and bitfield"); + } + } + + // struct handling + StructDescriptor structDesc = null; + if (type == StructFieldType.kStruct) { + // recursive definitions are not allowed + if (decl.typeString.equals(name)) { + throw new BadSchemaException(decl.name, "recursive struct reference"); + } + + // cross-reference struct, creating a placeholder if necessary + StructDescriptor aStruct = + m_structs.computeIfAbsent(decl.typeString, k -> new StructDescriptor(k)); + + // if the struct isn't valid, we can't be valid either + if (aStruct.isValid()) { + size = aStruct.getSize(); + } else { + isValid = false; + } + + // add to cross-references for when the struct does become valid + aStruct.m_references.add(theStruct); + structDesc = aStruct; + } + + // create field + StructFieldDescriptor fieldDesc = + new StructFieldDescriptor( + theStruct, + decl.name, + type, + size, + decl.arraySize, + decl.bitWidth, + decl.enumValues, + structDesc); + if (theStruct.m_fieldsByName.put(decl.name, fieldDesc) != null) { + throw new BadSchemaException(decl.name, "duplicate field name"); + } + + theStruct.m_fields.add(fieldDesc); + } + + theStruct.m_valid = isValid; + Stack stack = new Stack<>(); + if (isValid) { + // we have all the info needed, so calculate field offset & shift + theStruct.calculateOffsets(stack); + } else { + // check for circular reference + if (!theStruct.checkCircular(stack)) { + StringBuilder builder = new StringBuilder(); + builder.append("circular struct reference: "); + boolean first = true; + for (StructDescriptor elem : stack) { + if (!first) { + builder.append(" <- "); + } else { + first = false; + } + builder.append(elem.getName()); + } + throw new BadSchemaException(builder.toString()); + } + } + + return theStruct; + } + + /** + * Finds a structure in the database by name. + * + * @param name structure name + * @return struct descriptor, or null if not found + */ + public StructDescriptor find(String name) { + return m_structs.get(name); + } + + private final Map m_structs = new HashMap<>(); +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldDescriptor.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldDescriptor.java new file mode 100644 index 0000000000..25a69a575b --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldDescriptor.java @@ -0,0 +1,241 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct; + +import java.util.Map; + +/** Raw struct dynamic field descriptor. */ +public class StructFieldDescriptor { + private static int toBitWidth(int size, int bitWidth) { + if (bitWidth == 0) { + return size * 8; + } else { + return bitWidth; + } + } + + private static long toBitMask(int size, int bitWidth) { + if (size == 0) { + return 0; + } else { + return -1L >>> (64 - toBitWidth(size, bitWidth)); + } + } + + // does not fill in offset, shift + StructFieldDescriptor( + StructDescriptor parent, + String name, + StructFieldType type, + int size, + int arraySize, + int bitWidth, + Map enumValues, + StructDescriptor structDesc) { + m_parent = parent; + m_name = name; + m_size = size; + m_arraySize = arraySize; + m_enum = enumValues; + m_struct = structDesc; + m_bitMask = toBitMask(size, bitWidth); + m_type = type; + m_bitWidth = toBitWidth(size, bitWidth); + } + + /** + * Gets the dynamic struct this field is contained in. + * + * @return struct descriptor + */ + public StructDescriptor getParent() { + return m_parent; + } + + /** + * Gets the field name. + * + * @return field name + */ + public String getName() { + return m_name; + } + + /** + * Gets the field type. + * + * @return field type + */ + public StructFieldType getType() { + return m_type; + } + + /** + * Returns whether the field type is a signed integer. + * + * @return true if signed integer, false otherwise + */ + public boolean isInt() { + return m_type.isInt; + } + + /** + * Returns whether the field type is an unsigned integer. + * + * @return true if unsigned integer, false otherwise + */ + public boolean isUint() { + return m_type.isUint; + } + + /** + * Gets the underlying storage size of the field, in bytes. + * + * @return number of bytes + */ + public int getSize() { + return m_size; + } + + /** + * Gets the storage offset of the field, in bytes. + * + * @return number of bytes from the start of the struct + */ + public int getOffset() { + return m_offset; + } + + /** + * Gets the bit width of the field, in bits. + * + * @return number of bits + */ + public int getBitWidth() { + return m_bitWidth == 0 ? m_size * 8 : m_bitWidth; + } + + /** + * Gets the bit mask for the field. The mask is always the least significant bits (it is not + * shifted). + * + * @return bit mask + */ + public long getBitMask() { + return m_bitMask; + } + + /** + * Gets the bit shift for the field (LSB=0). + * + * @return number of bits + */ + public int getBitShift() { + return m_bitShift; + } + + /** + * Returns whether the field is an array. + * + * @return true if array + */ + public boolean isArray() { + return m_arraySize > 1; + } + + /** + * Gets the array size. Returns 1 if non-array. + * + * @return number of elements + */ + public int getArraySize() { + return m_arraySize; + } + + /** + * Returns whether the field has enumerated values. + * + * @return true if there are enumerated values + */ + public boolean hasEnum() { + return m_enum != null; + } + + /** + * Gets the enumerated values. + * + * @return set of enumerated values + */ + public Map getEnumValues() { + return m_enum; + } + + /** + * Gets the struct descriptor for a struct data type. + * + * @return struct descriptor; returns null for non-struct + */ + public StructDescriptor getStruct() { + return m_struct; + } + + /** + * Gets the minimum unsigned integer value that can be stored in this field. + * + * @return minimum value + */ + public long getUintMin() { + return 0; + } + + /** + * Gets the maximum unsigned integer value that can be stored in this field. Note this is not the + * actual maximum for uint64 (due to Java lacking support for 64-bit unsigned integers). + * + * @return maximum value + */ + public long getUintMax() { + return m_bitMask; + } + + /** + * Gets the minimum signed integer value that can be stored in this field. + * + * @return minimum value + */ + public long getIntMin() { + return (-(m_bitMask >> 1)) - 1; + } + + /** + * Gets the maximum signed integer value that can be stored in this field. + * + * @return maximum value + */ + public long getIntMax() { + return m_bitMask >> 1; + } + + /** + * Returns whether the field is a bitfield. + * + * @return true if bitfield + */ + public boolean isBitField() { + return m_bitShift != 0 || m_bitWidth != (m_size * 8); + } + + private final StructDescriptor m_parent; + private final String m_name; + int m_size; + int m_offset; + final int m_arraySize; // 1 for non-arrays + private final Map m_enum; + private final StructDescriptor m_struct; // null for non-structs + private final long m_bitMask; + private final StructFieldType m_type; + private final int m_bitWidth; + int m_bitShift; +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldType.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldType.java new file mode 100644 index 0000000000..28d5d8ec31 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/StructFieldType.java @@ -0,0 +1,67 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct; + +/** Known data types for raw struct dynamic fields (see StructFieldDescriptor). */ +public enum StructFieldType { + kBool("bool", false, false, 1), + kChar("char", false, false, 1), + kInt8("int8", true, false, 1), + kInt16("int16", true, false, 2), + kInt32("int32", true, false, 4), + kInt64("int64", true, false, 8), + kUint8("uint8", false, true, 1), + kUint16("uint16", false, true, 2), + kUint32("uint32", false, true, 4), + kUint64("uint64", false, true, 8), + kFloat("float", false, false, 4), + kDouble("double", false, false, 8), + kStruct("struct", false, false, 0); + + @SuppressWarnings("MemberName") + public final String name; + + @SuppressWarnings("MemberName") + public final boolean isInt; + + @SuppressWarnings("MemberName") + public final boolean isUint; + + @SuppressWarnings("MemberName") + public final int size; + + StructFieldType(String name, boolean isInt, boolean isUint, int size) { + this.name = name; + this.isInt = isInt; + this.isUint = isUint; + this.size = size; + } + + @Override + public String toString() { + return name; + } + + /** + * Get field type from string. + * + * @param str string + * @return field type + */ + public static StructFieldType fromString(String str) { + for (StructFieldType type : StructFieldType.values()) { + if (type.name.equals(str)) { + return type; + } + } + if ("float32".equals(str)) { + return kFloat; + } else if ("float64".equals(str)) { + return kDouble; + } else { + return kStruct; + } + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/Lexer.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/Lexer.java new file mode 100644 index 0000000000..6e3cd5dac7 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/Lexer.java @@ -0,0 +1,132 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct.parser; + +/** Raw struct schema lexer. */ +public class Lexer { + /** + * Construct a raw struct schema lexer. + * + * @param in schema + */ + public Lexer(String in) { + m_in = in; + } + + /** + * Gets the next token. + * + * @return Token kind; the token text can be retrieved using getTokenText() + */ + public TokenKind scan() { + // skip whitespace + do { + get(); + } while (m_current == ' ' || m_current == '\t' || m_current == '\n' || m_current == '\r'); + m_tokenStart = m_pos - 1; + + switch (m_current) { + case '[': + return TokenKind.kLeftBracket; + case ']': + return TokenKind.kRightBracket; + case '{': + return TokenKind.kLeftBrace; + case '}': + return TokenKind.kRightBrace; + case ':': + return TokenKind.kColon; + case ';': + return TokenKind.kSemicolon; + case ',': + return TokenKind.kComma; + case '=': + return TokenKind.kEquals; + case '-': + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + return scanInteger(); + case '\0': + return TokenKind.kEndOfInput; + default: + if (Character.isLetter(m_current) || m_current == '_') { + return scanIdentifier(); + } + return TokenKind.kUnknown; + } + } + + /** + * Gets the text of the last lexed token. + * + * @return token text + */ + public String getTokenText() { + if (m_tokenStart >= m_in.length()) { + return ""; + } + return m_in.substring(m_tokenStart, m_pos); + } + + /** + * Gets the starting position of the last lexed token. + * + * @return position (0 = first character) + */ + public int getPosition() { + return m_tokenStart; + } + + private TokenKind scanInteger() { + do { + get(); + } while (Character.isDigit(m_current)); + unget(); + return TokenKind.kInteger; + } + + private TokenKind scanIdentifier() { + do { + get(); + } while (Character.isLetterOrDigit(m_current) || m_current == '_'); + unget(); + return TokenKind.kIdentifier; + } + + private void get() { + if (m_pos < m_in.length()) { + m_current = m_in.charAt(m_pos); + } else { + m_current = '\0'; + } + ++m_pos; + } + + private void unget() { + if (m_pos > 0) { + m_pos--; + if (m_pos < m_in.length()) { + m_current = m_in.charAt(m_pos); + } else { + m_current = '\0'; + } + } else { + m_current = '\0'; + } + } + + final String m_in; + char m_current; + int m_tokenStart; + int m_pos; +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParseException.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParseException.java new file mode 100644 index 0000000000..9fa843ca70 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParseException.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct.parser; + +public class ParseException extends Exception { + private final int m_pos; + + public ParseException(int pos, String s) { + super(s); + m_pos = pos; + } + + public ParseException(int pos, String message, Throwable cause) { + super(message, cause); + m_pos = pos; + } + + public ParseException(int pos, Throwable cause) { + super(cause); + m_pos = pos; + } + + public int getPosition() { + return m_pos; + } + + @Override + public String toString() { + return m_pos + ": " + getMessage(); + } +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedDeclaration.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedDeclaration.java new file mode 100644 index 0000000000..8184ae5e56 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedDeclaration.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct.parser; + +import java.util.Map; + +/** Raw struct schema declaration. */ +public class ParsedDeclaration { + @SuppressWarnings("MemberName") + public String typeString; + + @SuppressWarnings("MemberName") + public String name; + + @SuppressWarnings("MemberName") + public Map enumValues; + + @SuppressWarnings("MemberName") + public int arraySize = 1; + + @SuppressWarnings("MemberName") + public int bitWidth; +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedSchema.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedSchema.java new file mode 100644 index 0000000000..2ca175314c --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/ParsedSchema.java @@ -0,0 +1,14 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct.parser; + +import java.util.ArrayList; +import java.util.List; + +/** Raw struct schema. */ +public class ParsedSchema { + @SuppressWarnings("MemberName") + public List declarations = new ArrayList<>(); +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/Parser.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/Parser.java new file mode 100644 index 0000000000..7aba1adaa6 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/Parser.java @@ -0,0 +1,157 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct.parser; + +import java.util.HashMap; +import java.util.Map; + +/** Raw struct schema parser. */ +public class Parser { + /** + * Construct a raw struct schema parser. + * + * @param in schema + */ + public Parser(String in) { + m_lexer = new Lexer(in); + } + + /** + * Parses the schema. + * + * @return parsed schema object + * @throws ParseException on parse error + */ + public ParsedSchema parse() throws ParseException { + ParsedSchema schema = new ParsedSchema(); + do { + getNextToken(); + if (m_token == TokenKind.kSemicolon) { + continue; + } + if (m_token == TokenKind.kEndOfInput) { + break; + } + schema.declarations.add(parseDeclaration()); + } while (m_token != TokenKind.kEndOfInput); + return schema; + } + + private ParsedDeclaration parseDeclaration() throws ParseException { + ParsedDeclaration decl = new ParsedDeclaration(); + + // optional enum specification + if (m_token == TokenKind.kIdentifier && "enum".equals(m_lexer.getTokenText())) { + getNextToken(); + expect(TokenKind.kLeftBrace); + decl.enumValues = parseEnum(); + getNextToken(); + } else if (m_token == TokenKind.kLeftBrace) { + decl.enumValues = parseEnum(); + getNextToken(); + } + + // type name + expect(TokenKind.kIdentifier); + decl.typeString = m_lexer.getTokenText(); + getNextToken(); + + // identifier name + expect(TokenKind.kIdentifier); + decl.name = m_lexer.getTokenText(); + getNextToken(); + + // array or bit field + if (m_token == TokenKind.kLeftBracket) { + getNextToken(); + expect(TokenKind.kInteger); + String valueStr = m_lexer.getTokenText(); + int value; + try { + value = Integer.parseInt(valueStr); + } catch (NumberFormatException e) { + value = 0; + } + if (value > 0) { + decl.arraySize = value; + } else { + throw new ParseException( + m_lexer.m_pos, "array size '" + valueStr + "' is not a positive integer"); + } + getNextToken(); + expect(TokenKind.kRightBracket); + getNextToken(); + } else if (m_token == TokenKind.kColon) { + getNextToken(); + expect(TokenKind.kInteger); + String valueStr = m_lexer.getTokenText(); + int value; + try { + value = Integer.parseInt(valueStr); + } catch (NumberFormatException e) { + value = 0; + } + if (value > 0) { + decl.bitWidth = value; + } else { + throw new ParseException( + m_lexer.m_pos, "bitfield width '" + valueStr + "' is not a positive integer"); + } + getNextToken(); + } + + // declaration must end with EOF or semicolon + if (m_token != TokenKind.kEndOfInput) { + expect(TokenKind.kSemicolon); + } + + return decl; + } + + private Map parseEnum() throws ParseException { + Map map = new HashMap<>(); + + // we start with current = '{' + getNextToken(); + while (m_token != TokenKind.kRightBrace) { + expect(TokenKind.kIdentifier); + final String name = m_lexer.getTokenText(); + getNextToken(); + expect(TokenKind.kEquals); + getNextToken(); + expect(TokenKind.kInteger); + String valueStr = m_lexer.getTokenText(); + long value; + try { + value = Long.parseLong(valueStr); + } catch (NumberFormatException e) { + throw new ParseException(m_lexer.m_pos, "could not parse enum value '" + valueStr + "'"); + } + map.put(name, value); + getNextToken(); + if (m_token == TokenKind.kRightBrace) { + break; + } + expect(TokenKind.kComma); + getNextToken(); + } + return map; + } + + private TokenKind getNextToken() { + m_token = m_lexer.scan(); + return m_token; + } + + private void expect(TokenKind kind) throws ParseException { + if (m_token != kind) { + throw new ParseException( + m_lexer.m_pos, "expected " + kind + ", got '" + m_lexer.getTokenText() + "'"); + } + } + + final Lexer m_lexer; + TokenKind m_token; +} diff --git a/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/TokenKind.java b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/TokenKind.java new file mode 100644 index 0000000000..85afa4a3bf --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/struct/parser/TokenKind.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct.parser; + +/** A lexed raw struct schema token. */ +public enum TokenKind { + kUnknown("unknown"), + kInteger("integer"), + kIdentifier("identifier"), + kLeftBracket("'['"), + kRightBracket("']'"), + kLeftBrace("'{'"), + kRightBrace("'}'"), + kColon("':'"), + kSemicolon("';'"), + kComma("','"), + kEquals("'='"), + kEndOfInput(""); + + private final String m_name; + + TokenKind(String name) { + this.m_name = name; + } + + @Override + public String toString() { + return m_name; + } +} diff --git a/wpiutil/src/main/native/cpp/DataLog.cpp b/wpiutil/src/main/native/cpp/DataLog.cpp index 441cd34de7..9994618fb5 100644 --- a/wpiutil/src/main/native/cpp/DataLog.cpp +++ b/wpiutil/src/main/native/cpp/DataLog.cpp @@ -31,6 +31,7 @@ #include "wpi/Endian.h" #include "wpi/Logger.h" #include "wpi/MathExtras.h" +#include "wpi/SmallString.h" #include "wpi/fs.h" #include "wpi/timestamp.h" @@ -211,6 +212,33 @@ void DataLog::Resume() { m_paused = false; } +bool DataLog::HasSchema(std::string_view name) const { + std::scoped_lock lock{m_mutex}; + wpi::SmallString<128> fullName{"/.schema/"}; + fullName += name; + auto it = m_entries.find(fullName); + return it != m_entries.end(); +} + +void DataLog::AddSchema(std::string_view name, std::string_view type, + std::span schema, int64_t timestamp) { + std::scoped_lock lock{m_mutex}; + wpi::SmallString<128> fullName{"/.schema/"}; + fullName += name; + auto& entryInfo = m_entries[fullName]; + if (entryInfo.id != 0) { + return; // don't add duplicates + } + int entry = StartImpl(fullName, type, {}, timestamp); + + // inline AppendRaw() without releasing lock + if (entry <= 0) { + [[unlikely]] return; // should never happen, but check anyway + } + StartRecord(entry, timestamp, schema.size(), 0); + AppendImpl(schema); +} + static void WriteToFile(fs::file_t f, std::span data, std::string_view filename, wpi::Logger& msglog) { do { @@ -484,6 +512,11 @@ void DataLog::WriterThreadMain( int DataLog::Start(std::string_view name, std::string_view type, std::string_view metadata, int64_t timestamp) { std::scoped_lock lock{m_mutex}; + return StartImpl(name, type, metadata, timestamp); +} + +int DataLog::StartImpl(std::string_view name, std::string_view type, + std::string_view metadata, int64_t timestamp) { auto& entryInfo = m_entries[name]; if (entryInfo.id == 0) { entryInfo.id = ++m_lastId; diff --git a/wpiutil/src/main/native/cpp/jni/DataLogJNI.cpp b/wpiutil/src/main/native/cpp/jni/DataLogJNI.cpp index 50d790f785..cbc233dc50 100644 --- a/wpiutil/src/main/native/cpp/jni/DataLogJNI.cpp +++ b/wpiutil/src/main/native/cpp/jni/DataLogJNI.cpp @@ -111,6 +111,47 @@ Java_edu_wpi_first_util_datalog_DataLogJNI_resume reinterpret_cast(impl)->Resume(); } +/* + * Class: edu_wpi_first_util_datalog_DataLogJNI + * Method: addSchema + * Signature: (JLjava/lang/String;Ljava/lang/String;[BJ)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_util_datalog_DataLogJNI_addSchema + (JNIEnv* env, jclass, jlong impl, jstring name, jstring type, + jbyteArray schema, jlong timestamp) +{ + if (impl == 0) { + wpi::ThrowNullPointerException(env, "impl is null"); + return; + } + reinterpret_cast(impl)->AddSchema( + JStringRef{env, name}, JStringRef{env, type}, + JSpan{env, schema}.uarray(), timestamp); +} + +/* + * Class: edu_wpi_first_util_datalog_DataLogJNI + * Method: addSchemaString + * Signature: (JLjava/lang/String;Ljava/lang/String;Ljava/lang/String;J)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_util_datalog_DataLogJNI_addSchemaString + (JNIEnv* env, jclass, jlong impl, jstring name, jstring type, jstring schema, + jlong timestamp) +{ + if (impl == 0) { + wpi::ThrowNullPointerException(env, "impl is null"); + return; + } + JStringRef schemaStr{env, schema}; + std::string_view schemaView = schemaStr.str(); + reinterpret_cast(impl)->AddSchema( + JStringRef{env, name}, JStringRef{env, type}, + {reinterpret_cast(schemaView.data()), schemaView.size()}, + timestamp); +} + /* * Class: edu_wpi_first_util_datalog_DataLogJNI * Method: start diff --git a/wpiutil/src/main/native/cpp/protobuf/Protobuf.cpp b/wpiutil/src/main/native/cpp/protobuf/Protobuf.cpp new file mode 100644 index 0000000000..4f2a616ae1 --- /dev/null +++ b/wpiutil/src/main/native/cpp/protobuf/Protobuf.cpp @@ -0,0 +1,194 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/protobuf/Protobuf.h" + +#include +#include +#include +#include +#include + +#include "wpi/SmallVector.h" + +using namespace wpi; + +using google::protobuf::Arena; +using google::protobuf::FileDescriptor; +using google::protobuf::FileDescriptorProto; + +namespace { +class VectorOutputStream final + : public google::protobuf::io::ZeroCopyOutputStream { + public: + // Create a StringOutputStream which appends bytes to the given string. + // The string remains property of the caller, but it is mutated in arbitrary + // ways and MUST NOT be accessed in any way until you're done with the + // stream. Either be sure there's no further usage, or (safest) destroy the + // stream before using the contents. + // + // Hint: If you call target->reserve(n) before creating the stream, + // the first call to Next() will return at least n bytes of buffer + // space. + explicit VectorOutputStream(std::vector& target) : target_{target} {} + VectorOutputStream(const VectorOutputStream&) = delete; + ~VectorOutputStream() override = default; + + VectorOutputStream& operator=(const VectorOutputStream&) = delete; + + // implements ZeroCopyOutputStream --------------------------------- + bool Next(void** data, int* size) override; + void BackUp(int count) override { target_.resize(target_.size() - count); } + int64_t ByteCount() const override { return target_.size(); } + + private: + static constexpr size_t kMinimumSize = 16; + + std::vector& target_; +}; + +class SmallVectorOutputStream final + : public google::protobuf::io::ZeroCopyOutputStream { + public: + // Create a StringOutputStream which appends bytes to the given string. + // The string remains property of the caller, but it is mutated in arbitrary + // ways and MUST NOT be accessed in any way until you're done with the + // stream. Either be sure there's no further usage, or (safest) destroy the + // stream before using the contents. + // + // Hint: If you call target->reserve(n) before creating the stream, + // the first call to Next() will return at least n bytes of buffer + // space. + explicit SmallVectorOutputStream(wpi::SmallVectorImpl& target) + : target_{target} { + target.resize(0); + } + SmallVectorOutputStream(const SmallVectorOutputStream&) = delete; + ~SmallVectorOutputStream() override = default; + + SmallVectorOutputStream& operator=(const SmallVectorOutputStream&) = delete; + + // implements ZeroCopyOutputStream --------------------------------- + bool Next(void** data, int* size) override; + void BackUp(int count) override { target_.resize(target_.size() - count); } + int64_t ByteCount() const override { return target_.size(); } + + private: + static constexpr size_t kMinimumSize = 16; + + wpi::SmallVectorImpl& target_; +}; +} // namespace + +bool VectorOutputStream::Next(void** data, int* size) { + size_t old_size = target_.size(); + + // Grow the string. + size_t new_size; + if (old_size < target_.capacity()) { + // Resize to match its capacity, since we can get away + // without a memory allocation this way. + new_size = target_.capacity(); + } else { + // Size has reached capacity, try to double it. + new_size = old_size * 2; + } + // Avoid integer overflow in returned '*size'. + new_size = (std::min)(new_size, old_size + (std::numeric_limits::max)()); + // Increase the size, also make sure that it is at least kMinimumSize. + target_.resize((std::max)(new_size, kMinimumSize)); + + *data = target_.data() + old_size; + *size = target_.size() - old_size; + return true; +} + +bool SmallVectorOutputStream::Next(void** data, int* size) { + size_t old_size = target_.size(); + + // Grow the string. + size_t new_size; + if (old_size < target_.capacity()) { + // Resize to match its capacity, since we can get away + // without a memory allocation this way. + new_size = target_.capacity(); + } else { + // Size has reached capacity, try to double it. + new_size = old_size * 2; + } + // Avoid integer overflow in returned '*size'. + new_size = (std::min)(new_size, old_size + (std::numeric_limits::max)()); + // Increase the size, also make sure that it is at least kMinimumSize. + target_.resize_for_overwrite((std::max)(new_size, kMinimumSize)); + + *data = target_.data() + old_size; + *size = target_.size() - old_size; + return true; +} + +void detail::DeleteProtobuf(google::protobuf::Message* msg) { + if (msg && !msg->GetArena()) { + delete msg; + } +} + +bool detail::ParseProtobuf(google::protobuf::Message* msg, + std::span data) { + return msg->ParseFromArray(data.data(), data.size()); +} + +bool detail::SerializeProtobuf(wpi::SmallVectorImpl& out, + const google::protobuf::Message& msg) { + SmallVectorOutputStream stream{out}; + return msg.SerializeToZeroCopyStream(&stream); +} + +bool detail::SerializeProtobuf(std::vector& out, + const google::protobuf::Message& msg) { + VectorOutputStream stream{out}; + return msg.SerializeToZeroCopyStream(&stream); +} + +std::string detail::GetTypeString(const google::protobuf::Message& msg) { + return fmt::format("proto:{}", msg.GetDescriptor()->full_name()); +} + +static void ForEachProtobufDescriptorImpl( + const FileDescriptor* desc, + function_ref exists, + function_ref schema)> + fn, + Arena* arena, FileDescriptorProto** descproto) { + std::string name = fmt::format("proto:{}", desc->name()); + if (exists(name)) { + return; + } + for (int i = 0, ndep = desc->dependency_count(); i < ndep; ++i) { + ForEachProtobufDescriptorImpl(desc->dependency(i), exists, fn, arena, + descproto); + } + if (!*descproto) { + *descproto = Arena::CreateMessage(arena); + } + (*descproto)->Clear(); + desc->CopyTo(*descproto); + SmallVector buf; + detail::SerializeProtobuf(buf, **descproto); + fn(name, buf); +} + +void detail::ForEachProtobufDescriptor( + const google::protobuf::Message& msg, + function_ref exists, + function_ref descriptor)> + fn) { + FileDescriptorProto* descproto = nullptr; + ForEachProtobufDescriptorImpl(msg.GetDescriptor()->file(), exists, fn, + msg.GetArena(), &descproto); + if (descproto && !msg.GetArena()) { + delete descproto; + } +} diff --git a/wpiutil/src/main/native/cpp/protobuf/ProtobufMessageDatabase.cpp b/wpiutil/src/main/native/cpp/protobuf/ProtobufMessageDatabase.cpp new file mode 100644 index 0000000000..35e17747ab --- /dev/null +++ b/wpiutil/src/main/native/cpp/protobuf/ProtobufMessageDatabase.cpp @@ -0,0 +1,119 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/protobuf/ProtobufMessageDatabase.h" + +#include + +using namespace wpi; + +using google::protobuf::Arena; +using google::protobuf::FileDescriptorProto; +using google::protobuf::Message; + +void ProtobufMessageDatabase::Add(std::string_view filename, + std::span data) { + auto& file = m_files[filename]; + if (file.complete) { + file.complete = false; + + // rebuild the pool EXCEPT for this descriptor + m_pool = std::make_unique(); + + for (auto&& p : m_files) { + p.second.inPool = false; + } + for (auto&& p : m_files) { + if (p.second.complete && !p.second.inPool) { + Rebuild(p.second); + } + } + + // clear messages and reset factory; Find() will recreate as needed + m_msgs.clear(); + m_factory = std::make_unique(); + } + + if (!file.proto) { + file.proto = std::unique_ptr{ + Arena::CreateMessage(nullptr)}; + } else { + // replacing an existing one; remove any previously existing refs + for (auto&& dep : file.proto->dependency()) { + auto& depFile = m_files[dep]; + std::erase(depFile.uses, filename); + } + file.proto->Clear(); + } + + // parse data + if (!file.proto->ParseFromArray(data.data(), data.size())) { + return; + } + + Build(filename, file); +} + +Message* ProtobufMessageDatabase::Find(std::string_view name) const { + // cached + auto& msg = m_msgs[name]; + if (msg) { + return msg.get(); + } + + // need to create it + auto desc = m_pool->FindMessageTypeByName(std::string{name}); + if (!desc) { + return nullptr; + } + msg = std::unique_ptr{m_factory->GetPrototype(desc)->New(nullptr)}; + return msg.get(); +} + +void ProtobufMessageDatabase::Build(std::string_view filename, + ProtoFile& file) { + if (file.complete) { + return; + } + // are all of the dependencies complete? + bool complete = true; + for (auto&& dep : file.proto->dependency()) { + auto& depFile = m_files[dep]; + if (!depFile.complete) { + complete = false; + } + depFile.uses.emplace_back(filename); + } + if (!complete) { + return; + } + + // add to pool + if (!m_pool->BuildFile(*file.proto)) { + return; + } + file.inPool = true; + file.complete = true; + + // recursively validate all uses + for (auto&& use : file.uses) { + Build(use, m_files[use]); + } +} + +bool ProtobufMessageDatabase::Rebuild(ProtoFile& file) { + for (auto&& dep : file.proto->dependency()) { + auto& depFile = m_files[dep]; + if (!depFile.inPool) { + if (!Rebuild(depFile)) { + return false; + } + } + } + if (!m_pool->BuildFile(*file.proto)) { + return false; + } + file.inPool = true; + return true; +} diff --git a/wpiutil/src/main/native/cpp/struct/DynamicStruct.cpp b/wpiutil/src/main/native/cpp/struct/DynamicStruct.cpp new file mode 100644 index 0000000000..7ae9271ae0 --- /dev/null +++ b/wpiutil/src/main/native/cpp/struct/DynamicStruct.cpp @@ -0,0 +1,444 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/struct/DynamicStruct.h" + +#include + +#include + +#include "wpi/Endian.h" +#include "wpi/SmallString.h" +#include "wpi/SmallVector.h" +#include "wpi/raw_ostream.h" +#include "wpi/struct/SchemaParser.h" + +using namespace wpi; + +static size_t TypeToSize(StructFieldType type) { + switch (type) { + case StructFieldType::kBool: + case StructFieldType::kChar: + case StructFieldType::kInt8: + case StructFieldType::kUint8: + return 1; + case StructFieldType::kInt16: + case StructFieldType::kUint16: + return 2; + case StructFieldType::kInt32: + case StructFieldType::kUint32: + case StructFieldType::kFloat: + return 4; + case StructFieldType::kInt64: + case StructFieldType::kUint64: + case StructFieldType::kDouble: + return 8; + default: + return 0; + } +} + +static StructFieldType TypeStringToType(std::string_view str) { + if (str == "bool") { + return StructFieldType::kBool; + } else if (str == "char") { + return StructFieldType::kChar; + } else if (str == "int8") { + return StructFieldType::kInt8; + } else if (str == "int16") { + return StructFieldType::kInt16; + } else if (str == "int32") { + return StructFieldType::kInt32; + } else if (str == "int64") { + return StructFieldType::kInt64; + } else if (str == "uint8") { + return StructFieldType::kUint8; + } else if (str == "uint16") { + return StructFieldType::kUint16; + } else if (str == "uint32") { + return StructFieldType::kUint32; + } else if (str == "uint64") { + return StructFieldType::kUint64; + } else if (str == "float" || str == "float32") { + return StructFieldType::kFloat; + } else if (str == "double" || str == "float64") { + return StructFieldType::kDouble; + } else { + return StructFieldType::kStruct; + } +} + +static inline unsigned int ToBitWidth(size_t size, unsigned int bitWidth) { + if (bitWidth == 0) { + return size * 8; + } else { + return bitWidth; + } +} + +static inline uint64_t ToBitMask(size_t size, unsigned int bitWidth) { + if (size == 0) { + return 0; + } else { + return UINT64_MAX >> (64 - ToBitWidth(size, bitWidth)); + } +} + +StructFieldDescriptor::StructFieldDescriptor( + const StructDescriptor* parent, std::string_view name, StructFieldType type, + size_t size, size_t arraySize, unsigned int bitWidth, EnumValues enumValues, + const StructDescriptor* structDesc, const private_init&) + : m_parent{parent}, + m_name{name}, + m_size{size}, + m_arraySize{arraySize}, + m_enum{std::move(enumValues)}, + m_struct{structDesc}, + m_bitMask{ToBitMask(size, bitWidth)}, + m_type{type}, + m_bitWidth{ToBitWidth(size, bitWidth)} {} + +const StructFieldDescriptor* StructDescriptor::FindFieldByName( + std::string_view name) const { + auto it = m_fieldsByName.find(name); + if (it == m_fieldsByName.end()) { + return nullptr; + } + return &m_fields[it->second]; +} + +bool StructDescriptor::CheckCircular( + wpi::SmallVectorImpl& stack) const { + stack.emplace_back(this); + for (auto&& ref : m_references) { + if (std::find(stack.begin(), stack.end(), ref) != stack.end()) { + [[unlikely]] return false; + } + if (!ref->CheckCircular(stack)) { + [[unlikely]] return false; + } + } + stack.pop_back(); + return true; +} + +std::string StructDescriptor::CalculateOffsets( + wpi::SmallVectorImpl& stack) { + size_t offset = 0; + unsigned int shift = 0; + size_t prevBitfieldSize = 0; + for (auto&& field : m_fields) { + if (!field.IsBitField()) { + [[likely]] shift = 0; // reset shift on non-bitfield element + offset += prevBitfieldSize; // finish bitfield if active + prevBitfieldSize = 0; // previous is now not bitfield + field.m_offset = offset; + if (field.m_struct) { + if (!field.m_struct->IsValid()) { + m_valid = false; + [[unlikely]] return {}; + } + field.m_size = field.m_struct->m_size; + } + offset += field.m_size * field.m_arraySize; + } else { + if (field.m_type == StructFieldType::kBool && prevBitfieldSize != 0 && + (shift + 1) <= (prevBitfieldSize * 8)) { + // bool takes on size of preceding bitfield type (if it fits) + field.m_size = prevBitfieldSize; + } else if (field.m_size != prevBitfieldSize || + (shift + field.m_bitWidth) > (field.m_size * 8)) { + shift = 0; + offset += prevBitfieldSize; + } + prevBitfieldSize = field.m_size; + field.m_offset = offset; + field.m_bitShift = shift; + shift += field.m_bitWidth; + } + } + + // update struct size + m_size = offset + prevBitfieldSize; + m_valid = true; + + // now that we're valid, referring types may be too + stack.emplace_back(this); + for (auto&& ref : m_references) { + if (std::find(stack.begin(), stack.end(), ref) != stack.end()) { + [[unlikely]] return fmt::format( + "internal error (inconsistent data): circular struct reference " + "between {} and {}", + m_name, ref->m_name); + } + auto err = ref->CalculateOffsets(stack); + if (!err.empty()) { + [[unlikely]] return err; + } + } + stack.pop_back(); + return {}; +} + +const StructDescriptor* StructDescriptorDatabase::Add(std::string_view name, + std::string_view schema, + std::string* err) { + structparser::Parser parser{schema}; + structparser::ParsedSchema parsed; + if (!parser.Parse(&parsed)) { + *err = fmt::format("parse error: {}", parser.GetError()); + [[unlikely]] return nullptr; + } + + // turn parsed schema into descriptors + auto& theStruct = m_structs[name]; + if (!theStruct) { + theStruct = std::make_unique( + name, StructDescriptor::private_init{}); + } + theStruct->m_schema = schema; + theStruct->m_fields.clear(); + theStruct->m_fields.reserve(parsed.declarations.size()); + bool isValid = true; + for (auto&& decl : parsed.declarations) { + auto type = TypeStringToType(decl.typeString); + size_t size = TypeToSize(type); + + // bitfield checks + if (decl.bitWidth != 0) { + // only integer or boolean types are allowed + if (type == StructFieldType::kChar || type == StructFieldType::kFloat || + type == StructFieldType::kDouble || + type == StructFieldType::kStruct) { + *err = fmt::format("field {}: type {} cannot be bitfield", decl.name, + decl.typeString); + [[unlikely]] return nullptr; + } + + // bit width cannot be larger than field size + if (decl.bitWidth > (size * 8)) { + *err = fmt::format("field {}: bit width {} exceeds type size", + decl.name, decl.bitWidth); + [[unlikely]] return nullptr; + } + + // bit width must be 1 for booleans + if (type == StructFieldType::kBool && decl.bitWidth != 1) { + *err = fmt::format("field {}: bit width must be 1 for bool type", + decl.name); + [[unlikely]] return nullptr; + } + + // cannot combine array and bitfield (shouldn't parse, but double-check) + if (decl.arraySize > 1) { + *err = fmt::format("field {}: cannot combine array and bitfield", + decl.name); + [[unlikely]] return nullptr; + } + } + + // struct handling + const StructDescriptor* structDesc = nullptr; + if (type == StructFieldType::kStruct) { + // recursive definitions are not allowed + if (decl.typeString == name) { + *err = fmt::format("field {}: recursive struct reference", decl.name); + [[unlikely]] return nullptr; + } + + // cross-reference struct, creating a placeholder if necessary + auto& aStruct = m_structs[decl.typeString]; + if (!aStruct) { + aStruct = std::make_unique( + decl.typeString, StructDescriptor::private_init{}); + } + + // if the struct isn't valid, we can't be valid either + if (aStruct->IsValid()) { + size = aStruct->GetSize(); + } else { + isValid = false; + } + + // add to cross-references for when the struct does become valid + aStruct->m_references.emplace_back(theStruct.get()); + structDesc = aStruct.get(); + } + + // create field + if (!theStruct->m_fieldsByName + .insert({decl.name, theStruct->m_fields.size()}) + .second) { + *err = fmt::format("duplicate field {}", decl.name); + [[unlikely]] return nullptr; + } + + theStruct->m_fields.emplace_back(theStruct.get(), decl.name, type, size, + decl.arraySize, decl.bitWidth, + std::move(decl.enumValues), structDesc, + StructFieldDescriptor::private_init{}); + } + + theStruct->m_valid = isValid; + if (isValid) { + // we have all the info needed, so calculate field offset & shift + wpi::SmallVector stack; + auto err2 = theStruct->CalculateOffsets(stack); + if (!err2.empty()) { + *err = std::move(err2); + [[unlikely]] return nullptr; + } + } else { + // check for circular reference + wpi::SmallVector stack; + if (!theStruct->CheckCircular(stack)) { + wpi::SmallString<128> buf; + wpi::raw_svector_ostream os{buf}; + for (auto&& elem : stack) { + if (!buf.empty()) { + os << " <- "; + } + os << elem->GetName(); + } + *err = fmt::format("circular struct reference: {}", os.str()); + [[unlikely]] return nullptr; + } + } + + return theStruct.get(); +} + +const StructDescriptor* StructDescriptorDatabase::Find( + std::string_view name) const { + auto it = m_structs.find(name); + if (it == m_structs.end()) { + return nullptr; + } + return it->second.get(); +} + +uint64_t DynamicStruct::GetFieldImpl(const StructFieldDescriptor* field, + size_t arrIndex) const { + assert(field->m_parent == m_desc); + assert(m_desc->IsValid()); + assert(arrIndex < field->m_arraySize); + uint64_t val; + switch (field->m_size) { + case 1: + val = m_data[field->m_offset + arrIndex]; + break; + case 2: + val = support::endian::read16le(&m_data[field->m_offset + arrIndex * 2]); + break; + case 4: + val = support::endian::read32le(&m_data[field->m_offset + arrIndex * 4]); + break; + case 8: + val = support::endian::read64le(&m_data[field->m_offset + arrIndex * 8]); + break; + default: + assert(false && "invalid field size"); + return 0; + } + return (val >> field->m_bitShift) & field->m_bitMask; +} + +void MutableDynamicStruct::SetData(std::span data) { + assert(data.size() >= m_desc->GetSize()); + std::copy(data.begin(), data.begin() + m_desc->GetSize(), m_data.begin()); +} + +void MutableDynamicStruct::SetStringField(const StructFieldDescriptor* field, + std::string_view value) { + assert(field->m_type == StructFieldType::kChar); + assert(field->m_parent == m_desc); + assert(m_desc->IsValid()); + size_t len = (std::min)(field->m_arraySize, value.size()); + std::copy(value.begin(), value.begin() + len, + reinterpret_cast(&m_data[field->m_offset])); + std::fill(&m_data[field->m_offset + len], + &m_data[field->m_offset + field->m_arraySize], 0); +} + +void MutableDynamicStruct::SetStructField(const StructFieldDescriptor* field, + const DynamicStruct& value, + size_t arrIndex) { + assert(field->m_type == StructFieldType::kStruct); + assert(field->m_parent == m_desc); + assert(m_desc->IsValid()); + assert(value.GetDescriptor() == field->m_struct); + assert(value.GetDescriptor()->IsValid()); + assert(arrIndex < field->m_arraySize); + auto source = value.GetData(); + size_t len = field->m_struct->GetSize(); + std::copy(source.begin(), source.begin() + len, + m_data.begin() + field->m_offset + arrIndex * len); +} + +void MutableDynamicStruct::SetFieldImpl(const StructFieldDescriptor* field, + uint64_t value, size_t arrIndex) { + assert(field->m_parent == m_desc); + assert(m_desc->IsValid()); + assert(arrIndex < field->m_arraySize); + + // common case is no bit shift and no masking + if (!field->IsBitField()) { + switch (field->m_size) { + case 1: + m_data[field->m_offset + arrIndex] = value; + break; + case 2: + support::endian::write16le(&m_data[field->m_offset + arrIndex * 2], + value); + break; + case 4: + support::endian::write32le(&m_data[field->m_offset + arrIndex * 4], + value); + break; + case 8: + support::endian::write64le(&m_data[field->m_offset + arrIndex * 8], + value); + break; + default: + assert(false && "invalid field size"); + } + return; + } + + // handle bit shifting and masking into current value + switch (field->m_size) { + case 1: { + uint8_t* data = &m_data[field->m_offset + arrIndex]; + *data &= ~(field->m_bitMask << field->m_bitShift); + *data |= (value & field->m_bitMask) << field->m_bitShift; + break; + } + case 2: { + uint8_t* data = &m_data[field->m_offset + arrIndex * 2]; + uint16_t val = support::endian::read16le(data); + val &= ~(field->m_bitMask << field->m_bitShift); + val |= (value & field->m_bitMask) << field->m_bitShift; + support::endian::write16le(data, val); + break; + } + case 4: { + uint8_t* data = &m_data[field->m_offset + arrIndex * 4]; + uint32_t val = support::endian::read32le(data); + val &= ~(field->m_bitMask << field->m_bitShift); + val |= (value & field->m_bitMask) << field->m_bitShift; + support::endian::write32le(data, val); + break; + } + case 8: { + uint8_t* data = &m_data[field->m_offset + arrIndex * 8]; + uint64_t val = support::endian::read64le(data); + val &= ~(field->m_bitMask << field->m_bitShift); + val |= (value & field->m_bitMask) << field->m_bitShift; + support::endian::write64le(data, val); + break; + } + default: + assert(false && "invalid field size"); + } +} diff --git a/wpiutil/src/main/native/cpp/struct/SchemaParser.cpp b/wpiutil/src/main/native/cpp/struct/SchemaParser.cpp new file mode 100644 index 0000000000..347019e9d3 --- /dev/null +++ b/wpiutil/src/main/native/cpp/struct/SchemaParser.cpp @@ -0,0 +1,238 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/struct/SchemaParser.h" + +#include + +#include "wpi/StringExtras.h" + +using namespace wpi::structparser; + +std::string_view wpi::structparser::ToString(Token::Kind kind) { + switch (kind) { + case Token::kInteger: + return "integer"; + case Token::kIdentifier: + return "identifier"; + case Token::kLeftBracket: + return "'['"; + case Token::kRightBracket: + return "']'"; + case Token::kLeftBrace: + return "'{'"; + case Token::kRightBrace: + return "'}'"; + case Token::kColon: + return "':'"; + case Token::kSemicolon: + return "';'"; + case Token::kComma: + return "','"; + case Token::kEquals: + return "'='"; + case Token::kEndOfInput: + return ""; + default: + return "unknown"; + } +} + +Token Lexer::Scan() { + // skip whitespace + do { + Get(); + } while (m_current == ' ' || m_current == '\t' || m_current == '\n' || + m_current == '\r'); + m_tokenStart = m_pos - 1; + + switch (m_current) { + case '[': + return MakeToken(Token::kLeftBracket); + case ']': + return MakeToken(Token::kRightBracket); + case '{': + return MakeToken(Token::kLeftBrace); + case '}': + return MakeToken(Token::kRightBrace); + case ':': + return MakeToken(Token::kColon); + case ';': + return MakeToken(Token::kSemicolon); + case ',': + return MakeToken(Token::kComma); + case '=': + return MakeToken(Token::kEquals); + case '-': + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + return ScanInteger(); + case -1: + return {Token::kEndOfInput, {}}; + default: + if (isAlpha(m_current) || m_current == '_') { + [[likely]] return ScanIdentifier(); + } + return MakeToken(Token::kUnknown); + } +} + +Token Lexer::ScanInteger() { + do { + Get(); + } while (isDigit(m_current)); + Unget(); + return MakeToken(Token::kInteger); +} + +Token Lexer::ScanIdentifier() { + do { + Get(); + } while (isAlnum(m_current) || m_current == '_'); + Unget(); + return MakeToken(Token::kIdentifier); +} + +void Parser::FailExpect(Token::Kind desired) { + Fail(fmt::format("expected {}, got '{}'", ToString(desired), m_token.text)); +} + +void Parser::Fail(std::string_view msg) { + m_error = fmt::format("{}: {}", m_lexer.GetPosition(), msg); +} + +bool Parser::Parse(ParsedSchema* out) { + do { + GetNextToken(); + if (m_token.Is(Token::kSemicolon)) { + continue; + } + if (m_token.Is(Token::kEndOfInput)) { + break; + } + if (!ParseDeclaration(&out->declarations.emplace_back())) { + [[unlikely]] return false; + } + } while (m_token.kind != Token::kEndOfInput); + return true; +} + +bool Parser::ParseDeclaration(ParsedDeclaration* out) { + // optional enum specification + if (m_token.Is(Token::kIdentifier) && m_token.text == "enum") { + GetNextToken(); + if (!Expect(Token::kLeftBrace)) { + [[unlikely]] return false; + } + if (!ParseEnum(&out->enumValues)) { + [[unlikely]] return false; + } + GetNextToken(); + } else if (m_token.Is(Token::kLeftBrace)) { + if (!ParseEnum(&out->enumValues)) { + [[unlikely]] return false; + } + GetNextToken(); + } + + // type name + if (!Expect(Token::kIdentifier)) { + [[unlikely]] return false; + } + out->typeString = m_token.text; + GetNextToken(); + + // identifier name + if (!Expect(Token::kIdentifier)) { + [[unlikely]] return false; + } + out->name = m_token.text; + GetNextToken(); + + // array or bit field + if (m_token.Is(Token::kLeftBracket)) { + GetNextToken(); + if (!Expect(Token::kInteger)) { + [[unlikely]] return false; + } + auto val = parse_integer(m_token.text, 10); + if (val && *val > 0) { + out->arraySize = *val; + } else { + Fail(fmt::format("array size '{}' is not a positive integer", + m_token.text)); + [[unlikely]] return false; + } + GetNextToken(); + if (!Expect(Token::kRightBracket)) { + [[unlikely]] return false; + } + GetNextToken(); + } else if (m_token.Is(Token::kColon)) { + GetNextToken(); + if (!Expect(Token::kInteger)) { + [[unlikely]] return false; + } + auto val = parse_integer(m_token.text, 10); + if (val && *val > 0) { + out->bitWidth = *val; + } else { + Fail(fmt::format("bitfield width '{}' is not a positive integer", + m_token.text)); + [[unlikely]] return false; + } + GetNextToken(); + } + + // declaration must end with EOF or semicolon + if (m_token.Is(Token::kEndOfInput)) { + return true; + } + return Expect(Token::kSemicolon); +} + +bool Parser::ParseEnum(EnumValues* out) { + // we start with current = '{' + GetNextToken(); + while (!m_token.Is(Token::kRightBrace)) { + if (!Expect(Token::kIdentifier)) { + [[unlikely]] return false; + } + std::string name; + name = m_token.text; + GetNextToken(); + if (!Expect(Token::kEquals)) { + [[unlikely]] return false; + } + GetNextToken(); + if (!Expect(Token::kInteger)) { + [[unlikely]] return false; + } + int64_t value; + if (auto val = parse_integer(m_token.text, 10)) { + value = *val; + } else { + Fail(fmt::format("could not parse enum value '{}'", m_token.text)); + [[unlikely]] return false; + } + out->emplace_back(std::move(name), value); + GetNextToken(); + if (m_token.Is(Token::kRightBrace)) { + break; + } + if (!Expect(Token::kComma)) { + [[unlikely]] return false; + } + GetNextToken(); + } + return true; +} diff --git a/wpiutil/src/main/native/include/wpi/DataLog.h b/wpiutil/src/main/native/include/wpi/DataLog.h index 5a3a76feb8..9e53bdcfa6 100644 --- a/wpiutil/src/main/native/include/wpi/DataLog.h +++ b/wpiutil/src/main/native/include/wpi/DataLog.h @@ -7,19 +7,27 @@ #include #ifdef __cplusplus +#include #include #include #include +#include #include #include #include #include +#include #include +#include #include "wpi/DenseMap.h" +#include "wpi/SmallVector.h" #include "wpi/StringMap.h" #include "wpi/condition_variable.h" #include "wpi/mutex.h" +#include "wpi/protobuf/Protobuf.h" +#include "wpi/struct/Struct.h" +#include "wpi/timestamp.h" #endif // __cplusplus /** @@ -170,6 +178,95 @@ class DataLog final { */ void Resume(); + /** + * Returns whether there is a data schema already registered with the given + * name. + * + * @param name Name (the string passed as the data type for records using this + * schema) + * @return True if schema already registered + */ + bool HasSchema(std::string_view name) const; + + /** + * Registers a data schema. Data schemas provide information for how a + * certain data type string can be decoded. The type string of a data schema + * indicates the type of the schema itself (e.g. "protobuf" for protobuf + * schemas, "struct" for struct schemas, etc). In the data log, schemas are + * saved just like normal records, with the name being generated from the + * provided name: "/.schema/". Duplicate calls to this function with + * the same name are silently ignored. + * + * @param name Name (the string passed as the data type for records using this + * schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + * @param timestamp Time stamp (may be 0 to indicate now) + */ + void AddSchema(std::string_view name, std::string_view type, + std::span schema, int64_t timestamp = 0); + + /** + * Registers a data schema. Data schemas provide information for how a + * certain data type string can be decoded. The type string of a data schema + * indicates the type of the schema itself (e.g. "protobuf" for protobuf + * schemas, "struct" for struct schemas, etc). In the data log, schemas are + * saved just like normal records, with the name being generated from the + * provided name: "/.schema/". Duplicate calls to this function with + * the same name are silently ignored. + * + * @param name Name (the string passed as the data type for records using this + * schema) + * @param type Type of schema (e.g. "protobuf", "struct", etc) + * @param schema Schema data + * @param timestamp Time stamp (may be 0 to indicate now) + */ + void AddSchema(std::string_view name, std::string_view type, + std::string_view schema, int64_t timestamp = 0) { + AddSchema( + name, type, + std::span{ + reinterpret_cast(schema.data()), schema.size()}, + timestamp); + } + + /** + * Registers a protobuf schema. Duplicate calls to this function with the same + * name are silently ignored. + * + * @tparam T protobuf serializable type + * @param msg protobuf message + * @param timestamp Time stamp (0 to indicate now) + */ + template + void AddProtobufSchema(ProtobufMessage& msg, int64_t timestamp = 0) { + if (timestamp == 0) { + timestamp = Now(); + } + msg.ForEachProtobufDescriptor( + [this](auto typeString) { return HasSchema(typeString); }, + [this, timestamp](auto typeString, auto schema) { + AddSchema(typeString, "proto:FileDescriptorProto", schema, timestamp); + }); + } + + /** + * Registers a struct schema. Duplicate calls to this function with the same + * name are silently ignored. + * + * @tparam T struct serializable type + * @param timestamp Time stamp (0 to indicate now) + */ + template + void AddStructSchema(int64_t timestamp = 0) { + if (timestamp == 0) { + timestamp = Now(); + } + ForEachStructSchema([this, timestamp](auto typeString, auto schema) { + AddSchema(typeString, "structschema", schema, timestamp); + }); + } + /** * Start an entry. Duplicate names are allowed (with the same type), and * result in the same index being returned (Start/Finish are reference @@ -364,6 +461,8 @@ class DataLog final { std::function data)> write); // must be called with m_mutex held + int StartImpl(std::string_view name, std::string_view type, + std::string_view metadata, int64_t timestamp); uint8_t* StartRecord(uint32_t entry, uint64_t timestamp, uint32_t payloadSize, size_t reserveSize); uint8_t* Reserve(size_t size); @@ -821,6 +920,128 @@ class StringArrayLogEntry : public DataLogEntry { } }; +/** + * Log raw struct serializable objects. + */ +template +class StructLogEntry : public DataLogEntry { + using S = Struct; + + public: + StructLogEntry() = default; + StructLogEntry(DataLog& log, std::string_view name, int64_t timestamp = 0) + : StructLogEntry{log, name, {}, timestamp} {} + StructLogEntry(DataLog& log, std::string_view name, std::string_view metadata, + int64_t timestamp = 0) { + m_log = &log; + log.AddStructSchema(timestamp); + m_entry = log.Start(name, S::kTypeString, metadata, timestamp); + } + + /** + * Appends a record to the log. + * + * @param data Data to record + * @param timestamp Time stamp (may be 0 to indicate now) + */ + void Append(const T& data, int64_t timestamp = 0) { + uint8_t buf[S::kSize]; + S::Pack(buf, data); + m_log->AppendRaw(m_entry, buf, timestamp); + } +}; + +/** + * Log raw struct serializable array of objects. + */ +template +class StructArrayLogEntry : public DataLogEntry { + using S = Struct; + + public: + StructArrayLogEntry() = default; + StructArrayLogEntry(DataLog& log, std::string_view name, + int64_t timestamp = 0) + : StructArrayLogEntry{log, name, {}, timestamp} {} + StructArrayLogEntry(DataLog& log, std::string_view name, + std::string_view metadata, int64_t timestamp = 0) { + m_log = &log; + log.AddStructSchema(timestamp); + m_entry = + log.Start(name, MakeStructArrayTypeString(), + metadata, timestamp); + } + + /** + * Appends a record to the log. + * + * @param data Data to record + * @param timestamp Time stamp (may be 0 to indicate now) + */ + template +#if __cpp_lib_ranges >= 201911L + requires std::ranges::range && + std::convertible_to, T> +#endif + void Append(U&& data, int64_t timestamp = 0) { + m_buf.Write(std::forward(data), [&](auto bytes) { + m_log->AppendRaw(m_entry, bytes, timestamp); + }); + } + + /** + * Appends a record to the log. + * + * @param data Data to record + * @param timestamp Time stamp (may be 0 to indicate now) + */ + void Append(std::span data, int64_t timestamp = 0) { + m_buf.Write( + data, [&](auto bytes) { m_log->AppendRaw(m_entry, bytes, timestamp); }); + } + + private: + StructArrayBuffer m_buf; +}; + +/** + * Log protobuf serializable objects. + */ +template +class ProtobufLogEntry : public DataLogEntry { + using P = Protobuf; + + public: + ProtobufLogEntry() = default; + ProtobufLogEntry(DataLog& log, std::string_view name, int64_t timestamp = 0) + : ProtobufLogEntry{log, name, {}, timestamp} {} + ProtobufLogEntry(DataLog& log, std::string_view name, + std::string_view metadata, int64_t timestamp = 0) { + m_log = &log; + log.AddProtobufSchema(m_msg, timestamp); + m_entry = log.Start(name, m_msg.GetTypeString(), metadata, timestamp); + } + + /** + * Appends a record to the log. + * + * @param data Data to record + * @param timestamp Time stamp (may be 0 to indicate now) + */ + void Append(const T& data, int64_t timestamp = 0) { + SmallVector buf; + { + std::scoped_lock lock{m_mutex}; + m_msg.Pack(buf, data); + } + m_log->AppendRaw(m_entry, buf, timestamp); + } + + private: + wpi::mutex m_mutex; + ProtobufMessage m_msg; +}; + } // namespace wpi::log extern "C" { diff --git a/wpiutil/src/main/native/include/wpi/protobuf/Protobuf.h b/wpiutil/src/main/native/include/wpi/protobuf/Protobuf.h new file mode 100644 index 0000000000..4eedcca297 --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/protobuf/Protobuf.h @@ -0,0 +1,260 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "wpi/function_ref.h" + +namespace google::protobuf { +class Arena; +class Message; +} // namespace google::protobuf + +namespace wpi { + +template +class SmallVectorImpl; + +/** + * Protobuf serialization template. Unspecialized class has no members; only + * specializations of this class are useful, and only if they meet the + * ProtobufSerializable concept. + * + * @tparam T type to serialize/deserialize + */ +template +struct Protobuf {}; + +/** + * Specifies that a type is capable of protobuf serialization and + * deserialization. + * + * This is designed for serializing complex flexible data structures using + * code generated from a .proto file. Serialization consists of writing + * values into a mutable protobuf Message and deserialization consists of + * reading values from an immutable protobuf Message. + * + * Implementations must define a template specialization for wpi::Protobuf with + * T being the type that is being serialized/deserialized, with the following + * static members (as enforced by this concept): + * - google::protobuf::Message* New(google::protobuf::Arena*): create a protobuf + * message + * - T Unpack(const google::protobuf::Message&): function for deserialization + * - void Pack(google::protobuf::Message*, T&& value): function for + * serialization + * + * To avoid pulling in the protobuf headers, these functions use + * google::protobuf::Message instead of a more specific type; implementations + * will need to static_cast to the correct type as created by New(). + * + * Additionally: In a static block, call StructRegistry.registerClass() to + * register the class + */ +template +concept ProtobufSerializable = requires( + google::protobuf::Arena* arena, const google::protobuf::Message& inmsg, + google::protobuf::Message* outmsg, const T& value) { + typename Protobuf>; + { + Protobuf>::New(arena) + } -> std::same_as; + { + Protobuf>::Unpack(inmsg) + } -> std::same_as>; + Protobuf>::Pack(outmsg, value); +}; + +/** + * Specifies that a type is capable of in-place protobuf deserialization. + * + * In addition to meeting ProtobufSerializable, implementations must define a + * wpi::Protobuf static member + * `void UnpackInto(T*, const google::protobuf::Message&)` to update the + * pointed-to T with the contents of the message. + */ +template +concept MutableProtobufSerializable = + ProtobufSerializable && + requires(T* out, const google::protobuf::Message& msg) { + Protobuf>::UnpackInto(out, msg); + }; + +/** + * Unpack a serialized protobuf message. + * + * @tparam T object type + * @param msg protobuf message + * @return Deserialized object + */ +template +inline T UnpackProtobuf(const google::protobuf::Message& msg) { + return Protobuf::Unpack(msg); +} + +/** + * Pack a serialized protobuf message. + * + * @param msg protobuf message (mutable, output) + * @param value object + */ +template +inline void PackProtobuf(google::protobuf::Message* msg, const T& value) { + Protobuf>::Pack(msg, value); +} + +/** + * Unpack a serialized struct into an existing object, overwriting its contents. + * + * @param out object (output) + * @param msg protobuf message + */ +template +inline void UnpackProtobufInto(T* out, const google::protobuf::Message& msg) { + if constexpr (MutableProtobufSerializable) { + Protobuf::UnpackInto(out, msg); + } else { + *out = UnpackProtobuf(msg); + } +} + +// these detail functions avoid the need to include protobuf headers +namespace detail { +void DeleteProtobuf(google::protobuf::Message* msg); +bool ParseProtobuf(google::protobuf::Message* msg, + std::span data); +bool SerializeProtobuf(wpi::SmallVectorImpl& out, + const google::protobuf::Message& msg); +bool SerializeProtobuf(std::vector& out, + const google::protobuf::Message& msg); +std::string GetTypeString(const google::protobuf::Message& msg); +void ForEachProtobufDescriptor( + const google::protobuf::Message& msg, + function_ref wants, + function_ref descriptor)> + fn); +} // namespace detail + +/** + * Owning wrapper (ala std::unique_ptr) for google::protobuf::Message* that does + * not require the protobuf headers be included. Note this object is not thread + * safe; users of this object are required to provide any necessary thread + * safety. + * + * @tparam T serialized object type + */ +template +class ProtobufMessage { + public: + explicit ProtobufMessage(google::protobuf::Arena* arena = nullptr) + : m_msg{Protobuf::New(arena)} {} + ~ProtobufMessage() { detail::DeleteProtobuf(m_msg); } + ProtobufMessage(const ProtobufMessage&) = delete; + ProtobufMessage& operator=(const ProtobufMessage&) = delete; + ProtobufMessage(ProtobufMessage&& rhs) : m_msg{rhs.m_msg} { + rhs.m_msg = nullptr; + } + ProtobufMessage& operator=(ProtobufMessage&& rhs) { + std::swap(m_msg, rhs.m_msg); + return *this; + } + + /** + * Gets the stored message object. + * + * @return google::protobuf::Message* + */ + google::protobuf::Message* GetMessage() { return m_msg; } + const google::protobuf::Message* GetMessage() const { return m_msg; } + + /** + * Unpacks from a byte array. + * + * @param data byte array + * @return Optional; empty if parsing failed + */ + std::optional Unpack(std::span data) { + if (!detail::ParseProtobuf(m_msg, data)) { + return std::nullopt; + } + return Protobuf::Unpack(*m_msg); + } + + /** + * Unpacks from a byte array into an existing object. + * + * @param[out] out output object + * @param[in] data byte array + * @return true if successful + */ + bool UnpackInto(T* out, std::span data) { + if (!detail::ParseProtobuf(m_msg, data)) { + return false; + } + UnpackProtobufInto(out, *m_msg); + return true; + } + + /** + * Packs object into a SmallVector. + * + * @param[out] out output bytes + * @param[in] value value + * @return true if successful + */ + bool Pack(wpi::SmallVectorImpl& out, const T& value) { + Protobuf::Pack(m_msg, value); + return detail::SerializeProtobuf(out, *m_msg); + } + + /** + * Packs object into a std::vector. + * + * @param[out] out output bytes + * @param[in] value value + * @return true if successful + */ + bool Pack(std::vector& out, const T& value) { + Protobuf::Pack(m_msg, value); + return detail::SerializeProtobuf(out, *m_msg); + } + + /** + * Gets the type string for the message. + * + * @return type string + */ + std::string GetTypeString() const { return detail::GetTypeString(*m_msg); } + + /** + * Loops over all protobuf descriptors including nested/referenced + * descriptors. + * + * @param exists function that returns false if fn should be called for the + * given type string + * @param fn function to call for each descriptor + */ + void ForEachProtobufDescriptor( + function_ref exists, + function_ref descriptor)> + fn) { + detail::ForEachProtobufDescriptor(*m_msg, exists, fn); + } + + private: + google::protobuf::Message* m_msg = nullptr; +}; + +} // namespace wpi diff --git a/wpiutil/src/main/native/include/wpi/protobuf/ProtobufMessageDatabase.h b/wpiutil/src/main/native/include/wpi/protobuf/ProtobufMessageDatabase.h new file mode 100644 index 0000000000..f2c6882e26 --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/protobuf/ProtobufMessageDatabase.h @@ -0,0 +1,76 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "wpi/StringMap.h" + +namespace wpi { + +/** + * Database of protobuf dynamic messages. Unlike the protobuf descriptor pools + * and databases, this gracefully handles adding descriptors out of order and + * descriptors being replaced. + */ +class ProtobufMessageDatabase { + public: + /** + * Adds a file descriptor to the database. If the file references other + * files that have not yet been added, no messages in that file will be + * available until those files are added. Replaces any existing file with + * the same name. + * + * @param filename filename + * @param data FileDescriptorProto serialized data + */ + void Add(std::string_view filename, std::span data); + + /** + * Finds a message in the database by name. + * + * @param name type name + * @return protobuf message, or nullptr if not found + */ + google::protobuf::Message* Find(std::string_view name) const; + + /** + * Gets message factory. + * + * @return message factory + */ + google::protobuf::MessageFactory* GetMessageFactory() { + return m_factory.get(); + } + + private: + struct ProtoFile { + std::unique_ptr proto; + std::vector uses; + bool complete = false; + bool inPool = false; + }; + + void Build(std::string_view filename, ProtoFile& file); + bool Rebuild(ProtoFile& file); + + std::unique_ptr m_pool = + std::make_unique(); + std::unique_ptr m_factory = + std::make_unique(); + wpi::StringMap m_files; // indexed by filename + // indexed by type string + mutable wpi::StringMap> m_msgs; +}; + +} // namespace wpi diff --git a/wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h b/wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h new file mode 100644 index 0000000000..b88894c09d --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/struct/DynamicStruct.h @@ -0,0 +1,682 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "wpi/MathExtras.h" +#include "wpi/StringMap.h" +#include "wpi/bit.h" + +namespace wpi { + +template +class SmallVectorImpl; + +class DynamicStruct; +class MutableDynamicStruct; +class StructDescriptor; +class StructDescriptorDatabase; + +/** + * Known data types for raw struct dynamic fields (see StructFieldDescriptor). + */ +enum class StructFieldType { + kBool, + kChar, + kInt8, + kInt16, + kInt32, + kInt64, + kUint8, + kUint16, + kUint32, + kUint64, + kFloat, + kDouble, + kStruct +}; + +/** + * Raw struct dynamic field descriptor. + */ +class StructFieldDescriptor { + struct private_init {}; + friend class DynamicStruct; + friend class MutableDynamicStruct; + friend class StructDescriptor; + friend class StructDescriptorDatabase; + + public: + /** + * Set of enumerated values. + */ + using EnumValues = std::vector>; + + StructFieldDescriptor(const StructDescriptor* parent, std::string_view name, + StructFieldType type, size_t size, size_t arraySize, + unsigned int bitWidth, EnumValues enumValues, + const StructDescriptor* structDesc, + const private_init&); + + /** + * Gets the dynamic struct this field is contained in. + * + * @return struct descriptor + */ + const StructDescriptor* GetParent() const { return m_parent; } + + /** + * Gets the field name. + * + * @return field name + */ + const std::string& GetName() const { return m_name; } + + /** + * Gets the field type. + * + * @return field type + */ + StructFieldType GetType() const { return m_type; } + + /** + * Returns whether the field type is a signed integer. + * + * @return true if signed integer, false otherwise + */ + bool IsInt() const { + return m_type == StructFieldType::kInt8 || + m_type == StructFieldType::kInt16 || + m_type == StructFieldType::kInt32 || + m_type == StructFieldType::kInt64; + } + + /** + * Returns whether the field type is an unsigned integer. + * + * @return true if unsigned integer, false otherwise + */ + bool IsUint() const { + return m_type == StructFieldType::kUint8 || + m_type == StructFieldType::kUint16 || + m_type == StructFieldType::kUint32 || + m_type == StructFieldType::kUint64; + } + + /** + * Gets the underlying storage size of the field, in bytes. + * + * @return number of bytes + */ + size_t GetSize() const { return m_size; } + + /** + * Gets the storage offset of the field, in bytes. + * + * @return number of bytes from the start of the struct + */ + size_t GetOffset() const { return m_offset; } + + /** + * Gets the bit width of the field, in bits. + * + * @return number of bits + */ + unsigned int GetBitWidth() const { + return m_bitWidth == 0 ? m_size * 8 : m_bitWidth; + } + + /** + * Gets the bit mask for the field. The mask is always the least significant + * bits (it is not shifted). + * + * @return bit mask + */ + uint64_t GetBitMask() const { return m_bitMask; } + + /** + * Gets the bit shift for the field (LSB=0). + * + * @return number of bits + */ + unsigned int GetBitShift() const { return m_bitShift; } + + /** + * Returns whether the field is an array. + * + * @return true if array + */ + bool IsArray() const { return m_arraySize > 1; } + + /** + * Gets the array size. Returns 1 if non-array. + * + * @return number of elements + */ + size_t GetArraySize() const { return m_arraySize; } + + /** + * Returns whether the field has enumerated values. + * + * @return true if there are enumerated values + */ + bool HasEnum() const { return !m_enum.empty(); } + + /** + * Gets the enumerated values. + * + * @return set of enumerated values + */ + const EnumValues& GetEnumValues() { return m_enum; } + + /** + * Gets the struct descriptor for a struct data type. + * + * @return struct descriptor; returns null for non-struct + */ + const StructDescriptor* GetStruct() const { return m_struct; } + + /** + * Gets the minimum unsigned integer value that can be stored in this field. + * + * @return minimum value + */ + uint64_t GetUintMin() const { return 0; } + + /** + * Gets the maximum unsigned integer value that can be stored in this field. + * + * @return maximum value + */ + uint64_t GetUintMax() const { return m_bitMask; } + + /** + * Gets the minimum signed integer value that can be stored in this field. + * + * @return minimum value + */ + int64_t GetIntMin() const { + return static_cast(-(m_bitMask >> 1)) - 1; + } + + /** + * Gets the maximum signed integer value that can be stored in this field. + * + * @return maximum value + */ + int64_t GetIntMax() const { return m_bitMask >> 1; } + + /** + * Returns whether the field is a bitfield. + * + * @return true if bitfield + */ + bool IsBitField() const { + return m_bitShift != 0 || m_bitWidth != (m_size * 8); + } + + private: + // note: constructor fills in everything except offset and shift + const StructDescriptor* m_parent; + std::string m_name; + size_t m_size; + size_t m_offset = 0; + size_t m_arraySize; // 1 for non-arrays + EnumValues m_enum; + const StructDescriptor* m_struct; // nullptr for non-structs + uint64_t m_bitMask; + StructFieldType m_type; + unsigned int m_bitWidth; + unsigned int m_bitShift = 0; +}; + +/** + * Raw struct dynamic struct descriptor. + */ +class StructDescriptor { + struct private_init {}; + friend class StructDescriptorDatabase; + + public: + StructDescriptor(std::string_view name, const private_init&) : m_name{name} {} + + /** + * Gets the struct name. + * + * @return name + */ + const std::string& GetName() const { return m_name; } + + /** + * Gets the struct schema. + * + * @return schema + */ + const std::string& GetSchema() const { return m_schema; } + + /** + * Returns whether the struct is valid (e.g. the struct is fully defined and + * field offsets computed). + * + * @return true if valid + */ + bool IsValid() const { return m_valid; } + + /** + * Returns the struct size, in bytes. Not valid unless IsValid() is true. + * + * @return size in bytes + */ + size_t GetSize() const { + assert(m_valid); + return m_size; + } + + /** + * Gets a field descriptor by name. Note the field cannot be accessed until + * the struct is valid. + * + * @param name field name + * @return field descriptor, or nullptr if not found + */ + const StructFieldDescriptor* FindFieldByName(std::string_view name) const; + + /** + * Gets all field descriptors. Note fields cannot be accessed until the struct + * is valid. + * + * @return field descriptors + */ + const std::vector& GetFields() const { + return m_fields; + } + + private: + bool CheckCircular( + wpi::SmallVectorImpl& stack) const; + std::string CalculateOffsets( + wpi::SmallVectorImpl& stack); + + std::string m_name; + std::string m_schema; + std::vector m_references; + std::vector m_fields; + StringMap m_fieldsByName; + size_t m_size = 0; + bool m_valid = false; +}; + +/** + * Database of raw struct dynamic descriptors. + */ +class StructDescriptorDatabase { + public: + /** + * Adds a structure schema to the database. If the struct references other + * structs that have not yet been added, it will not be valid until those + * structs are also added. + * + * @param[in] name structure name + * @param[in] schema structure schema + * @param[out] err detailed error, if nullptr is returned + * @return Added struct, or nullptr on error + */ + const StructDescriptor* Add(std::string_view name, std::string_view schema, + std::string* err); + + /** + * Finds a structure in the database by name. + * + * @param name structure name + * @return struct descriptor, or nullptr if not found + */ + const StructDescriptor* Find(std::string_view name) const; + + private: + StringMap> m_structs; +}; + +/** + * Dynamic (run-time) read-only access to a serialized raw struct. + */ +class DynamicStruct { + public: + /** + * Constructs a new dynamic struct. Note: the passed data is a span; no copy + * is made, so it's necessary for the lifetime of the referenced data to be + * longer than this object. + * + * @param desc struct descriptor + * @param data serialized data + */ + DynamicStruct(const StructDescriptor* desc, std::span data) + : m_desc{desc}, m_data{data} {} + + /** + * Gets the struct descriptor. + * + * @return struct descriptor + */ + const StructDescriptor* GetDescriptor() const { return m_desc; } + + /** + * Gets the serialized data. + * + * @return data + */ + std::span GetData() const { return m_data; } + + /** + * Gets a struct field descriptor by name. + * + * @param name field name + * @return field descriptor, or nullptr if no field with that name exists + */ + const StructFieldDescriptor* FindField(std::string_view name) const { + return m_desc->FindFieldByName(name); + } + + /** + * Gets the value of a boolean field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return field value + */ + bool GetBoolField(const StructFieldDescriptor* field, + size_t arrIndex = 0) const { + assert(field->m_type == StructFieldType::kBool); + return GetFieldImpl(field, arrIndex); + } + + /** + * Gets the value of a signed integer field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return field value + */ + int64_t GetIntField(const StructFieldDescriptor* field, + size_t arrIndex = 0) const { + assert(field->IsInt()); + return GetFieldImpl(field, arrIndex); + } + + /** + * Gets the value of an unsigned integer field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return field value + */ + uint64_t GetUintField(const StructFieldDescriptor* field, + size_t arrIndex = 0) const { + assert(field->IsUint()); + return GetFieldImpl(field, arrIndex); + } + + /** + * Gets the value of a float field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return field value + */ + float GetFloatField(const StructFieldDescriptor* field, + size_t arrIndex = 0) const { + assert(field->m_type == StructFieldType::kFloat); + return bit_cast( + static_cast(GetFieldImpl(field, arrIndex))); + } + + /** + * Gets the value of a double field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return field value + */ + double GetDoubleField(const StructFieldDescriptor* field, + size_t arrIndex = 0) const { + assert(field->m_type == StructFieldType::kDouble); + return bit_cast(GetFieldImpl(field, arrIndex)); + } + + /** + * Gets the value of a char or char array field. + * + * @param field field descriptor + * @return field value + */ + std::string_view GetStringField(const StructFieldDescriptor* field) const { + assert(field->m_type == StructFieldType::kChar); + assert(field->m_parent == m_desc); + assert(m_desc->IsValid()); + return {reinterpret_cast(&m_data[field->m_offset]), + field->m_arraySize}; + } + + /** + * Gets the value of a struct field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return field value + */ + DynamicStruct GetStructField(const StructFieldDescriptor* field, + size_t arrIndex = 0) const { + assert(field->m_type == StructFieldType::kStruct); + assert(field->m_parent == m_desc); + assert(m_desc->IsValid()); + assert(arrIndex < field->m_arraySize); + return DynamicStruct{field->m_struct, + m_data.subspan(field->m_offset + + arrIndex * field->m_struct->GetSize())}; + } + + protected: + const StructDescriptor* m_desc; + + private: + uint64_t GetFieldImpl(const StructFieldDescriptor* field, + size_t arrIndex) const; + + std::span m_data; +}; + +/** + * Dynamic (run-time) mutable access to a serialized raw struct. + */ +class MutableDynamicStruct : public DynamicStruct { + public: + /** + * Constructs a new dynamic struct. Note: the passed data is a span; no copy + * is made, so it's necessary for the lifetime of the referenced data to be + * longer than this object. + * + * @param desc struct descriptor + * @param data serialized data + */ + MutableDynamicStruct(const StructDescriptor* desc, std::span data) + : DynamicStruct{desc, data}, m_data{data} {} + + /** + * Gets the serialized data. + * + * @return data + */ + std::span GetData() { return m_data; } + + using DynamicStruct::GetData; + + /** + * Overwrites the entire serialized struct by copying data from a span. + * + * @param data replacement data for the struct + */ + void SetData(std::span data); + + /** + * Sets the value of a boolean field. + * + * @param field field descriptor + * @param value field value + * @param arrIndex array index (must be less than field array size) + */ + void SetBoolField(const StructFieldDescriptor* field, bool value, + size_t arrIndex = 0) { + assert(field->m_type == StructFieldType::kBool); + SetFieldImpl(field, value ? 1 : 0, arrIndex); + } + + /** + * Sets the value of a signed integer field. + * + * @param field field descriptor + * @param value field value + * @param arrIndex array index (must be less than field array size) + */ + void SetIntField(const StructFieldDescriptor* field, int64_t value, + size_t arrIndex = 0) { + assert(field->IsInt()); + SetFieldImpl(field, value, arrIndex); + } + + /** + * Sets the value of an unsigned integer field. + * + * @param field field descriptor + * @param value field value + * @param arrIndex array index (must be less than field array size) + */ + void SetUintField(const StructFieldDescriptor* field, uint64_t value, + size_t arrIndex = 0) { + assert(field->IsUint()); + SetFieldImpl(field, value, arrIndex); + } + + /** + * Sets the value of a float field. + * + * @param field field descriptor + * @param value field value + * @param arrIndex array index (must be less than field array size) + */ + void SetFloatField(const StructFieldDescriptor* field, float value, + size_t arrIndex = 0) { + assert(field->m_type == StructFieldType::kFloat); + SetFieldImpl(field, bit_cast(value), arrIndex); + } + + /** + * Sets the value of a double field. + * + * @param field field descriptor + * @param value field value + * @param arrIndex array index (must be less than field array size) + */ + void SetDoubleField(const StructFieldDescriptor* field, double value, + size_t arrIndex = 0) { + assert(field->m_type == StructFieldType::kDouble); + SetFieldImpl(field, bit_cast(value), arrIndex); + } + + /** + * Sets the value of a char or char array field. + * + * @param field field descriptor + * @param value field value + */ + void SetStringField(const StructFieldDescriptor* field, + std::string_view value); + + /** + * Sets the value of a struct field. + * + * @param field field descriptor + * @param value field value + * @param arrIndex array index (must be less than field array size) + */ + void SetStructField(const StructFieldDescriptor* field, + const DynamicStruct& value, size_t arrIndex = 0); + + /** + * Gets the value of a struct field. + * + * @param field field descriptor + * @param arrIndex array index (must be less than field array size) + * @return field value + */ + MutableDynamicStruct GetStructField(const StructFieldDescriptor* field, + size_t arrIndex = 0) { + assert(field->m_type == StructFieldType::kStruct); + assert(field->m_parent == m_desc); + assert(m_desc->IsValid()); + assert(arrIndex < field->m_arraySize); + return MutableDynamicStruct{ + field->m_struct, m_data.subspan(field->m_offset + + arrIndex * field->m_struct->GetSize())}; + } + + using DynamicStruct::GetStructField; + + private: + void SetFieldImpl(const StructFieldDescriptor* field, uint64_t value, + size_t arrIndex); + + std::span m_data; +}; + +namespace impl { +struct DSOData { + explicit DSOData(size_t size) : m_dataStore(size) {} + explicit DSOData(std::span data) + : m_dataStore{data.begin(), data.end()} {} + + std::vector m_dataStore; +}; +} // namespace impl + +/** + * Dynamic (run-time) mutable access to a serialized raw struct, with internal + * data storage. + */ +class DynamicStructObject : private impl::DSOData, public MutableDynamicStruct { + /** + * Constructs a new dynamic struct object. The descriptor must be valid. + * + * @param desc struct descriptor + */ + explicit DynamicStructObject(const StructDescriptor* desc) + : DSOData{desc->GetSize()}, MutableDynamicStruct{desc, m_dataStore} {} + + /** + * Constructs a new dynamic struct object. Makes a copy of the serialized + * data so there are no lifetime constraints on the data parameter. + * + * @param desc struct descriptor + * @param data serialized data + */ + DynamicStructObject(const StructDescriptor* desc, + std::span data) + : DSOData{data}, MutableDynamicStruct{desc, m_dataStore} { + assert(data.size() >= desc->GetSize()); + } + + // can't be movable due to span references + DynamicStructObject(DynamicStructObject&&) = delete; + DynamicStructObject& operator=(DynamicStructObject&&) = delete; +}; + +} // namespace wpi diff --git a/wpiutil/src/main/native/include/wpi/struct/SchemaParser.h b/wpiutil/src/main/native/include/wpi/struct/SchemaParser.h new file mode 100644 index 0000000000..a8cf1a403f --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/struct/SchemaParser.h @@ -0,0 +1,186 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include +#include + +namespace wpi::structparser { + +/** + * A lexed raw struct schema token. + */ +struct Token { + enum Kind { + kUnknown, + kInteger, + kIdentifier, + kLeftBracket, + kRightBracket, + kLeftBrace, + kRightBrace, + kColon, + kSemicolon, + kComma, + kEquals, + kEndOfInput, + }; + + Token() = default; + Token(Kind kind, std::string_view text) : kind{kind}, text{text} {} + + bool Is(Kind k) const { return kind == k; } + + Kind kind = kUnknown; + std::string_view text; +}; + +std::string_view ToString(Token::Kind kind); + +/** + * Raw struct schema lexer. + */ +class Lexer { + public: + /** + * Construct a raw struct schema lexer. + * + * @param in schema + */ + explicit Lexer(std::string_view in) : m_in{in} {} + + /** + * Gets the next token. + * + * @return Token + */ + [[nodiscard]] + Token Scan(); + + /** + * Gets the starting position of the last lexed token. + * + * @return position (0 = first character) + */ + size_t GetPosition() const { return m_tokenStart; } + + private: + Token ScanInteger(); + Token ScanIdentifier(); + + void Get() { + if (m_pos < m_in.size()) { + [[likely]] m_current = m_in[m_pos]; + } else { + m_current = -1; + } + ++m_pos; + } + + void Unget() { + if (m_pos > 0) { + [[likely]] m_pos--; + if (m_pos < m_in.size()) { + [[likely]] m_current = m_in[m_pos]; + } else { + m_current = -1; + } + } else { + m_current = -1; + } + } + + Token MakeToken(Token::Kind kind) { + return {kind, m_in.substr(m_tokenStart, m_pos - m_tokenStart)}; + } + + std::string_view m_in; + int m_current = -1; + size_t m_tokenStart = 0; + size_t m_pos = 0; +}; + +/** + * Raw struct set of enumerated values. + */ +using EnumValues = std::vector>; + +/** + * Raw struct schema declaration. + */ +struct ParsedDeclaration { + std::string typeString; + std::string name; + EnumValues enumValues; + size_t arraySize = 1; + unsigned int bitWidth = 0; +}; + +/** + * Raw struct schema. + */ +struct ParsedSchema { + std::vector declarations; +}; + +/** + * Raw struct schema parser. + */ +class Parser { + public: + /** + * Construct a raw struct schema parser. + * + * @param in schema + */ + explicit Parser(std::string_view in) : m_lexer{in} {} + + /** + * Parses the schema. + * + * @param[out] out parsed schema object + * @return true on success, false on failure (use GetError() to get error) + */ + [[nodiscard]] + bool Parse(ParsedSchema* out); + + /** + * Gets the parser error if one occurred. + * + * @return parser error; blank if no error occurred + */ + const std::string& GetError() const { return m_error; } + + private: + [[nodiscard]] + bool ParseDeclaration(ParsedDeclaration* out); + [[nodiscard]] + bool ParseEnum(EnumValues* out); + + Token::Kind GetNextToken() { + m_token = m_lexer.Scan(); + return m_token.kind; + } + [[nodiscard]] + bool Expect(Token::Kind kind) { + if (m_token.Is(kind)) { + [[likely]] return true; + } + FailExpect(kind); + return false; + } + void FailExpect(Token::Kind desired); + void Fail(std::string_view msg); + + Lexer m_lexer; + Token m_token; + std::string m_error; +}; + +} // namespace wpi::structparser diff --git a/wpiutil/src/main/native/include/wpi/struct/Struct.h b/wpiutil/src/main/native/include/wpi/struct/Struct.h new file mode 100644 index 0000000000..1336707973 --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/struct/Struct.h @@ -0,0 +1,533 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "wpi/Endian.h" +#include "wpi/MathExtras.h" +#include "wpi/bit.h" +#include "wpi/ct_string.h" +#include "wpi/function_ref.h" +#include "wpi/mutex.h" + +namespace wpi { + +/** + * Struct serialization template. Unspecialized class has no members; only + * specializations of this class are useful, and only if they meet the + * StructSerializable concept. + * + * @tparam T type to serialize/deserialize + */ +template +struct Struct {}; + +/** + * Specifies that a type is capable of raw struct serialization and + * deserialization. + * + * This is designed for serializing small fixed-size data structures in the + * fastest and most compact means possible. Serialization consists of writing + * values into a mutable std::span and deserialization consists of reading + * values from an immutable std::span. + * + * Implementations must define a template specialization for wpi::Struct with + * T being the type that is being serialized/deserialized, with the following + * static members (as enforced by this concept): + * - std::string_view kTypeString: the type string + * - size_t kSize: the structure size in bytes + * - std::string_view kSchema: the struct schema + * - T Unpack(std::span): function for deserialization + * - void Pack(std::span, T&& value): function for + * serialization + * + * If the struct has nested structs, implementations should also meet the + * requirements of HasNestedStruct. + */ +template +concept StructSerializable = + requires(std::span in, std::span out, T&& value) { + typename Struct>; + { + Struct>::kTypeString + } -> std::convertible_to; + { + Struct>::kSize + } -> std::convertible_to; + { + Struct>::kSchema + } -> std::convertible_to; + { + Struct>::Unpack( + in.subspan<0, Struct>::kSize>()) + } -> std::same_as>; + Struct>::Pack( + out.subspan<0, Struct>::kSize>(), + std::forward(value)); + }; + +/** + * Specifies that a type is capable of in-place raw struct deserialization. + * + * In addition to meeting StructSerializable, implementations must define a + * wpi::Struct static member `void UnpackInto(T*, std::span)` + * to update the pointed-to T with the contents of the span. + */ +template +concept MutableStructSerializable = + StructSerializable && requires(T* out, std::span in) { + Struct>::UnpackInto( + out, in.subspan<0, Struct>::kSize>()); + }; + +/** + * Specifies that a struct type has nested struct declarations. + * + * In addition to meeting StructSerializable, implementations must define a + * wpi::Struct static member + * `void ForEachNested(std::invocable on each nested struct + * type. + */ +template +concept HasNestedStruct = + StructSerializable && + requires(function_ref fn) { + Struct>::ForEachNested(fn); + }; + +/** + * Unpack a serialized struct. + * + * @tparam T object type + * @param data raw struct data + * @return Deserialized object + */ +template +inline T UnpackStruct(std::span::kSize> data) { + return Struct::Unpack(data); +} + +/** + * Unpack a serialized struct starting at a given offset within the data. + * This is primarily useful in unpack implementations to unpack nested structs. + * + * @tparam T object type + * @tparam Offset starting offset + * @param data raw struct data + * @return Deserialized object + */ +template +inline T UnpackStruct(std::span data) { + return Struct::Unpack(data.template subspan::kSize>()); +} + +/** + * Pack a serialized struct. + * + * @param data struct storage (mutable, output) + * @param value object + */ +template +inline void PackStruct( + std::span>::kSize> data, + T&& value) { + Struct>::Pack(data, std::forward(value)); +} + +/** + * Pack a serialized struct starting at a given offset within the data. This is + * primarily useful in pack implementations to pack nested structs. + * + * @tparam Offset starting offset + * @param data struct storage (mutable, output) + * @param value object + */ +template +inline void PackStruct(std::span data, T&& value) { + Struct>::Pack( + data.template subspan>::kSize>(), + std::forward(value)); +} + +/** + * Unpack a serialized struct into an existing object, overwriting its contents. + * + * @param out object (output) + * @param data raw struct data + */ +template +inline void UnpackStructInto(T* out, + std::span::kSize> data) { + if constexpr (MutableStructSerializable) { + Struct::UnpackInto(out, data); + } else { + *out = UnpackStruct(data); + } +} + +/** + * Unpack a serialized struct into an existing object, overwriting its contents, + * and starting at a given offset within the data. + * This is primarily useful in unpack implementations to unpack nested structs. + * + * @tparam Offset starting offset + * @param out object (output) + * @param data raw struct data + */ +template +inline void UnpackStructInto(T* out, std::span data) { + if constexpr (MutableStructSerializable) { + Struct::UnpackInto(out, + data.template subspan::kSize>()); + } else { + *out = UnpackStruct(data); + } +} + +/** + * Get the type string for a raw struct serializable type + * + * @tparam T serializable type + * @return type string + */ +template +constexpr auto GetStructTypeString() { + return Struct::kTypeString; +} + +template +consteval auto MakeStructArrayTypeString() { + using namespace literals; + if constexpr (N == std::dynamic_extent) { + return Concat( + ct_string, Struct::kTypeString.size()>{ + Struct::kTypeString}, + "[]"_ct_string); + } else { + return Concat( + ct_string, Struct::kTypeString.size()>{ + Struct::kTypeString}, + "["_ct_string, NumToCtString(), "]"_ct_string); + } +} + +template +consteval auto MakeStructArraySchema() { + using namespace literals; + if constexpr (N == std::dynamic_extent) { + return Concat( + ct_string, Struct::kSchema.size()>{ + Struct::kSchema}, + "[]"_ct_string); + } else { + return Concat( + ct_string, Struct::kSchema.size()>{ + Struct::kSchema}, + "["_ct_string, NumToCtString(), "]"_ct_string); + } +} + +template +constexpr std::string_view GetStructSchema() { + return Struct::kSchema; +} + +template +constexpr std::span GetStructSchemaBytes() { + return {reinterpret_cast(Struct::kSchema.data()), + Struct::kSchema.size()}; +} + +template +void ForEachStructSchema( + std::invocable auto fn) { + if constexpr (HasNestedStruct) { + Struct::ForEachNested(fn); + } + fn(Struct::kTypeString, Struct::kSchema); +} + +template +class StructArrayBuffer { + using S = Struct; + + public: + StructArrayBuffer() = default; + StructArrayBuffer(const StructArrayBuffer&) = delete; + StructArrayBuffer& operator=(const StructArrayBuffer&) = delete; + StructArrayBuffer(StructArrayBuffer&& rhs) : m_buf{std::move(rhs.m_buf)} {} + StructArrayBuffer& operator=(StructArrayBuffer&& rhs) { + m_buf = std::move(rhs.m_buf); + return *this; + } + + template + requires +#if __cpp_lib_ranges >= 201911L + std::ranges::range && + std::convertible_to, T> && +#endif + std::invocable> + void Write(U&& data, F&& func) { + if ((std::size(data) * S::kSize) < 256) { + // use the stack + uint8_t buf[256]; + auto out = buf; + for (auto&& val : data) { + S::Pack(std::span{out, out + S::kSize}, + std::forward(val)); + out += S::kSize; + } + func(std::span{buf, out}); + } else { + std::scoped_lock lock{m_mutex}; + m_buf.resize(std::size(data) * S::kSize); + auto out = m_buf.begin(); + for (auto&& val : data) { + S::Pack(std::span{out, out + S::kSize}, + std::forward(val)); + out += S::kSize; + } + func(m_buf); + } + } + + private: + wpi::mutex m_mutex; + std::vector m_buf; +}; + +/** + * Raw struct support for fixed-size arrays of other structs. + */ +template +struct Struct> { + static constexpr auto kTypeString = MakeStructArrayTypeString(); + static constexpr size_t kSize = N * Struct::kSize; + static constexpr auto kSchema = MakeStructArraySchema(); + static std::array Unpack(std::span data) { + std::array result; + for (size_t i = 0; i < N; ++i) { + result[i] = UnpackStruct(data); + data = data.subspan(Struct::kSize); + } + return result; + } + static void Pack(std::span data, + std::span values) { + std::span unsizedData = data; + for (auto&& val : values) { + PackStruct<0>(unsizedData, val); + unsizedData = unsizedData.subspan(Struct::kSize); + } + } + static void UnpackInto(std::array* out, + std::span data) { + UnpackInto(std::span{*out}, data); + } + // alternate span-based function + static void UnpackInto(std::span out, + std::span data) { + std::span unsizedData = data; + for (size_t i = 0; i < N; ++i) { + UnpackStructInto<0, T>(&out[i], unsizedData); + unsizedData = unsizedData.subspan(Struct::kSize); + } + } +}; + +/** + * Raw struct support for boolean values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view kTypeString = "struct:bool"; + static constexpr size_t kSize = 1; + static constexpr std::string_view kSchema = "bool value"; + static bool Unpack(std::span data) { return data[0]; } + static void Pack(std::span data, bool value) { + data[0] = static_cast(value ? 1 : 0); + } +}; + +/** + * Raw struct support for uint8_t values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view kTypeString = "struct:uint8"; + static constexpr size_t kSize = 1; + static constexpr std::string_view kSchema = "uint8 value"; + static uint8_t Unpack(std::span data) { return data[0]; } + static void Pack(std::span data, uint8_t value) { + data[0] = value; + } +}; + +/** + * Raw struct support for int8_t values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view kTypeString = "struct:int8"; + static constexpr size_t kSize = 1; + static constexpr std::string_view kSchema = "int8 value"; + static int8_t Unpack(std::span data) { return data[0]; } + static void Pack(std::span data, int8_t value) { + data[0] = value; + } +}; + +/** + * Raw struct support for uint16_t values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view kTypeString = "struct:uint16"; + static constexpr size_t kSize = 2; + static constexpr std::string_view kSchema = "uint16 value"; + static uint16_t Unpack(std::span data) { + return support::endian::read16le(data.data()); + } + static void Pack(std::span data, uint16_t value) { + support::endian::write16le(data.data(), value); + } +}; + +/** + * Raw struct support for int16_t values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view kTypeString = "struct:int16"; + static constexpr size_t kSize = 2; + static constexpr std::string_view kSchema = "int16 value"; + static int16_t Unpack(std::span data) { + return support::endian::read16le(data.data()); + } + static void Pack(std::span data, int16_t value) { + support::endian::write16le(data.data(), value); + } +}; + +/** + * Raw struct support for uint32_t values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view kTypeString = "struct:uint32"; + static constexpr size_t kSize = 4; + static constexpr std::string_view kSchema = "uint32 value"; + static uint32_t Unpack(std::span data) { + return support::endian::read32le(data.data()); + } + static void Pack(std::span data, uint32_t value) { + support::endian::write32le(data.data(), value); + } +}; + +/** + * Raw struct support for int32_t values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view kTypeString = "struct:int32"; + static constexpr size_t kSize = 4; + static constexpr std::string_view kSchema = "int32 value"; + static int32_t Unpack(std::span data) { + return support::endian::read32le(data.data()); + } + static void Pack(std::span data, int32_t value) { + support::endian::write32le(data.data(), value); + } +}; + +/** + * Raw struct support for uint64_t values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view kTypeString = "struct:uint64"; + static constexpr size_t kSize = 8; + static constexpr std::string_view kSchema = "uint64 value"; + static uint64_t Unpack(std::span data) { + return support::endian::read64le(data.data()); + } + static void Pack(std::span data, uint64_t value) { + support::endian::write64le(data.data(), value); + } +}; + +/** + * Raw struct support for int64_t values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view kTypeString = "struct:int64"; + static constexpr size_t kSize = 8; + static constexpr std::string_view kSchema = "int64 value"; + static int64_t Unpack(std::span data) { + return support::endian::read64le(data.data()); + } + static void Pack(std::span data, int64_t value) { + support::endian::write64le(data.data(), value); + } +}; + +/** + * Raw struct support for float values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view kTypeString = "struct:float"; + static constexpr size_t kSize = 4; + static constexpr std::string_view kSchema = "float value"; + static float Unpack(std::span data) { + return bit_cast(support::endian::read32le(data.data())); + } + static void Pack(std::span data, float value) { + support::endian::write32le(data.data(), bit_cast(value)); + } +}; + +/** + * Raw struct support for double values. + * Primarily useful for higher level struct implementations. + */ +template <> +struct Struct { + static constexpr std::string_view kTypeString = "struct:double"; + static constexpr size_t kSize = 8; + static constexpr std::string_view kSchema = "double value"; + static double Unpack(std::span data) { + return bit_cast(support::endian::read64le(data.data())); + } + static void Pack(std::span data, double value) { + support::endian::write64le(data.data(), bit_cast(value)); + } +}; + +} // namespace wpi diff --git a/wpiutil/src/test/java/edu/wpi/first/util/struct/DynamicStructTest.java b/wpiutil/src/test/java/edu/wpi/first/util/struct/DynamicStructTest.java new file mode 100644 index 0000000000..9e77c80aee --- /dev/null +++ b/wpiutil/src/test/java/edu/wpi/first/util/struct/DynamicStructTest.java @@ -0,0 +1,390 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.stream.Stream; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.Arguments; +import org.junit.jupiter.params.provider.MethodSource; + +class DynamicStructTest { + @SuppressWarnings("MemberName") + private StructDescriptorDatabase db; + + @BeforeEach + public void init() { + db = new StructDescriptorDatabase(); + } + + @Test + void testEmpty() { + var desc = assertDoesNotThrow(() -> db.add("test", "")); + assertEquals(desc.getName(), "test"); + assertEquals(desc.getSchema(), ""); + assertTrue(desc.getFields().isEmpty()); + assertTrue(desc.isValid()); + assertEquals(desc.getSize(), 0); + } + + @Test + void testNestedStruct() { + var desc = assertDoesNotThrow(() -> db.add("test", "int32 a")); + assertTrue(desc.isValid()); + var desc2 = assertDoesNotThrow(() -> db.add("test2", "test a")); + assertTrue(desc2.isValid()); + assertEquals(desc2.getSize(), 4); + } + + @Test + void testDelayedValid() { + var desc = assertDoesNotThrow(() -> db.add("test", "foo a")); + assertFalse(desc.isValid()); + var desc2 = assertDoesNotThrow(() -> db.add("test2", "foo a[2]")); + assertFalse(desc2.isValid()); + var desc3 = assertDoesNotThrow(() -> db.add("foo", "int32 a")); + assertTrue(desc3.isValid()); + assertTrue(desc.isValid()); + assertEquals(desc.getSize(), 4); + assertTrue(desc2.isValid()); + assertEquals(desc2.getSize(), 8); + } + + @Test + void testInvalidBitfield() { + assertThrows( + BadSchemaException.class, + () -> db.add("test", "float a:1"), + "field a: type float cannot be bitfield"); + assertThrows( + BadSchemaException.class, + () -> db.add("test", "double a:1"), + "field a: type double cannot be bitfield"); + assertThrows( + BadSchemaException.class, + () -> db.add("test", "foo a:1"), + "field a: type foo cannot be bitfield"); + } + + @Test + void testCircularStructReference() { + assertThrows( + BadSchemaException.class, + () -> db.add("test", "test a"), + "field a: recursive struct reference"); + } + + @Test + void testNestedCircularStructRef() { + assertDoesNotThrow(() -> db.add("test", "foo a")); + assertDoesNotThrow(() -> db.add("foo", "bar a")); + assertThrows( + BadSchemaException.class, + () -> db.add("bar", "test a"), + "circular struct reference: bar <- foo <- test"); + + // ok + var desc = assertDoesNotThrow(() -> db.add("baz", "bar a")); + assertFalse(desc.isValid()); + } + + @Test + void testNestedCircularStructRef2() { + assertDoesNotThrow(() -> db.add("test", "foo a")); + assertDoesNotThrow(() -> db.add("bar", "test a")); + assertThrows( + BadSchemaException.class, + () -> db.add("foo", "bar a"), + "circular struct reference: foo <- test <- bar"); + } + + @Test + void testBitfieldBasic() { + var desc = assertDoesNotThrow(() -> db.add("test", "int32 a:2; uint32 b:30")); + assertEquals(desc.getSize(), 4); + var fields = desc.getFields(); + assertEquals(fields.size(), 2); + var field = fields.get(0); + assertEquals(field.getBitWidth(), 2); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getBitMask(), 0x3); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 4); + field = fields.get(1); + assertEquals(field.getBitWidth(), 30); + assertEquals(field.getBitShift(), 2); + assertEquals(field.getBitMask(), 0x3fffffff); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 4); + } + + @Test + void testBitfieldDiffType() { + var desc = assertDoesNotThrow(() -> db.add("test", "int32 a:2; int16 b:2")); + assertEquals(desc.getSize(), 6); + var fields = desc.getFields(); + assertEquals(fields.size(), 2); + var field = fields.get(0); + assertEquals(field.getBitWidth(), 2); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getBitMask(), 0x3); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 4); + field = fields.get(1); + assertEquals(field.getBitWidth(), 2); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getBitMask(), 0x3); + assertEquals(field.getOffset(), 4); + assertEquals(field.getSize(), 2); + } + + @Test + void testBitfieldOverflow() { + var desc = assertDoesNotThrow(() -> db.add("test", "int8 a:4; int8 b:5")); + assertEquals(desc.getSize(), 2); + var fields = desc.getFields(); + assertEquals(fields.size(), 2); + var field = fields.get(0); + assertEquals(field.getBitWidth(), 4); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getBitMask(), 0xf); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 1); + field = fields.get(1); + assertEquals(field.getBitWidth(), 5); + assertEquals(field.getBitMask(), 0x1f); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getOffset(), 1); + assertEquals(field.getSize(), 1); + } + + @Test + void testBitfieldBoolBegin8() { + var desc = assertDoesNotThrow(() -> db.add("test", "bool a:1; int8 b:5")); + assertEquals(desc.getSize(), 1); + var fields = desc.getFields(); + assertEquals(fields.size(), 2); + var field = fields.get(0); + assertEquals(field.getBitWidth(), 1); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getBitMask(), 0x1); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 1); + field = fields.get(1); + assertEquals(field.getBitWidth(), 5); + assertEquals(field.getBitMask(), 0x1f); + assertEquals(field.getBitShift(), 1); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 1); + } + + @Test + void testBitfieldBoolBegin16() { + var desc = assertDoesNotThrow(() -> db.add("test", "bool a:1; int16 b:5")); + assertEquals(desc.getSize(), 3); + var fields = desc.getFields(); + assertEquals(fields.size(), 2); + var field = fields.get(0); + assertEquals(field.getBitWidth(), 1); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getBitMask(), 0x1); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 1); + field = fields.get(1); + assertEquals(field.getBitWidth(), 5); + assertEquals(field.getBitMask(), 0x1f); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getOffset(), 1); + assertEquals(field.getSize(), 2); + } + + @Test + void testBitfieldBoolMid() { + var desc = + assertDoesNotThrow(() -> db.add("test", "int16 a:2; bool b:1; bool c:1; uint16 d:5")); + assertEquals(desc.getSize(), 2); + var fields = desc.getFields(); + assertEquals(fields.size(), 4); + var field = fields.get(0); + assertEquals(field.getBitWidth(), 2); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getBitMask(), 0x3); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 2); + field = fields.get(1); + assertEquals(field.getBitWidth(), 1); + assertEquals(field.getBitMask(), 0x1); + assertEquals(field.getBitShift(), 2); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 2); + field = fields.get(2); + assertEquals(field.getBitWidth(), 1); + assertEquals(field.getBitMask(), 0x1); + assertEquals(field.getBitShift(), 3); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 2); + field = fields.get(3); + assertEquals(field.getBitWidth(), 5); + assertEquals(field.getBitMask(), 0x1f); + assertEquals(field.getBitShift(), 4); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 2); + } + + @Test + void testBitfieldBoolEnd() { + var desc = assertDoesNotThrow(() -> db.add("test", "int16 a:15; bool b:1")); + assertEquals(desc.getSize(), 2); + var fields = desc.getFields(); + assertEquals(fields.size(), 2); + var field = fields.get(0); + assertEquals(field.getBitWidth(), 15); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getBitMask(), 0x7fff); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 2); + field = fields.get(1); + assertEquals(field.getBitWidth(), 1); + assertEquals(field.getBitMask(), 0x1); + assertEquals(field.getBitShift(), 15); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 2); + } + + @Test + void testBitfieldBoolEnd2() { + var desc = assertDoesNotThrow(() -> db.add("test", "int16 a:16; bool b:1")); + assertEquals(desc.getSize(), 3); + var fields = desc.getFields(); + assertEquals(fields.size(), 2); + var field = fields.get(0); + assertEquals(field.getBitWidth(), 16); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getBitMask(), 0xffff); + assertEquals(field.getOffset(), 0); + assertEquals(field.getSize(), 2); + field = fields.get(1); + assertEquals(field.getBitWidth(), 1); + assertEquals(field.getBitMask(), 0x1); + assertEquals(field.getBitShift(), 0); + assertEquals(field.getOffset(), 2); + assertEquals(field.getSize(), 1); + } + + @Test + void testBitfieldBoolWrongSize() { + assertThrows( + BadSchemaException.class, + () -> db.add("test", "bool a:2"), + "field a: bit width must be 1 for bool type"); + } + + @Test + void testBitfieldTooBig() { + assertThrows( + BadSchemaException.class, + () -> db.add("test", "int16 a:17"), + "field a: bit width 17 exceeds type size"); + } + + @Test + void testDuplicateFieldName() { + assertThrows( + BadSchemaException.class, + () -> db.add("test", "int16 a; int8 a"), + "field a: duplicate field name"); + } + + private static Stream provideSimpleTestParams() { + return Stream.of( + Arguments.of("bool a", 1, StructFieldType.kBool, false, false, 8, 0xff), + Arguments.of("char a", 1, StructFieldType.kChar, false, false, 8, 0xff), + Arguments.of("int8 a", 1, StructFieldType.kInt8, true, false, 8, 0xff), + Arguments.of("int16 a", 2, StructFieldType.kInt16, true, false, 16, 0xffff), + Arguments.of("int32 a", 4, StructFieldType.kInt32, true, false, 32, 0xffffffffL), + Arguments.of("int64 a", 8, StructFieldType.kInt64, true, false, 64, -1), + Arguments.of("uint8 a", 1, StructFieldType.kUint8, false, true, 8, 0xff), + Arguments.of("uint16 a", 2, StructFieldType.kUint16, false, true, 16, 0xffff), + Arguments.of("uint32 a", 4, StructFieldType.kUint32, false, true, 32, 0xffffffffL), + Arguments.of("uint64 a", 8, StructFieldType.kUint64, false, true, 64, -1), + Arguments.of("float a", 4, StructFieldType.kFloat, false, false, 32, 0xffffffffL), + Arguments.of("float32 a", 4, StructFieldType.kFloat, false, false, 32, 0xffffffffL), + Arguments.of("double a", 8, StructFieldType.kDouble, false, false, 64, -1), + Arguments.of("float64 a", 8, StructFieldType.kDouble, false, false, 64, -1), + Arguments.of("foo a", 0, StructFieldType.kStruct, false, false, 0, 0)); + } + + @ParameterizedTest + @MethodSource("provideSimpleTestParams") + void testStandardCheck( + String schema, + int size, + StructFieldType type, + boolean isInt, + boolean isUint, + int bitWidth, + long bitMask) { + var desc = assertDoesNotThrow(() -> db.add("test", schema)); + assertEquals(desc.getName(), "test"); + assertEquals(desc.getSchema(), schema); + var fields = desc.getFields(); + assertEquals(fields.size(), 1); + var field = fields.get(0); + assertEquals(field.getParent(), desc); + assertEquals(field.getName(), "a"); + assertEquals(field.isInt(), isInt); + assertEquals(field.isUint(), isUint); + assertFalse(field.isArray()); + if (type != StructFieldType.kStruct) { + assertTrue(desc.isValid()); + assertEquals(desc.getSize(), size); + assertEquals(field.getSize(), size); + assertEquals(field.getBitWidth(), bitWidth); + assertEquals(field.getBitMask(), bitMask); + } else { + assertFalse(desc.isValid()); + assertNotNull(field.getStruct()); + } + } + + @ParameterizedTest + @MethodSource("provideSimpleTestParams") + void testStandardArray( + String schema, + int size, + StructFieldType type, + boolean isInt, + boolean isUint, + int bitWidth, + long bitMask) { + var desc = assertDoesNotThrow(() -> db.add("test", schema + "[2]")); + assertEquals(desc.getName(), "test"); + assertEquals(desc.getSchema(), schema + "[2]"); + var fields = desc.getFields(); + assertEquals(fields.size(), 1); + var field = fields.get(0); + assertEquals(field.getParent(), desc); + assertEquals(field.getName(), "a"); + assertEquals(field.isInt(), isInt); + assertEquals(field.isUint(), isUint); + assertTrue(field.isArray()); + assertEquals(field.getArraySize(), 2); + if (type != StructFieldType.kStruct) { + assertTrue(desc.isValid()); + assertEquals(desc.getSize(), size * 2); + } else { + assertFalse(desc.isValid()); + assertNotNull(field.getStruct()); + } + } +} diff --git a/wpiutil/src/test/java/edu/wpi/first/util/struct/parser/ParserTest.java b/wpiutil/src/test/java/edu/wpi/first/util/struct/parser/ParserTest.java new file mode 100644 index 0000000000..52ba8dafcf --- /dev/null +++ b/wpiutil/src/test/java/edu/wpi/first/util/struct/parser/ParserTest.java @@ -0,0 +1,212 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.util.struct.parser; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.Test; + +class ParserTest { + @Test + void testEmpty() { + Parser p = new Parser(""); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertTrue(schema.declarations.isEmpty()); + } + + @Test + void testEmptySemicolon() { + Parser p = new Parser(";"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertTrue(schema.declarations.isEmpty()); + } + + @Test + void testSimple() { + Parser p = new Parser("int32 a"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 1); + var decl = schema.declarations.get(0); + assertEquals(decl.typeString, "int32"); + assertEquals(decl.name, "a"); + assertEquals(decl.arraySize, 1); + } + + @Test + void testSimpleTrailingSemi() { + Parser p = new Parser("int32 a;"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 1); + } + + @Test + void testArray() { + Parser p = new Parser("int32 a[2]"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 1); + var decl = schema.declarations.get(0); + assertEquals(decl.typeString, "int32"); + assertEquals(decl.name, "a"); + assertEquals(decl.arraySize, 2); + } + + @Test + void testArrayTrailingSemi() { + Parser p = new Parser("int32 a[2];"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 1); + } + + @Test + void testBitfield() { + Parser p = new Parser("int32 a:2"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 1); + var decl = schema.declarations.get(0); + assertEquals(decl.typeString, "int32"); + assertEquals(decl.name, "a"); + assertEquals(decl.bitWidth, 2); + } + + @Test + void testBitfieldTrailingSemi() { + Parser p = new Parser("int32 a:2;"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 1); + } + + @Test + void testEnumKeyword() { + Parser p = new Parser("enum {x=1} int32 a;"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 1); + var decl = schema.declarations.get(0); + assertEquals(decl.typeString, "int32"); + assertEquals(decl.name, "a"); + assertEquals(decl.enumValues.size(), 1); + assertEquals(decl.enumValues.get("x"), 1); + } + + @Test + void testEnumNoKeyword() { + Parser p = new Parser("{x=1} int32 a;"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 1); + var decl = schema.declarations.get(0); + assertEquals(decl.typeString, "int32"); + assertEquals(decl.name, "a"); + assertEquals(decl.enumValues.size(), 1); + assertEquals(decl.enumValues.get("x"), 1); + } + + @Test + void testEnumNoValues() { + Parser p = new Parser("{} int32 a;"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 1); + var decl = schema.declarations.get(0); + assertEquals(decl.typeString, "int32"); + assertEquals(decl.name, "a"); + assertTrue(decl.enumValues.isEmpty()); + } + + @Test + void testEnumMultipleValues() { + Parser p = new Parser("{x=1,y=-2} int32 a;"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 1); + var decl = schema.declarations.get(0); + assertEquals(decl.typeString, "int32"); + assertEquals(decl.name, "a"); + assertEquals(decl.enumValues.size(), 2); + assertEquals(decl.enumValues.get("x"), 1); + assertEquals(decl.enumValues.get("y"), -2); + } + + @Test + void testEnumTrailingComma() { + Parser p = new Parser("{x=1,y=2,} int32 a;"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 1); + var decl = schema.declarations.get(0); + assertEquals(decl.typeString, "int32"); + assertEquals(decl.name, "a"); + assertEquals(decl.enumValues.size(), 2); + assertEquals(decl.enumValues.get("x"), 1); + assertEquals(decl.enumValues.get("y"), 2); + } + + @Test + void testMultipleNoTrailingSemi() { + Parser p = new Parser("int32 a; int16 b"); + ParsedSchema schema = assertDoesNotThrow(() -> p.parse()); + assertEquals(schema.declarations.size(), 2); + assertEquals(schema.declarations.get(0).typeString, "int32"); + assertEquals(schema.declarations.get(0).name, "a"); + assertEquals(schema.declarations.get(1).typeString, "int16"); + assertEquals(schema.declarations.get(1).name, "b"); + } + + @Test + void testErrBitfieldArray() { + Parser p = new Parser("int32 a[1]:2"); + assertThrows(ParseException.class, () -> p.parse(), "10: expected ';', got ':'"); + } + + @Test + void testErrNoArrayValue() { + Parser p = new Parser("int32 a[]"); + assertThrows(ParseException.class, () -> p.parse(), "8: expected integer, got ']'"); + } + + @Test + void testErrNoBitfieldValue() { + Parser p = new Parser("int32 a:"); + assertThrows(ParseException.class, () -> p.parse(), "8: expected integer, got ''"); + } + + @Test + void testErrNoNameArray() { + Parser p = new Parser("int32 [2]"); + assertThrows(ParseException.class, () -> p.parse(), "6: expected identifier, got '['"); + } + + @Test + void testErrNoNameBitField() { + Parser p = new Parser("int32 :2"); + assertThrows(ParseException.class, () -> p.parse(), "6: expected identifier, got ':'"); + } + + @Test + void testNegativeBitField() { + Parser p = new Parser("int32 a:-1"); + assertThrows( + ParseException.class, () -> p.parse(), "8: bitfield width '-1' is not a positive integer"); + } + + @Test + void testNegativeArraySize() { + Parser p = new Parser("int32 a[-1]"); + assertThrows( + ParseException.class, () -> p.parse(), "8: array size '-1' is not a positive integer"); + } + + @Test + void testZeroBitField() { + Parser p = new Parser("int32 a:0"); + assertThrows( + ParseException.class, () -> p.parse(), "8: bitfield width '0' is not a positive integer"); + } + + @Test + void testZeroArraySize() { + Parser p = new Parser("int32 a[0]"); + assertThrows( + ParseException.class, () -> p.parse(), "8: array size '0' is not a positive integer"); + } +} diff --git a/wpiutil/src/test/native/cpp/struct/DynamicStructTest.cpp b/wpiutil/src/test/native/cpp/struct/DynamicStructTest.cpp new file mode 100644 index 0000000000..d5d21d17b6 --- /dev/null +++ b/wpiutil/src/test/native/cpp/struct/DynamicStructTest.cpp @@ -0,0 +1,358 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/struct/DynamicStruct.h" // NOLINT(build/include_order) + +#include + +#include + +using namespace wpi; + +class DynamicStructTest : public ::testing::Test { + protected: + StructDescriptorDatabase db; + std::string err; +}; + +TEST_F(DynamicStructTest, Empty) { + auto desc = db.Add("test", "", &err); + ASSERT_TRUE(desc); + ASSERT_EQ(desc->GetName(), "test"); + ASSERT_EQ(desc->GetSchema(), ""); + ASSERT_TRUE(desc->GetFields().empty()); + ASSERT_TRUE(desc->IsValid()); + ASSERT_EQ(desc->GetSize(), 0u); +} + +TEST_F(DynamicStructTest, NestedStruct) { + auto desc = db.Add("test", "int32 a", &err); + ASSERT_TRUE(desc); + ASSERT_TRUE(desc->IsValid()); + auto desc2 = db.Add("test2", "test a", &err); + ASSERT_TRUE(desc2); + ASSERT_TRUE(desc2->IsValid()); + ASSERT_EQ(desc2->GetSize(), 4u); +} + +TEST_F(DynamicStructTest, DelayedValid) { + auto desc = db.Add("test", "foo a", &err); + ASSERT_TRUE(desc); + ASSERT_FALSE(desc->IsValid()); + auto desc2 = db.Add("test2", "foo a[2]", &err); + ASSERT_TRUE(desc2); + ASSERT_FALSE(desc2->IsValid()); + auto desc3 = db.Add("foo", "int32 a", &err); + ASSERT_TRUE(desc3); + ASSERT_TRUE(desc3->IsValid()); + ASSERT_TRUE(desc->IsValid()); + ASSERT_EQ(desc->GetSize(), 4u); + ASSERT_TRUE(desc2->IsValid()); + ASSERT_EQ(desc2->GetSize(), 8u); +} + +TEST_F(DynamicStructTest, InvalidBitfield) { + auto desc = db.Add("test", "float a:1", &err); + EXPECT_FALSE(desc); + EXPECT_EQ(err, "field a: type float cannot be bitfield"); + + desc = db.Add("test", "double a:1", &err); + EXPECT_FALSE(desc); + EXPECT_EQ(err, "field a: type double cannot be bitfield"); + + desc = db.Add("test", "foo a:1", &err); + EXPECT_FALSE(desc); + EXPECT_EQ(err, "field a: type foo cannot be bitfield"); +} + +TEST_F(DynamicStructTest, CircularStructReference) { + auto desc = db.Add("test", "test a", &err); + ASSERT_FALSE(desc); + ASSERT_EQ(err, "field a: recursive struct reference"); +} + +TEST_F(DynamicStructTest, NestedCircularStructRef) { + auto desc = db.Add("test", "foo a", &err); + ASSERT_TRUE(desc); + auto desc2 = db.Add("foo", "bar a", &err); + ASSERT_TRUE(desc2); + auto desc3 = db.Add("bar", "test a", &err); + ASSERT_FALSE(desc3); + ASSERT_EQ(err, "circular struct reference: bar <- foo <- test"); + + // ok + auto desc4 = db.Add("baz", "bar a", &err); + ASSERT_TRUE(desc4); + ASSERT_FALSE(desc4->IsValid()); +} + +TEST_F(DynamicStructTest, NestedCircularStructRef2) { + auto desc = db.Add("test", "foo a", &err); + ASSERT_TRUE(desc); + auto desc2 = db.Add("bar", "test a", &err); + ASSERT_TRUE(desc2); + auto desc3 = db.Add("foo", "bar a", &err); + ASSERT_FALSE(desc3); + ASSERT_EQ(err, "circular struct reference: foo <- test <- bar"); +} + +TEST_F(DynamicStructTest, BitfieldBasic) { + auto desc = db.Add("test", "int32 a:2; uint32 b:30", &err); + ASSERT_TRUE(desc); + EXPECT_EQ(desc->GetSize(), 4u); + auto& fields = desc->GetFields(); + ASSERT_EQ(fields.size(), 2u); + EXPECT_EQ(fields[0].GetBitWidth(), 2u); + EXPECT_EQ(fields[0].GetBitShift(), 0u); + EXPECT_EQ(fields[0].GetBitMask(), 0x3u); + EXPECT_EQ(fields[0].GetOffset(), 0u); + EXPECT_EQ(fields[0].GetSize(), 4u); + EXPECT_EQ(fields[1].GetBitWidth(), 30u); + EXPECT_EQ(fields[1].GetBitShift(), 2u); + EXPECT_EQ(fields[1].GetBitMask(), 0x3fffffffu); + EXPECT_EQ(fields[1].GetOffset(), 0u); + EXPECT_EQ(fields[1].GetSize(), 4u); +} + +TEST_F(DynamicStructTest, BitfieldDiffType) { + auto desc = db.Add("test", "int32 a:2; int16 b:2", &err); + ASSERT_TRUE(desc); + EXPECT_EQ(desc->GetSize(), 6u); + auto& fields = desc->GetFields(); + ASSERT_EQ(fields.size(), 2u); + EXPECT_EQ(fields[0].GetBitWidth(), 2u); + EXPECT_EQ(fields[0].GetBitShift(), 0u); + EXPECT_EQ(fields[0].GetBitMask(), 0x3u); + EXPECT_EQ(fields[0].GetOffset(), 0u); + EXPECT_EQ(fields[0].GetSize(), 4u); + EXPECT_EQ(fields[1].GetBitWidth(), 2u); + EXPECT_EQ(fields[1].GetBitShift(), 0u); + EXPECT_EQ(fields[1].GetBitMask(), 0x3u); + EXPECT_EQ(fields[1].GetOffset(), 4u); + EXPECT_EQ(fields[1].GetSize(), 2u); +} + +TEST_F(DynamicStructTest, BitfieldOverflow) { + auto desc = db.Add("test", "int8 a:4; int8 b:5", &err); + ASSERT_TRUE(desc); + EXPECT_EQ(desc->GetSize(), 2u); + auto& fields = desc->GetFields(); + ASSERT_EQ(fields.size(), 2u); + EXPECT_EQ(fields[0].GetBitWidth(), 4u); + EXPECT_EQ(fields[0].GetBitShift(), 0u); + EXPECT_EQ(fields[0].GetBitMask(), 0xfu); + EXPECT_EQ(fields[0].GetOffset(), 0u); + EXPECT_EQ(fields[0].GetSize(), 1u); + EXPECT_EQ(fields[1].GetBitWidth(), 5u); + EXPECT_EQ(fields[1].GetBitMask(), 0x1fu); + EXPECT_EQ(fields[1].GetBitShift(), 0u); + EXPECT_EQ(fields[1].GetOffset(), 1u); + EXPECT_EQ(fields[1].GetSize(), 1u); +} + +TEST_F(DynamicStructTest, BitfieldBoolBegin8) { + auto desc = db.Add("test", "bool a:1; int8 b:5", &err); + ASSERT_TRUE(desc); + EXPECT_EQ(desc->GetSize(), 1u); + auto& fields = desc->GetFields(); + ASSERT_EQ(fields.size(), 2u); + EXPECT_EQ(fields[0].GetBitWidth(), 1u); + EXPECT_EQ(fields[0].GetBitShift(), 0u); + EXPECT_EQ(fields[0].GetBitMask(), 0x1u); + EXPECT_EQ(fields[0].GetOffset(), 0u); + EXPECT_EQ(fields[0].GetSize(), 1u); + EXPECT_EQ(fields[1].GetBitWidth(), 5u); + EXPECT_EQ(fields[1].GetBitMask(), 0x1fu); + EXPECT_EQ(fields[1].GetBitShift(), 1u); + EXPECT_EQ(fields[1].GetOffset(), 0u); + EXPECT_EQ(fields[1].GetSize(), 1u); +} + +TEST_F(DynamicStructTest, BitfieldBoolBegin16) { + auto desc = db.Add("test", "bool a:1; int16 b:5", &err); + ASSERT_TRUE(desc); + EXPECT_EQ(desc->GetSize(), 3u); + auto& fields = desc->GetFields(); + ASSERT_EQ(fields.size(), 2u); + EXPECT_EQ(fields[0].GetBitWidth(), 1u); + EXPECT_EQ(fields[0].GetBitShift(), 0u); + EXPECT_EQ(fields[0].GetBitMask(), 0x1u); + EXPECT_EQ(fields[0].GetOffset(), 0u); + EXPECT_EQ(fields[0].GetSize(), 1u); + EXPECT_EQ(fields[1].GetBitWidth(), 5u); + EXPECT_EQ(fields[1].GetBitMask(), 0x1fu); + EXPECT_EQ(fields[1].GetBitShift(), 0u); + EXPECT_EQ(fields[1].GetOffset(), 1u); + EXPECT_EQ(fields[1].GetSize(), 2u); +} + +TEST_F(DynamicStructTest, BitfieldBoolMid) { + auto desc = db.Add("test", "int16 a:2; bool b:1; bool c:1; uint16 d:5", &err); + ASSERT_TRUE(desc); + EXPECT_EQ(desc->GetSize(), 2u); + auto& fields = desc->GetFields(); + ASSERT_EQ(fields.size(), 4u); + EXPECT_EQ(fields[0].GetBitWidth(), 2u); + EXPECT_EQ(fields[0].GetBitShift(), 0u); + EXPECT_EQ(fields[0].GetBitMask(), 0x3u); + EXPECT_EQ(fields[0].GetOffset(), 0u); + EXPECT_EQ(fields[0].GetSize(), 2u); + EXPECT_EQ(fields[1].GetBitWidth(), 1u); + EXPECT_EQ(fields[1].GetBitMask(), 0x1u); + EXPECT_EQ(fields[1].GetBitShift(), 2u); + EXPECT_EQ(fields[1].GetOffset(), 0u); + EXPECT_EQ(fields[1].GetSize(), 2u); + EXPECT_EQ(fields[2].GetBitWidth(), 1u); + EXPECT_EQ(fields[2].GetBitMask(), 0x1u); + EXPECT_EQ(fields[2].GetBitShift(), 3u); + EXPECT_EQ(fields[2].GetOffset(), 0u); + EXPECT_EQ(fields[2].GetSize(), 2u); + EXPECT_EQ(fields[3].GetBitWidth(), 5u); + EXPECT_EQ(fields[3].GetBitMask(), 0x1fu); + EXPECT_EQ(fields[3].GetBitShift(), 4u); + EXPECT_EQ(fields[3].GetOffset(), 0u); + EXPECT_EQ(fields[3].GetSize(), 2u); +} + +TEST_F(DynamicStructTest, BitfieldBoolEnd) { + auto desc = db.Add("test", "int16 a:15; bool b:1", &err); + ASSERT_TRUE(desc); + EXPECT_EQ(desc->GetSize(), 2u); + auto& fields = desc->GetFields(); + ASSERT_EQ(fields.size(), 2u); + EXPECT_EQ(fields[0].GetBitWidth(), 15u); + EXPECT_EQ(fields[0].GetBitShift(), 0u); + EXPECT_EQ(fields[0].GetBitMask(), 0x7fffu); + EXPECT_EQ(fields[0].GetOffset(), 0u); + EXPECT_EQ(fields[0].GetSize(), 2u); + EXPECT_EQ(fields[1].GetBitWidth(), 1u); + EXPECT_EQ(fields[1].GetBitMask(), 0x1u); + EXPECT_EQ(fields[1].GetBitShift(), 15u); + EXPECT_EQ(fields[1].GetOffset(), 0u); + EXPECT_EQ(fields[1].GetSize(), 2u); +} + +TEST_F(DynamicStructTest, BitfieldBoolEnd2) { + auto desc = db.Add("test", "int16 a:16; bool b:1", &err); + ASSERT_TRUE(desc); + EXPECT_EQ(desc->GetSize(), 3u); + auto& fields = desc->GetFields(); + ASSERT_EQ(fields.size(), 2u); + EXPECT_EQ(fields[0].GetBitWidth(), 16u); + EXPECT_EQ(fields[0].GetBitShift(), 0u); + EXPECT_EQ(fields[0].GetBitMask(), 0xffffu); + EXPECT_EQ(fields[0].GetOffset(), 0u); + EXPECT_EQ(fields[0].GetSize(), 2u); + EXPECT_EQ(fields[1].GetBitWidth(), 1u); + EXPECT_EQ(fields[1].GetBitMask(), 0x1u); + EXPECT_EQ(fields[1].GetBitShift(), 0u); + EXPECT_EQ(fields[1].GetOffset(), 2u); + EXPECT_EQ(fields[1].GetSize(), 1u); +} + +TEST_F(DynamicStructTest, BitfieldBoolWrongSize) { + auto desc = db.Add("test", "bool a:2", &err); + ASSERT_FALSE(desc); + ASSERT_EQ(err, "field a: bit width must be 1 for bool type"); +} + +TEST_F(DynamicStructTest, BitfieldTooBig) { + auto desc = db.Add("test", "int16 a:17", &err); + ASSERT_FALSE(desc); + ASSERT_EQ(err, "field a: bit width 17 exceeds type size"); +} + +TEST_F(DynamicStructTest, DuplicateFieldName) { + auto desc = db.Add("test", "int16 a; int8 a", &err); + ASSERT_FALSE(desc); + ASSERT_EQ(err, "duplicate field a"); +} + +struct SimpleTestParam { + const char* schema; + size_t size; + StructFieldType type; + bool isInt; + bool isUint; + unsigned int bitWidth; + uint64_t bitMask; +}; + +std::ostream& operator<<(std::ostream& os, const SimpleTestParam& param) { + return os << "SimpleTestParam(Schema: \"" << param.schema << "\")"; +} + +class DynamicSimpleStructTest + : public ::testing::TestWithParam { + protected: + StructDescriptorDatabase db; + std::string err; +}; + +TEST_P(DynamicSimpleStructTest, Check) { + auto desc = db.Add("test", GetParam().schema, &err); + ASSERT_TRUE(desc); + ASSERT_EQ(desc->GetName(), "test"); + ASSERT_EQ(desc->GetSchema(), GetParam().schema); + auto& fields = desc->GetFields(); + ASSERT_EQ(fields.size(), 1u); + EXPECT_EQ(fields[0].GetParent(), desc); + EXPECT_EQ(fields[0].GetName(), "a"); + EXPECT_EQ(fields[0].IsInt(), GetParam().isInt); + EXPECT_EQ(fields[0].IsUint(), GetParam().isUint); + EXPECT_FALSE(fields[0].IsArray()); + if (GetParam().type != StructFieldType::kStruct) { + ASSERT_TRUE(desc->IsValid()); + ASSERT_EQ(desc->GetSize(), GetParam().size); + ASSERT_EQ(fields[0].GetSize(), GetParam().size); + ASSERT_EQ(fields[0].GetBitWidth(), GetParam().bitWidth); + ASSERT_EQ(fields[0].GetBitMask(), GetParam().bitMask); + } else { + ASSERT_FALSE(desc->IsValid()); + ASSERT_TRUE(fields[0].GetStruct()); + } +} + +TEST_P(DynamicSimpleStructTest, Array) { + auto desc = db.Add("test", GetParam().schema + std::string{"[2]"}, &err); + ASSERT_TRUE(desc); + ASSERT_EQ(desc->GetName(), "test"); + ASSERT_EQ(desc->GetSchema(), GetParam().schema + std::string{"[2]"}); + auto& fields = desc->GetFields(); + ASSERT_EQ(fields.size(), 1u); + EXPECT_EQ(fields[0].GetParent(), desc); + EXPECT_EQ(fields[0].GetName(), "a"); + EXPECT_EQ(fields[0].IsInt(), GetParam().isInt); + EXPECT_EQ(fields[0].IsUint(), GetParam().isUint); + EXPECT_TRUE(fields[0].IsArray()); + EXPECT_EQ(fields[0].GetArraySize(), 2u); + if (GetParam().type != StructFieldType::kStruct) { + ASSERT_TRUE(desc->IsValid()); + ASSERT_EQ(desc->GetSize(), GetParam().size * 2u); + } else { + ASSERT_FALSE(desc->IsValid()); + ASSERT_TRUE(fields[0].GetStruct()); + } +} + +static SimpleTestParam simpleTests[] = { + {"bool a", 1, StructFieldType::kBool, false, false, 8, UINT8_MAX}, + {"char a", 1, StructFieldType::kChar, false, false, 8, UINT8_MAX}, + {"int8 a", 1, StructFieldType::kInt8, true, false, 8, UINT8_MAX}, + {"int16 a", 2, StructFieldType::kInt16, true, false, 16, UINT16_MAX}, + {"int32 a", 4, StructFieldType::kInt32, true, false, 32, UINT32_MAX}, + {"int64 a", 8, StructFieldType::kInt64, true, false, 64, UINT64_MAX}, + {"uint8 a", 1, StructFieldType::kUint8, false, true, 8, UINT8_MAX}, + {"uint16 a", 2, StructFieldType::kUint16, false, true, 16, UINT16_MAX}, + {"uint32 a", 4, StructFieldType::kUint32, false, true, 32, UINT32_MAX}, + {"uint64 a", 8, StructFieldType::kUint64, false, true, 64, UINT64_MAX}, + {"float a", 4, StructFieldType::kFloat, false, false, 32, UINT32_MAX}, + {"float32 a", 4, StructFieldType::kFloat, false, false, 32, UINT32_MAX}, + {"double a", 8, StructFieldType::kDouble, false, false, 64, UINT64_MAX}, + {"float64 a", 8, StructFieldType::kDouble, false, false, 64, UINT64_MAX}, + {"foo a", 0, StructFieldType::kStruct, false, false, 0, 0}, +}; + +INSTANTIATE_TEST_SUITE_P(DynamicSimpleStructTests, DynamicSimpleStructTest, + ::testing::ValuesIn(simpleTests)); diff --git a/wpiutil/src/test/native/cpp/struct/SchemaParserTest.cpp b/wpiutil/src/test/native/cpp/struct/SchemaParserTest.cpp new file mode 100644 index 0000000000..bf241419a0 --- /dev/null +++ b/wpiutil/src/test/native/cpp/struct/SchemaParserTest.cpp @@ -0,0 +1,218 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/struct/SchemaParser.h" // NOLINT(build/include_order) + +#include + +using namespace wpi::structparser; + +TEST(StructParserTest, Empty) { + Parser p{""}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_TRUE(schema.declarations.empty()); +} + +TEST(StructParserTest, EmptySemicolon) { + Parser p{";"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_TRUE(schema.declarations.empty()); +} + +TEST(StructParserTest, Simple) { + Parser p{"int32 a"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 1u); + auto& decl = schema.declarations[0]; + EXPECT_EQ(decl.typeString, "int32"); + EXPECT_EQ(decl.name, "a"); + EXPECT_EQ(decl.arraySize, 1u); +} + +TEST(StructParserTest, SimpleTrailingSemi) { + Parser p{"int32 a;"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 1u); +} + +TEST(StructParserTest, Array) { + Parser p{"int32 a[2]"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 1u); + auto& decl = schema.declarations[0]; + EXPECT_EQ(decl.typeString, "int32"); + EXPECT_EQ(decl.name, "a"); + EXPECT_EQ(decl.arraySize, 2u); +} + +TEST(StructParserTest, ArrayTrailingSemi) { + Parser p{"int32 a[2];"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 1u); +} + +TEST(StructParserTest, Bitfield) { + Parser p{"int32 a:2"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 1u); + auto& decl = schema.declarations[0]; + EXPECT_EQ(decl.typeString, "int32"); + EXPECT_EQ(decl.name, "a"); + EXPECT_EQ(decl.bitWidth, 2u); +} + +TEST(StructParserTest, BitfieldTrailingSemi) { + Parser p{"int32 a:2;"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 1u); +} + +TEST(StructParserTest, EnumKeyword) { + Parser p{"enum {x=1} int32 a;"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 1u); + auto& decl = schema.declarations[0]; + EXPECT_EQ(decl.typeString, "int32"); + EXPECT_EQ(decl.name, "a"); + ASSERT_EQ(decl.enumValues.size(), 1u); + EXPECT_EQ(decl.enumValues[0].first, "x"); + EXPECT_EQ(decl.enumValues[0].second, 1); +} + +TEST(StructParserTest, EnumNoKeyword) { + Parser p{"{x=1} int32 a;"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 1u); + auto& decl = schema.declarations[0]; + EXPECT_EQ(decl.typeString, "int32"); + EXPECT_EQ(decl.name, "a"); + ASSERT_EQ(decl.enumValues.size(), 1u); + EXPECT_EQ(decl.enumValues[0].first, "x"); + EXPECT_EQ(decl.enumValues[0].second, 1); +} + +TEST(StructParserTest, EnumNoValues) { + Parser p{"{} int32 a;"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 1u); + auto& decl = schema.declarations[0]; + EXPECT_EQ(decl.typeString, "int32"); + EXPECT_EQ(decl.name, "a"); + ASSERT_TRUE(decl.enumValues.empty()); +} + +TEST(StructParserTest, EnumMultipleValues) { + Parser p{"{x=1,y=-2} int32 a;"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 1u); + auto& decl = schema.declarations[0]; + EXPECT_EQ(decl.typeString, "int32"); + EXPECT_EQ(decl.name, "a"); + ASSERT_EQ(decl.enumValues.size(), 2u); + EXPECT_EQ(decl.enumValues[0].first, "x"); + EXPECT_EQ(decl.enumValues[0].second, 1); + EXPECT_EQ(decl.enumValues[1].first, "y"); + EXPECT_EQ(decl.enumValues[1].second, -2); +} + +TEST(StructParserTest, EnumTrailingComma) { + Parser p{"{x=1,y=2,} int32 a;"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 1u); + auto& decl = schema.declarations[0]; + EXPECT_EQ(decl.typeString, "int32"); + EXPECT_EQ(decl.name, "a"); + ASSERT_EQ(decl.enumValues.size(), 2u); + EXPECT_EQ(decl.enumValues[0].first, "x"); + EXPECT_EQ(decl.enumValues[0].second, 1); + EXPECT_EQ(decl.enumValues[1].first, "y"); + EXPECT_EQ(decl.enumValues[1].second, 2); +} + +TEST(StructParserTest, MultipleNoTrailingSemi) { + Parser p{"int32 a; int16 b"}; + ParsedSchema schema; + ASSERT_TRUE(p.Parse(&schema)); + ASSERT_EQ(schema.declarations.size(), 2u); + EXPECT_EQ(schema.declarations[0].typeString, "int32"); + EXPECT_EQ(schema.declarations[0].name, "a"); + EXPECT_EQ(schema.declarations[1].typeString, "int16"); + EXPECT_EQ(schema.declarations[1].name, "b"); +} + +TEST(StructParserTest, ErrBitfieldArray) { + Parser p{"int32 a[1]:2"}; + ParsedSchema schema; + ASSERT_FALSE(p.Parse(&schema)); + ASSERT_EQ(p.GetError(), "10: expected ';', got ':'"); +} + +TEST(StructParserTest, ErrNoArrayValue) { + Parser p{"int32 a[]"}; + ParsedSchema schema; + ASSERT_FALSE(p.Parse(&schema)); + ASSERT_EQ(p.GetError(), "8: expected integer, got ']'"); +} + +TEST(StructParserTest, ErrNoBitfieldValue) { + Parser p{"int32 a:"}; + ParsedSchema schema; + ASSERT_FALSE(p.Parse(&schema)); + ASSERT_EQ(p.GetError(), "8: expected integer, got ''"); +} + +TEST(StructParserTest, ErrNoNameArray) { + Parser p{"int32 [2]"}; + ParsedSchema schema; + ASSERT_FALSE(p.Parse(&schema)); + ASSERT_EQ(p.GetError(), "6: expected identifier, got '['"); +} + +TEST(StructParserTest, ErrNoNameBitField) { + Parser p{"int32 :2"}; + ParsedSchema schema; + ASSERT_FALSE(p.Parse(&schema)); + ASSERT_EQ(p.GetError(), "6: expected identifier, got ':'"); +} + +TEST(StructParserTest, NegativeBitField) { + Parser p{"int32 a:-1"}; + ParsedSchema schema; + ASSERT_FALSE(p.Parse(&schema)); + ASSERT_EQ(p.GetError(), "8: bitfield width '-1' is not a positive integer"); +} + +TEST(StructParserTest, NegativeArraySize) { + Parser p{"int32 a[-1]"}; + ParsedSchema schema; + ASSERT_FALSE(p.Parse(&schema)); + ASSERT_EQ(p.GetError(), "8: array size '-1' is not a positive integer"); +} + +TEST(StructParserTest, ZeroBitField) { + Parser p{"int32 a:0"}; + ParsedSchema schema; + ASSERT_FALSE(p.Parse(&schema)); + ASSERT_EQ(p.GetError(), "8: bitfield width '0' is not a positive integer"); +} + +TEST(StructParserTest, ZeroArraySize) { + Parser p{"int32 a[0]"}; + ParsedSchema schema; + ASSERT_FALSE(p.Parse(&schema)); + ASSERT_EQ(p.GetError(), "8: array size '0' is not a positive integer"); +}