From eef70197cdb89c4b121b088a88d5cc702880dfba Mon Sep 17 00:00:00 2001 From: Michael Jansen Date: Mon, 30 Sep 2024 21:47:08 -0400 Subject: [PATCH] Upgrade telemetry to use structs --- lib/pages/telemetry_page.dart | 28 ++-------- lib/services/pplib_telemetry.dart | 28 +++++----- lib/util/wpimath/geometry.dart | 26 ++++----- .../pathplannerlib/telemetry.py | 25 ++++----- .../pathplanner/lib/util/PPLibTelemetry.java | 40 ++++++------- .../pathplanner/lib/util/PPLibTelemetry.cpp | 32 +++-------- .../pathplanner/lib/util/PPLibTelemetry.h | 20 ++++--- test/pages/telemetry_page_test.dart | 30 ++++++---- test/util/wpimath/geometry_test.dart | 56 +++++++++++++------ 9 files changed, 135 insertions(+), 150 deletions(-) diff --git a/lib/pages/telemetry_page.dart b/lib/pages/telemetry_page.dart index 77297ef3..cda5cb06 100644 --- a/lib/pages/telemetry_page.dart +++ b/lib/pages/telemetry_page.dart @@ -31,7 +31,7 @@ class _TelemetryPageState extends State { bool _connected = false; final List> _velData = []; final List _inaccuracyData = []; - List? _currentPath; + List? _currentPath; Pose2d? _currentPose; Pose2d? _targetPose; late final Size _robotSize; @@ -79,14 +79,7 @@ class _TelemetryPageState extends State { 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; }); } }); @@ -94,14 +87,7 @@ class _TelemetryPageState extends State { 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? currentPath; + final List? 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 { diff --git a/lib/services/pplib_telemetry.dart b/lib/services/pplib_telemetry.dart index 9104fd28..119e3f70 100644 --- a/lib/services/pplib_telemetry.dart +++ b/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?> currentPoseStream() { - return _currentPoseSub - .stream() - .map((pose) => (pose as List?)?.map((e) => e as num).toList()); + Stream currentPoseStream() { + return _currentPoseSub.stream().map((pose) => (pose is List) + ? Pose2d.fromBytes(Uint8List.fromList(pose)) + : null); } - Stream?> currentPathStream() { - return _activePathSub - .stream() - .map((path) => (path as List?)?.map((e) => e as num).toList()); + Stream?> currentPathStream() { + return _activePathSub.stream().map((poses) => (poses is List) + ? Pose2d.listFromBytes(Uint8List.fromList(poses)) + : null); } Stream connectionStatusStream() { return _client.connectionStatusStream().asBroadcastStream(); } - Stream?> targetPoseStream() { - return _targetPoseSub - .stream() - .map((pose) => (pose as List?)?.map((e) => e as num).toList()); + Stream targetPoseStream() { + return _targetPoseSub.stream().map((pose) => (pose is List) + ? Pose2d.fromBytes(Uint8List.fromList(pose)) + : null); } bool get isConnected => _isConnected; diff --git a/lib/util/wpimath/geometry.dart b/lib/util/wpimath/geometry.dart index fc10f3a4..6e51b6bc 100644 --- a/lib/util/wpimath/geometry.dart +++ b/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 listFromBytes(Uint8List bytes) { + List 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; diff --git a/pathplannerlib-python/pathplannerlib/telemetry.py b/pathplannerlib-python/pathplannerlib/telemetry.py index 5b57ddd4..57cad3a0 100644 --- a/pathplannerlib-python/pathplannerlib/telemetry.py +++ b/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()) diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java index d6bd9e7f..c1f107f5 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java +++ b/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 posePub = + NetworkTableInstance.getDefault() + .getStructTopic("/PathPlanner/currentPose", Pose2d.struct) + .publish(); + private static final StructArrayPublisher pathPub = + NetworkTableInstance.getDefault() + .getStructArrayTopic("/PathPlanner/activePath", Pose2d.struct) + .publish(); + private static final StructPublisher targetPosePub = + NetworkTableInstance.getDefault() + .getStructTopic("/PathPlanner/targetPose", Pose2d.struct) + .publish(); private static final Map> hotReloadPaths = new HashMap<>(); private static final Map> 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); } /** diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PPLibTelemetry.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PPLibTelemetry.cpp index 514f30e6..23820b25 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/PPLibTelemetry.cpp +++ b/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 PPLibTelemetry::m_posePub = + nt::NetworkTableInstance::GetDefault().GetStructTopic < frc::Pose2d + > ("/PathPlanner/currentPose").Publish(); +nt::StructArrayPublisher PPLibTelemetry::m_pathPub = + nt::NetworkTableInstance::GetDefault().GetStructArrayTopic < frc::Pose2d + > ("/PathPlanner/activePath").Publish(); +nt::StructPublisher PPLibTelemetry::m_targetPosePub = + nt::NetworkTableInstance::GetDefault().GetStructTopic < frc::Pose2d + > ("/PathPlanner/targetPose").Publish(); std::unordered_map>> PPLibTelemetry::m_hotReloadPaths = std::unordered_map>> P std::optional PPLibTelemetry::m_hotReloadPathListener = std::nullopt; -void PPLibTelemetry::setCurrentPath(std::shared_ptr path) { - std::vector 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(); diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h index e1ff5a52..7fe88d2a 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/util/PPLibTelemetry.h @@ -3,6 +3,8 @@ #include #include #include +#include +#include #include #include #include @@ -37,16 +39,16 @@ class PPLibTelemetry { } static inline void setCurrentPose(frc::Pose2d pose) { - m_posePub.Set( - std::span( { pose.X()(), pose.Y()(), - pose.Rotation().Radians()() })); + m_posePub.Set(pose); } - static void setCurrentPath(std::shared_ptr path); + static inline void setCurrentPath(std::shared_ptr 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( { 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 m_posePub; + static nt::StructArrayPublisher m_pathPub; + static nt::StructPublisher m_targetPosePub; static std::unordered_map>> m_hotReloadPaths; diff --git a/test/pages/telemetry_page_test.dart b/test/pages/telemetry_page_test.dart index 20cfa460..0c87a73b 100644 --- a/test/pages/telemetry_page_test.dart +++ b/test/pages/telemetry_page_test.dart @@ -7,6 +7,7 @@ import 'package:mockito/annotations.dart'; import 'package:mockito/mockito.dart'; import 'package:pathplanner/pages/telemetry_page.dart'; import 'package:pathplanner/services/pplib_telemetry.dart'; +import 'package:pathplanner/util/wpimath/geometry.dart'; import 'package:pathplanner/widgets/field_image.dart'; import 'package:shared_preferences/shared_preferences.dart'; @@ -65,9 +66,9 @@ void main() { final connectionStatusController = StreamController(); final velocitiesController = StreamController>(); final inaccuracyController = StreamController(); - final currentPoseController = StreamController>(); - final targetPoseController = StreamController>(); - final currentPathController = StreamController>(); + final currentPoseController = StreamController(); + final targetPoseController = StreamController(); + final currentPathController = StreamController?>(); when(telemetry.getServerAddress()).thenReturn('localhost:5811'); when(telemetry.connectionStatusStream()) @@ -100,9 +101,14 @@ void main() { // Add some data to the streams velocitiesController.add([1.0, 2.0, 0.5, 1.5]); inaccuracyController.add(0.25); - currentPoseController.add([2.1, 2.1, 20]); - targetPoseController.add([2, 2, 0]); - currentPathController.add([1, 5, 2, 4, 3, 5]); + currentPoseController + .add(Pose2d(Translation2d(2.1, 2.1), Rotation2d.fromDegrees(20))); + targetPoseController.add(Pose2d(Translation2d(2, 2), Rotation2d())); + currentPathController.add([ + Pose2d(Translation2d(1, 5), Rotation2d()), + Pose2d(Translation2d(2, 4), Rotation2d()), + Pose2d(Translation2d(3, 5), Rotation2d()) + ]); // Allow time for the streams to emit some values await widgetTester.pump(const Duration(milliseconds: 100)); @@ -133,9 +139,9 @@ void main() { final connectionStatusController = StreamController.broadcast(); final velocitiesController = StreamController>(); final inaccuracyController = StreamController(); - final currentPoseController = StreamController?>(); - final targetPoseController = StreamController?>(); - final currentPathController = StreamController?>(); + final currentPoseController = StreamController(); + final targetPoseController = StreamController(); + final currentPathController = StreamController?>(); when(telemetry.connectionStatusStream()) .thenAnswer((_) => connectionStatusController.stream); @@ -205,9 +211,9 @@ void main() { final connectionStatusController = StreamController.broadcast(); final velocitiesController = StreamController>(); final inaccuracyController = StreamController.broadcast(); - final currentPoseController = StreamController?>(); - final targetPoseController = StreamController?>(); - final currentPathController = StreamController?>(); + final currentPoseController = StreamController(); + final targetPoseController = StreamController(); + final currentPathController = StreamController?>(); when(telemetry.connectionStatusStream()) .thenAnswer((_) => connectionStatusController.stream); diff --git a/test/util/wpimath/geometry_test.dart b/test/util/wpimath/geometry_test.dart index 77eeae42..6bd2b02b 100644 --- a/test/util/wpimath/geometry_test.dart +++ b/test/util/wpimath/geometry_test.dart @@ -33,7 +33,7 @@ void main() { }); group('struct decoding', () { - test('valid data', () { + test('single pose', () { List rawBytes = [ 0x00, 0x00, @@ -69,7 +69,7 @@ void main() { expect(pose.rotation.radians, closeTo(pi, epsilon)); }); - test('missing bytes', () { + test('list of poses', () { List rawBytes = [ 0x00, 0x00, @@ -90,26 +90,48 @@ void main() { 0x18, 0x2d, 0x44, - 0x54 + 0x54, + 0xfb, + 0x21, + 0x09, + 0x40, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x14, + 0x40, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00, + 0x14, + 0x40, + 0x18, + 0x2d, + 0x44, + 0x54, + 0xfb, + 0x21, + 0x09, + 0x40 ]; Uint8List data = Uint8List.fromList(rawBytes); - Pose2d pose = Pose2d.fromBytes(data); + final poses = Pose2d.listFromBytes(data); - expect(pose.x, closeTo(5.0, epsilon)); - expect(pose.y, closeTo(5.0, epsilon)); - expect(pose.rotation.radians, closeTo(0.0, epsilon)); - }); - - test('no bytes', () { - List rawBytes = []; - Uint8List data = Uint8List.fromList(rawBytes); - - Pose2d pose = Pose2d.fromBytes(data); + expect(poses.length, 2); - expect(pose.x, closeTo(0.0, epsilon)); - expect(pose.y, closeTo(0.0, epsilon)); - expect(pose.rotation.radians, closeTo(0.0, epsilon)); + expect(poses[0].x, closeTo(5.0, epsilon)); + expect(poses[0].y, closeTo(5.0, epsilon)); + expect(poses[0].rotation.radians, closeTo(pi, epsilon)); + expect(poses[1].x, closeTo(5.0, epsilon)); + expect(poses[1].y, closeTo(5.0, epsilon)); + expect(poses[1].rotation.radians, closeTo(pi, epsilon)); }); }); });