דלג לתוכן הראשי

תכנות FRC עם Limelight Lib‏ (WPILib Java & C++)

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

שימוש

זוהי ספריה המורכבת מקובץ בודד. כל שעליך לעשות הוא להעתיק את הקובץ LimelightHelpers.java מהגרסה האחרונה (https://github.com/LimelightVision/limelightlib-wpijava/releases) לתיקיית "robot" בפרויקט Java שלך. אין צורך ליצור אובייקטים עבור ה-Limelights שלך - הספריה פשוטה ופונקציונלית כדי למקסם את קלות השימוש והאמינות.

טיפים להצלחה

  • התחילו בפשטות! הרבה קבוצות FRC מצליחות משתמשות בגישות בסיסיות באופן יעיל. לדוגמה, קבוצה 2056 בשנת 2024 השתמשה בצינור עיבוד צבע סטנדרטי של 90FPS במקום ברשתות נוירונים למעקב אחר חלקי המשחק.
  • חשבו על מה שאתם באמת צריכים: האם אתם צריכים מיקום מלא בשדה, או שמספיק מירכוז מטרה פשוט (לדוגמה, driveSpeed = result.getTx() * 0.03)?

מושגי מפתח

1. שימוש בסיסי

כל מתודה מקבלת פרמטר שם Limelight. השאר ריק או null כדי להשתמש ב-"limelight":

// נתוני מיקוד בסיסיים
double tx = LimelightHelpers.getTX(""); // היסט אופקי מהצלב למטרה במעלות
double ty = LimelightHelpers.getTY(""); // היסט אנכי מהצלב למטרה במעלות
double ta = LimelightHelpers.getTA(""); // שטח המטרה (0% עד 100% מהתמונה)
boolean hasTarget = LimelightHelpers.getTV(""); // האם יש לך מטרה תקפה?

double txnc = LimelightHelpers.getTXNC(""); // היסט אופקי מהפיקסל/נקודה הראשית למטרה במעלות
double tync = LimelightHelpers.getTYNC(""); // היסט אנכי מהפיקסל/נקודה הראשית למטרה במעלות

2. ניהול Pipeline

Pipeline הם כמו תוכניות שניתן להחליף באופן מיידי המשנות את האופן שבו Limelight מעבד תמונות. ניתן להגדיר 10 pipeline שונים בממשק האינטרנט:

// עבור ל-pipeline 0
LimelightHelpers.setPipelineIndex("", 0);

3. בקרת LED

עבור Limelight עם נורות LED בהירות, ניתן לשלוט ב-LED למצבים שונים:

// תן ל-pipeline הנוכחי לשלוט ב-LED
LimelightHelpers.setLEDMode_PipelineControl("");

// כפה הדלקת/כיבוי/הבהוב LED
LimelightHelpers.setLEDMode_ForceOn("");
LimelightHelpers.setLEDMode_ForceOff("");
LimelightHelpers.setLEDMode_ForceBlink("");

4. מיקום בשדה עם MegaTag

(ראה את התיעוד של MegaTag ואת המדריך לאומדן מיקום Swerve לפרטים נוספים.)

// בפונקציה המחזורית שלך:
LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
if (limelightMeasurement.tagCount >= 2) { // סמוך על המדידה רק אם רואים מספר תגים
m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 9999999));
m_poseEstimator.addVisionMeasurement(
limelightMeasurement.pose,
limelightMeasurement.timestampSeconds
);
}

5. מיקום בשדה עם MegaTag2

(ראה את התיעוד של MegaTag2 ואת המדריך לאומדן מיקום Swerve לפרטים נוספים.)

MegaTag2 משפר את דיוק המיקום על ידי שילוב נתוני כיוון הרובוט עם ראייה. על ידי אספקת נתוני הג'ירו, אתה עוזר ל-MegaTag2 להגביל את בעיית המיקום ולספק תוצאות מצוינות גם אם רק תג אחד נראה:

// ראשית, ספר ל-Limelight את הכיוון הנוכחי של הרובוט
double robotYaw = m_gyro.getYaw();
LimelightHelpers.SetRobotOrientation("", robotYaw, 0.0, 0.0, 0.0, 0.0, 0.0);

// קבל את אומדן המיקום
LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue("");

// הוסף אותו לאומדן המיקום שלך
m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999));
m_poseEstimator.addVisionMeasurement(
limelightMeasurement.pose,
limelightMeasurement.timestampSeconds
);

6. פונקציונליות מיוחדת של AprilTag

// הגדר חלון חיתוך מותאם אישית לביצועים משופרים (-1 עד 1 לכל ערך)
LimelightHelpers.setCropWindow("", -0.5, 0.5, -0.5, 0.5);

