Ability to add custom features to robot preview

Writerside/topics/Robot-Config.md

@@ -81,8 +81,7 @@ True Max Drive Speed
 >
 > It is very important that the True Max Drive Speed is measured for your actual robot. This value is not simply a "max
 > velocity" limit on the robot. It encodes information about how much motor torque can actually be used to accelerate
-> the
-> robot.
+> the robot.
 >
 > This can be easily measured by driving the robot in a straight line as fast as possible on a charged battery, and
 > measuring the robot's maximum velocity.
@@ -107,4 +106,9 @@ Drive Current Limit
 ## Module Offsets
 
 The locations of each swerve module relative to the center of the robot, in meters. These should be the same offsets
-used to create your kinematics in code. Only available for swerve drive robots.
+used to create your kinematics in code. Only available for swerve drive robots.
+
+## Robot Features
+
+Robot features are shapes that can be added to the robot in the GUI. These can be used to represent intakes, shooting
+trajectories, etc. Current features include: rectangle, circle, and lines.

lib/commands/command.dart

@@ -25,43 +25,30 @@ abstract class Command {
 
   static Command? fromJson(Map<String, dynamic> json) {
     String? type = json['type'];
-
-    if (type == 'wait') {
-      return WaitCommand.fromDataJson(json['data'] ?? {});
-    } else if (type == 'named') {
-      return NamedCommand.fromDataJson(json['data'] ?? {});
-    } else if (type == 'path') {
-      return PathCommand.fromDataJson(json['data'] ?? {});
-    } else if (type == 'sequential') {
-      return SequentialCommandGroup.fromDataJson(json['data'] ?? {});
-    } else if (type == 'parallel') {
-      return ParallelCommandGroup.fromDataJson(json['data'] ?? {});
-    } else if (type == 'race') {
-      return RaceCommandGroup.fromDataJson(json['data'] ?? {});
-    } else if (type == 'deadline') {
-      return DeadlineCommandGroup.fromDataJson(json['data'] ?? {});
-    }
-
-    return null;
+    Map<String, dynamic> data = json['data'] ?? {};
+
+    return switch (type) {
+      'wait' => WaitCommand.fromDataJson(data),
+      'named' => NamedCommand.fromDataJson(data),
+      'path' => PathCommand.fromDataJson(data),
+      'sequential' => SequentialCommandGroup.fromDataJson(data),
+      'parallel' => ParallelCommandGroup.fromDataJson(data),
+      'race' => RaceCommandGroup.fromDataJson(data),
+      'deadline' => DeadlineCommandGroup.fromDataJson(data),
+      _ => null,
+    };
   }
 
   static Command? fromType(String type, {List<Command>? commands}) {
-    if (type == 'named') {
-      return NamedCommand();
-    } else if (type == 'wait') {
-      return WaitCommand();
-    } else if (type == 'path') {
-      return PathCommand();
-    } else if (type == 'sequential') {
-      return SequentialCommandGroup(commands: commands ?? []);
-    } else if (type == 'parallel') {
-      return ParallelCommandGroup(commands: commands ?? []);
-    } else if (type == 'race') {
-      return RaceCommandGroup(commands: commands ?? []);
-    } else if (type == 'deadline') {
-      return DeadlineCommandGroup(commands: commands ?? []);
-    }
-
-    return null;
+    return switch (type) {
+      'named' => NamedCommand(),
+      'wait' => WaitCommand(),
+      'path' => PathCommand(),
+      'sequential' => SequentialCommandGroup(commands: commands ?? []),
+      'parallel' => ParallelCommandGroup(commands: commands ?? []),
+      'race' => RaceCommandGroup(commands: commands ?? []),
+      'deadline' => DeadlineCommandGroup(commands: commands ?? []),
+      _ => null,
+    };
   }
 }

lib/pages/home_page.dart

@@ -548,6 +548,8 @@
         json, PrefsKeys.defaultMaxAngVel, Defaults.defaultMaxAngVel);
     _setPrefDoubleFromJSON(
         json, PrefsKeys.defaultMaxAngAccel, Defaults.defaultMaxAngAccel);
+    _setPrefDoubleFromJSON(
+        json, PrefsKeys.defaultNominalVoltage, Defaults.defaultNominalVoltage);
     _setPrefDoubleFromJSON(json, PrefsKeys.robotMass, Defaults.robotMass);
     _setPrefDoubleFromJSON(json, PrefsKeys.robotMOI, Defaults.robotMOI);
     _setPrefDoubleFromJSON(
@@ -574,6 +576,8 @@
         json, PrefsKeys.bumperOffsetX, Defaults.bumperOffsetX);
     _setPrefDoubleFromJSON(
         json, PrefsKeys.bumperOffsetY, Defaults.bumperOffsetY);
