Aller au contenu principal

Programmation FRC avec Limelight Lib (WPILib Java & C++)

JavaDocs : https://limelightlib-wpijava-reference.limelightvision.io

Utilisation

Il s'agit d'une bibliothèque à fichier unique. Tout ce que vous devez faire est de copier le fichier LimelightHelpers.java de la dernière version (https://github.com/LimelightVision/limelightlib-wpijava/releases) dans le dossier "robot" de votre projet Java. Vous n'avez pas besoin de créer d'objets pour vos Limelights - la bibliothèque est simple et fonctionnelle pour maximiser la facilité d'utilisation et la fiabilité.

Conseils pour réussir

  • Commencez simplement ! De nombreuses équipes FRC performantes utilisent efficacement des approches basiques. Par exemple, l'équipe 2056 en 2024 a utilisé un pipeline de couleur standard à 90FPS au lieu de réseaux neuronaux pour le suivi des pièces de jeu.
  • Réfléchissez à vos besoins réels : Avez-vous besoin d'une localisation complète sur le terrain, ou est-ce qu'un simple centrage de cible suffirait (par exemple, driveSpeed = result.getTx() * 0.03) ?

Concepts Clés

1. Utilisation de Base

Chaque méthode accepte un paramètre de nom Limelight. Laissez-le vide ou null pour utiliser "limelight" :

// Données de ciblage basiques
double tx = LimelightHelpers.getTX(""); // Décalage horizontal du réticule à la cible en degrés
double ty = LimelightHelpers.getTY(""); // Décalage vertical du réticule à la cible en degrés
double ta = LimelightHelpers.getTA(""); // Zone de la cible (0% à 100% de l'image)
boolean hasTarget = LimelightHelpers.getTV(""); // Avez-vous une cible valide ?

double txnc = LimelightHelpers.getTXNC(""); // Décalage horizontal du pixel/point principal à la cible en degrés
double tync = LimelightHelpers.getTYNC(""); // Décalage vertical du pixel/point principal à la cible en degrés

2. Gestion des Pipelines

Les pipelines sont comme des programmes instantanément interchangeables qui modifient la façon dont Limelight traite les images. Vous pouvez configurer 10 pipelines différentes dans l'interface web :

// Passer à la pipeline 0
LimelightHelpers.setPipelineIndex("", 0);

3. Contrôle des LED

Pour les Limelights avec des LED d'illumination brillantes, vous pouvez contrôler les LED pour différentes situations :

// Laisser la pipeline actuelle contrôler les LED
LimelightHelpers.setLEDMode_PipelineControl("");

// Forcer les LED allumées/éteintes/clignotantes
LimelightHelpers.setLEDMode_ForceOn("");
LimelightHelpers.setLEDMode_ForceOff("");
LimelightHelpers.setLEDMode_ForceBlink("");

4. Localisation sur le Terrain avec MegaTag

(Voir la documentation MegaTag et le Tutoriel d'Estimation de Pose Swerve pour plus de détails.)

// Dans votre fonction périodique :
LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
if (limelightMeasurement.tagCount >= 2) { // Ne faire confiance à la mesure que si nous voyons plusieurs tags
m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 9999999));
m_poseEstimator.addVisionMeasurement(
limelightMeasurement.pose,
limelightMeasurement.timestampSeconds
);
}

5. Localisation sur le Terrain avec MegaTag2

(Voir la documentation MegaTag2 et le Tutoriel d'Estimation de Pose Swerve pour plus de détails.)

MegaTag2 améliore la précision de localisation en fusionnant les données d'orientation du robot avec la vision. En fournissant les données du gyroscope, vous aidez MegaTag2 à contraindre le problème de localisation et à fournir d'excellents résultats même si un seul tag est visible :

// D'abord, indiquez à Limelight l'orientation actuelle de votre robot
double robotYaw = m_gyro.getYaw();
LimelightHelpers.SetRobotOrientation("", robotYaw, 0.0, 0.0, 0.0, 0.0, 0.0);

// Obtenir l'estimation de pose
LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue("");

// L'ajouter à votre estimateur de pose
m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999));
m_poseEstimator.addVisionMeasurement(
limelightMeasurement.pose,
limelightMeasurement.timestampSeconds
);

6. Fonctionnalités Spéciales AprilTag

// Définir une fenêtre de recadrage personnalisée pour de meilleures performances (-1 à 1 pour chaque valeur)
LimelightHelpers.setCropWindow("", -0.5, 0.5, -0.5, 0.5);