// שנה את מיקום המצלמה ביחס למרכז הרובוט (x קדימה, y שמאלה, z למעלה, מעלות)
LimelightHelpers.setCameraPose_RobotSpace("",
0.5, // היסט קדימה (מטרים)
0.0, // היסט צד (מטרים)
0.5, // היסט גובה (מטרים)
0.0, // גלגול (מעלות)
30.0, // עלרוד (מעלות)
0.0 // סבסוב (מעלות)
);

// הגדר נקודת מעקב היסט AprilTag (מטרים)
LimelightHelpers.setFiducial3DOffset("",
0.0, // היסט קדימה
0.0, // היסט צד
0.5 // היסט גובה
);

// הגדר זיהוי AprilTag
LimelightHelpers.SetFiducialIDFiltersOverride("", new int[]{1, 2, 3, 4}); // עקוב רק אחרי מזהי תגים אלה
LimelightHelpers.SetFiducialDownscalingOverride("", 2.0f); // עבד ברזולוציה חצויה לשיפור קצב הפריימים וטווח מופחת

7. אינטגרציה עם Python

תקשר עם סקריפטי SnapScript מותאמים אישית של Python:

// שלח נתונים ל-Python
double[] dataToSend = {1.0, 2.0, 3.0};
LimelightHelpers.setPythonScriptData("", dataToSend);

// קבל נתונים מ-Python
double[] dataFromPython = LimelightHelpers.getPythonScriptData("");

8. קבלת תוצאות מפורטות מ-NetworkTables (RawTargets)

לביצועים מקסימליים, ניתן לגשת לנתוני מטרה גולמיים ישירות מ-NetworkTables, עוקף ניתוח JSON:

// קבל נתוני AprilTag/Fiducial גולמיים
RawFiducial[] fiducials = LimelightHelpers.getRawFiducials("");
for (RawFiducial fiducial : fiducials) {
int id = fiducial.id; // מזהה תג
double txnc = fiducial.txnc; // היסט X (ללא צלב)
double tync = fiducial.tync; // היסט Y (ללא צלב)
double ta = fiducial.ta; // שטח מטרה
double distToCamera = fiducial.distToCamera; // מרחק למצלמה
double distToRobot = fiducial.distToRobot; // מרחק לרובוט
double ambiguity = fiducial.ambiguity; // עמימות מיקום התג
}

// קבל תוצאות גלאי נוירוני גולמיות
RawDetection[] detections = LimelightHelpers.getRawDetections("");
for (RawDetection detection : detections) {
int classID = detection.classId;
double txnc = detection.txnc;
double tync = detection.tync;
double ta = detection.ta;
// גש לקואורדינטות פינה במידת הצורך
double corner0X = detection.corner0_X;
double corner0Y = detection.corner0_Y;
// ... פינות 1-3 זמינות באופן דומה
}

9. קבלת תוצאות מפורטות מ-JSON (אובייקט Results)

אובייקט Results מספק גישה מקיפה לכל סוגי הנתונים של Limelight. זה יכול לקחת עד 2 מילישניות לניתוח על ה-RoboRIO, לכן איננו ממליצים להשתמש בגישה זו ב-FRC:

LimelightResults results = LimelightHelpers.getLatestResults("");
if (results.valid) {
// מטרות צבע/רטרו-רפלקטיביות
if (results.targets_Retro.length > 0) {
LimelightTarget_Retro target = results.targets_Retro[0];
double skew = target.ts; // הטיית/סיבוב מטרה
double shortSide = target.short_side; // הצד הקצר בפיקסלים
double longSide = target.long_side; // הצד הארוך בפיקסלים
Pose3d targetPose = target.getCameraPose_TargetSpace();
}

// AprilTags/Fiducials
if (results.targets_Fiducials.length > 0) {
LimelightTarget_Fiducial tag = results.targets_Fiducials[0];
double id = tag.fiducialID; // מזהה תג
String family = tag.fiducialFamily; // משפחת תג (למשל, "16h5")

// נתוני מיקום תלת-ממדי
Pose3d robotPoseField = tag.getRobotPose_FieldSpace(); // מיקום הרובוט במרחב השדה
Pose3d cameraPoseTag = tag.getCameraPose_TargetSpace(); // מיקום המצלמה ביחס לתג
Pose3d robotPoseTag = tag.getRobotPose_TargetSpace(); // מיקום הרובוט ביחס לתג
Pose3d tagPoseCamera = tag.getTargetPose_CameraSpace(); // מיקום התג ביחס למצלמה
Pose3d tagPoseRobot = tag.getTargetPose_RobotSpace(); // מיקום התג ביחס לרובוט

// נתוני מיקוד דו-ממדי
double tx = tag.tx; // היסט אופקי מהצלב
double ty = tag.ty; // היסט אנכי מהצלב
double ta = tag.ta; // שטח מטרה (0-100% מהתמונה)
}

// זיהויי רשת נוירונית
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;
}

