Skip to content

Show multiple vision targets simultaneously #161

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 4 commits into from
Apr 29, 2024
Merged

Show multiple vision targets simultaneously #161

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4216a95
show multiple vision targets simultaneously
having11 Apr 29, 2024
e5dd1bb
linter
having11 Apr 29, 2024
8dea70d
Don't mark explicit
having11 Apr 29, 2024
507df7f
add comment
having11 Apr 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/main/cpp/RobotContainer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
94 changes: 82 additions & 12 deletions src/main/cpp/utils/TargetTracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<TrackedTarget>(m_config.maxTrackedItems);
}

std::vector<DetectedObject> 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;

Expand All @@ -37,14 +63,43 @@ std::vector<DetectedObject> TargetTracker::GetTargets() {
return objects;
}

std::optional<DetectedObject> TargetTracker::GetBestTarget(
std::vector<DetectedObject>& 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<DetectedObject>& _objects) {
std::vector<DetectedObject> objects = _objects;
SortTargetsByProximity(objects);

// Update with the latest targets
for (auto i = 0; i < std::min(static_cast<size_t>(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<DetectedObject> TargetTracker::GetBestTarget(
std::vector<DetectedObject>& targets) {
if (targets.empty()) {
return std::nullopt;
}
Expand All @@ -71,10 +126,6 @@ bool TargetTracker::HasTargetLock(std::vector<DetectedObject>& targets) {

std::optional<frc::Pose2d> TargetTracker::GetBestTargetPose(
std::vector<DetectedObject>& targets) {
if (!frc::RobotBase::IsReal()) {
return m_config.simGamepiecePose;
}

if (!HasTargetLock(targets)) {
return std::nullopt;
}
Expand Down Expand Up @@ -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;

Expand All @@ -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<DetectedObject>& 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);
}
54 changes: 30 additions & 24 deletions src/main/include/RobotContainer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down Expand Up @@ -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<units::radians>::Constraints{
Expand Down
22 changes: 22 additions & 0 deletions src/main/include/utils/TargetTracker.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ struct DetectedCorners {

struct DetectedObject {
uint8_t classId;
std::string className;
double confidence;
// Positive-right, center-zero
units::degree_t centerX;
Expand All @@ -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<std::vector<double>> corners)
: classId{id},
className{"unknown"},
confidence{conf},
centerX{cX},
centerY{cY},
Expand All @@ -73,6 +77,7 @@ struct DetectedObject {
explicit DetectedObject(
const LimelightHelpers::DetectionResultClass& detectionResult)
: classId{static_cast<uint8_t>(detectionResult.m_classID)},
className{detectionResult.m_className},
confidence{detectionResult.m_confidence},
centerX{detectionResult.m_TargetXDegreesCrosshairAdjusted},
centerY{detectionResult.m_TargetYDegreesCrosshairAdjusted},
Expand All @@ -84,6 +89,12 @@ struct DetectedObject {
}
};

struct TrackedTarget {
DetectedObject object;
frc::Pose2d currentPose;
bool valid;
};

class TargetTracker {
public:
struct TargetTrackerConfig {
Expand All @@ -99,20 +110,31 @@ 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<DetectedObject> GetTargets();
void UpdateTrackedTargets(const std::vector<DetectedObject>& objects);
std::optional<DetectedObject> GetBestTarget(std::vector<DetectedObject>&);
bool HasTargetLock(std::vector<DetectedObject>&);
std::optional<frc::Pose2d> GetTargetPose(const DetectedObject&);
std::optional<frc::Pose2d> GetBestTargetPose(std::vector<DetectedObject>&);
units::inch_t GetDistanceToTarget(const DetectedObject&);

private:
/**
* Sort targets by ASC distance to camera
*/
void SortTargetsByProximity(std::vector<DetectedObject>& objects);
void PublishTrackedTarget(const TrackedTarget& target, int index);

TargetTrackerConfig m_config;
IntakeSubsystem* m_intake;
ScoringSubsystem* m_scoring;
DriveSubsystem* m_drive;
std::vector<TrackedTarget> m_trackedTargets;
};