Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update to wpilib 2023 beta 7 (#607)

Browse Source 
We now need platform specific jars -- reworks actions to support that. Currently only generates 32 bit pi images.
This commit is contained in:
shueja-personal
2022-12-16 17:05:23 -08:00
committed by GitHub
parent da1aabae3a
commit bb63af601d
198 changed files with 6339 additions and 4525 deletions
Download Patch File Download Diff File Expand all files Collapse all files

200
photon-lib/src/main/native/include/frc/geometry/Pose3d.h
View File

@@ -1,200 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/geometry/Pose2d.h>
#include <wpi/SymbolExports.h>
#include "Transform3d.h"
#include "Translation3d.h"
#include "Twist3d.h"
namespace frc {
/**
* Represents a 3D pose containing translational and rotational elements.
*/
class WPILIB_DLLEXPORT Pose3d {
public:
/**
* Constructs a pose at the origin facing toward the positive X axis.
*/
constexpr Pose3d() = default;
/**
* Constructs a pose with the specified translation and rotation.
*
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose3d(Translation3d translation, Rotation3d rotation);
/**
* Constructs a pose with x, y, and z translations instead of a separate
* Translation3d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param z The z component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose3d(units::meter_t x, units::meter_t y, units::meter_t z,
Rotation3d rotation);
/**
* Transforms the pose by the given transformation and returns the new
* transformed pose.
*
* @param other The transform to transform the pose by.
*
* @return The transformed pose.
*/
Pose3d operator+(const Transform3d& other) const;
/**
* Returns the Transform3d that maps the one pose to another.
*
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
Transform3d operator-(const Pose3d& other) const;
/**
* Checks equality between this Pose3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Pose3d& other) const;
/**
* Checks inequality between this Pose3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Pose3d& other) const;
/**
* Returns the underlying translation.
*
* @return Reference to the translational component of the pose.
*/
const Translation3d& Translation() const { return m_translation; }
/**
* Returns the X component of the pose's translation.
*
* @return The x component of the pose's translation.
*/
units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the pose's translation.
*
* @return The y component of the pose's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the pose's translation.
*
* @return The z component of the pose's translation.
*/
units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the underlying rotation.
*
* @return Reference to the rotational component of the pose.
*/
const Rotation3d& Rotation() const { return m_rotation; }
/**
* Transforms the pose by the given transformation and returns the new pose.
* See + operator for the matrix multiplication performed.
*
* @param other The transform to transform the pose by.
*
* @return The transformed pose.
*/
Pose3d TransformBy(const Transform3d& other) const;
/**
* Returns the other pose relative to the current pose.
*
* This function can often be used for trajectory tracking or pose
* stabilization algorithms to get the error between the reference and the
* current pose.
*
* @param other The pose that is the origin of the new coordinate frame that
* the current pose will be converted into.
*
* @return The current pose relative to the new origin pose.
*/
Pose3d RelativeTo(const Pose3d& other) const;
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*
* The twist is a change in pose in the robot's coordinate frame since the
* previous pose update. When the user runs exp() on the previous known
* field-relative pose with the argument being the twist, the user will
* receive the new field-relative pose.
*
* "Exp" represents the pose exponential, which is solving a differential
* equation moving the pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
* 0.01 meters and changes angle by 0.5 degrees since the previous pose
* update, the twist would be Twist3d{0.01_m, 0_m, 0_m, Rotation3d{0.0, 0.0,
* 0.5_deg}}.
*
* @return The new pose of the robot.
*/
Pose3d Exp(const Twist3d& twist) const;
/**
* Returns a Twist3d that maps this pose to the end pose. If c is the output
* of a.Log(b), then a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
*
* @return The twist that maps this to end.
*/
Twist3d Log(const Pose3d& end) const;
/**
* Returns a Pose2d representing this Pose3d projected into the X-Y plane.
*/
Pose2d ToPose2d() const;
private:
Translation3d m_translation;
Rotation3d m_rotation;
};
} // namespace frc

114
photon-lib/src/main/native/include/frc/geometry/Quaternion.h
View File

@@ -1,114 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/EigenCore.h>
#include <wpi/SymbolExports.h>
namespace frc {
class WPILIB_DLLEXPORT Quaternion {
public:
/**
* Constructs a quaternion with a default angle of 0 degrees.
*/
Quaternion() = default;
/**
* Constructs a quaternion with the given components.
*
* @param w W component of the quaternion.
* @param x X component of the quaternion.
* @param y Y component of the quaternion.
* @param z Z component of the quaternion.
*/
Quaternion(double w, double x, double y, double z);
/**
* Multiply with another quaternion.
*
* @param other The other quaternion.
*/
Quaternion operator*(const Quaternion& other) const;
/**
* Checks equality between this Quaternion and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Quaternion& other) const;
/**
* Checks inequality between this Quaternion and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Quaternion& other) const;
/**
* Returns the inverse of the quaternion.
*/
Quaternion Inverse() const;
/**
* Normalizes the quaternion.
*/
Quaternion Normalize() const;
/**
* Returns W component of the quaternion.
*/
double W() const;
/**
* Returns X component of the quaternion.
*/
double X() const;
/**
* Returns Y component of the quaternion.
*/
double Y() const;
/**
* Returns Z component of the quaternion.
*/
double Z() const;
/**
* Returns the rotation vector representation of this quaternion.
*
* This is also the log operator of SO(3).
*/
Eigen::Vector3d ToRotationVector() const;
private:
double m_r = 1.0;
Eigen::Vector3d m_v{0.0, 0.0, 0.0};
};
} // namespace frc

193
photon-lib/src/main/native/include/frc/geometry/Rotation3d.h
View File

@@ -1,193 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/EigenCore.h>
#include <frc/geometry/Rotation2d.h>
#include <units/angle.h>
#include <wpi/SymbolExports.h>
#include "Quaternion.h"
namespace frc {
/**
* A rotation in a 3D coordinate frame represented by a quaternion.
*/
class WPILIB_DLLEXPORT Rotation3d {
public:
/**
* Constructs a Rotation3d with a default angle of 0 degrees.
*/
Rotation3d() = default;
/**
* Constructs a Rotation3d from a quaternion.
*
* @param q The quaternion.
*/
explicit Rotation3d(const Quaternion& q);
/**
* Constructs a Rotation3d from extrinsic roll, pitch, and yaw.
*
* Extrinsic rotations occur in that order around the axes in the fixed global
* frame rather than the body frame.
*
* @param roll The counterclockwise rotation angle around the X axis (roll).
* @param pitch The counterclockwise rotation angle around the Y axis (pitch).
* @param yaw The counterclockwise rotation angle around the Z axis (yaw).
*/
Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw);
/**
* Constructs a Rotation3d with the given axis-angle representation. The axis
* doesn't have to be normalized.
*
* @param axis The rotation axis.
* @param angle The rotation around the axis.
*/
Rotation3d(const Vectord<3>& axis, units::radian_t angle);
/**
* Constructs a Rotation3d from a rotation matrix.
*
* @param rotationMatrix The rotation matrix.
* @throws std::domain_error if the rotation matrix isn't special orthogonal.
*/
explicit Rotation3d(const Matrixd<3, 3>& rotationMatrix);
/**
* Constructs a Rotation3d that rotates the initial vector onto the final
* vector.
*
* This is useful for turning a 3D vector (final) into an orientation relative
* to a coordinate system vector (initial).
*
* @param initial The initial vector.
* @param final The final vector.
*/
Rotation3d(const Vectord<3>& initial, const Vectord<3>& final);
/**
* Adds two rotations together.
*
* @param other The rotation to add.
*
* @return The sum of the two rotations.
*/
Rotation3d operator+(const Rotation3d& other) const;
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation.
*
* @param other The rotation to subtract.
*
* @return The difference between the two rotations.
*/
Rotation3d operator-(const Rotation3d& other) const;
/**
* Takes the inverse of the current rotation.
*
* @return The inverse of the current rotation.
*/
Rotation3d operator-() const;
/**
* Multiplies the current rotation by a scalar.
* @param scalar The scalar.
*
* @return The new scaled Rotation3d.
*/
Rotation3d operator*(double scalar) const;
/**
* Checks equality between this Rotation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Rotation3d& other) const;
/**
* Checks inequality between this Rotation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Rotation3d& other) const;
/**
* Adds the new rotation to the current rotation.
*
* @param other The rotation to rotate by.
*
* @return The new rotated Rotation3d.
*/
Rotation3d RotateBy(const Rotation3d& other) const;
/**
* Returns the quaternion representation of the Rotation3d.
*/
const Quaternion& GetQuaternion() const;
/**
* Returns the counterclockwise rotation angle around the X axis (roll).
*/
units::radian_t X() const;
/**
* Returns the counterclockwise rotation angle around the Y axis (pitch).
*/
units::radian_t Y() const;
/**
* Returns the counterclockwise rotation angle around the Z axis (yaw).
*/
units::radian_t Z() const;
/**
* Returns the axis in the axis-angle representation of this rotation.
*/
Vectord<3> Axis() const;
/**
* Returns the angle in the axis-angle representation of this rotation.
*/
units::radian_t Angle() const;
/**
* Returns a Rotation2d representing this Rotation3d projected into the X-Y
* plane.
*/
Rotation2d ToRotation2d() const;
private:
Quaternion m_q;
};
} // namespace frc