// תוצאות מסווג
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;
}

// תוצאות ברקוד/QR
if (results.targets_Barcode.length > 0) {
LimelightTarget_Barcode barcode = results.targets_Barcode[0];
String data = barcode.data;
String family = barcode.family;
}
}

10. ניהול צילומי מסך

צלם צילומי מסך לכיוון pipeline לא מקוון:

// צלם צילום מסך לצפייה מאוחרת יותר בממשק האינטרנט
LimelightHelpers.takeSnapshot("", "auto_shot_1");

11. פונקציונליות שונה

// שנה מצבי הזרמה
LimelightHelpers.setStreamMode_Standard(""); // הזרמות זו לצד זו
LimelightHelpers.setStreamMode_PiPMain(""); // הזרמה משנית בפינה
LimelightHelpers.setStreamMode_PiPSecondary(""); // הזרמה ראשית בפינה

שיטות סטטיות של LimelightHelpers

נתוני מטרה

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)

תוצאות רשת נוירונים

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

הערכת תנוחה

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)

תצורת מצלמה/צינור

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)

הגדרות מתקדמות של 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)

תוצאות וגישה לנתונים

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)

ממשק סקריפט Python

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

שיטות שירות

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)

מחלקות ליבה

PoseEstimate

מייצג הערכת תנוחה תלת-ממדית

מאפיינים ציבוריים:

  • Pose2d pose - התנוחה המוערכת
  • double timestampSeconds - חותמת זמן של ההערכה
  • double latency - השהיית עיבוד
  • int tagCount - מספר התגים שנעשה בהם שימוש
  • double tagSpan - מרווח בין התגים שזוהו
  • double avgTagDist - מרחק ממוצע לתגים
  • double avgTagArea - שטח ממוצע של התגים
  • RawFiducial[] rawFiducials - זיהויי פידוציאל גולמיים
  • boolean isMegaTag2 - האם זו תוצאת MegaTag2

(NetworkTables-derived) RawFiducial

מייצג נתוני זיהוי AprilTag/Fiducial גולמיים

מאפיינים ציבוריים:

  • int id - מספר זיהוי AprilTag
  • double txnc - היסט אופקי ממרכז המצלמה במעלות
  • double tync - היסט אנכי ממרכז המצלמה במעלות
  • double ta - שטח מטרה (0-100% מהתמונה)
  • double distToCamera - מרחק מהמצלמה למטרה במטרים
  • double distToRobot - מרחק מהרובוט למטרה במטרים
  • double ambiguity - ציון עמימות תנוחת AprilTag

(NetworkTables-derived) RawDetection

מייצג נתוני זיהוי רשת נוירונים גולמיים

מאפיינים ציבוריים:

  • int classId - מזהה מחלקת רשת נוירונים
  • double txnc - היסט אופקי ממרכז המצלמה במעלות
  • double tync - היסט אנכי ממרכז המצלמה במעלות
  • double ta - שטח מטרה (0-100% מהתמונה)
  • double corner0_X - קואורדינטת X של הפינה הראשונה
  • double corner0_Y - קואורדינטת Y של הפינה הראשונה
  • double corner1_X - קואורדינטת X של הפינה השנייה
  • double corner1_Y - קואורדינטת Y של הפינה השנייה
  • double corner2_X - קואורדינטת X של הפינה השלישית
  • double corner2_Y - קואורדינטת Y של הפינה השלישית
  • double corner3_X - קואורדינטת X של הפינה הרביעית
  • double corner3_Y - קואורדינטת Y של הפינה הרביעית

(JSON-derived) LimelightResults

מכיל את כל התוצאות מפלט ה-JSON של Limelight

