Add JSON support for Trajectories (#2025)

wpilibc/src/main/native/cpp/geometry/Pose2d.cpp

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

wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp

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

wpilibc/src/main/native/cpp/geometry/Translation2d.cpp

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

wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp

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

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

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