From 77377c4b9dd33fe456e23952971017411a90c854 Mon Sep 17 00:00:00 2001 From: Jade Date: Mon, 9 Dec 2024 13:32:46 +0800 Subject: [PATCH] Add support for choreo trajectories (#271) * Add support for choreo trajectories Signed-off-by: Jade Turner * fix some bugs Signed-off-by: Jade Turner * review Signed-off-by: Jade Turner * 3d field Signed-off-by: Jade Turner * remove support from non trajectory keys Signed-off-by: Jade Turner --------- Signed-off-by: Jade Turner --- .../controllers/OdometryController_Config.ts | 6 ++++- .../ThreeDimensionController_Config.ts | 2 ++ src/shared/geometry.ts | 25 +++++++++++++++++++ 3 files changed, 32 insertions(+), 1 deletion(-) diff --git a/src/hub/controllers/OdometryController_Config.ts b/src/hub/controllers/OdometryController_Config.ts index f262f09e..f3df13e7 100644 --- a/src/hub/controllers/OdometryController_Config.ts +++ b/src/hub/controllers/OdometryController_Config.ts @@ -348,7 +348,9 @@ const OdometryController_Config: SourceListConfig = { "Transform3d[]", "Translation2d[]", "Translation3d[]", - "Trajectory" + "Trajectory", + "DifferentialSample[]", + "SwerveSample[]" ], showDocs: true, options: [ @@ -501,6 +503,8 @@ const OdometryController_Config: SourceListConfig = { "Transform3d", "Transform2d[]", "Transform3d[]", + "DifferentialSample[]", + "SwerveSample[]", "Trajectory" ], showDocs: true, diff --git a/src/hub/controllers/ThreeDimensionController_Config.ts b/src/hub/controllers/ThreeDimensionController_Config.ts index 332140c5..48d50314 100644 --- a/src/hub/controllers/ThreeDimensionController_Config.ts +++ b/src/hub/controllers/ThreeDimensionController_Config.ts @@ -446,6 +446,8 @@ const ThreeDimensionController_Config: SourceListConfig = { "Transform3d[]", "Translation2d[]", "Translation3d[]", + "SwerveSample[]", + "DifferentialSample[]", "Trajectory" ], showDocs: true, diff --git a/src/shared/geometry.ts b/src/shared/geometry.ts index e8931fbd..c4649a4d 100644 --- a/src/shared/geometry.ts +++ b/src/shared/geometry.ts @@ -214,6 +214,9 @@ export function grabPosesAuto( } else { return []; } + case "DifferentialSample[]": + case "SwerveSample[]": + return grabChoreoSampleArray(log, key, timestamp, uuid); default: return []; } @@ -438,6 +441,21 @@ export function grabPose2d(log: Log, key: string, timestamp: number, uuid?: stri ]; } +export function grabChoreoSample(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 +484,13 @@ export function grabPose2dArray(log: Log, key: string, timestamp: number, uuid?: ); } +export function grabChoreoSampleArray(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(grabChoreoSample(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)),