מאפיינים ציבוריים:

  • String error - הודעת שגיאה אם קיימת
  • double pipelineID - אינדקס צינור הנוכחי
  • double latency_pipeline - השהיית עיבוד צינור (מילישניות)
  • double latency_capture - השהיית לכידת תמונה (מילישניות)
  • double latency_jsonParse - השהיית ניתוח JSON (מילישניות)
  • double timestamp_LIMELIGHT_publish - חותמת זמן כאשר המידע פורסם
  • double timestamp_RIOFPGA_capture - חותמת זמן כאשר RoboRIO FPGA לכד את המידע
  • boolean valid - האם המטרה תקפה
  • double[] botpose - תנוחת רובוט במרחב השדה
  • double[] botpose_wpired - תנוחת רובוט במרחב ברית אדומה של WPILib
  • double[] botpose_wpiblue - תנוחת רובוט במרחב ברית כחולה של WPILib
  • double botpose_tagcount - מספר התגים שנעשה בהם שימוש להערכת תנוחה
  • double botpose_span - מרווח בין התגים שזוהו
  • double botpose_avgdist - מרחק ממוצע לתגים שזוהו
  • double botpose_avgarea - שטח ממוצע של התגים שזוהו
  • double[] camerapose_robotspace - תנוחת מצלמה ביחס לרובוט
  • LimelightTarget_Retro[] targets_Retro - מערך מטרות רטרו-רפלקטיביות
  • LimelightTarget_Fiducial[] targets_Fiducials - מערך מטרות AprilTag
  • LimelightTarget_Classifier[] targets_Classifier - מערך תוצאות מסווג
  • LimelightTarget_Detector[] targets_Detector - מערך תוצאות גלאי
  • LimelightTarget_Barcode[] targets_Barcode - מערך תוצאות ברקוד

שיטות ציבוריות:

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

(JSON-derived) LimelightTarget_Retro

מייצג תוצאת מטרה צבעונית/רטרו-רפלקטיבית

מאפיינים ציבוריים:

  • double ta - שטח מטרה (0-100% מהתמונה)
  • double tx - היסט אופקי מכוונת למטרה (-29.8 עד 29.8 מעלות)
  • double ty - היסט אנכי מכוונת למטרה (-24.85 עד 24.85 מעלות)
  • double tx_pixels - היסט אופקי בפיקסלים
  • double ty_pixels - היסט אנכי בפיקסלים
  • double tx_nocrosshair - היסט אופקי ממרכז המצלמה
  • double ty_nocrosshair - היסט אנכי ממרכז המצלמה
  • double ts - הטיה או סיבוב של המטרה (-90 עד 0 מעלות)

שיטות ציבוריות:

  • 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()

(JSON-derived) LimelightTarget_Fiducial

מייצג תוצאת מטרת AprilTag/Fiducial

מאפיינים ציבוריים:

  • double fiducialID - מספר זיהוי AprilTag
  • String fiducialFamily - סוג משפחת AprilTag
  • כל המאפיינים מ-LimelightTarget_Retro

שיטות ציבוריות:

  • כל השיטות מ-LimelightTarget_Retro

(JSON-derived) LimelightTarget_Barcode

מייצג תוצאת מטרת ברקוד

מאפיינים ציבוריים:

  • String family - סוג משפחת ברקוד (למשל "QR", "DataMatrix")
  • String data - תוכן נתוני הברקוד המפוענח
  • double tx_pixels - היסט אופקי בפיקסלים
  • double ty_pixels - היסט אנכי בפיקסלים
  • double tx - היסט אופקי מכוונת
  • double ty - היסט אנכי מכוונת
  • double tx_nocrosshair - היסט אופקי ממרכז המצלמה
  • double ty_nocrosshair - היסט אנכי ממרכז המצלמה
  • double ta - שטח מטרה
  • double[][] corners - קואורדינטות פינות

שיטות ציבוריות:

  • String getFamily()

(JSON-derived) LimelightTarget_Classifier

מייצג תוצאת צינור מסווג נוירוני

מאפיינים ציבוריים:

  • String className - שם המחלקה שזוהתה
  • double classID - מספר זיהוי המחלקה שזוהתה
  • double confidence - ביטחון בזיהוי (0-100%)
  • double zone - אזור זיהוי
  • double tx - היסט אופקי מכוונת
  • double ty - היסט אנכי מכוונת
  • double txp - היסט אופקי בפיקסלים
  • double typ - היסט אנכי בפיקסלים

(JSON-derived) LimelightTarget_Detector

מייצג תוצאת צינור גלאי נוירוני

מאפיינים ציבוריים:

  • String className - שם המחלקה שזוהתה
  • double classID - מספר זיהוי המחלקה שזוהתה
  • double confidence - ביטחון בזיהוי (0-100%)
  • double ta - שטח מטרה
  • double tx - היסט אופקי מכוונת
  • double ty - היסט אנכי מכוונת
  • double txp - היסט אופקי בפיקסלים
  • double typ - היסט אנכי בפיקסלים
  • double tx_nocrosshair - היסט אופקי ממרכז המצלמה
  • double ty_nocrosshair - היסט אנכי ממרכז המצלמה