+    widget.prefs.setString(PrefsKeys.robotFeatures,
+        json[PrefsKeys.robotFeatures] ?? Defaults.robotFeatures);
   }
 
   void _setPrefDoubleFromJSON(
@@ -616,6 +620,9 @@
       PrefsKeys.defaultMaxAngAccel:
           widget.prefs.getDouble(PrefsKeys.defaultMaxAngAccel) ??
               Defaults.defaultMaxAccel,
+      PrefsKeys.defaultNominalVoltage:
+          widget.prefs.getDouble(PrefsKeys.defaultNominalVoltage) ??
+              Defaults.defaultNominalVoltage,
       PrefsKeys.robotMass:
           widget.prefs.getDouble(PrefsKeys.robotMass) ?? Defaults.robotMass,
       PrefsKeys.robotMOI:
@@ -660,6 +667,9 @@
       PrefsKeys.bumperOffsetY:
           widget.prefs.getDouble(PrefsKeys.bumperOffsetY) ??
               Defaults.bumperOffsetY,
+      PrefsKeys.robotFeatures:
+          widget.prefs.getStringList(PrefsKeys.robotFeatures) ??
+              Defaults.robotFeatures,
     };
 
     settingsFile.writeAsString(encoder.convert(settings)).then((_) {

lib/pages/telemetry_page.dart

@@ -1,10 +1,12 @@
 import 'dart:collection';
+import 'dart:convert';
 import 'dart:math';
 
 import 'package:fl_chart/fl_chart.dart';
 import 'package:flutter/foundation.dart';
 import 'package:flutter/material.dart';
 import 'package:flutter_animate/flutter_animate.dart';
+import 'package:pathplanner/robot_features/feature.dart';
 import 'package:pathplanner/services/pplib_telemetry.dart';
 import 'package:pathplanner/util/path_painter_util.dart';
 import 'package:pathplanner/util/prefs.dart';
@@ -39,6 +41,7 @@
   late final Size _robotSize;
   late final Translation2d _bumperOffset;
   late bool _useSim;
+  final List<Feature> _robotFeatures = [];
 
   bool _gotCurrentPose = false;
   bool _gotTargetPose = false;
@@ -62,6 +65,16 @@
     _useSim = widget.prefs.getBool(PrefsKeys.telemetryUseSim) ??
         Defaults.telemetryUseSim;
 
+    for (String featureJson
+        in widget.prefs.getStringList(PrefsKeys.robotFeatures) ??
+            Defaults.robotFeatures) {
+      try {
+        _robotFeatures.add(Feature.fromJson(jsonDecode(featureJson))!);
+      } catch (_) {
+        // Ignore and skip loading this feature
+      }
+    }
+
     widget.telemetry.connectionStatusStream().listen((connected) {
       if (mounted) {
         setState(() {
@@ -232,6 +245,7 @@
                                 targetPose: _targetPose,
                                 currentPath: _currentPath,
                                 colorScheme: Theme.of(context).colorScheme,
+                                robotFeatures: _robotFeatures,
                               ),
                             ),
                           ),
@@ -577,6 +591,7 @@
   final Pose2d? targetPose;
   final List<Pose2d>? currentPath;
   final ColorScheme colorScheme;
+  final List<Feature> robotFeatures;
 
   static double scale = 1;
 
@@ -588,6 +603,7 @@
     this.targetPose,
     this.currentPath,
     required this.colorScheme,
+    required this.robotFeatures,
   });
 
   @override
@@ -623,7 +639,8 @@
           scale,
           canvas,
           Colors.grey[600]!.withOpacity(0.75),
-          colorScheme.surfaceContainer);
+          colorScheme.surfaceContainer,
+          robotFeatures);
     }
 
     if (currentPose != null) {
@@ -635,7 +652,8 @@
           scale,
           canvas,
           colorScheme.primary,
-          colorScheme.surfaceContainer);
+          colorScheme.surfaceContainer,
+          robotFeatures);
     }
   }
 

lib/robot_features/circle_feature.dart

