[wpimath] Add structured data support for DifferentialDriveWheelPosit…

…ions (wpilibsuite#6412)

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java

@@ -7,6 +7,8 @@
 import static edu.wpi.first.units.Units.Meters;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelPositionsProto;
+import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import java.util.Objects;
@@ -20,6 +22,14 @@ public class DifferentialDriveWheelPositions
   /** Distance measured by the right side. */
   public double rightMeters;
 
+  /** DifferentialDriveWheelPostions struct for serialization. */
+  public static final DifferentialDriveWheelPositionsStruct struct =
+      new DifferentialDriveWheelPositionsStruct();
+
+  /** DifferentialDriveWheelPostions struct for serialization. */
+  public static final DifferentialDriveWheelPositionsProto proto =
+      new DifferentialDriveWheelPositionsProto();
+
   /**
    * Constructs a DifferentialDriveWheelPositions.
    *

wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java

@@ -0,0 +1,40 @@
+// 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.math.kinematics.proto;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
+import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelPositions;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class DifferentialDriveWheelPositionsProto
+    implements Protobuf<DifferentialDriveWheelPositions, ProtobufDifferentialDriveWheelPositions> {
+  @Override
+  public Class<DifferentialDriveWheelPositions> getTypeClass() {
+    return DifferentialDriveWheelPositions.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufDifferentialDriveWheelPositions.getDescriptor();
+  }
+
+  @Override
+  public ProtobufDifferentialDriveWheelPositions createMessage() {
+    return ProtobufDifferentialDriveWheelPositions.newInstance();
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions unpack(ProtobufDifferentialDriveWheelPositions msg) {
+    return new DifferentialDriveWheelPositions(msg.getLeft(), msg.getRight());
+  }
+
+  @Override
+  public void pack(
+      ProtobufDifferentialDriveWheelPositions msg, DifferentialDriveWheelPositions value) {
+    msg.setLeft(value.leftMeters);
+    msg.setRight(value.rightMeters);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java

@@ -0,0 +1,45 @@
+// 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.math.kinematics.struct;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public class DifferentialDriveWheelPositionsStruct
+    implements Struct<DifferentialDriveWheelPositions> {
+  @Override
+  public Class<DifferentialDriveWheelPositions> getTypeClass() {
+    return DifferentialDriveWheelPositions.class;
+  }
+
+  @Override
+  public String getTypeString() {
+    return "struct:DifferentialDriveWheelPositions";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 2;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double left;double right";
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions unpack(ByteBuffer bb) {
+    double left = bb.getDouble();
+    double right = bb.getDouble();
+    return new DifferentialDriveWheelPositions(left, right);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, DifferentialDriveWheelPositions value) {
+    bb.putDouble(value.leftMeters);
+    bb.putDouble(value.rightMeters);
+  }
+}

wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelPositionsProto.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/kinematics/proto/DifferentialDriveWheelPositionsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<
+    frc::DifferentialDriveWheelPositions>::New(google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufDifferentialDriveWheelPositions>(arena);
+}
+
+frc::DifferentialDriveWheelPositions
+wpi::Protobuf<frc::DifferentialDriveWheelPositions>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m =
+      static_cast<const wpi::proto::ProtobufDifferentialDriveWheelPositions*>(
+          &msg);
+  return frc::DifferentialDriveWheelPositions{
+      units::meter_t{m->left()},
+      units::meter_t{m->right()},
+  };
+}
+
+void wpi::Protobuf<frc::DifferentialDriveWheelPositions>::Pack(
+    google::protobuf::Message* msg,
+    const frc::DifferentialDriveWheelPositions& value) {
+  auto m =
+      static_cast<wpi::proto::ProtobufDifferentialDriveWheelPositions*>(msg);
+  m->set_left(value.left.value());
+  m->set_right(value.right.value());
+}

wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStruct.cpp

@@ -0,0 +1,26 @@
+// 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/kinematics/struct/DifferentialDriveWheelPositionsStruct.h"
+
+namespace {
+constexpr size_t kLeftOff = 0;
+constexpr size_t kRightOff = kLeftOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::DifferentialDriveWheelPositions>;
+
+frc::DifferentialDriveWheelPositions StructType::Unpack(
+    std::span<const uint8_t> data) {
+  return frc::DifferentialDriveWheelPositions{
+      units::meter_t{wpi::UnpackStruct<double, kLeftOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kRightOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+                      const frc::DifferentialDriveWheelPositions& value) {
+  wpi::PackStruct<kLeftOff>(data, value.left.value());
+  wpi::PackStruct<kRightOff>(data, value.right.value());
+}

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h

@@ -49,3 +49,6 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
   }
 };
 }  // namespace frc
+
+#include "frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h"
+#include "frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h"

wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h

@@ -0,0 +1,19 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DifferentialDriveWheelPositions> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::DifferentialDriveWheelPositions Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::DifferentialDriveWheelPositions& value);
+};

wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h

@@ -0,0 +1,28 @@
+// 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 <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveWheelPositions> {
+  static constexpr std::string_view GetTypeString() {
+    return "struct:DifferentialDriveWheelPositions";
+  }
+  static constexpr size_t GetSize() { return 16; }
+  static constexpr std::string_view GetSchema() {
+    return "double left;double right";
+  }
+
+  static frc::DifferentialDriveWheelPositions Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::DifferentialDriveWheelPositions& value);
+};
+
+static_assert(wpi::StructSerializable<frc::DifferentialDriveWheelPositions>);

wpimath/src/main/proto/kinematics.proto

@@ -21,6 +21,11 @@ message ProtobufDifferentialDriveWheelSpeeds {
   double right = 2;
 }
 
+message ProtobufDifferentialDriveWheelPositions {
+  double left = 1;
+  double right = 2;
+}
+
 message ProtobufMecanumDriveKinematics {
   ProtobufTranslation2d front_left = 1;
   ProtobufTranslation2d front_right = 2;

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java

@@ -0,0 +1,29 @@
+// 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.math.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelPositionsStructTest {
+  private static final DifferentialDriveWheelPositions DATA =
+      new DifferentialDriveWheelPositions(1.74, 35.04);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelPositions.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    DifferentialDriveWheelPositions.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    DifferentialDriveWheelPositions data = DifferentialDriveWheelPositions.struct.unpack(buffer);
+    assertEquals(DATA.leftMeters, data.leftMeters);
+    assertEquals(DATA.rightMeters, data.rightMeters);
+  }
+}

wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStructTest.cpp

@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::DifferentialDriveWheelPositions>;
+const DifferentialDriveWheelPositions kExpectedData{
+    DifferentialDriveWheelPositions{1.74_m, 35.04_m}};
+}  // namespace
+
+TEST(DifferentialDriveWheelPositionsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::GetSize()];
+  std::memset(buffer, 0, StructType::GetSize());
+  StructType::Pack(buffer, kExpectedData);
+
+  DifferentialDriveWheelPositions unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value());
+  EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value());
+}