// Modifier la pose de la caméra par rapport au centre du robot (x avant, y gauche, z haut, degrés)
LimelightHelpers.setCameraPose_RobotSpace("",
0.5, // Décalage avant (mètres)
0.0, // Décalage latéral (mètres)
0.5, // Décalage en hauteur (mètres)
0.0, // Roulis (degrés)
30.0, // Tangage (degrés)
0.0 // Lacet (degrés)
);

// Définir le point de suivi de décalage AprilTag (mètres)
LimelightHelpers.setFiducial3DOffset("",
0.0, // Décalage avant
0.0, // Décalage latéral
0.5 // Décalage en hauteur
);

// Configurer la détection AprilTag
LimelightHelpers.SetFiducialIDFiltersOverride("", new int[]{1, 2, 3, 4}); // Suivre uniquement ces IDs de tag
LimelightHelpers.SetFiducialDownscalingOverride("", 2.0f); // Traiter à mi-résolution pour améliorer la fréquence d'images et réduire la portée

7. Intégration Python

Communiquer avec des SnapScripts Python personnalisés :

// Envoyer des données à Python
double[] dataToSend = {1.0, 2.0, 3.0};
LimelightHelpers.setPythonScriptData("", dataToSend);

// Obtenir des données de Python
double[] dataFromPython = LimelightHelpers.getPythonScriptData("");

8. Obtention de Résultats Détaillés depuis NetworkTables (RawTargets)

Pour des performances maximales, vous pouvez accéder aux données brutes des cibles directement depuis NetworkTables, en contournant l'analyse JSON :

// Obtenir les données brutes AprilTag/Fiducial
RawFiducial[] fiducials = LimelightHelpers.getRawFiducials("");
for (RawFiducial fiducial : fiducials) {
int id = fiducial.id; // ID du tag
double txnc = fiducial.txnc; // Décalage X (sans réticule)
double tync = fiducial.tync; // Décalage Y (sans réticule)
double ta = fiducial.ta; // Zone de la cible
double distToCamera = fiducial.distToCamera; // Distance à la caméra
double distToRobot = fiducial.distToRobot; // Distance au robot
double ambiguity = fiducial.ambiguity; // Ambiguïté de pose du tag
}

// Obtenir les résultats bruts du détecteur neural
RawDetection[] detections = LimelightHelpers.getRawDetections("");
for (RawDetection detection : detections) {
int classID = detection.classId;
double txnc = detection.txnc;
double tync = detection.tync;
double ta = detection.ta;
// Accéder aux coordonnées des coins si nécessaire
double corner0X = detection.corner0_X;
double corner0Y = detection.corner0_Y;
// ... coins 1-3 disponibles de manière similaire
}

9. Obtention de Résultats Détaillés depuis JSON (Objet Results)

L'objet Results fournit un accès complet à tous les types de données Limelight. L'analyse peut prendre jusqu'à 2 ms sur le RoboRIO, nous ne recommandons donc pas cette approche en FRC :

LimelightResults results = LimelightHelpers.getLatestResults("");
if (results.valid) {
// Cibles de couleur/rétroréfléchissantes
if (results.targets_Retro.length > 0) {
LimelightTarget_Retro target = results.targets_Retro[0];
double skew = target.ts; // Inclinaison/rotation de la cible
double shortSide = target.short_side; // Côté le plus court en pixels
double longSide = target.long_side; // Côté le plus long en pixels
Pose3d targetPose = target.getCameraPose_TargetSpace();
}

// AprilTags/Fiducials
if (results.targets_Fiducials.length > 0) {
LimelightTarget_Fiducial tag = results.targets_Fiducials[0];
double id = tag.fiducialID; // ID du tag
String family = tag.fiducialFamily; // Famille du tag (ex: "16h5")

// Données de pose 3D
Pose3d robotPoseField = tag.getRobotPose_FieldSpace(); // Pose du robot dans l'espace du terrain
Pose3d cameraPoseTag = tag.getCameraPose_TargetSpace(); // Pose de la caméra par rapport au tag
Pose3d robotPoseTag = tag.getRobotPose_TargetSpace(); // Pose du robot par rapport au tag
Pose3d tagPoseCamera = tag.getTargetPose_CameraSpace(); // Pose du tag par rapport à la caméra
Pose3d tagPoseRobot = tag.getTargetPose_RobotSpace(); // Pose du tag par rapport au robot

// Données de ciblage 2D
double tx = tag.tx; // Décalage horizontal du réticule
double ty = tag.ty; // Décalage vertical du réticule
double ta = tag.ta; // Zone de la cible (0-100% de l'image)
}

// Détections du réseau neuronal
if (results.targets_Detector.length > 0) {
LimelightTarget_Detector detection = results.targets_Detector[0];
String className = detection.className;
double confidence = detection.confidence;
double area = detection.ta;
}

// Résultats du classificateur
if (results.targets_Classifier.length > 0) {
LimelightTarget_Classifier result = results.targets_Classifier[0];
String class_name = result.className;
double confidence = result.confidence;
int classID = (int)result.classID;
}

// Résultats des codes-barres/QR
if (results.targets_Barcode.length > 0) {
LimelightTarget_Barcode barcode = results.targets_Barcode[0];
String data = barcode.data;
String family = barcode.family;
}
}

