Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add JSON support for Trajectories (#2025)

Browse Source
This commit is contained in:
carbotaniuman
2019-11-02 13:35:03 -05:00
committed by Peter Johnson
parent 2b6811eddb
commit ed30d5d40e
23 changed files with 581 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -9,6 +9,8 @@
#include <cmath>
#include <wpi/json.h>
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<Translation2d>(),
json.at("rotation").get<Rotation2d>()};
}

10
wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
View File

@@ -9,6 +9,8 @@
#include <cmath>
#include <wpi/json.h>
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<double>()}};
}
void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
}

12
wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -7,6 +7,8 @@
#include "frc/geometry/Translation2d.h"
#include <wpi/json.h>
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<double>()},
{"y", translation.Y().to<double>()}};
}
void frc::from_json(const wpi::json& json, Translation2d& translation) {
translation = Translation2d{units::meter_t{json.at("x").get<double>()},
units::meter_t{json.at("y").get<double>()}};
}

30
wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
View File

@@ -7,8 +7,20 @@
#include "frc/trajectory/Trajectory.h"
#include <wpi/json.h>
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<double>()},
{"velocity", state.velocity.to<double>()},
{"acceleration", state.acceleration.to<double>()},
{"pose", state.pose},
{"curvature", state.curvature.to<double>()}};
}
void frc::from_json(const wpi::json& json, Trajectory::State& state) {
state.pose = json.at("pose").get<Pose2d>();
state.t = units::second_t{json.at("time").get<double>()};
state.velocity =
units::meters_per_second_t{json.at("velocity").get<double>()};
state.acceleration =
units::meters_per_second_squared_t{json.at("acceleration").get<double>()};
state.curvature = frc::curvature_t{json.at("curvature").get<double>()};
}

58
wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp Normal file
View File

@@ -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 <system_error>
#include <wpi/SmallString.h>
#include <wpi/json.h>
#include <wpi/raw_istream.h>
#include <wpi/raw_ostream.h>
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::vector<Trajectory::State>>()};
}
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<std::vector<Trajectory::State>>()};
}

9
wpilibc/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -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

9
wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -10,6 +10,10 @@
#include <units/units.h>
#include <wpi/math>
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

9
wpilibc/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -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

25
wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
View File

@@ -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

59
wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h Normal file
View File

@@ -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 <string>
#include <wpi/StringRef.h>
#include <wpi/Twine.h>
#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

23
wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp Normal file
View File

@@ -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());
}

5
wpilibj/CMakeLists.txt
View File

@@ -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}")

48
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
View File

@@ -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<Pose2d> {
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<Pose2d> {
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);
}
}
}

45
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
View File

@@ -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<Rotation2d> {
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<Rotation2d> {
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);
}
}
}

47
wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
View File

@@ -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<Translation2d> {
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<Translation2d> {
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);
}
}
}

31
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
View File

@@ -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

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
View File

@@ -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<TrajectoryConstraint> constraints) {
public TrajectoryConfig addConstraints(List<? extends TrajectoryConstraint> constraints) {
m_constraints.addAll(constraints);
return this;
}

17
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
View File

@@ -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<Pose2d> 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<Spline.ControlVector> {
public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
public ControlVectorList(int initialCapacity) {
super(initialCapacity);
}
public ControlVectorList() {
super();
}
public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
super(collection);
}
}
}

75
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java Normal file
View File

@@ -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]));
}
}

2
wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
View File

@@ -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<TrajectoryConstraint> constraints) {
static Trajectory getTrajectory(List<? extends TrajectoryConstraint> constraints) {
final double maxVelocity = feetToMeters(12.0);
final double maxAccel = feetToMeters(12);

33
wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java Normal file
View File

@@ -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());
}
}

29
wpiutil/CMakeLists.txt
View File

@@ -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)

3
wpiutil/build.gradle
View File

@@ -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")