Add JSON support for Trajectories (#2025)

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

wpilibc/src/main/native/include/frc/geometry/Rotation2d.h

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

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

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

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