10. Gestion des Captures d'Écran

Prendre des captures d'écran pour l'ajustement hors ligne des pipelines :

// Prendre une capture d'écran pour la visualiser plus tard dans l'interface web
LimelightHelpers.takeSnapshot("", "auto_shot_1");

11. Fonctionnalités Diverses

// Changer les modes de flux
LimelightHelpers.setStreamMode_Standard(""); // Flux côte à côte
LimelightHelpers.setStreamMode_PiPMain(""); // Flux secondaire dans le coin
LimelightHelpers.setStreamMode_PiPSecondary(""); // Flux principal dans le coin

Méthodes Statiques LimelightHelpers

Données de Cible

public static boolean getTV(String limelightName)
public static double getTX(String limelightName)
public static double getTY(String limelightName)
public static double getTXNC(String limelightName)
public static double getTYNC(String limelightName)
public static double getTA(String limelightName)
public static double[] getT2DArray(String limelightName)
public static int getTargetCount(String limelightName)
public static RawFiducial[] getRawFiducials(String limelightName)
public static RawDetection[] getRawDetections(String limelightName)

Résultats du Réseau Neuronal

public static int getClassifierClassIndex(String limelightName)  
public static int getDetectorClassIndex(String limelightName)
public static String getClassifierClass(String limelightName)
public static String getDetectorClass(String limelightName)

Estimation de Pose

public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName)
public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName)
public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName)
public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName)

public static Pose3d getBotPose3d(String limelightName)
public static Pose3d getBotPose3d_wpiRed(String limelightName)
public static Pose3d getBotPose3d_wpiBlue(String limelightName)
public static Pose3d getBotPose3d_TargetSpace(String limelightName)
public static Pose3d getCameraPose3d_TargetSpace(String limelightName)
public static Pose3d getTargetPose3d_CameraSpace(String limelightName)
public static Pose3d getTargetPose3d_RobotSpace(String limelightName)
public static Pose3d getCameraPose3d_RobotSpace(String limelightName)

public static Pose2d getBotPose2d(String limelightName)
public static Pose2d getBotPose2d_wpiBlue(String limelightName)
public static Pose2d getBotPose2d_wpiRed(String limelightName)

public static double[] getBotPose(String limelightName)
public static double[] getBotPose_wpiRed(String limelightName)
public static double[] getBotPose_wpiBlue(String limelightName)
public static double[] getBotPose_TargetSpace(String limelightName)
public static double[] getCameraPose_TargetSpace(String limelightName)
public static double[] getTargetPose_CameraSpace(String limelightName)
public static double[] getTargetPose_RobotSpace(String limelightName)

Configuration Caméra/Pipeline

public static void setPipelineIndex(String limelightName, int pipelineIndex)
public static void setPriorityTagID(String limelightName, int ID)
public static double getCurrentPipelineIndex(String limelightName)
public static String getCurrentPipelineType(String limelightName)

public static void setLEDMode_PipelineControl(String limelightName)
public static void setLEDMode_ForceOff(String limelightName)
public static void setLEDMode_ForceBlink(String limelightName)
public static void setLEDMode_ForceOn(String limelightName)

public static void setStreamMode_Standard(String limelightName)
public static void setStreamMode_PiPMain(String limelightName)
public static void setStreamMode_PiPSecondary(String limelightName)

public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax)
public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw)

Paramètres Avancés AprilTag/Fiducial

public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ)
public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, double pitch, double pitchRate, double roll, double rollRate)
public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs)
public static void SetFiducialDownscalingOverride(String limelightName, float downscale)

Résultats et Accès aux Données

