From ed30d5d40e5f44ddd748e8024ae82b51a03384c5 Mon Sep 17 00:00:00 2001 From: carbotaniuman <41451839+carbotaniuman@users.noreply.github.com> Date: Sat, 2 Nov 2019 13:35:03 -0500 Subject: [PATCH] Add JSON support for Trajectories (#2025) --- .../src/main/native/cpp/geometry/Pose2d.cpp | 12 +++ .../main/native/cpp/geometry/Rotation2d.cpp | 10 +++ .../native/cpp/geometry/Translation2d.cpp | 12 +++ .../main/native/cpp/trajectory/Trajectory.cpp | 30 ++++++++ .../native/cpp/trajectory/TrajectoryUtil.cpp | 58 ++++++++++++++ .../main/native/include/frc/geometry/Pose2d.h | 9 +++ .../native/include/frc/geometry/Rotation2d.h | 9 +++ .../include/frc/geometry/Translation2d.h | 9 +++ .../include/frc/trajectory/Trajectory.h | 25 +++++++ .../include/frc/trajectory/TrajectoryUtil.h | 59 +++++++++++++++ .../cpp/trajectory/TrajectoryJsonTest.cpp | 23 ++++++ wpilibj/CMakeLists.txt | 5 +- .../wpi/first/wpilibj/geometry/Pose2d.java | 48 ++++++++++++ .../first/wpilibj/geometry/Rotation2d.java | 45 +++++++++++ .../first/wpilibj/geometry/Translation2d.java | 47 ++++++++++++ .../first/wpilibj/trajectory/Trajectory.java | 31 ++++++++ .../wpilibj/trajectory/TrajectoryConfig.java | 2 +- .../trajectory/TrajectoryGenerator.java | 17 ++++- .../wpilibj/trajectory/TrajectoryUtil.java | 75 +++++++++++++++++++ .../trajectory/TrajectoryGeneratorTest.java | 2 +- .../trajectory/TrajectoryJsonTest.java | 33 ++++++++ wpiutil/CMakeLists.txt | 29 +++++-- wpiutil/build.gradle | 3 + 23 files changed, 581 insertions(+), 12 deletions(-) create mode 100644 wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp create mode 100644 wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h create mode 100644 wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java diff --git a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp index 42d20f709e..1754830303 100644 --- a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp @@ -9,6 +9,8 @@ #include +#include + using namespace frc; Pose2d::Pose2d(Translation2d translation, Rotation2d rotation) @@ -96,3 +98,13 @@ Twist2d Pose2d::Log(const Pose2d& end) const { return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)}; } + +void frc::to_json(wpi::json& json, const Pose2d& pose) { + json = wpi::json{{"translation", pose.Translation()}, + {"rotation", pose.Rotation()}}; +} + +void frc::from_json(const wpi::json& json, Pose2d& pose) { + pose = Pose2d{json.at("translation").get(), + json.at("rotation").get()}; +} diff --git a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp index 2723442480..a3c4bea932 100644 --- a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp @@ -9,6 +9,8 @@ #include +#include + using namespace frc; Rotation2d::Rotation2d(units::radian_t value) @@ -68,3 +70,11 @@ Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const { return {Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos()}; } + +void frc::to_json(wpi::json& json, const Rotation2d& rotation) { + json = wpi::json{{"radians", rotation.Radians().to()}}; +} + +void frc::from_json(const wpi::json& json, Rotation2d& rotation) { + rotation = Rotation2d{units::radian_t{json.at("radians").get()}}; +} diff --git a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp index ca287d120e..c3db7e3a69 100644 --- a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp @@ -7,6 +7,8 @@ #include "frc/geometry/Translation2d.h" +#include + using namespace frc; Translation2d::Translation2d(units::meter_t x, units::meter_t y) @@ -73,3 +75,13 @@ Translation2d& Translation2d::operator/=(double scalar) { *this *= (1.0 / scalar); return *this; } + +void frc::to_json(wpi::json& json, const Translation2d& translation) { + json = wpi::json{{"x", translation.X().to()}, + {"y", translation.Y().to()}}; +} + +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()}}; +} diff --git a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp index 9b4b4f5611..7b2b2b6e02 100644 --- a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp +++ b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp @@ -7,8 +7,20 @@ #include "frc/trajectory/Trajectory.h" +#include + using namespace frc; +bool Trajectory::State::operator==(const Trajectory::State& other) const { + return t == other.t && velocity == other.velocity && + acceleration == other.acceleration && pose == other.pose && + curvature == other.curvature; +} + +bool Trajectory::State::operator!=(const Trajectory::State& other) const { + return !operator==(other); +} + Trajectory::State Trajectory::State::Interpolate(State endValue, double i) const { // Find the new [t] value. @@ -94,3 +106,21 @@ Trajectory::State Trajectory::Sample(units::second_t t) const { return prevSample.Interpolate(sample, (t - prevSample.t) / (sample.t - prevSample.t)); } + +void frc::to_json(wpi::json& json, const Trajectory::State& state) { + json = wpi::json{{"time", state.t.to()}, + {"velocity", state.velocity.to()}, + {"acceleration", state.acceleration.to()}, + {"pose", state.pose}, + {"curvature", state.curvature.to()}}; +} + +void frc::from_json(const wpi::json& json, Trajectory::State& state) { + state.pose = json.at("pose").get(); + state.t = units::second_t{json.at("time").get()}; + state.velocity = + units::meters_per_second_t{json.at("velocity").get()}; + state.acceleration = + units::meters_per_second_squared_t{json.at("acceleration").get()}; + state.curvature = frc::curvature_t{json.at("curvature").get()}; +} diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp new file mode 100644 index 0000000000..f3cf30d893 --- /dev/null +++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/trajectory/TrajectoryUtil.h" + +#include + +#include +#include +#include +#include + +using namespace frc; + +void TrajectoryUtil::ToPathweaverJson(const Trajectory& trajectory, + const wpi::Twine& path) { + std::error_code error_code; + + wpi::SmallString<128> buf; + wpi::raw_fd_ostream output{path.toStringRef(buf), error_code}; + if (error_code) { + throw std::runtime_error(("Cannot open file: " + path).str()); + } + + wpi::json json = trajectory.States(); + output << json; + output.flush(); +} + +Trajectory TrajectoryUtil::FromPathweaverJson(const wpi::Twine& path) { + std::error_code error_code; + + wpi::SmallString<128> buf; + wpi::raw_fd_istream input{path.toStringRef(buf), error_code}; + if (error_code) { + throw std::runtime_error(("Cannot open file: " + path).str()); + } + + wpi::json json; + input >> json; + + return Trajectory{json.get>()}; +} + +std::string TrajectoryUtil::SerializeTrajectory(const Trajectory& trajectory) { + wpi::json json = trajectory.States(); + return json.dump(); +} + +Trajectory TrajectoryUtil::DeserializeTrajectory( + const wpi::StringRef json_str) { + wpi::json json = wpi::json::parse(json_str); + return Trajectory{json.get>()}; +} diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h index 2161e8280b..85c715fe52 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h @@ -11,6 +11,10 @@ #include "Translation2d.h" #include "Twist2d.h" +namespace wpi { +class json; +} // namespace wpi + namespace frc { /** @@ -167,4 +171,9 @@ class Pose2d { Translation2d m_translation; Rotation2d m_rotation; }; + +void to_json(wpi::json& json, const Pose2d& pose); + +void from_json(const wpi::json& json, Pose2d& pose); + } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h index 9453e0cb78..246a407b6e 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h @@ -10,6 +10,10 @@ #include #include +namespace wpi { +class json; +} // namespace wpi + namespace frc { /** @@ -175,4 +179,9 @@ class Rotation2d { double m_cos = 1; double m_sin = 0; }; + +void to_json(wpi::json& json, const Rotation2d& rotation); + +void from_json(const wpi::json& json, Rotation2d& rotation); + } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h index a53abf118d..90c61e26fe 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h @@ -11,6 +11,10 @@ #include "Rotation2d.h" +namespace wpi { +class json; +} // namespace wpi + namespace frc { /** @@ -211,4 +215,9 @@ class Translation2d { units::meter_t m_x = 0_m; units::meter_t m_y = 0_m; }; + +void to_json(wpi::json& json, const Translation2d& state); + +void from_json(const wpi::json& json, Translation2d& state); + } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h index 0a384fbb00..96c2c6e7d1 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h +++ b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h @@ -13,6 +13,10 @@ #include "frc/geometry/Pose2d.h" +namespace wpi { +class json; +} // namespace wpi + namespace frc { /** @@ -47,6 +51,22 @@ class Trajectory { // The curvature at that point of the trajectory. curvature_t curvature{0.0}; + /** + * Checks equality between this State and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const State& other) const; + + /** + * Checks inequality between this State and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const State& other) const; + /** * Interpolates between two States. * @@ -103,4 +123,9 @@ class Trajectory { return startValue + (endValue - startValue) * t; } }; + +void to_json(wpi::json& json, const Trajectory::State& state); + +void from_json(const wpi::json& json, Trajectory::State& state); + } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h new file mode 100644 index 0000000000..700ed9c214 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include + +#include "frc/trajectory/Trajectory.h" + +namespace frc { +class TrajectoryUtil { + public: + TrajectoryUtil() = delete; + + /** + * Exports a Trajectory to a PathWeaver-style JSON file. + * + * @param trajectory the trajectory to export + * @param path the path of the file to export to + * + * @return The interpolated state. + */ + static void ToPathweaverJson(const Trajectory& trajectory, + const wpi::Twine& path); + /** + * Imports a Trajectory from a PathWeaver-style JSON file. + * + * @param path The path of the json file to import from. + * + * @return The trajectory represented by the file. + */ + static Trajectory FromPathweaverJson(const wpi::Twine& path); + + /** + * Deserializes a Trajectory from PathWeaver-style JSON. + + * @param json the string containing the serialized JSON + + * @return the trajectory represented by the JSON + */ + static std::string SerializeTrajectory(const Trajectory& trajectory); + + /** + * Serializes a Trajectory to PathWeaver-style JSON. + + * @param trajectory the trajectory to export + + * @return the string containing the serialized JSON + */ + static Trajectory DeserializeTrajectory(wpi::StringRef json_str); +}; +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp new file mode 100644 index 0000000000..999d2bbd85 --- /dev/null +++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp @@ -0,0 +1,23 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/trajectory/TrajectoryConfig.h" +#include "frc/trajectory/TrajectoryUtil.h" +#include "gtest/gtest.h" +#include "trajectory/TestTrajectory.h" + +using namespace frc; + +TEST(TrajectoryJsonTest, DeserializeMatches) { + TrajectoryConfig config{12_fps, 12_fps_sq}; + auto trajectory = TestTrajectory::GetTrajectory(config); + + Trajectory deserialized; + EXPECT_NO_THROW(deserialized = TrajectoryUtil::DeserializeTrajectory( + TrajectoryUtil::SerializeTrajectory(trajectory))); + EXPECT_EQ(trajectory.States(), deserialized.States()); +} diff --git a/wpilibj/CMakeLists.txt b/wpilibj/CMakeLists.txt index ad554c7d23..7874f7fdf4 100644 --- a/wpilibj/CMakeLists.txt +++ b/wpilibj/CMakeLists.txt @@ -15,9 +15,10 @@ if (NOT WITHOUT_JAVA) configure_file(src/generate/WPILibVersion.java.in WPILibVersion.java) file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) - file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/*.jar") + file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/*.jar") + file(GLOB JACKSON_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar") - add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpiutil_jar OUTPUT_NAME wpilibj) + add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpiutil_jar OUTPUT_NAME wpilibj) get_property(WPILIBJ_JAR_FILE TARGET wpilibj_jar PROPERTY JAR_FILE) install(FILES ${WPILIBJ_JAR_FILE} DESTINATION "${java_lib_dest}") diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java index f3f76a7ff1..018e1ecee4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java @@ -7,11 +7,25 @@ package edu.wpi.first.wpilibj.geometry; +import java.io.IOException; import java.util.Objects; +import com.fasterxml.jackson.core.JsonGenerator; +import com.fasterxml.jackson.core.JsonParser; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationContext; +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.SerializerProvider; +import com.fasterxml.jackson.databind.annotation.JsonDeserialize; +import com.fasterxml.jackson.databind.annotation.JsonSerialize; +import com.fasterxml.jackson.databind.deser.std.StdDeserializer; +import com.fasterxml.jackson.databind.ser.std.StdSerializer; + /** * Represents a 2d pose containing translational and rotational elements. */ +@JsonSerialize(using = Pose2d.PoseSerializer.class) +@JsonDeserialize(using = Pose2d.PoseDeserializer.class) public class Pose2d { private final Translation2d m_translation; private final Rotation2d m_rotation; @@ -220,4 +234,38 @@ public class Pose2d { public int hashCode() { return Objects.hash(m_translation, m_rotation); } + + static class PoseSerializer extends StdSerializer { + PoseSerializer() { + super(Pose2d.class); + } + + @Override + public void serialize( + Pose2d value, JsonGenerator jgen, SerializerProvider provider) + throws IOException, JsonProcessingException { + + jgen.writeStartObject(); + jgen.writeObjectField("translation", value.m_translation); + jgen.writeObjectField("rotation", value.m_rotation); + jgen.writeEndObject(); + } + } + + static class PoseDeserializer extends StdDeserializer { + PoseDeserializer() { + super(Pose2d.class); + } + + @Override + public Pose2d deserialize(JsonParser jp, DeserializationContext ctxt) + throws IOException, JsonProcessingException { + JsonNode node = jp.getCodec().readTree(jp); + + Translation2d translation = + jp.getCodec().treeToValue(node.get("translation"), Translation2d.class); + Rotation2d rotation = jp.getCodec().treeToValue(node.get("rotation"), Rotation2d.class); + return new Pose2d(translation, rotation); + } + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java index f697a63907..288fa5c8aa 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java @@ -7,12 +7,26 @@ package edu.wpi.first.wpilibj.geometry; +import java.io.IOException; import java.util.Objects; +import com.fasterxml.jackson.core.JsonGenerator; +import com.fasterxml.jackson.core.JsonParser; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationContext; +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.SerializerProvider; +import com.fasterxml.jackson.databind.annotation.JsonDeserialize; +import com.fasterxml.jackson.databind.annotation.JsonSerialize; +import com.fasterxml.jackson.databind.deser.std.StdDeserializer; +import com.fasterxml.jackson.databind.ser.std.StdSerializer; + /** * A rotation in a 2d coordinate frame represented a point on the unit circle * (cosine and sine). */ +@JsonSerialize(using = Rotation2d.RotationSerializer.class) +@JsonDeserialize(using = Rotation2d.RotationDeserializer.class) public class Rotation2d { private final double m_value; private final double m_cos; @@ -203,4 +217,35 @@ public class Rotation2d { public int hashCode() { return Objects.hash(m_value); } + + static class RotationSerializer extends StdSerializer { + RotationSerializer() { + super(Rotation2d.class); + } + + @Override + public void serialize( + Rotation2d value, JsonGenerator jgen, SerializerProvider provider) + throws IOException, JsonProcessingException { + + jgen.writeStartObject(); + jgen.writeNumberField("radians", value.m_value); + jgen.writeEndObject(); + } + } + + static class RotationDeserializer extends StdDeserializer { + RotationDeserializer() { + super(Rotation2d.class); + } + + @Override + public Rotation2d deserialize(JsonParser jp, DeserializationContext ctxt) + throws IOException, JsonProcessingException { + JsonNode node = jp.getCodec().readTree(jp); + double radians = node.get("radians").numberValue().doubleValue(); + + return new Rotation2d(radians); + } + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java index 49bd3f52fe..73b0257dc3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java @@ -7,8 +7,20 @@ package edu.wpi.first.wpilibj.geometry; +import java.io.IOException; import java.util.Objects; +import com.fasterxml.jackson.core.JsonGenerator; +import com.fasterxml.jackson.core.JsonParser; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationContext; +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.SerializerProvider; +import com.fasterxml.jackson.databind.annotation.JsonDeserialize; +import com.fasterxml.jackson.databind.annotation.JsonSerialize; +import com.fasterxml.jackson.databind.deser.std.StdDeserializer; +import com.fasterxml.jackson.databind.ser.std.StdSerializer; + /** * Represents a translation in 2d space. * This object can be used to represent a point or a vector. @@ -17,6 +29,8 @@ import java.util.Objects; * When the robot is placed on the origin, facing toward the X direction, * moving forward increases the X, whereas moving to the left increases the Y. */ +@JsonSerialize(using = Translation2d.TranslationSerializer.class) +@JsonDeserialize(using = Translation2d.TranslationDeserializer.class) @SuppressWarnings({"ParameterName", "MemberName"}) public class Translation2d { private final double m_x; @@ -189,4 +203,37 @@ public class Translation2d { public int hashCode() { return Objects.hash(m_x, m_y); } + + static class TranslationSerializer extends StdSerializer { + TranslationSerializer() { + super(Translation2d.class); + } + + @Override + public void serialize( + Translation2d value, JsonGenerator jgen, SerializerProvider provider) + throws IOException, JsonProcessingException { + + jgen.writeStartObject(); + jgen.writeNumberField("x", value.m_x); + jgen.writeNumberField("y", value.m_y); + jgen.writeEndObject(); + } + } + + static class TranslationDeserializer extends StdDeserializer { + TranslationDeserializer() { + super(Translation2d.class); + } + + @Override + public Translation2d deserialize(JsonParser jp, DeserializationContext ctxt) + throws IOException, JsonProcessingException { + JsonNode node = jp.getCodec().readTree(jp); + double xval = node.get("x").numberValue().doubleValue(); + double yval = node.get("y").numberValue().doubleValue(); + + return new Translation2d(xval, yval); + } + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java index 934602a19b..4f49244015 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java @@ -8,8 +8,11 @@ package edu.wpi.first.wpilibj.trajectory; import java.util.List; +import java.util.Objects; import java.util.stream.Collectors; +import com.fasterxml.jackson.annotation.JsonProperty; + import edu.wpi.first.wpilibj.geometry.Pose2d; /** @@ -137,18 +140,23 @@ public class Trajectory { @SuppressWarnings("MemberName") public static class State { // The time elapsed since the beginning of the trajectory. + @JsonProperty("time") public double timeSeconds; // The speed at that point of the trajectory. + @JsonProperty("velocity") public double velocityMetersPerSecond; // The acceleration at that point of the trajectory. + @JsonProperty("acceleration") public double accelerationMetersPerSecondSq; // The pose at that point of the trajectory. + @JsonProperty("pose") public Pose2d poseMeters; // The curvature at that point of the trajectory. + @JsonProperty("curvature") public double curvatureRadPerMeter; public State() { @@ -228,6 +236,29 @@ public class Trajectory { timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq, poseMeters, curvatureRadPerMeter); } + + @Override + public boolean equals(Object obj) { + if (this == obj) { + return true; + } + if (!(obj instanceof State)) { + return false; + } + State state = (State) obj; + return Double.compare(state.timeSeconds, timeSeconds) == 0 + && Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0 + && Double.compare(state.accelerationMetersPerSecondSq, + accelerationMetersPerSecondSq) == 0 + && Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0 + && Objects.equals(poseMeters, state.poseMeters); + } + + @Override + public int hashCode() { + return Objects.hash(timeSeconds, velocityMetersPerSecond, + accelerationMetersPerSecondSq, poseMeters, curvatureRadPerMeter); + } } @Override diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java index 7bff2cc3ba..7051a0fdde 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java @@ -59,7 +59,7 @@ public class TrajectoryConfig { * @param constraints List of user-defined constraints. * @return Instance of the current config object. */ - public TrajectoryConfig addConstraints(List constraints) { + public TrajectoryConfig addConstraints(List constraints) { m_constraints.addAll(constraints); return this; } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java index aa3840a1e0..a4d6898597 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java @@ -8,6 +8,7 @@ package edu.wpi.first.wpilibj.trajectory; import java.util.ArrayList; +import java.util.Collection; import java.util.List; import edu.wpi.first.wpilibj.geometry.Pose2d; @@ -160,8 +161,7 @@ public final class TrajectoryGenerator { @SuppressWarnings("LocalVariableName") public static Trajectory generateTrajectory(List waypoints, TrajectoryConfig config) { var originalList = SplineHelper.getQuinticControlVectorsFromWaypoints(waypoints); - var newList = new ControlVectorList(); - newList.addAll(originalList); + var newList = new ControlVectorList(originalList); return generateTrajectory(newList, config); } @@ -194,6 +194,17 @@ public final class TrajectoryGenerator { } // Work around type erasure signatures - private static class ControlVectorList extends ArrayList { + public static class ControlVectorList extends ArrayList { + public ControlVectorList(int initialCapacity) { + super(initialCapacity); + } + + public ControlVectorList() { + super(); + } + + public ControlVectorList(Collection collection) { + super(collection); + } } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java new file mode 100644 index 0000000000..f4b1b12349 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory; + +import java.io.BufferedReader; +import java.io.BufferedWriter; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.util.Arrays; + +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; +import com.fasterxml.jackson.databind.ObjectReader; +import com.fasterxml.jackson.databind.ObjectWriter; + +public final class TrajectoryUtil { + private static final ObjectReader READER = new ObjectMapper().readerFor(Trajectory.State[].class); + private static final ObjectWriter WRITER = new ObjectMapper().writerFor(Trajectory.State[].class); + + private TrajectoryUtil() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Imports a Trajectory from a PathWeaver-style JSON file. + * @param path the path of the json file to import from + * @return The trajectory represented by the file. + * @throws IOException if reading from the file fails + */ + public static Trajectory fromPathweaverJson(Path path) throws IOException { + try (BufferedReader reader = Files.newBufferedReader(path)) { + Trajectory.State[] state = READER.readValue(reader); + return new Trajectory(Arrays.asList(state)); + } + } + + /** + * Exports a Trajectory to a PathWeaver-style JSON file. + * @param trajectory the trajectory to export + * @param path the path of the file to export to + * @throws IOException if writing to the file fails + */ + public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException { + try (BufferedWriter writer = Files.newBufferedWriter(path)) { + WRITER.writeValue(writer, trajectory.getStates().toArray(new Trajectory.State[0])); + } + } + + /** + * Deserializes a Trajectory from PathWeaver-style JSON. + * @param json the string containing the serialized JSON + * @return the trajectory represented by the JSON + * @throws JsonProcessingException if deserializing the JSON fails + */ + public static Trajectory deserializeTrajectory(String json) throws JsonProcessingException { + Trajectory.State[] state = READER.readValue(json); + return new Trajectory(Arrays.asList(state)); + } + + /** + * Serializes a Trajectory to PathWeaver-style JSON. + * @param trajectory the trajectory to export + * @return the string containing the serialized JSON + * @throws JsonProcessingException if serializing the Trajectory fails + */ + public static String serializeTrajectory(Trajectory trajectory) throws JsonProcessingException { + return WRITER.writeValueAsString(trajectory.getStates().toArray(new Trajectory.State[0])); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java index 19b93740e9..5b45e3e590 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java @@ -23,7 +23,7 @@ import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertTrue; class TrajectoryGeneratorTest { - static Trajectory getTrajectory(List constraints) { + static Trajectory getTrajectory(List constraints) { final double maxVelocity = feetToMeters(12.0); final double maxAccel = feetToMeters(12); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java new file mode 100644 index 0000000000..e389a4fb79 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory; + +import java.util.List; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class TrajectoryJsonTest { + @Test + void deserializeMatches() { + var config = List.of(new DifferentialDriveKinematicsConstraint( + new DifferentialDriveKinematics(20), 3)); + var trajectory = TrajectoryGeneratorTest.getTrajectory(config); + + var deserialized = + assertDoesNotThrow(() -> + TrajectoryUtil.deserializeTrajectory(TrajectoryUtil.serializeTrajectory(trajectory))); + + assertEquals(trajectory.getStates(), deserialized.getStates()); + } +} diff --git a/wpiutil/CMakeLists.txt b/wpiutil/CMakeLists.txt index 0ec1724198..93e0191158 100644 --- a/wpiutil/CMakeLists.txt +++ b/wpiutil/CMakeLists.txt @@ -14,9 +14,9 @@ if (NOT WITHOUT_JAVA) include(UseJava) set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked") - if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml-simple-0.38.jar") + if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/ejml-simple-0.38.jar") set(BASE_URL "https://search.maven.org/remotecontent?filepath=") - set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpiutil/thirdparty") + set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml") message(STATUS "Downloading EJML jarfiles...") @@ -38,8 +38,27 @@ if (NOT WITHOUT_JAVA) message(STATUS "All files downloaded.") endif() + if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/jackson-core-2.10.0.jar") + set(BASE_URL "https://search.maven.org/remotecontent?filepath=") + set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson") + + message(STATUS "Downloading Jackson jarfiles...") + + file(DOWNLOAD "${BASE_URL}com/fasterxml/jackson/core/jackson-core/2.10.0/jackson-core-2.10.0.jar" + "${JAR_ROOT}/jackson-core-2.10.0.jar") + file(DOWNLOAD "${BASE_URL}com/fasterxml/jackson/core/jackson-databind/2.10.0/jackson-databind-2.10.0.jar" + "${JAR_ROOT}/jackson-databind-2.10.0.jar") + file(DOWNLOAD "${BASE_URL}com/fasterxml/jackson/core/jackson-annotations/2.10.0/jackson-annotations-2.10.0.jar" + "${JAR_ROOT}/jackson-annotations-2.10.0.jar") + + message(STATUS "All files downloaded.") + endif() + file(GLOB EJML_JARS - ${CMAKE_BINARY_DIR}/wpiutil/thirdparty/*.jar) + ${CMAKE_BINARY_DIR}/wpiutil/thirdparty/ejml/*.jar) + + file(GLOB JACKSON_JARS + ${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar) set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${EJML_JARS}) @@ -51,9 +70,9 @@ if (NOT WITHOUT_JAVA) if(${CMAKE_VERSION} VERSION_LESS "3.11.0") set(CMAKE_JAVA_COMPILE_FLAGS "-h" "${CMAKE_CURRENT_BINARY_DIR}/jniheaders") - add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} OUTPUT_NAME wpiutil) + add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} ${JACKSON_JARS} OUTPUT_NAME wpiutil) else() - add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} OUTPUT_NAME wpiutil GENERATE_NATIVE_HEADERS wpiutil_jni_headers) + add_jar(wpiutil_jar ${JAVA_SOURCES} INCLUDE_JARS ${EJML_JARS} ${JACKSON_JARS} OUTPUT_NAME wpiutil GENERATE_NATIVE_HEADERS wpiutil_jni_headers) endif() get_property(WPIUTIL_JAR_FILE TARGET wpiutil_jar PROPERTY JAR_FILE) diff --git a/wpiutil/build.gradle b/wpiutil/build.gradle index 588316ef8b..9af91a7891 100644 --- a/wpiutil/build.gradle +++ b/wpiutil/build.gradle @@ -243,6 +243,9 @@ model { dependencies { compile "org.ejml:ejml-simple:0.38" + compile "com.fasterxml.jackson.core:jackson-annotations:2.10.0" + compile "com.fasterxml.jackson.core:jackson-core:2.10.0" + compile "com.fasterxml.jackson.core:jackson-databind:2.10.0" } def wpilibNumberFileInput = file("src/generate/GenericNumber.java.in")