fixed vision std devs

frc2053 · Oct 28, 2024 · 9c45cc1 · 9c45cc1
1 parent 7ab879e
commit 9c45cc1
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 19 deletions.
diff --git a/src/main/cpp/str/Camera.cpp b/src/main/cpp/str/Camera.cpp
@@ -30,7 +30,7 @@ Camera::Camera(std::string cameraName, frc::Transform3d robotToCamera,
     : simulate(simulate),
       posePub(nt->GetStructTopic<frc::Pose2d>(cameraName + "PoseEstimation")
                   .Publish()),
-      singleTagDevs(singleTagDevs),
+      singleTagDevs(singleTagStdDev),
       multiTagDevs(multiTagDevs),
       stdDevXPosePub(nt->GetDoubleTopic(cameraName + "StdDevsX").Publish()),
       stdDevYPosePub(nt->GetDoubleTopic(cameraName + "StdDevsY").Publish()),
@@ -133,10 +133,6 @@ photon::PhotonPipelineResult Camera::GetLatestResult() {
 }
 
 std::optional<photon::EstimatedRobotPose> Camera::GetEstimatedGlobalPose() {
-  if (!simulate) {
-    return std::nullopt;
-  }
-
   std::optional<photon::EstimatedRobotPose> visionEst;
 
   auto result = camera->GetLatestResult();
@@ -149,16 +145,19 @@ std::optional<photon::EstimatedRobotPose> Camera::GetEstimatedGlobalPose() {
   }
 
   latestResult = result;
+
+  const auto& targetsSpan = result.GetTargets();
+  targetsCopy = std::vector<photon::PhotonTrackedTarget>(targetsSpan.begin(), targetsSpan.end());
+
   return visionEst;
 }
 
 Eigen::Matrix<double, 3, 1> Camera::GetEstimationStdDevs(
     frc::Pose2d estimatedPose) {
   Eigen::Matrix<double, 3, 1> estStdDevs = singleTagDevs;
-  auto targets = GetLatestResult().GetTargets();
   int numTags = 0;
   units::meter_t avgDist = 0_m;
-  for (const auto& tgt : targets) {
+  for (const auto& tgt : targetsCopy) {
     auto tagPose =
         photonEstimator->GetFieldLayout().GetTagPose(tgt.GetFiducialId());
     if (tagPose.has_value()) {
@@ -180,15 +179,17 @@ Eigen::Matrix<double, 3, 1> Camera::GetEstimationStdDevs(
          std::numeric_limits<double>::max(), std::numeric_limits<double>::max())
             .finished();
   } else {
-    estStdDevs = estStdDevs * (1 + (avgDist.value() * avgDist.value() / 30));
+    estStdDevs = estStdDevs * (1 + (avgDist.value() * avgDist.value() / 30.0));
   }
 
-  if (!simulate) {
-    stdDevXPosePub.Set(estStdDevs(0));
-    stdDevYPosePub.Set(estStdDevs(1));
-    stdDevRotPosePub.Set(estStdDevs(2));
+  if(estStdDevs(0) == 0 || estStdDevs(1) == 0 || estStdDevs(2) == 0) {
+    fmt::print("ERROR STD DEV IS ZERO!\n");
   }
 
+  stdDevXPosePub.Set(estStdDevs(0));
+  stdDevYPosePub.Set(estStdDevs(1));
+  stdDevRotPosePub.Set(estStdDevs(2));
+
   return estStdDevs;
 }
 

diff --git a/src/main/cpp/str/SwerveDrive.cpp b/src/main/cpp/str/SwerveDrive.cpp
@@ -98,12 +98,8 @@ void SwerveDrive::Drive(units::meters_per_second_t xVel,
   speedsToSend =
       frc::ChassisSpeeds::Discretize(speedsToSend, consts::LOOP_PERIOD);
 
-  fmt::print("x: {}, y: {}, rot: {}\n", speedsToSend.vx, speedsToSend.vy, speedsToSend.omega);
-
   std::array<frc::SwerveModuleState, 4> states = consts::swerve::physical::KINEMATICS.ToSwerveModuleStates(speedsToSend);
 
-  fmt::print("fl speed: {} | fl angle: {}\n", states[0].speed, states[0].angle.Degrees());
-
   SetModuleStates(states,
       true, openLoop,
       ConvertModuleForcesToTorqueCurrent(xModuleForce, yModuleForce));

diff --git a/src/main/cpp/str/Vision.cpp b/src/main/cpp/str/Vision.cpp
@@ -16,8 +16,8 @@ std::vector<std::optional<Eigen::Matrix<double, 3, 1>>> Vision::GetPoseStdDevs(
   int i = 0;
   for (auto& cam : cameras) {
     if (poses[i].has_value()) {
-      allStdDevs.push_back(
-          cam.GetEstimationStdDevs(poses[i].value().estimatedPose.ToPose2d()));
+      auto stddev = cam.GetEstimationStdDevs(poses[i].value().estimatedPose.ToPose2d());
+      allStdDevs.push_back(stddev);
     } else {
       allStdDevs.push_back(std::nullopt);
     }

diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt
@@ -1 +1 @@
-465a551
+7ab879e
diff --git a/src/main/include/str/Camera.h b/src/main/include/str/Camera.h
@@ -43,6 +43,7 @@ class Camera {
   std::shared_ptr<photon::PhotonCameraSim> cameraSim;
 
   photon::PhotonPipelineResult latestResult;
+  std::vector<photon::PhotonTrackedTarget> targetsCopy;
 
   Eigen::Matrix<double, 3, 1> singleTagDevs;
   Eigen::Matrix<double, 3, 1> multiTagDevs;