From 2f6ea2d54e54e894eb4e4026d064924f9dcbebad Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Wed, 13 Nov 2024 21:25:56 +0800 Subject: [PATCH 1/2] Add support for choreo trajectories Signed-off-by: Jade Turner --- src/hub/controllers/OdometryController.ts | 12 +++++-- .../controllers/OdometryController_Config.ts | 32 ++++++++++++++--- src/shared/geometry.ts | 34 +++++++++++++++++-- 3 files changed, 68 insertions(+), 10 deletions(-) diff --git a/src/hub/controllers/OdometryController.ts b/src/hub/controllers/OdometryController.ts index 81096eeb..13a5c9d7 100644 --- a/src/hub/controllers/OdometryController.ts +++ b/src/hub/controllers/OdometryController.ts @@ -275,7 +275,13 @@ export default class OdometryController implements TabController { } // Get pose data - let numberArrayFormat: "Translation2d" | "Translation3d" | "Pose2d" | "Pose3d" = "Pose2d"; + let numberArrayFormat: + | "Translation2d" + | "Translation3d" + | "DifferentialSample" + | "SwerveSample" + | "Pose2d" + | "Pose3d" = "Pose2d"; let numberArrayUnits: "radians" | "degrees" = "radians"; if ("format" in source.options) { let formatRaw = source.options.format; @@ -283,7 +289,9 @@ export default class OdometryController implements TabController { formatRaw === "Pose2d" || formatRaw === "Pose3d" || formatRaw === "Translation2d" || - formatRaw === "Translation3d" + formatRaw === "Translation3d" || + formatRaw === "DifferentialSample" || + formatRaw === "SwerveSample" ? formatRaw : "Pose2d"; } diff --git a/src/hub/controllers/OdometryController_Config.ts b/src/hub/controllers/OdometryController_Config.ts index f262f09e..caa17de5 100644 --- a/src/hub/controllers/OdometryController_Config.ts +++ b/src/hub/controllers/OdometryController_Config.ts @@ -23,7 +23,11 @@ const OdometryController_Config: SourceListConfig = { "Transform2d", "Transform3d", "Transform2d[]", - "Transform3d[]" + "Transform3d[]", + "DifferentialSample", + "DifferentialSample[]", + "SwerveSample", + "SwerveSample[]" ], showDocs: true, options: [], @@ -79,7 +83,11 @@ const OdometryController_Config: SourceListConfig = { "Transform2d", "Transform3d", "Transform2d[]", - "Transform3d[]" + "Transform3d[]", + "DifferentialSample", + "DifferentialSample[]", + "SwerveSample", + "SwerveSample[]" ], showDocs: true, options: [ @@ -174,7 +182,11 @@ const OdometryController_Config: SourceListConfig = { "Translation2d", "Translation3d", "Translation2d[]", - "Translation3d[]" + "Translation3d[]", + "DifferentialSample", + "DifferentialSample[]", + "SwerveSample", + "SwerveSample[]" ], showDocs: true, options: [ @@ -348,7 +360,9 @@ const OdometryController_Config: SourceListConfig = { "Transform3d[]", "Translation2d[]", "Translation3d[]", - "Trajectory" + "Trajectory", + "DifferentialSample[]", + "SwerveSample[]" ], showDocs: true, options: [ @@ -428,6 +442,10 @@ const OdometryController_Config: SourceListConfig = { "Translation3d", "Translation2d[]", "Translation3d[]", + "DifferentialSample", + "DifferentialSample[]", + "SwerveSample", + "SwerveSample[]", "ZebraTranslation" ], showDocs: true, @@ -501,7 +519,11 @@ const OdometryController_Config: SourceListConfig = { "Transform3d", "Transform2d[]", "Transform3d[]", - "Trajectory" + "Trajectory", + "DifferentialSample", + "DifferentialSample[]", + "SwerveSample", + "SwerveSample[]" ], showDocs: true, options: [ diff --git a/src/shared/geometry.ts b/src/shared/geometry.ts index e8931fbd..dda48411 100644 --- a/src/shared/geometry.ts +++ b/src/shared/geometry.ts @@ -161,7 +161,7 @@ export function grabPosesAuto( logType: string, timestamp: number, uuid?: string, - numberArrayFormat?: "Translation2d" | "Translation3d" | "Pose2d" | "Pose3d", + numberArrayFormat?: "Translation2d" | "Translation3d" | "Pose2d" | "Pose3d" | "DifferentialSample" | "SwerveSample", numberArrayUnits?: "radians" | "degrees", zebraOrigin?: "blue" | "red", zebraFieldWidth?: number, @@ -214,6 +214,12 @@ export function grabPosesAuto( } else { return []; } + case "DifferentialSample": + case "SwerveSample": + return grabSample(log, key + "/pose", timestamp, uuid); // TODO + case "DifferentialSample[]": + case "SwerveSample[]": + return grabSampleArray(log, key + "/pose", timestamp, uuid); // TODO! default: return []; } @@ -247,7 +253,7 @@ export function grabNumberArray( log: Log, key: string, timestamp: number, - format: "Translation2d" | "Translation3d" | "Pose2d" | "Pose3d", + format: "Translation2d" | "Translation3d" | "Pose2d" | "Pose3d" | "DifferentialSample" | "SwerveSample", unit?: "radians" | "degrees", uuid?: string ): AnnotatedPose3d[] { @@ -438,6 +444,21 @@ export function grabPose2d(log: Log, key: string, timestamp: number, uuid?: stri ]; } +export function grabSample(log: Log, key: string, timestamp: number, uuid?: string): AnnotatedPose3d[] { + return [ + { + pose: pose2dTo3d({ + translation: [ + getOrDefault(log, key + "/pose/translation/x", LoggableType.Number, timestamp, 0, uuid), + getOrDefault(log, key + "/pose/translation/y", LoggableType.Number, timestamp, 0, uuid) + ], + rotation: getOrDefault(log, key + "/pose/rotation/value", LoggableType.Number, timestamp, 0, uuid) + }), + annotation: { is2DSource: true } + } + ]; +} + export function grabPose3d(log: Log, key: string, timestamp: number, uuid?: string): AnnotatedPose3d[] { return [ { @@ -466,6 +487,13 @@ export function grabPose2dArray(log: Log, key: string, timestamp: number, uuid?: ); } +export function grabSampleArray(log: Log, key: string, timestamp: number, uuid?: string): AnnotatedPose3d[] { + return indexArray(getOrDefault(log, key + "/length", LoggableType.Number, timestamp, 0, uuid)).reduce( + (array, index) => array.concat(grabSample(log, key + "/" + index.toString(), timestamp)), + [] as AnnotatedPose3d[] + ); +} + export function grabPose3dArray(log: Log, key: string, timestamp: number, uuid?: string): AnnotatedPose3d[] { return indexArray(getOrDefault(log, key + "/length", LoggableType.Number, timestamp, 0, uuid)).reduce( (array, index) => array.concat(grabPose3d(log, key + "/" + index.toString(), timestamp)), @@ -577,7 +605,7 @@ export function grabHeatmapData( logType: string, timeRange: "enabled" | "auto" | "teleop" | "teleop-no-endgame" | "full", uuid?: string, - numberArrayFormat?: "Translation2d" | "Translation3d" | "Pose2d" | "Pose3d", + numberArrayFormat?: "Translation2d" | "Translation3d" | "Pose2d" | "Pose3d" | "DifferentialSample" | "SwerveSample", numberArrayUnits?: "radians" | "degrees", zebraOrigin?: "blue" | "red", zebraFieldWidth?: number, From b7f789b8dcbaedf27585d8e37d7971c1588e6d13 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Wed, 13 Nov 2024 22:04:25 +0800 Subject: [PATCH 2/2] fix some bugs Signed-off-by: Jade Turner --- src/shared/geometry.ts | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/shared/geometry.ts b/src/shared/geometry.ts index dda48411..ce6dc75d 100644 --- a/src/shared/geometry.ts +++ b/src/shared/geometry.ts @@ -216,10 +216,10 @@ export function grabPosesAuto( } case "DifferentialSample": case "SwerveSample": - return grabSample(log, key + "/pose", timestamp, uuid); // TODO + return grabSample(log, key, timestamp, uuid); case "DifferentialSample[]": case "SwerveSample[]": - return grabSampleArray(log, key + "/pose", timestamp, uuid); // TODO! + return grabSampleArray(log, key, timestamp, uuid); default: return []; } @@ -300,6 +300,10 @@ export function grabNumberArray( }); } break; + case "DifferentialSample": + case "SwerveSample": + //TODO + break; case "Pose3d": for (let i = 0; i < value.length - 6; i += 7) { poses.push({