mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
cf54d9ccb7
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.
114 lines
3.4 KiB
C++
114 lines
3.4 KiB
C++
// 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 <wpi/struct/Struct.h>
|
|
|
|
#include "frc/geometry/Rotation3d.h"
|
|
#include "units/angle.h"
|
|
#include "units/length.h"
|
|
#include "units/math.h"
|
|
|
|
namespace frc {
|
|
/**
|
|
* A change in distance along a 3D arc since the last pose update. We can use
|
|
* ideas from differential calculus to create new Pose3ds from a Twist3d and
|
|
* vice versa.
|
|
*
|
|
* A Twist can be used to represent a difference between two poses.
|
|
*/
|
|
struct WPILIB_DLLEXPORT Twist3d {
|
|
/**
|
|
* Linear "dx" component
|
|
*/
|
|
units::meter_t dx = 0_m;
|
|
|
|
/**
|
|
* Linear "dy" component
|
|
*/
|
|
units::meter_t dy = 0_m;
|
|
|
|
/**
|
|
* Linear "dz" component
|
|
*/
|
|
units::meter_t dz = 0_m;
|
|
|
|
/**
|
|
* Rotation vector x component.
|
|
*/
|
|
units::radian_t rx = 0_rad;
|
|
|
|
/**
|
|
* Rotation vector y component.
|
|
*/
|
|
units::radian_t ry = 0_rad;
|
|
|
|
/**
|
|
* Rotation vector z component.
|
|
*/
|
|
units::radian_t rz = 0_rad;
|
|
|
|
/**
|
|
* Checks equality between this Twist3d and another object.
|
|
*
|
|
* @param other The other object.
|
|
* @return Whether the two objects are equal.
|
|
*/
|
|
bool operator==(const Twist3d& other) const {
|
|
return units::math::abs(dx - other.dx) < 1E-9_m &&
|
|
units::math::abs(dy - other.dy) < 1E-9_m &&
|
|
units::math::abs(dz - other.dz) < 1E-9_m &&
|
|
units::math::abs(rx - other.rx) < 1E-9_rad &&
|
|
units::math::abs(ry - other.ry) < 1E-9_rad &&
|
|
units::math::abs(rz - other.rz) < 1E-9_rad;
|
|
}
|
|
|
|
/**
|
|
* Scale this by a given factor.
|
|
*
|
|
* @param factor The factor by which to scale.
|
|
* @return The scaled Twist3d.
|
|
*/
|
|
constexpr Twist3d operator*(double factor) const {
|
|
return Twist3d{dx * factor, dy * factor, dz * factor,
|
|
rx * factor, ry * factor, rz * factor};
|
|
}
|
|
};
|
|
} // namespace frc
|
|
|
|
template <>
|
|
struct wpi::Struct<frc::Twist3d> {
|
|
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<const uint8_t, 48> data) {
|
|
return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
|
|
units::meter_t{wpi::UnpackStruct<double, 8>(data)},
|
|
units::meter_t{wpi::UnpackStruct<double, 16>(data)},
|
|
units::radian_t{wpi::UnpackStruct<double, 24>(data)},
|
|
units::radian_t{wpi::UnpackStruct<double, 32>(data)},
|
|
units::radian_t{wpi::UnpackStruct<double, 40>(data)}};
|
|
}
|
|
static void Pack(std::span<uint8_t, 48> 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<frc::Twist3d> {
|
|
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);
|
|
};
|