diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 26967e0..ffa4a00 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -391,6 +391,7 @@ void RobotContainer::Periodic() { m_turnToPose.Update(); auto targets = m_tracker.GetTargets(); + m_tracker.UpdateTrackedTargets(targets); if (m_intake.NotePresent()) { // Note is present, get ready to score it diff --git a/src/main/cpp/utils/TargetTracker.cpp b/src/main/cpp/utils/TargetTracker.cpp index 3543dc7..0c07bc3 100644 --- a/src/main/cpp/utils/TargetTracker.cpp +++ b/src/main/cpp/utils/TargetTracker.cpp @@ -11,9 +11,35 @@ TargetTracker::TargetTracker(TargetTrackerConfig config, IntakeSubsystem* intake, ScoringSubsystem* scoring, DriveSubsystem* drive) - : m_config{config}, m_intake{intake}, m_scoring{scoring}, m_drive{drive} {} + : m_config{config}, m_intake{intake}, m_scoring{scoring}, m_drive{drive} { + m_trackedTargets = std::vector(m_config.maxTrackedItems); +} std::vector TargetTracker::GetTargets() { + if (!frc::RobotBase::IsReal()) { + static int counter = 0; + return { + DetectedObject(1, 0.75, + units::degree_t((counter++ % (31 * 20)) / 20 - 15), + 10_deg, 0.08, + { + {0, 0}, + {0, 0}, + {0, 0}, + {0, 0}, + }), + DetectedObject(1, 0.66, + units::degree_t((counter++ % (51 * 30 + 40)) / 30 - 25), + 15_deg, 0.06, + { + {0, 0}, + {0, 0}, + {0, 0}, + {0, 0}, + }), + }; + } + auto llResult = LimelightHelpers::getLatestResults(m_config.limelightName); auto& detectionResults = llResult.targetingResults.DetectionResults; @@ -37,14 +63,43 @@ std::vector TargetTracker::GetTargets() { return objects; } -std::optional TargetTracker::GetBestTarget( - std::vector& targets) { - static int counter = 0; - if (!frc::RobotBase::IsReal()) { - return DetectedObject(1, 0.75, units::degree_t((counter++ % (51 * 20)) / 20 - 25), 30_deg, - 0.08, {}); +void TargetTracker::UpdateTrackedTargets( + const std::vector& _objects) { + std::vector objects = _objects; + SortTargetsByProximity(objects); + + // Update with the latest targets + for (auto i = 0; i < std::min(static_cast(m_config.maxTrackedItems), + objects.size()); + i++) { + // TODO: split by class name + auto trackedPose = GetTargetPose(objects[i]); + + m_trackedTargets[i] = { + .object = objects[i], + .currentPose = trackedPose.value_or(m_trackedTargets[i].currentPose), + .valid = true, + }; + } + + // Invalidate the old ones + for (auto i = objects.size(); i < m_config.maxTrackedItems; i++) { + m_trackedTargets[i] = { + .object = DetectedObject(), + .currentPose = m_config.invalidTrackedPose, + .valid = false, + }; + } + + // Push them all to NT + // TODO: Move to separate method + for (auto i = 0; i < m_trackedTargets.size(); i++) { + PublishTrackedTarget(m_trackedTargets[i], i); } +} +std::optional TargetTracker::GetBestTarget( + std::vector& targets) { if (targets.empty()) { return std::nullopt; } @@ -71,10 +126,6 @@ bool TargetTracker::HasTargetLock(std::vector& targets) { std::optional TargetTracker::GetBestTargetPose( std::vector& targets) { - if (!frc::RobotBase::IsReal()) { - return m_config.simGamepiecePose; - } - if (!HasTargetLock(targets)) { return std::nullopt; } @@ -120,7 +171,9 @@ units::inch_t TargetTracker::GetDistanceToTarget(const DetectedObject& target) { frc::SmartDashboard::PutNumber("TargetTracker Pixel Width", pixelWidth); auto otherDistance = - pixelWidth > 0 + // TODO: Replace with constant + // This ensures we don't get a wildly small width (= very far away) if bounding box is wrong/missing + pixelWidth > 3 ? ((m_config.gamepieceWidth * m_config.focalLength) / pixelWidth) : 0_in; @@ -136,4 +189,21 @@ units::inch_t TargetTracker::GetDistanceToTarget(const DetectedObject& target) { std::to_string(otherDistance.value()) + " in"); return combinedDistance; +} + +void TargetTracker::SortTargetsByProximity( + std::vector& objects) { + std::sort(objects.begin(), objects.end(), + [this](const auto& a, const auto& b) { + auto distanceA = GetDistanceToTarget(a); + auto distanceB = GetDistanceToTarget(b); + + return distanceA < distanceB; + }); +} + +void TargetTracker::PublishTrackedTarget(const TrackedTarget& target, + int index) { + std::string key = "object[" + std::to_string(index) + "]"; + m_drive->GetField()->GetObject(key)->SetPose(target.currentPose); } \ No newline at end of file diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 80bebf5..c560223 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -56,7 +56,8 @@ class RobotContainer { void Initialize(); void StopMotors(); void Periodic(); - units::degree_t CurveRotation(double sensitivity, double val, double inMin, double inMax, double outMin, double outMax); + units::degree_t CurveRotation(double sensitivity, double val, double inMin, + double inMax, double outMin, double outMax); private: frc::Mechanism2d m_mech{1, 1}; @@ -111,29 +112,34 @@ class RobotContainer { StateSubsystem m_state{m_subsystems, m_driverController, m_operatorController}; - TargetTracker m_tracker{{// Camera angle - VisionConstants::kCameraAngle, - // Camera lens height - VisionConstants::kCameraLensHeight, - // Confidence threshold - VisionConstants::kConfidenceThreshold, - // Limelight name - VisionConstants::kLimelightName, - // Gamepiece width - VisionConstants::kNoteWidth, - // Focal length - VisionConstants::focalLength, - // Sim gamepiece pose - VisionConstants::kSimGamepiecePose, - // Gamepiece rotation - VisionConstants::kGamepieceRotation, - // Trig-based distance percentage - VisionConstants::kTrigDistancePercentage, - // Area percentage threshold - VisionConstants::kAreaPercentageThreshold}, - &m_intake, - &m_scoring, - &m_drive}; + TargetTracker m_tracker{ + {// Camera angle + VisionConstants::kCameraAngle, + // Camera lens height + VisionConstants::kCameraLensHeight, + // Confidence threshold + VisionConstants::kConfidenceThreshold, + // Limelight name + VisionConstants::kLimelightName, + // Gamepiece width + VisionConstants::kNoteWidth, + // Focal length + VisionConstants::focalLength, + // Sim gamepiece pose + VisionConstants::kSimGamepiecePose, + // Gamepiece rotation + VisionConstants::kGamepieceRotation, + // Trig-based distance percentage + VisionConstants::kTrigDistancePercentage, + // Area percentage threshold + VisionConstants::kAreaPercentageThreshold, + // Max # of tracked objects + 10, + // Default pose to use when tracked target isn't found + frc::Pose2d{100_m, 100_m, frc::Rotation2d{0_deg}}}, + &m_intake, + &m_scoring, + &m_drive}; TurnToPose m_turnToPose{{// Rotation constraints frc::TrapezoidProfile::Constraints{ diff --git a/src/main/include/utils/TargetTracker.h b/src/main/include/utils/TargetTracker.h index 0bd3827..87b5d82 100644 --- a/src/main/include/utils/TargetTracker.h +++ b/src/main/include/utils/TargetTracker.h @@ -52,6 +52,7 @@ struct DetectedCorners { struct DetectedObject { uint8_t classId; + std::string className; double confidence; // Positive-right, center-zero units::degree_t centerX; @@ -60,10 +61,13 @@ struct DetectedObject { double areaPercentage; DetectedCorners detectedCorners; + DetectedObject() {} + explicit DetectedObject(uint8_t id, double conf, units::degree_t cX, units::degree_t cY, double area, std::vector> corners) : classId{id}, + className{"unknown"}, confidence{conf}, centerX{cX}, centerY{cY}, @@ -73,6 +77,7 @@ struct DetectedObject { explicit DetectedObject( const LimelightHelpers::DetectionResultClass& detectionResult) : classId{static_cast(detectionResult.m_classID)}, + className{detectionResult.m_className}, confidence{detectionResult.m_confidence}, centerX{detectionResult.m_TargetXDegreesCrosshairAdjusted}, centerY{detectionResult.m_TargetYDegreesCrosshairAdjusted}, @@ -84,6 +89,12 @@ struct DetectedObject { } }; +struct TrackedTarget { + DetectedObject object; + frc::Pose2d currentPose; + bool valid; +}; + class TargetTracker { public: struct TargetTrackerConfig { @@ -99,11 +110,15 @@ class TargetTracker { /// applies the inverse to width-based estimate double trigDistancePercentage; double areaPercentageThreshold; + + uint8_t maxTrackedItems; + frc::Pose2d invalidTrackedPose; }; TargetTracker(TargetTrackerConfig config, IntakeSubsystem* intake, ScoringSubsystem* scoring, DriveSubsystem* drive); std::vector GetTargets(); + void UpdateTrackedTargets(const std::vector& objects); std::optional GetBestTarget(std::vector&); bool HasTargetLock(std::vector&); std::optional GetTargetPose(const DetectedObject&); @@ -111,8 +126,15 @@ class TargetTracker { units::inch_t GetDistanceToTarget(const DetectedObject&); private: + /** + * Sort targets by ASC distance to camera + */ + void SortTargetsByProximity(std::vector& objects); + void PublishTrackedTarget(const TrackedTarget& target, int index); + TargetTrackerConfig m_config; IntakeSubsystem* m_intake; ScoringSubsystem* m_scoring; DriveSubsystem* m_drive; + std::vector m_trackedTargets; }; \ No newline at end of file