public static double getLatency_Pipeline(String limelightName)
public static double getLatency_Capture(String limelightName)
public static double[] getTargetColor(String limelightName)
public static double getFiducialID(String limelightName)
public static String getNeuralClassID(String limelightName)
public static String[] getRawBarcodeData(String limelightName)
public static String getJSONDump(String limelightName)
public static LimelightResults getLatestResults(String limelightName)

Interface Script Python

public static void setPythonScriptData(String limelightName, double[] outgoingPythonData)
public static double[] getPythonScriptData(String limelightName)

Méthodes Utilitaires

public static CompletableFuture<Boolean> takeSnapshot(String tableName, String snapshotName)
public static NetworkTable getLimelightNTTable(String tableName)
public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName)
public static double getLimelightNTDouble(String tableName, String entryName)
public static void setLimelightNTDouble(String tableName, String entryName, double val)
public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val)
public static double[] getLimelightNTDoubleArray(String tableName, String entryName)
public static String getLimelightNTString(String tableName, String entryName)
public static URL getLimelightURLString(String tableName, String request)

Classes principales

PoseEstimate

Représente une estimation de pose 3D

Propriétés publiques :

  • Pose2d pose - La pose estimée
  • double timestampSeconds - Horodatage de l'estimation
  • double latency - Latence de traitement
  • int tagCount - Nombre de tags utilisés
  • double tagSpan - Écart entre les tags détectés
  • double avgTagDist - Distance moyenne aux tags
  • double avgTagArea - Surface moyenne des tags
  • RawFiducial[] rawFiducials - Détections brutes des marqueurs
  • boolean isMegaTag2 - Indique si c'est un résultat MegaTag2

(Dérivé de NetworkTables) RawFiducial

Représente les données brutes de détection d'AprilTag/marqueur

