added trajectory vis in ascope and simgui

frc2053 · Nov 13, 2024 · f162e8d · f162e8d
1 parent e5d3105
commit f162e8d
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 11 deletions.
diff --git a/aslayout.json b/aslayout.json
@@ -10,14 +10,14 @@
           "width": 431,
           "expanded": [
             "/Vision/str_fr_camPoseEstimation/translation",
-            "/Swerve",
             "/Swerve/OdometryPose/rotation",
             "/Swerve/PoseEstimatorPose/rotation",
-            "/Vision"
+            "/SmartDashboard/VisionSystemSim-str_fl_cam/Sim Field",
+            "/Swerve"
           ]
         },
         "tabs": {
-          "selected": 6,
+          "selected": 1,
           "tabs": [
             {
               "type": 0,
@@ -141,6 +141,16 @@
                     "options": {
                       "color": "#ff8c00"
                     }
+                  },
+                  {
+                    "type": "trajectory",
+                    "logKey": "NT:/Swerve/ActivePath",
+                    "logType": "Pose2d[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#00ff00",
+                      "size": "normal"
+                    }
                   }
                 ],
                 "game": "2024 Field",
@@ -237,11 +247,21 @@
                       "size": "normal"
                     }
                   },
+                  {
+                    "type": "swerveStates",
+                    "logKey": "NT:/Swerve/PathForcesPub",
+                    "logType": "SwerveModuleState[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#ff8c00",
+                      "arrangement": "0,1,2,3"
+                    }
+                  },
                   {
                     "type": "ghost",
                     "logKey": "NT:/Swerve/OdometryPose",
                     "logType": "Pose2d",
-                    "visible": true,
+                    "visible": false,
                     "options": {
                       "model": "LynkClone",
                       "color": "#0000ff"
@@ -251,7 +271,7 @@
                     "type": "ghost",
                     "logKey": "NT:/Swerve/AddedVisionPoses",
                     "logType": "Pose2d",
-                    "visible": true,
+                    "visible": false,
                     "options": {
                       "model": "LynkClone",
                       "color": "#00ff00"
@@ -303,6 +323,16 @@
                       "model": "LynkClone",
                       "color": "#ffff00"
                     }
+                  },
+                  {
+                    "type": "trajectory",
+                    "logKey": "NT:/Swerve/ActivePath",
+                    "logType": "Pose2d[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#00ffff",
+                      "size": "normal"
+                    }
                   }
                 ],
                 "game": "2024 Field",
@@ -313,14 +343,14 @@
                 "cameraIndex": -1,
                 "orbitFov": 50,
                 "cameraPosition": [
-                  10.37608674425006,
-                  5.14018959610576,
-                  0.16767350540132944
+                  -4.421077625054751,
+                  5.352729764849907,
+                  4.334578707441611
                 ],
                 "cameraTarget": [
-                  5.574306668907712,
-                  0.26632949711303977,
-                  -0.03370745348536627
+                  -3.4524484976375676,
+                  1.0991429624851292,
+                  0.18463123604264106
                 ]
               },
               "controlsHeight": 441

diff --git a/simgui.json b/simgui.json
@@ -55,6 +55,10 @@
         "Robot": {
           "weight": 2.0
         },
+        "activePath": {
+          "arrows": false,
+          "style": "Line"
+        },
         "bottom": 1476,
         "height": 8.210550308227539,
         "left": 150,

diff --git a/src/main/cpp/str/swerve/SwerveDrive.cpp b/src/main/cpp/str/swerve/SwerveDrive.cpp
@@ -57,6 +57,7 @@ void SwerveDrive::SetYModuleForces(const std::vector<units::newton_t>& yForce) {
 }
 
 void SwerveDrive::SetActivePath(std::vector<frc::Pose2d> poses) {
+  activePathPub.Set(poses);
   swerveField.GetObject("activePath")->SetPoses(poses);
 }
 

diff --git a/src/main/include/str/swerve/SwerveDrive.h b/src/main/include/str/swerve/SwerveDrive.h
@@ -137,6 +137,8 @@ class SwerveDrive {
   nt::StructArrayPublisher<frc::SwerveModuleState> currentStatesPub{
       nt->GetStructArrayTopic<frc::SwerveModuleState>("CurrentStates")
           .Publish()};
+  nt::StructArrayPublisher<frc::Pose2d> activePathPub{
+      nt->GetStructArrayTopic<frc::Pose2d>("ActivePath").Publish()};
   nt::StructArrayPublisher<frc::SwerveModulePosition> currentPositionsPub{
       nt->GetStructArrayTopic<frc::SwerveModulePosition>("CurrentPositions")
           .Publish()};
-Original file line number
+Diff line change
@@ Expand Up @@
     }
     void SwerveDrive::SetActivePath(std::vector<frc::Pose2d> poses) {
+      activePathPub.Set(poses);
       swerveField.GetObject("activePath")->SetPoses(poses);
     }
@@ Expand Down @@