Add JSON support for Trajectories (#2025)

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<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);
+    }
+  }
 }

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<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);
+    }
+  }
 }

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<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);
+    }
+  }
 }

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

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

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<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);
+    }
   }
 }

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