added back vision

frc2053 · Nov 12, 2024 · 128fd5b · 128fd5b
1 parent 6029627
commit 128fd5b
Show file tree

Hide file tree

Showing 17 changed files with 725 additions and 4 deletions.
diff --git a/elastic-layout.json b/elastic-layout.json
@@ -35,7 +35,7 @@
             "title": "SwerveField",
             "x": 640.0,
             "y": 0.0,
-            "width": 896.0,
+            "width": 768.0,
             "height": 640.0,
             "type": "Field",
             "properties": {
@@ -83,6 +83,54 @@
               "red_start_time": 15,
               "yellow_start_time": 30
             }
+          },
+          {
+            "title": "str_bl_cam-processed",
+            "x": 1408.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/str_bl_cam-processed",
+              "period": 0.06
+            }
+          },
+          {
+            "title": "str_br_cam-processed",
+            "x": 1664.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/str_br_cam-processed",
+              "period": 0.06
+            }
+          },
+          {
+            "title": "str_fl_cam-processed",
+            "x": 1408.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/str_fl_cam-processed",
+              "period": 0.06
+            }
+          },
+          {
+            "title": "str_fr_cam-processed",
+            "x": 1664.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/str_fr_cam-processed",
+              "period": 0.06
+            }
           }
         ]
       }
@@ -227,4 +275,4 @@
       }
     }
   ]
-}
+}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
@@ -30,12 +30,38 @@ void Robot::RobotPeriodic() {
   loopTimePub.Set((1 / loopTime).value());
 
   frc2::CommandScheduler::GetInstance().Run();
+  UpdateVision();
 
   lastTotalLoopTime = now;
   matchTimePub.Set(frc::DriverStation::GetMatchTime().value());
   battVoltagePub.Set(frc::RobotController::GetBatteryVoltage().value());
 }
 
+void Robot::SimulationPeriodic() {
+  m_container.GetVision().SimulationPeriodic(
+      m_container.GetDrive().GetOdomPose());
+}
+
+void Robot::UpdateVision() {
+  auto visionEstimates = m_container.GetVision().GetCameraEstimatedPoses(
+      frc::Pose3d{m_container.GetDrive().GetRobotPose()});
+  auto stdDevs = m_container.GetVision().GetPoseStdDevs(visionEstimates);
+
+  frc::Pose3d pose = frc::Pose3d{m_container.GetDrive().GetRobotPose()};
+
+  m_container.GetVision().UpdateCameraPositionVis(pose);
+
+  int i = 0;
+  for (const auto& est : visionEstimates) {
+    if (est.has_value()) {
+      m_container.GetDrive().AddVisionMeasurement(
+          est.value().estimatedPose.ToPose2d(), est.value().timestamp,
+          stdDevs[i].value());
+    }
+    i++;
+  }
+}
+
 void Robot::DisabledInit() {}
 
 void Robot::DisabledPeriodic() {}

diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp
@@ -10,6 +10,7 @@
 #include "constants/SwerveConstants.h"
 #include "frc2/command/sysid/SysIdRoutine.h"
 #include "str/DriverstationUtils.h"
+#include "str/vision/VisionSystem.h"
 #include "subsystems/Drive.h"
 
 RobotContainer::RobotContainer() {
@@ -125,3 +126,7 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
 Drive& RobotContainer::GetDrive() {
   return driveSub;
 }
+
+str::vision::VisionSystem& RobotContainer::GetVision() {
+  return vision;
+}
diff --git a/src/main/cpp/str/swerve/SwerveDrive.cpp b/src/main/cpp/str/swerve/SwerveDrive.cpp
@@ -16,6 +16,7 @@
 #include "frc/kinematics/SwerveModuleState.h"
 #include "frc/smartdashboard/SmartDashboard.h"
 #include "str/DriverstationUtils.h"
+#include "str/Math.h"
 #include "str/swerve/SwerveModuleHelpers.h"
 #include "units/angle.h"
 #include "units/angular_velocity.h"
@@ -37,6 +38,10 @@ frc::Pose2d SwerveDrive::GetPose() const {
   return poseEstimator.GetEstimatedPosition();
 }
 
+frc::Pose2d SwerveDrive::GetOdomPose() const {
+  return odom.GetPose();
+}
+
 void SwerveDrive::SetXModuleForces(
     const std::array<units::newton_t, 4>& xForce) {
   xModuleForce = xForce;
@@ -75,6 +80,21 @@ void SwerveDrive::UpdateOdom() {
   lastOdomUpdateTime = now;
 }
 
+void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& measurement,
+                                       units::second_t timestamp,
+                                       const Eigen::Vector3d& stdDevs) {
+  if (str::math::IsRobotInsideField(consts::swerve::physical::TOTAL_LENGTH,
+                                    consts::swerve::physical::TOTAL_WIDTH,
+                                    measurement)) {
+    wpi::array<double, 3> newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)};
+    addedVisionPosesPub.Set(measurement);
+    poseEstimator.AddVisionMeasurement(measurement, timestamp, newStdDevs);
+  } else {
+    frc::DataLogManager::Log(
+        "WARNING: Vision pose was outside of field! Not adding to estimator!");
+  }
+}
+
 void SwerveDrive::UpdateSimulation() {
   std::array<frc::SwerveModuleState, 4> simState;
   int i = 0;