From 36ea865edccdc526253eb6c063b41d3f8bc3bfa8 Mon Sep 17 00:00:00 2001
From: Banks T <btrout.dhrs@gmail.com>
Date: Sat, 26 Oct 2019 11:18:56 -0400
Subject: [PATCH] Add toString for geometry and trajectory classes (#1991)

---
 .../edu/wpi/first/wpilibj/geometry/Pose2d.java    |  5 +++++
 .../wpi/first/wpilibj/geometry/Rotation2d.java    |  5 +++++
 .../wpi/first/wpilibj/geometry/Transform2d.java   |  5 +++++
 .../wpi/first/wpilibj/geometry/Translation2d.java |  5 +++++
 .../edu/wpi/first/wpilibj/geometry/Twist2d.java   |  5 +++++
 .../wpi/first/wpilibj/trajectory/Trajectory.java  | 15 +++++++++++++++
 6 files changed, 40 insertions(+)

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 8f819ad475..f3f76a7ff1 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
@@ -196,6 +196,11 @@ public class Pose2d {
     return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
   }
 
+  @Override
+  public String toString() {
+    return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
+  }
+
   /**
    * Checks equality between this Pose2d and another object.
    *
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 7b916c1148..f697a63907 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
@@ -180,6 +180,11 @@ public class Rotation2d {
     return m_sin / m_cos;
   }
 
+  @Override
+  public String toString() {
+    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
+  }
+
   /**
    * Checks equality between this Rotation2d and another object.
    *
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
index dceb502078..507ebfe400 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
@@ -79,6 +79,11 @@ public class Transform2d {
     return m_rotation;
   }
 
+  @Override
+  public String toString() {
+    return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
+  }
+
   /**
    * Checks equality between this Transform2d and another object.
    *
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 83028d3863..49bd3f52fe 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
@@ -165,6 +165,11 @@ public class Translation2d {
     return new Translation2d(m_x / scalar, m_y / scalar);
   }
 
+  @Override
+  public String toString() {
+    return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
+  }
+
   /**
    * Checks equality between this Translation2d and another object.
    *
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
index 191a616e96..f2def815e0 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
@@ -48,6 +48,11 @@ public class Twist2d {
     this.dtheta = dtheta;
   }
 
+  @Override
+  public String toString() {
+    return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
+  }
+
   /**
    * Checks equality between this Twist2d and another object.
    *
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 aa92831481..934602a19b 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,6 +8,7 @@
 package edu.wpi.first.wpilibj.trajectory;
 
 import java.util.List;
+import java.util.stream.Collectors;
 
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 
@@ -219,5 +220,19 @@ public class Trajectory {
           lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac)
       );
     }
+
+    @Override
+    public String toString() {
+      return String.format(
+        "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
+        timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq,
+        poseMeters, curvatureRadPerMeter);
+    }
+  }
+
+  @Override
+  public String toString() {
+    String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n"));
+    return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList);
   }
 }