Upgrade telemetry to use structs

lib/pages/telemetry_page.dart

@@ -31,7 +31,7 @@ class _TelemetryPageState extends State<TelemetryPage> {
   bool _connected = false;
   final List<List<num>> _velData = [];
   final List<num> _inaccuracyData = [];
-  List<num>? _currentPath;
+  List<Pose2d>? _currentPath;
   Pose2d? _currentPose;
   Pose2d? _targetPose;
   late final Size _robotSize;
@@ -79,29 +79,15 @@ class _TelemetryPageState extends State<TelemetryPage> {
     widget.telemetry.currentPoseStream().listen((pose) {
       if (mounted) {
         setState(() {
-          if (pose == null) {
-            _currentPose = null;
-          } else {
-            _currentPose = Pose2d(
-              Translation2d(pose[0], pose[1]),
-              Rotation2d(pose[2]),
-            );
-          }
+          _currentPose = pose;
         });
       }
     });
 
     widget.telemetry.targetPoseStream().listen((pose) {
       if (mounted) {
         setState(() {
-          if (pose == null) {
-            _targetPose = null;
-          } else {
-            _targetPose = Pose2d(
-              Translation2d(pose[0], pose[1]),
-              Rotation2d(pose[2]),
-            );
-          }
+          _targetPose = pose;
         });
       }
     });
@@ -491,7 +477,7 @@ class TelemetryPainter extends CustomPainter {
   final Size robotSize;
   final Pose2d? currentPose;
   final Pose2d? targetPose;
-  final List<num>? currentPath;
+  final List<Pose2d>? currentPath;
 
   static double scale = 1;
 
@@ -514,11 +500,9 @@ class TelemetryPainter extends CustomPainter {
         ..strokeWidth = 2;
 
       Path path = Path();
-      for (int i = 0; i < currentPath!.length - 3; i += 3) {
+      for (int i = 0; i < currentPath!.length; i++) {
         Offset offset = PathPainterUtil.pointToPixelOffset(
-            Translation2d(currentPath![i], currentPath![i + 1]),
-            scale,
-            fieldImage);
+            currentPath![i].translation, scale, fieldImage);
         if (i == 0) {
           path.moveTo(offset.dx, offset.dy);
         } else {

lib/services/pplib_telemetry.dart

@@ -1,8 +1,10 @@
 import 'dart:convert';
+import 'dart:typed_data';
 
 import 'package:nt4/nt4.dart';
 import 'package:pathplanner/auto/pathplanner_auto.dart';
 import 'package:pathplanner/path/pathplanner_path.dart';
+import 'package:pathplanner/util/wpimath/geometry.dart';
 
 class PPLibTelemetry {
   late NT4Client _client;
@@ -29,7 +31,7 @@ class PPLibTelemetry {
         _client.subscribePeriodic('/PathPlanner/inaccuracy', 0.033);
     _currentPoseSub =
         _client.subscribePeriodic('/PathPlanner/currentPose', 0.033);
-    _activePathSub = _client.subscribePeriodic('/PathPlanner/activePath', 0.1);
+    _activePathSub = _client.subscribeAllSamples('/PathPlanner/activePath');
     _targetPoseSub =
         _client.subscribePeriodic('/PathPlanner/targetPose', 0.033);
 
@@ -80,26 +82,26 @@ class PPLibTelemetry {
         .map((inaccuracy) => (inaccuracy as num?) ?? 0);
   }
 
-  Stream<List<num>?> currentPoseStream() {
-    return _currentPoseSub
-        .stream()
-        .map((pose) => (pose as List?)?.map((e) => e as num).toList());
+  Stream<Pose2d?> currentPoseStream() {
+    return _currentPoseSub.stream().map((pose) => (pose is List<int>)
+        ? Pose2d.fromBytes(Uint8List.fromList(pose))
+        : null);
   }
 
-  Stream<List<num>?> currentPathStream() {
-    return _activePathSub
-        .stream()
-        .map((path) => (path as List?)?.map((e) => e as num).toList());
+  Stream<List<Pose2d>?> currentPathStream() {
+    return _activePathSub.stream().map((poses) => (poses is List<int>)
+        ? Pose2d.listFromBytes(Uint8List.fromList(poses))
+        : null);
   }
 
   Stream<bool> connectionStatusStream() {
     return _client.connectionStatusStream().asBroadcastStream();
   }
 
-  Stream<List<num>?> targetPoseStream() {
-    return _targetPoseSub
-        .stream()
-        .map((pose) => (pose as List?)?.map((e) => e as num).toList());
+  Stream<Pose2d?> targetPoseStream() {
+    return _targetPoseSub.stream().map((pose) => (pose is List<int>)
+        ? Pose2d.fromBytes(Uint8List.fromList(pose))
+        : null);
   }
 
   bool get isConnected => _isConnected;

lib/util/wpimath/geometry.dart

@@ -16,26 +16,22 @@ class Pose2d {
   factory Pose2d.fromBytes(Uint8List bytes) {
     final view = ByteData.view(bytes.buffer);
 
-    int length = view.lengthInBytes;
-
-    double xMeters = 0.0;
-    double yMeters = 0.0;
-    double angleRadians = 0.0;
-
-    if (length >= 8) {
-      xMeters = view.getFloat64(0, Endian.little);
-    }
-    if (length >= 16) {
-      yMeters = view.getFloat64(8, Endian.little);
-    }
-    if (length >= 24) {
-      angleRadians = view.getFloat64(16, Endian.little);
-    }
+    double xMeters = view.getFloat64(0, Endian.little);
+    double yMeters = view.getFloat64(8, Endian.little);
+    double angleRadians = view.getFloat64(16, Endian.little);
 
     return Pose2d(
         Translation2d(xMeters, yMeters), Rotation2d.fromRadians(angleRadians));
   }
 
+  static List<Pose2d> listFromBytes(Uint8List bytes) {
+    List<Pose2d> poses = [];
+    for (int i = 0; i < bytes.length; i += 24) {
+      poses.add(Pose2d.fromBytes(bytes.sublist(i, i + 24)));
+    }
+    return poses;
+  }
+
   Pose2d interpolate(Pose2d endValue, num t) {
     if (t < 0) {
       return this;

pathplannerlib-python/pathplannerlib/telemetry.py

@@ -1,4 +1,4 @@
-from ntcore import DoubleArrayPublisher, DoublePublisher, NetworkTableInstance
+from ntcore import DoubleArrayPublisher, DoublePublisher, NetworkTableInstance, StructPublisher, StructArrayPublisher
 from wpimath.geometry import Pose2d
 from .path import PathPlannerPath
 
@@ -7,12 +7,12 @@ class PPLibTelemetry:
     _velPub: DoubleArrayPublisher = NetworkTableInstance.getDefault().getDoubleArrayTopic('/PathPlanner/vel').publish()
     _inaccuracyPub: DoublePublisher = NetworkTableInstance.getDefault().getDoubleTopic(
         '/PathPlanner/inaccuracy').publish()
-    _posePub: DoubleArrayPublisher = NetworkTableInstance.getDefault().getDoubleArrayTopic(
-        '/PathPlanner/currentPose').publish()
-    _pathPub: DoubleArrayPublisher = NetworkTableInstance.getDefault().getDoubleArrayTopic(
-        '/PathPlanner/activePath').publish()
-    _targetPosePub: DoubleArrayPublisher = NetworkTableInstance.getDefault().getDoubleArrayTopic(
-        '/PathPlanner/targetPose').publish()
+    _posePub: StructPublisher = NetworkTableInstance.getDefault().getStructTopic(
+        '/PathPlanner/currentPose', Pose2d.WPIStruct).publish()
+    _pathPub: StructArrayPublisher = NetworkTableInstance.getDefault().getStructArrayTopic(
+        '/PathPlanner/activePath', Pose2d.WPIStruct).publish()
+    _targetPosePub: StructPublisher = NetworkTableInstance.getDefault().getStructTopic(
+        '/PathPlanner/targetPose', Pose2d.WPIStruct).publish()
 
     @staticmethod
     def setVelocities(actual_vel: float, commanded_vel: float, actual_ang_vel: float, commanded_ang_vel: float) -> None:
@@ -24,17 +24,12 @@ def setPathInaccuracy(inaccuracy: float) -> None:
 
     @staticmethod
     def setCurrentPose(pose: Pose2d) -> None:
-        PPLibTelemetry._posePub.set([pose.X(), pose.Y(), pose.rotation().radians()])
+        PPLibTelemetry._posePub.set(pose)
 
     @staticmethod
     def setTargetPose(pose: Pose2d) -> None:
-        PPLibTelemetry._targetPosePub.set([pose.X(), pose.Y(), pose.rotation().radians()])
+        PPLibTelemetry._targetPosePub.set(pose)
 
     @staticmethod
     def setCurrentPath(path: PathPlannerPath) -> None:
-        arr = []
-
-        for p in path.getAllPathPoints():
-            arr.extend([p.position.X(), p.position.Y(), 0.0])
-
-        PPLibTelemetry._pathPub.set(arr)
+        PPLibTelemetry._pathPub.set(path.getPathPoses())

pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java

@@ -2,9 +2,7 @@
 
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import com.pathplanner.lib.path.PathPlannerPath;
-import com.pathplanner.lib.path.PathPoint;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.*;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
@@ -24,12 +22,18 @@ public class PPLibTelemetry {
       NetworkTableInstance.getDefault().getDoubleArrayTopic("/PathPlanner/vel").publish();
   private static final DoublePublisher inaccuracyPub =
       NetworkTableInstance.getDefault().getDoubleTopic("/PathPlanner/inaccuracy").publish();
-  private static final DoubleArrayPublisher posePub =
-      NetworkTableInstance.getDefault().getDoubleArrayTopic("/PathPlanner/currentPose").publish();
-  private static final DoubleArrayPublisher pathPub =
-      NetworkTableInstance.getDefault().getDoubleArrayTopic("/PathPlanner/activePath").publish();
-  private static final DoubleArrayPublisher targetPosePub =
-      NetworkTableInstance.getDefault().getDoubleArrayTopic("/PathPlanner/targetPose").publish();
+  private static final StructPublisher<Pose2d> posePub =
+      NetworkTableInstance.getDefault()
+          .getStructTopic("/PathPlanner/currentPose", Pose2d.struct)
+          .publish();
+  private static final StructArrayPublisher<Pose2d> pathPub =
+      NetworkTableInstance.getDefault()
+          .getStructArrayTopic("/PathPlanner/activePath", Pose2d.struct)
+          .publish();
+  private static final StructPublisher<Pose2d> targetPosePub =
+      NetworkTableInstance.getDefault()
+          .getStructTopic("/PathPlanner/targetPose", Pose2d.struct)
+          .publish();
 
   private static final Map<String, List<PathPlannerPath>> hotReloadPaths = new HashMap<>();
   private static final Map<String, List<PathPlannerAuto>> hotReloadAutos = new HashMap<>();
@@ -69,7 +73,7 @@ public static void setPathInaccuracy(double inaccuracy) {
    * @param pose Current robot pose
    */
   public static void setCurrentPose(Pose2d pose) {
-    posePub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getRadians()});
+    posePub.set(pose);
   }
 
   /**
@@ -78,19 +82,8 @@ public static void setCurrentPose(Pose2d pose) {
    * @param path The current path
    */
   public static void setCurrentPath(PathPlannerPath path) {
-    double[] arr = new double[path.numPoints() * 3];
-
-    int ndx = 0;
-    for (PathPoint p : path.getAllPathPoints()) {
-      Translation2d pos = p.position;
-      arr[ndx] = pos.getX();
-      arr[ndx + 1] = pos.getY();
-      // Just add 0 as a heading since it's not needed for displaying a path
-      arr[ndx + 2] = 0.0;
-      ndx += 3;
-    }
-
-    pathPub.set(arr);
+    // Use poses for simplicity
+    pathPub.set(path.getPathPoses().toArray(new Pose2d[0]));
   }
 
   /**
@@ -99,8 +92,7 @@ public static void setCurrentPath(PathPlannerPath path) {
    * @param targetPose Target robot pose
    */
   public static void setTargetPose(Pose2d targetPose) {
-    targetPosePub.set(
-        new double[] {targetPose.getX(), targetPose.getY(), targetPose.getRotation().getRadians()});
+    targetPosePub.set(targetPose);
   }
 
   /**

pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PPLibTelemetry.cpp

@@ -15,15 +15,15 @@ nt::DoubleArrayPublisher PPLibTelemetry::m_velPub =
 nt::DoublePublisher PPLibTelemetry::m_inaccuracyPub =
 		nt::NetworkTableInstance::GetDefault().GetDoubleTopic(
 				"/PathPlanner/inaccuracy").Publish();
-nt::DoubleArrayPublisher PPLibTelemetry::m_posePub =
-		nt::NetworkTableInstance::GetDefault().GetDoubleArrayTopic(
-				"/PathPlanner/currentPose").Publish();
-nt::DoubleArrayPublisher PPLibTelemetry::m_pathPub =
-		nt::NetworkTableInstance::GetDefault().GetDoubleArrayTopic(
-				"/PathPlanner/activePath").Publish();
-nt::DoubleArrayPublisher PPLibTelemetry::m_targetPosePub =
-		nt::NetworkTableInstance::GetDefault().GetDoubleArrayTopic(
-				"/PathPlanner/targetPose").Publish();
+nt::StructPublisher<frc::Pose2d> PPLibTelemetry::m_posePub =
+		nt::NetworkTableInstance::GetDefault().GetStructTopic < frc::Pose2d
+				> ("/PathPlanner/currentPose").Publish();
+nt::StructArrayPublisher<frc::Pose2d> PPLibTelemetry::m_pathPub =
+		nt::NetworkTableInstance::GetDefault().GetStructArrayTopic < frc::Pose2d
+				> ("/PathPlanner/activePath").Publish();
+nt::StructPublisher<frc::Pose2d> PPLibTelemetry::m_targetPosePub =
+		nt::NetworkTableInstance::GetDefault().GetStructTopic < frc::Pose2d
+				> ("/PathPlanner/targetPose").Publish();
 
 std::unordered_map<std::string, std::vector<std::shared_ptr<PathPlannerPath>>> PPLibTelemetry::m_hotReloadPaths =
 		std::unordered_map<std::string,
@@ -32,20 +32,6 @@ std::unordered_map<std::string, std::vector<std::shared_ptr<PathPlannerPath>>> P
 std::optional<NT_Listener> PPLibTelemetry::m_hotReloadPathListener =
 		std::nullopt;
 
-void PPLibTelemetry::setCurrentPath(std::shared_ptr<PathPlannerPath> path) {
-	std::vector<double> arr;
-
-	for (const PathPoint &p : path->getAllPathPoints()) {
-		frc::Translation2d pos = p.position;
-		arr.push_back(pos.X()());
-		arr.push_back(pos.Y()());
-		// Just add 0 as a heading since it's not needed for displaying a path
-		arr.push_back(0.0);
-	}
-
-	m_pathPub.Set(std::span { arr.data(), arr.size() });
-}
-
 void PPLibTelemetry::ensureHotReloadListenersInitialized() {
 	if (!m_hotReloadPathListener) {
 		nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();

pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h

@@ -3,6 +3,8 @@
 #include <networktables/NetworkTableInstance.h>
 #include <networktables/DoubleArrayTopic.h>
 #include <networktables/DoubleTopic.h>
+#include <networktables/StructTopic.h>
+#include <networktables/StructArrayTopic.h>
 #include <networktables/NetworkTableListener.h>
 #include <string>
 #include <unordered_map>
@@ -37,16 +39,16 @@ class PPLibTelemetry {
 	}
 
 	static inline void setCurrentPose(frc::Pose2d pose) {
-		m_posePub.Set(
-				std::span<const double>( { pose.X()(), pose.Y()(),
-						pose.Rotation().Radians()() }));
+		m_posePub.Set(pose);
 	}
 
-	static void setCurrentPath(std::shared_ptr<PathPlannerPath> path);
+	static inline void setCurrentPath(std::shared_ptr<PathPlannerPath> path) {
+		auto poses = path->getPathPoses();
+		m_pathPub.Set(std::span { poses.data(), poses.size() });
+	}
 
 	static inline void setTargetPose(frc::Pose2d targetPose) {
-		m_targetPosePub.Set(std::span<const double>( { targetPose.X()(),
-				targetPose.Y()(), targetPose.Rotation().Radians()() }));
+		m_targetPosePub.Set(targetPose);
 	}
 
 	static void registerHotReloadPath(std::string pathName,
@@ -61,9 +63,9 @@ class PPLibTelemetry {
 
 	static nt::DoubleArrayPublisher m_velPub;
 	static nt::DoublePublisher m_inaccuracyPub;
-	static nt::DoubleArrayPublisher m_posePub;
-	static nt::DoubleArrayPublisher m_pathPub;
-	static nt::DoubleArrayPublisher m_targetPosePub;
+	static nt::StructPublisher<frc::Pose2d> m_posePub;
+	static nt::StructArrayPublisher<frc::Pose2d> m_pathPub;
+	static nt::StructPublisher<frc::Pose2d> m_targetPosePub;
 
 	static std::unordered_map<std::string,
 			std::vector<std::shared_ptr<PathPlannerPath>>> m_hotReloadPaths;