@@ -0,0 +1,63 @@
+import 'package:flutter/material.dart';
+import 'package:pathplanner/robot_features/feature.dart';
+import 'package:pathplanner/util/wpimath/geometry.dart';
+
+class CircleFeature extends Feature {
+  Translation2d center;
+  num radius;
+  num strokeWidth;
+  bool filled;
+
+  CircleFeature({
+    this.center = const Translation2d(),
+    this.radius = 0.1,
+    this.strokeWidth = 0.02,
+    this.filled = false,
+    required super.name,
+  }) : super(type: 'circle');
+
+  CircleFeature.fromDataJson(Map<String, dynamic> dataJson, String name)
+      : this(
+          center: Translation2d.fromJson(dataJson['center']),
+          radius: dataJson['radius'],
+          strokeWidth: dataJson['strokeWidth'],
+          filled: dataJson['filled'],
+          name: name,
+        );
+
+  @override
+  Map<String, dynamic> dataToJson() {
+    return {
+      'center': center.toJson(),
+      'radius': radius,
+      'strokeWidth': strokeWidth,
+      'filled': filled,
+    };
+  }
+
+  @override
+  void draw(Canvas canvas, double pixelsPerMeter, Color color) {
+    Paint paint = Paint()
+      ..style = filled ? PaintingStyle.fill : PaintingStyle.stroke
+      ..strokeWidth = strokeWidth * pixelsPerMeter
+      ..color = color;
+
+    Offset centerPixels =
+        Offset(center.x * pixelsPerMeter, -center.y * pixelsPerMeter);
+    double radiusPixels = radius * pixelsPerMeter;
+
+    canvas.drawCircle(centerPixels, radiusPixels, paint);
+  }
+
+  @override
+  int get hashCode => Object.hash(type, center, radius, strokeWidth, filled);
+
+  @override
+  bool operator ==(Object other) {
+    return other is CircleFeature &&
+        other.center == center &&
+        other.radius == radius &&
+        other.strokeWidth == strokeWidth &&
+        other.filled == filled;
+  }
+}

lib/robot_features/feature.dart

@@ -0,0 +1,41 @@
+import 'package:flutter/foundation.dart';
+import 'package:flutter/material.dart';
+import 'package:pathplanner/robot_features/circle_feature.dart';
+import 'package:pathplanner/robot_features/line_feature.dart';
+import 'package:pathplanner/robot_features/rounded_rect_feature.dart';
+
+abstract class Feature {
+  String name;
+  final String type;
+
+  Feature({required this.name, required this.type});
+
+  /// Draw the feature on the canvas with the given pixels per meter ratio
+  /// The canvas should already be transformed to the center of the robot
+  /// and its rotation. This just needs to draw relative to the robot.
+  void draw(Canvas canvas, double pixelsPerMeter, Color color);
+
+  Map<String, dynamic> dataToJson();
+
+  @nonVirtual
+  Map<String, dynamic> toJson() {
+    return {
+      'name': name,
+      'type': type,
+      'data': dataToJson(),
+    };
+  }
+
+  static Feature? fromJson(Map<String, dynamic> json) {
+    String name = json['name'];
+    String type = json['type'];
+    Map<String, dynamic> data = json['data'] ?? {};
+
+    return switch (type) {
+      'rounded_rect' => RoundedRectFeature.fromDataJson(data, name),
+      'circle' => CircleFeature.fromDataJson(data, name),
+      'line' => LineFeature.fromDataJson(data, name),
+      _ => null,
+    };
+  }
+}

lib/robot_features/line_feature.dart

@@ -0,0 +1,58 @@
+import 'package:flutter/material.dart';
+import 'package:pathplanner/robot_features/feature.dart';
+import 'package:pathplanner/util/wpimath/geometry.dart';
+
+class LineFeature extends Feature {
+  Translation2d start;
+  Translation2d end;
+  num strokeWidth;
+
+  LineFeature({
+    this.start = const Translation2d(),
+    this.end = const Translation2d(0.2, 0.0),
+    this.strokeWidth = 0.02,
+    required super.name,
+  }) : super(type: 'line');
+
+  LineFeature.fromDataJson(Map<String, dynamic> dataJson, String name)
+      : this(
+          start: Translation2d.fromJson(dataJson['start']),
+          end: Translation2d.fromJson(dataJson['end']),
+          strokeWidth: dataJson['strokeWidth'],
+          name: name,
+        );
+
+  @override
+  Map<String, dynamic> dataToJson() {
+    return {
+      'start': start.toJson(),
+      'end': end.toJson(),
+      'strokeWidth': strokeWidth,
+    };
+  }
+
+  @override
+  void draw(Canvas canvas, double pixelsPerMeter, Color color) {
+    Paint paint = Paint()
+      ..style = PaintingStyle.stroke
+      ..strokeWidth = strokeWidth * pixelsPerMeter
+      ..color = color;
+
+    Offset startPixels =
+        Offset(start.x * pixelsPerMeter, -start.y * pixelsPerMeter);
+    Offset endPixels = Offset(end.x * pixelsPerMeter, -end.y * pixelsPerMeter);
+
+    canvas.drawLine(startPixels, endPixels, paint);
+  }
+
+  @override
+  int get hashCode => Object.hash(type, start, end, strokeWidth);
+
+  @override
+  bool operator ==(Object other) {
+    return other is LineFeature &&
+        other.start == start &&
+        other.end == end &&
+        other.strokeWidth == strokeWidth;
+  }
+}