Propriétés publiques :

  • int id - Numéro d'ID AprilTag
  • double txnc - Décalage horizontal par rapport au centre de la caméra en degrés
  • double tync - Décalage vertical par rapport au centre de la caméra en degrés
  • double ta - Surface de la cible (0-100% de l'image)
  • double distToCamera - Distance de la caméra à la cible en mètres
  • double distToRobot - Distance du robot à la cible en mètres
  • double ambiguity - Score d'ambiguïté de pose AprilTag

(Dérivé de NetworkTables) RawDetection

Représente les données brutes de détection du réseau neuronal

Propriétés publiques :

  • int classId - ID de classe du réseau neuronal
  • double txnc - Décalage horizontal par rapport au centre de la caméra en degrés
  • double tync - Décalage vertical par rapport au centre de la caméra en degrés
  • double ta - Surface de la cible (0-100% de l'image)
  • double corner0_X - Coordonnée X du premier coin
  • double corner0_Y - Coordonnée Y du premier coin
  • double corner1_X - Coordonnée X du deuxième coin
  • double corner1_Y - Coordonnée Y du deuxième coin
  • double corner2_X - Coordonnée X du troisième coin
  • double corner2_Y - Coordonnée Y du troisième coin
  • double corner3_X - Coordonnée X du quatrième coin
  • double corner3_Y - Coordonnée Y du quatrième coin

(Dérivé de JSON) LimelightResults

Contient tous les résultats de la sortie JSON d'une Limelight

Propriétés publiques :

  • String error - Message d'erreur le cas échéant
  • double pipelineID - Index du pipeline actuel
  • double latency_pipeline - Latence de traitement du pipeline (ms)
  • double latency_capture - Latence de capture d'image (ms)
  • double latency_jsonParse - Latence d'analyse JSON (ms)
  • double timestamp_LIMELIGHT_publish - Horodatage de publication des données
  • double timestamp_RIOFPGA_capture - Horodatage de capture par le FPGA RoboRIO
  • boolean valid - Indique si la cible est valide
  • double[] botpose - Pose du robot dans l'espace du terrain
  • double[] botpose_wpired - Pose du robot dans l'espace WPILib alliance rouge
  • double[] botpose_wpiblue - Pose du robot dans l'espace WPILib alliance bleue
  • double botpose_tagcount - Nombre de tags utilisés pour l'estimation de pose
  • double botpose_span - Écart entre les tags détectés
  • double botpose_avgdist - Distance moyenne aux tags détectés
  • double botpose_avgarea - Surface moyenne des tags détectés
  • double[] camerapose_robotspace - Pose de la caméra par rapport au robot
  • LimelightTarget_Retro[] targets_Retro - Tableau des cibles rétroréfléchissantes
  • LimelightTarget_Fiducial[] targets_Fiducials - Tableau des cibles AprilTag
  • LimelightTarget_Classifier[] targets_Classifier - Tableau des résultats du classificateur
  • LimelightTarget_Detector[] targets_Detector - Tableau des résultats du détecteur
  • LimelightTarget_Barcode[] targets_Barcode - Tableau des résultats de codes-barres

Méthodes publiques :

  • Pose3d getBotPose3d()
  • Pose3d getBotPose3d_wpiRed()
  • Pose3d getBotPose3d_wpiBlue()
  • Pose2d getBotPose2d()
  • Pose2d getBotPose2d_wpiRed()
  • Pose2d getBotPose2d_wpiBlue()

(Dérivé de JSON) LimelightTarget_Retro

Représente un résultat de cible colorée/rétroréfléchissante

Propriétés publiques :

  • double ta - Surface de la cible (0-100% de l'image)
  • double tx - Décalage horizontal du réticule à la cible (-29.8 à 29.8 degrés)
  • double ty - Décalage vertical du réticule à la cible (-24.85 à 24.85 degrés)
  • double tx_pixels - Décalage horizontal en pixels
  • double ty_pixels - Décalage vertical en pixels
  • double tx_nocrosshair - Décalage horizontal depuis le centre de la caméra
  • double ty_nocrosshair - Décalage vertical depuis le centre de la caméra
  • double ts - Inclinaison ou rotation de la cible (-90 à 0 degrés)

Méthodes publiques :

  • Pose3d getCameraPose_TargetSpace()
  • Pose3d getRobotPose_FieldSpace()
  • Pose3d getRobotPose_TargetSpace()
  • Pose3d getTargetPose_CameraSpace()
  • Pose3d getTargetPose_RobotSpace()
  • Pose2d getCameraPose_TargetSpace2D()
  • Pose2d getRobotPose_FieldSpace2D()
  • Pose2d getRobotPose_TargetSpace2D()
  • Pose2d getTargetPose_CameraSpace2D()
  • Pose2d getTargetPose_RobotSpace2D()

(Dérivé de JSON) LimelightTarget_Fiducial

Représente un résultat de cible AprilTag/marqueur

Propriétés publiques :

  • double fiducialID - Numéro d'ID AprilTag
  • String fiducialFamily - Type de famille AprilTag
  • Toutes les propriétés de LimelightTarget_Retro

Méthodes publiques :

  • Toutes les méthodes de LimelightTarget_Retro

(Dérivé de JSON) LimelightTarget_Barcode

Représente un résultat de cible code-barres

Propriétés publiques :

  • String family - Type de famille de code-barres (ex: "QR", "DataMatrix")
  • String data - Contenu décodé du code-barres
  • double tx_pixels - Décalage horizontal en pixels
  • double ty_pixels - Décalage vertical en pixels
  • double tx - Décalage horizontal du réticule
  • double ty - Décalage vertical du réticule
  • double tx_nocrosshair - Décalage horizontal depuis le centre de la caméra
  • double ty_nocrosshair - Décalage vertical depuis le centre de la caméra
  • double ta - Surface de la cible
  • double[][] corners - Coordonnées des coins

Méthodes publiques :

  • String getFamily()

(Dérivé de JSON) LimelightTarget_Classifier

Représente un résultat de pipeline de classification neuronale

Propriétés publiques :

  • String className - Nom de la classe détectée
  • double classID - Numéro d'ID de la classe détectée
  • double confidence - Confiance de détection (0-100%)
  • double zone - Zone de détection
  • double tx - Décalage horizontal du réticule
  • double ty - Décalage vertical du réticule
  • double txp - Décalage horizontal en pixels
  • double typ - Décalage vertical en pixels

(Dérivé de JSON) LimelightTarget_Detector

Représente un résultat de pipeline de détection neuronale

Propriétés publiques :

  • String className - Nom de la classe détectée
  • double classID - Numéro d'ID de la classe détectée
  • double confidence - Confiance de détection (0-100%)
  • double ta - Surface de la cible
  • double tx - Décalage horizontal du réticule
  • double ty - Décalage vertical du réticule
  • double txp - Décalage horizontal en pixels
  • double typ - Décalage vertical en pixels
  • double tx_nocrosshair - Décalage horizontal depuis le centre de la caméra
  • double ty_nocrosshair - Décalage vertical depuis le centre de la caméra