diff --git a/aslayout.json b/aslayout.json new file mode 100644 index 0000000..623af54 --- /dev/null +++ b/aslayout.json @@ -0,0 +1,468 @@ +{ + "hubs": [ + { + "x": -2568, + "y": -191, + "width": 2576, + "height": 1408, + "state": { + "sidebar": { + "width": 431, + "expanded": [ + "/Vision/str_fr_camPoseEstimation/translation", + "/Swerve", + "/Swerve/OdometryPose/rotation", + "/Swerve/PoseEstimatorPose/rotation", + "/Vision" + ] + }, + "tabs": { + "selected": 6, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "y9i2u21zv9stde3c7oetits5laxivwvx", + "renderer": "#/", + "controlsHeight": 0 + }, + { + "type": 2, + "title": "Odometry", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "NT:/Swerve/PoseEstimatorPose", + "logType": "Pose2d", + "visible": true, + "options": {} + }, + { + "type": "swerveStates", + "logKey": "NT:/Swerve/DesiredStates", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "swerveStates", + "logKey": "NT:/Swerve/CurrentStates", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "swerveStates", + "logKey": "NT:/Swerve/SimStates", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#00ff00", + "arrangement": "0,1,2,3" + } + }, + { + "type": "swerveStates", + "logKey": "NT:/Swerve/PathForcesPub", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff8c00", + "arrangement": "0,1,2,3" + } + }, + { + "type": "ghost", + "logKey": "NT:/Swerve/AddedVisionPoses", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#00ff00" + } + }, + { + "type": "ghost", + "logKey": "NT:/Swerve/OdometryPose", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#0000ff" + } + }, + { + "type": "arrow", + "logKey": "NT:/Vision/CameraLocations", + "logType": "Pose3d[]", + "visible": false, + "options": { + "position": "center" + } + }, + { + "type": "ghost", + "logKey": "NT:/Vision/str_fl_camPoseEstimation", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ffff00" + } + }, + { + "type": "ghost", + "logKey": "NT:/Vision/str_fr_camPoseEstimation", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#00ffff" + } + }, + { + "type": "ghost", + "logKey": "NT:/Vision/str_bl_camPoseEstimation", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ff0000" + } + }, + { + "type": "ghost", + "logKey": "NT:/Vision/str_br_camPoseEstimation", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ff8c00" + } + } + ], + "game": "2024 Field", + "bumpers": "auto", + "origin": "blue", + "orientation": 0, + "size": 30 + }, + "controllerUUID": "2z6sfpv8l6xi8ghwo0dqtbi3ln5zq2up", + "renderer": null, + "controlsHeight": 570 + }, + { + "type": 3, + "title": "3D Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "NT:/Swerve/PoseEstimatorPose", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Crab Bot" + } + }, + { + "type": "swerveStates", + "logKey": "NT:/Swerve/CurrentStates", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "swerveStates", + "logKey": "NT:/Swerve/DesiredStates", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "swerveStates", + "logKey": "NT:/Swerve/SimStates", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#00ff00", + "arrangement": "0,1,2,3" + } + }, + { + "type": "vision", + "logKey": "NT:/Vision/str_bl_camtargetPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "vision", + "logKey": "NT:/Vision/str_br_camtargetPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "vision", + "logKey": "NT:/Vision/str_fl_camtargetPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "vision", + "logKey": "NT:/Vision/str_fr_camtargetPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00", + "size": "normal" + } + }, + { + "type": "ghost", + "logKey": "NT:/Swerve/OdometryPose", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Crab Bot", + "color": "#0000ff" + } + }, + { + "type": "ghost", + "logKey": "NT:/Swerve/AddedVisionPoses", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Crab Bot", + "color": "#00ff00" + } + }, + { + "type": "axes", + "logKey": "NT:/Vision/CameraLocations", + "logType": "Pose3d[]", + "visible": true, + "options": {} + }, + { + "type": "ghost", + "logKey": "NT:/Vision/str_bl_camPoseEstimation", + "logType": "Pose2d", + "visible": false, + "options": { + "model": "Crab Bot", + "color": "#ff0000" + } + }, + { + "type": "ghost", + "logKey": "NT:/Vision/str_br_camPoseEstimation", + "logType": "Pose2d", + "visible": false, + "options": { + "model": "Crab Bot", + "color": "#ff8c00" + } + }, + { + "type": "ghost", + "logKey": "NT:/Vision/str_fr_camPoseEstimation", + "logType": "Pose2d", + "visible": false, + "options": { + "model": "Crab Bot", + "color": "#00ffff" + } + }, + { + "type": "ghost", + "logKey": "NT:/Vision/str_fl_camPoseEstimation", + "logType": "Pose2d", + "visible": false, + "options": { + "model": "Crab Bot", + "color": "#ffff00" + } + } + ], + "game": "2024 Field", + "origin": "blue" + }, + "controllerUUID": "ooyjnq5zdslv0xp52opjoj9p45qiwow0", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + 2.293574925668202, + 2.1071062567321013, + 3.4533555903128077 + ], + "cameraTarget": [ + 5.4215286608684865, + 1.0209803434297497, + 2.0276851647304728 + ] + }, + "controlsHeight": 702 + }, + { + "type": 8, + "title": "Joysticks", + "controller": [ + "Xbox Controller (White)", + "Xbox Controller (White)", + "None", + "None", + "None", + "None" + ], + "controllerUUID": "4exedvr8yrfmep3g021mb9wnf5cb774c", + "renderer": null, + "controlsHeight": 85 + }, + { + "type": 9, + "title": "Swerve", + "controller": { + "sources": [ + { + "type": "states", + "logKey": "NT:/Swerve/DesiredStates", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "NT:/Swerve/CurrentStates", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "chassisSpeeds", + "logKey": "NT:/Swerve/SimChassisSpeeds", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#00ff00" + } + }, + { + "type": "rotationLegacy", + "logKey": "NT:/Swerve/PoseEstimatorPose/rotation/value", + "logType": "Number", + "visible": true, + "options": { + "units": "radians" + } + } + ], + "maxSpeed": 5, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 1 + }, + "controllerUUID": "bt9ftgdxdwv6tw7rc580fys90shxu0oh", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 5, + "title": "Console", + "controller": null, + "controllerUUID": "9ewg9pkpcrwgwoo3plgfrgegqnbznf2g", + "renderer": { + "highlight": false + }, + "controlsHeight": 0 + }, + { + "type": 11, + "title": "Points", + "controller": { + "sources": [ + { + "type": "plus", + "logKey": "NT:/Vision/str_fl_camtargetCorners", + "logType": "Translation2d[]", + "visible": true, + "options": { + "size": "medium", + "groupSize": "0" + } + }, + { + "type": "plus", + "logKey": "NT:/Vision/str_fr_camtargetCorners", + "logType": "Translation2d[]", + "visible": true, + "options": { + "size": "medium", + "groupSize": "0" + } + }, + { + "type": "plus", + "logKey": "NT:/Vision/str_bl_camtargetCorners", + "logType": "Translation2d[]", + "visible": true, + "options": { + "size": "medium", + "groupSize": "0" + } + }, + { + "type": "plus", + "logKey": "NT:/Vision/str_br_camtargetCorners", + "logType": "Translation2d[]", + "visible": true, + "options": { + "size": "medium", + "groupSize": "0" + } + } + ], + "width": 1600, + "height": 1200, + "orientation": "xr,yd", + "origin": "ul" + }, + "controllerUUID": "w1jv7a9993vo67dwlchj0bonml8sl6s8", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "4.0.0-beta-3" +} diff --git a/src/main/cpp/str/vision/Camera.cpp b/src/main/cpp/str/vision/Camera.cpp index c24d862..5403b60 100644 --- a/src/main/cpp/str/vision/Camera.cpp +++ b/src/main/cpp/str/vision/Camera.cpp @@ -133,44 +133,41 @@ std::optional Camera::GetAngleToNote() { return angleToNote; } -photon::PhotonPipelineResult Camera::GetLatestResult() { - return latestResult; -} - std::optional Camera::GetEstimatedGlobalPose( frc::Pose3d robotPose) { std::optional visionEst; - auto result = camera->GetLatestResult(); - visionEst = photonEstimator->Update(result); + auto allUnread = camera->GetAllUnreadResults(); - if (visionEst.has_value()) { - posePub.Set(visionEst.value().estimatedPose.ToPose2d()); - } else { - posePub.Set({}); - } + for (const auto& result : allUnread) { + visionEst = photonEstimator->Update(result); - latestResult = result; - - const auto& targetsSpan = result.GetTargets(); - targetsCopy = std::vector(targetsSpan.begin(), - targetsSpan.end()); - - std::vector targetPoses; - std::vector cornerPxs; - for (const auto& target : targetsCopy) { - targetPoses.emplace_back( - robotPose.TransformBy(photonEstimator->GetRobotToCameraTransform()) - .TransformBy(target.bestCameraToTarget)); - for (const auto& corner : target.GetDetectedCorners()) { - // YEAH I KNOW ITS NOT A METER ITS PIXELS BUT ASCOPE NEEDS A TRANSLATION - // TYPE - cornerPxs.emplace_back(frc::Translation2d{units::meter_t{corner.x}, - units::meter_t{corner.y}}); + if (visionEst.has_value()) { + posePub.Set(visionEst.value().estimatedPose.ToPose2d()); + } else { + posePub.Set({}); + } + + const auto& targetsSpan = result.GetTargets(); + targetsCopy = std::vector(targetsSpan.begin(), + targetsSpan.end()); + + std::vector targetPoses; + std::vector cornerPxs; + for (const auto& target : targetsCopy) { + targetPoses.emplace_back( + robotPose.TransformBy(photonEstimator->GetRobotToCameraTransform()) + .TransformBy(target.bestCameraToTarget)); + for (const auto& corner : target.GetDetectedCorners()) { + // YEAH I KNOW ITS NOT A METER ITS PIXELS BUT ASCOPE NEEDS A TRANSLATION + // TYPE + cornerPxs.emplace_back(frc::Translation2d{units::meter_t{corner.x}, + units::meter_t{corner.y}}); + } } + targetPosesPub.Set(targetPoses); + cornersPub.Set(cornerPxs); } - targetPosesPub.Set(targetPoses); - cornersPub.Set(cornerPxs); return visionEst; } diff --git a/src/main/include/str/vision/Camera.h b/src/main/include/str/vision/Camera.h index 76bd3bb..e421315 100644 --- a/src/main/include/str/vision/Camera.h +++ b/src/main/include/str/vision/Camera.h @@ -29,7 +29,6 @@ class Camera { Eigen::Matrix singleTagStdDev, Eigen::Matrix multiTagDevs, bool simulate); void SimPeriodic(frc::Pose2d robotSimPose); - photon::PhotonPipelineResult GetLatestResult(); std::optional GetEstimatedGlobalPose( frc::Pose3d robotPose); Eigen::Matrix GetEstimationStdDevs(frc::Pose2d estimatedPose);