141
photon-lib/src/main/native/include/frc/geometry/Transform3d.h
View File

@@ -1,141 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <wpi/SymbolExports.h>
#include "Translation3d.h"
namespace frc {
class WPILIB_DLLEXPORT Pose3d;
/**
* Represents a transformation for a Pose3d.
*/
class WPILIB_DLLEXPORT Transform3d {
public:
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param final The final pose for the transformation.
*/
Transform3d(Pose3d initial, Pose3d final);
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
Transform3d(Translation3d translation, Rotation3d rotation);
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
constexpr Transform3d() = default;
/**
* Returns the translation component of the transformation.
*
* @return Reference to the translational component of the transform.
*/
const Translation3d& Translation() const { return m_translation; }
/**
* Returns the X component of the transformation's translation.
*
* @return The x component of the transformation's translation.
*/
units::meter_t X() const { return m_translation.X(); }
/**
* Returns the Y component of the transformation's translation.
*
* @return The y component of the transformation's translation.
*/
units::meter_t Y() const { return m_translation.Y(); }
/**
* Returns the Z component of the transformation's translation.
*
* @return The z component of the transformation's translation.
*/
units::meter_t Z() const { return m_translation.Z(); }
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
const Rotation3d& Rotation() const { return m_rotation; }
/**
* Invert the transformation. This is useful for undoing a transformation.
*
* @return The inverted transformation.
*/
Transform3d Inverse() const;
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform3d.
*/
Transform3d operator*(double scalar) const {
return Transform3d(m_translation * scalar, m_rotation * scalar);
}
/**
* Composes two transformations.
*
* @param other The transform to compose with this one.
* @return The composition of the two transformations.
*/
Transform3d operator+(const Transform3d& other) const;
/**
* Checks equality between this Transform3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Transform3d& other) const;
/**
* Checks inequality between this Transform3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Transform3d& other) const;
private:
Translation3d m_translation;
Rotation3d m_rotation;
};
} // namespace frc

205
photon-lib/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -1,205 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/geometry/Translation2d.h>
#include <units/length.h>
#include <wpi/SymbolExports.h>
#include "Rotation3d.h"
namespace frc {
/**
* Represents a translation in 3D space.
* This object can be used to represent a point or a vector.
*
* This assumes that you are using conventional mathematical axes. When the
* robot is at the origin facing in the positive X direction, forward is
* positive X, left is positive Y, and up is positive Z.
*/
class WPILIB_DLLEXPORT Translation3d {
public:
/**
* Constructs a Translation3d with X, Y, and Z components equal to zero.
*/
constexpr Translation3d() = default;
/**
* Constructs a Translation3d with the X, Y, and Z components equal to the
* provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
Translation3d(units::meter_t x, units::meter_t y, units::meter_t z);
/**
* Constructs a Translation3d with the provided distance and angle. This is
* essentially converting from polar coordinates to Cartesian coordinates.
*
* @param distance The distance from the origin to the end of the translation.
* @param angle The angle between the x-axis and the translation vector.
*/
Translation3d(units::meter_t distance, const Rotation3d& angle);
/**
* Calculates the distance between two translations in 3D space.
*
* The distance between translations is defined as
* √((x₂−x₁)²+(y₂−y₁)²+(z₂−z₁)²).
*
* @param other The translation to compute the distance to.
*
* @return The distance between the two translations.
*/
units::meter_t Distance(const Translation3d& other) const;
/**
* Returns the X component of the translation.
*
* @return The Z component of the translation.
*/
units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The Y component of the translation.
*/
units::meter_t Y() const { return m_y; }
/**
* Returns the Z component of the translation.
*
* @return The Z component of the translation.
*/
units::meter_t Z() const { return m_z; }
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
units::meter_t Norm() const;
/**
* Applies a rotation to the translation in 3D space.
*
* For example, rotating a Translation3d of &lt;2, 0, 0&gt; by 90 degrees
* around the Z axis will return a Translation3d of &lt;0, 2, 0&gt;.
*
* @param other The rotation to rotate the translation by.
*
* @return The new rotated translation.
*/
Translation3d RotateBy(const Rotation3d& other) const;
/**
* Returns a Translation2d representing this Translation3d projected into the
* X-Y plane.
*/
Translation2d ToTranslation2d() const;
/**
* Returns the sum of two translations in 3D space.
*
* For example, Translation3d{1.0, 2.5, 3.5} + Translation3d{2.0, 5.5, 7.5} =
* Translation3d{3.0, 8.0, 11.0}.
*
* @param other The translation to add.
*
* @return The sum of the translations.
*/
Translation3d operator+(const Translation3d& other) const;
/**
* Returns the difference between two translations.
*
* For example, Translation3d{5.0, 4.0, 3.0} - Translation3d{1.0, 2.0, 3.0} =
* Translation3d{4.0, 2.0, 0.0}.
*
* @param other The translation to subtract.
*
* @return The difference between the two translations.
*/
Translation3d operator-(const Translation3d& other) const;
/**
* Returns the inverse of the current translation. This is equivalent to
* negating all components of the translation.
*
* @return The inverse of the current translation.
*/
Translation3d operator-() const;
/**
* Returns the translation multiplied by a scalar.
*
* For example, Translation3d{2.0, 2.5, 4.5} * 2 = Translation3d{4.0, 5.0,
* 9.0}.
*
* @param scalar The scalar to multiply by.
*
* @return The scaled translation.
*/
Translation3d operator*(double scalar) const;
/**
* Returns the translation divided by a scalar.
*
* For example, Translation3d{2.0, 2.5, 4.5} / 2 = Translation3d{1.0, 1.25,
* 2.25}.
*
* @param scalar The scalar to divide by.
*
* @return The scaled translation.
*/
Translation3d operator/(double scalar) const;
/**
* Checks equality between this Translation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const Translation3d& other) const;
/**
* Checks inequality between this Translation3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Translation3d& other) const;
private:
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
units::meter_t m_z = 0_m;
};
} // namespace frc

106
photon-lib/src/main/native/include/frc/geometry/Twist3d.h
View File

@@ -1,106 +0,0 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/geometry/Rotation3d.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/math.h>
#include <wpi/SymbolExports.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
* vise 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;
}
/**
* Checks inequality between this Twist3d and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const Twist3d& other) const { return !operator==(other); }
/**
* Scale this by a given factor.
*
* @param factor The factor by which to scale.
* @return The scaled Twist3d.
*/
Twist3d operator*(double factor) const {
return Twist3d{dx * factor, dy * factor, dz * factor,
rx * factor, ry * factor, rz * factor};
}
};
} // namespace frc

8
photon-lib/src/main/native/include/photonlib/Packet.h
View File

@@ -46,7 +46,7 @@ class Packet {
* Constructs a packet with the given data.
* @param data The packet data.
*/
explicit Packet(std::vector<char> data) : packetData(data) {}
explicit Packet(std::vector<uint8_t> data) : packetData(data) {}
/**
* Clears the packet and resets the read and write positions.
@@ -61,7 +61,7 @@ class Packet {
* Returns the packet data.
* @return The packet data.
*/
const std::vector<char>& GetData() { return packetData; }
const std::vector<uint8_t>& GetData() { return packetData; }
/**
* Returns the number of bytes in the data.
@@ -105,7 +105,7 @@ class Packet {
if constexpr (wpi::support::endian::system_endianness() ==
wpi::support::endianness::little) {
// Reverse to little endian for host.
char& raw = reinterpret_cast<char&>(value);
uint8_t& raw = reinterpret_cast<uint8_t&>(value);
std::reverse(&raw, &raw + sizeof(T));
}
}
@@ -121,7 +121,7 @@ class Packet {
private:
// Data stored in the packet
std::vector<char> packetData;
std::vector<uint8_t> packetData;
size_t readPos = 0;
size_t writePos = 0;

24
photon-lib/src/main/native/include/photonlib/PhotonCamera.h
View File

@@ -27,9 +27,13 @@
#include <memory>
#include <string>
#include <networktables/BooleanTopic.h>
#include <networktables/DoubleTopic.h>
#include <networktables/IntegerTopic.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableInstance.h>
#include <networktables/RawTopic.h>
#include <networktables/StringTopic.h>
#include <units/time.h>
#include <wpi/deprecated.h>
@@ -146,13 +150,17 @@ class PhotonCamera {
protected:
std::shared_ptr<nt::NetworkTable> mainTable;
std::shared_ptr<nt::NetworkTable> rootTable;
nt::NetworkTableEntry rawBytesEntry;
nt::NetworkTableEntry driverModeEntry;
nt::NetworkTableEntry inputSaveImgEntry;
nt::NetworkTableEntry outputSaveImgEntry;
nt::NetworkTableEntry pipelineIndexEntry;
nt::NetworkTableEntry ledModeEntry;
nt::NetworkTableEntry versionEntry;
nt::RawSubscriber rawBytesEntry;
nt::BooleanPublisher driverModeEntry;
nt::BooleanPublisher inputSaveImgEntry;
nt::BooleanPublisher outputSaveImgEntry;
nt::IntegerPublisher pipelineIndexEntry;
nt::IntegerPublisher ledModeEntry;
nt::StringSubscriber versionEntry;
nt::BooleanSubscriber driverModeSubscriber;
nt::IntegerSubscriber pipelineIndexSubscriber;
nt::IntegerSubscriber ledModeSubscriber;
std::string path;

6
photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h
View File

@@ -24,12 +24,12 @@
#pragma once
#include <span>
#include <string>
#include <frc/Errors.h>
#include <units/time.h>
#include <wpi/SmallVector.h>
#include <wpi/span.h>
#include "photonlib/Packet.h"
#include "photonlib/PhotonTrackedTarget.h"
@@ -51,7 +51,7 @@ class PhotonPipelineResult {
* @param targets The list of targets identified by the pipeline.
*/
PhotonPipelineResult(units::second_t latency,
wpi::span<const PhotonTrackedTarget> targets);
std::span<const PhotonTrackedTarget> targets);
/**
* Returns the best target in this pipeline result. If there are no targets,
@@ -105,7 +105,7 @@ class PhotonPipelineResult {
* Returns a reference to the vector of targets.
* @return A reference to the vector of targets.
*/
const wpi::span<const PhotonTrackedTarget> GetTargets() const {
const std::span<const PhotonTrackedTarget> GetTargets() const {
return targets;
}

6
photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h
View File

@@ -83,6 +83,12 @@ class PhotonTrackedTarget {
*/
double GetSkew() const { return skew; }
/**
* Get the Fiducial ID of the target currently being tracked,
* or -1 if not set.
*/
double GetFiducialId() const { return fiducialId; }
/**
* Returns the corners of the minimum area rectangle bounding this target.
*/

19
photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h
View File

@@ -47,6 +47,7 @@ class SimPhotonCamera : public PhotonCamera {
targetAreaEntry = rootTable->GetEntry("targetAreaEntry");
targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("raw");
versionEntry = instance->GetTable("photonvision")->GetEntry("version");
// versionEntry.SetString(PhotonVersion.versionString);
}
@@ -86,10 +87,9 @@ class SimPhotonCamera : public PhotonCamera {
PhotonPipelineResult newResult{latency, targetList};
Packet packet{};
packet << newResult;
rawBytesEntry.SetRaw(
std::string_view{packet.GetData().data(), packet.GetDataSize()});
std::string rawBytesGet = rawBytesEntry.GetRaw("ohono");
rawBytesPublisher.Set(
std::span{packet.GetData().data(), packet.GetDataSize()});
bool hasTargets = newResult.HasTargets();
hasTargetEntry.SetBoolean(hasTargets);
@@ -97,7 +97,8 @@ class SimPhotonCamera : public PhotonCamera {
targetPitchEntry.SetDouble(0.0);
targetYawEntry.SetDouble(0.0);
targetAreaEntry.SetDouble(0.0);
targetPoseEntry.SetDoubleArray({0.0, 0.0, 0.0});
targetPoseEntry.SetDoubleArray(
std::vector<double>{0.0, 0.0, 0.0, 0, 0, 0, 0});
targetSkewEntry.SetDouble(0.0);
} else {
PhotonTrackedTarget bestTarget = newResult.GetBestTarget();
@@ -107,9 +108,12 @@ class SimPhotonCamera : public PhotonCamera {
targetSkewEntry.SetDouble(bestTarget.GetSkew());
frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
targetPoseEntry.SetDoubleArray(
{transform.X().to<double>(), transform.Y().to<double>(),
transform.Rotation().ToRotation2d().Degrees().to<double>()});
targetPoseEntry.SetDoubleArray(std::vector<double>{
transform.X().to<double>(), transform.Y().to<double>(),
transform.Z().to<double>(), transform.Rotation().GetQuaternion().W(),
transform.Rotation().GetQuaternion().X(),
transform.Rotation().GetQuaternion().Y(),
transform.Rotation().GetQuaternion().Z()});
}
}
@@ -122,5 +126,6 @@ class SimPhotonCamera : public PhotonCamera {
nt::NetworkTableEntry targetSkewEntry;
nt::NetworkTableEntry targetPoseEntry;
nt::NetworkTableEntry versionEntry;
nt::RawPublisher rawBytesPublisher;
};
} // namespace photonlib