This is the latest WPILib src, VisionSample2013, cRIO image, ... pulled down from firstforge.wpi.edu.
There might be risks in using the top of tree rather than an official release, but the commit messages do mention fixes for some deadlocks and race conditions.
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4066 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Vision2009/TrackAPI.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Vision2009/TrackAPI.cpp
new file mode 100644
index 0000000..41d40bc
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Vision2009/TrackAPI.cpp
@@ -0,0 +1,481 @@
+/********************************************************************************
+* Project : FIRST Motor Controller
+* File Name : TrackAPI.cpp
+* Contributors : ELF, DWD
+* Creation Date : August 10, 2008
+* Revision History : Source code & revision history maintained at sourceforge.WPI.edu
+* File Description : Tracking Routines for FIRST Vision API
+*/
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "string.h"
+#include "vxWorks.h"
+
+#include "AxisCamera.h"
+#include "FrcError.h"
+#include "TrackAPI.h"
+#include "VisionAPI.h"
+
+int TrackAPI_debugFlag = 0;
+#define DPRINTF if(TrackAPI_debugFlag)dprintf
+
+/**
+* @brief Find the largest particle that meets a criteria
+* @param binaryImage Image to inspect
+* @param rect area to search
+* @return 0 = error
+*/
+bool InArea(Image* binaryImage, int particleIndex, Rect rect)
+{
+ double position;
+
+ imaqMeasureParticle(binaryImage, particleIndex, 0,
+ IMAQ_MT_BOUNDING_RECT_LEFT, &position);
+ if ( position < (rect.left ) ) return false; // outside left of rectangle?
+
+ imaqMeasureParticle(binaryImage, particleIndex, 0,
+ IMAQ_MT_BOUNDING_RECT_TOP, &position);
+ if ( position < (rect.top ) ) return false; // outside top of rectangle ?
+
+ imaqMeasureParticle(binaryImage, particleIndex, 0,
+ IMAQ_MT_BOUNDING_RECT_RIGHT, &position);
+ if (position > (rect.left + rect.width) ) return false; // outside right of rectangle ?
+
+ imaqMeasureParticle(binaryImage, particleIndex, 0,
+ IMAQ_MT_BOUNDING_RECT_BOTTOM, &position);
+ if (position > (rect.top + rect.height) ) return false; // outside bottom of rectangle ?
+
+ DPRINTF(LOG_INFO, "particle %i is in (%i %i) height %i width %i\n",
+ particleIndex, rect.left, rect.top, rect.height, rect.width);
+ return true;
+}
+
+/**
+* @brief Find the largest particle that meets a criteria
+* @param binaryImage Image to inspect
+* @param largestParticleIndex Index of the largest particle
+* @param rect area to search
+* @return 0 = error
+*/
+int GetLargestParticle(Image* binaryImage, int* largestParticleIndex)
+{ return GetLargestParticle(binaryImage, largestParticleIndex, IMAQ_NO_RECT); }
+
+int GetLargestParticle(Image* binaryImage, int* largestParticleIndex, Rect rect)
+{
+ *largestParticleIndex = 0; // points to caller-provided variable
+
+ /* determine number of particles in thresholded image */
+ int numParticles;
+ int success = frcCountParticles(binaryImage, &numParticles);
+ if ( !success ) { return success; }
+
+ /* if no particles found we can quit here */
+ if (numParticles == 0) { return 0; } // unsuccessful if zero particles found
+
+ // find the largest particle
+ double largestParticleArea = 0;
+ double particleArea;
+ for (int i = 0; i < numParticles; ++i) {
+ success = imaqMeasureParticle(binaryImage, i, 0, IMAQ_MT_AREA, &particleArea);
+ if ( !success ) { return success; }
+ if (particleArea > largestParticleArea) {
+ // see if is in the right area
+ if ( InArea(binaryImage, i, rect) ) {
+ largestParticleArea = particleArea;
+ *largestParticleIndex = i; // return index to caller
+ }
+ }
+ }
+
+ return success;
+}
+
+/**
+* @brief Search for a color. Supports IMAQ_IMAGE_HSL.
+* @param color Definition for the hue range
+* @param trackReport Values for tracking: center of particle, particle size, color
+* @return 0 = error
+*/
+int FindColor(FrcHue color, ParticleAnalysisReport* trackReport)
+{
+ int success = 0; // return: 0 = error
+
+ /* track color */
+ // use ACTIVE_LIGHT or WHITE_LIGHT for brightly lit objects
+ TrackingThreshold td = GetTrackingData(color, PASSIVE_LIGHT);
+
+ success = FindColor(IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, trackReport);
+ if ( !success ) {
+ DPRINTF (LOG_INFO, "did not find color - errorCode= %i",GetLastVisionError());
+ return success;
+ }
+
+ //PrintReport(par);
+
+ /* set an image quality restriction */
+ if (trackReport->particleToImagePercent < PARTICLE_TO_IMAGE_PERCENT) {
+ imaqSetError(ERR_PARTICLE_TOO_SMALL, __FUNCTION__);
+ success = 0;
+ }
+ return success;
+}
+
+/**
+* @brief Search for a color. Supports IMAQ_IMAGE_HSL.
+* @param hueRange The range for the first plane
+* @param trackReport Values for tracking: center of particle, particle size, color
+* @return 0 = error
+*/
+int FindColor(const Range* hueRange, ParticleAnalysisReport *trackReport)
+{ return FindColor(hueRange, DEFAULT_SATURATION_THRESHOLD, trackReport); }
+
+/**
+* @brief Search for a color. Supports IMAQ_IMAGE_HSL.
+* @param hueRange The range for the first plane
+* @param minSaturation The lower range saturation
+* @param trackReport Values for tracking: center of particle, particle size, color
+* @return 0 = error
+*/
+int FindColor(const Range* hueRange, int minSaturation, ParticleAnalysisReport *trackReport)
+{
+ Range satRange;
+ satRange.minValue = minSaturation;
+ satRange.maxValue = 255;
+ Range lumRange;
+ lumRange.minValue = 0;
+ lumRange.maxValue = 255;
+ ColorMode cmode = IMAQ_HSL;
+ return FindColor(cmode, hueRange, &satRange, &lumRange, trackReport);
+}
+
+/**
+* @brief Search for a color. Supports IMAQ_IMAGE_HSL and IMAQ_IMAGE_RGB.
+* @param mode Color mode, either IMAQ_HSL or IMAQ_RGB
+* @param plane1Range The range for the first plane (hue or red)
+* @param plane2Range The range for the second plane (saturation or green)
+* @param plane3Range The range for the third plane (luminance or blue)
+* @param trackReport Values for tracking: center of particle, particle size, etc
+* @return 0 = error
+*/
+int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range,
+ const Range* plane3Range, ParticleAnalysisReport *trackReport)
+{
+ return FindColor(mode, plane1Range, plane2Range, plane3Range, trackReport, NULL);
+}
+
+/**
+* @brief Search for a color. Supports IMAQ_IMAGE_HSL and IMAQ_IMAGE_RGB.
+* @param mode Color mode, either IMAQ_HSL or IMAQ_RGB
+* @param plane1Range The range for the first plane (hue or red)
+* @param plane2Range The range for the second plane (saturation or green)
+* @param plane3Range The range for the third plane (luminance or blue)
+* @param trackReport Values for tracking: center of particle, particle size, etc
+* @param colorReport Color charactaristics of the particle
+* @return 0 = error
+*/
+int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range,
+ const Range* plane3Range, ParticleAnalysisReport *trackReport,
+ ColorReport *colorReport)
+{
+ return FindColor(mode, plane1Range, plane2Range, plane3Range, trackReport,
+ NULL, IMAQ_NO_RECT);
+}
+
+/**
+* @brief Search for a color. Supports IMAQ_IMAGE_HSL and IMAQ_IMAGE_RGB.
+* @param mode Color mode, either IMAQ_HSL or IMAQ_RGB
+* @param plane1Range The range for the first plane (hue or red)
+* @param plane2Range The range for the second plane (saturation or green)
+* @param plane3Range The range for the third plane (luminance or blue)
+* @param trackReport Values for tracking: center of particle, particle size, etc
+* @param colorReport Color charactaristics of the particle
+* @param rect Rectangle to confine search to
+* @return 0 = error
+*/
+int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range,
+ const Range* plane3Range, ParticleAnalysisReport *trackReport,
+ ColorReport *colorReport, Rect rect)
+{
+ int errorCode = 0;
+ int success = 0;
+
+ /* create an image object */
+ Image* cameraImage = frcCreateImage(IMAQ_IMAGE_HSL);
+ if (!cameraImage) { return success; }
+
+ /* get image from camera - if the camera has not finished initializing,
+ * this will fail
+ */
+ double imageTime;
+ success = GetImage(cameraImage, &imageTime);
+ if (!success){
+ DPRINTF(LOG_INFO, "No camera Image available Error = %i %s",
+ errorCode, GetVisionErrorText(errorCode));
+ frcDispose(cameraImage);
+ imaqSetError(errorCode, __FUNCTION__); //reset error code for the caller
+ return success;
+ }
+
+ /* save a copy of the image to another image for color thresholding later */
+ Image* histImage = frcCreateImage(IMAQ_IMAGE_HSL);
+ if (!histImage) { frcDispose(cameraImage); return success; }
+ success = frcCopyImage(histImage,cameraImage);
+ if ( !success ) {
+ errorCode = GetLastVisionError();
+ frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
+ return success;
+ }
+
+ /* Color threshold the image */
+ success = frcColorThreshold(cameraImage, cameraImage, mode, plane1Range, plane2Range, plane3Range);
+ if ( !success ) {
+ errorCode = GetLastVisionError();
+ DPRINTF (LOG_DEBUG, "Error = %i %s ", errorCode, GetVisionErrorText(errorCode));
+ frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
+ return success;
+ }
+
+ int largestParticleIndex = 0;
+ success = GetLargestParticle(cameraImage, &largestParticleIndex, rect );
+ if ( !success ) {
+ errorCode = GetLastVisionError();
+ DPRINTF (LOG_DEBUG, "Error after GetLargestParticle = %i %s ", errorCode, GetVisionErrorText(errorCode));
+ frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
+ imaqSetError(ERR_COLOR_NOT_FOUND, __FUNCTION__);
+ return success;
+ }
+ DPRINTF(LOG_INFO, "largestParticleIndex = %i\n", largestParticleIndex);
+
+ /* Particles were found */
+ /*
+ * Fill in report information for largest particle found
+ */
+ success = frcParticleAnalysis(cameraImage, largestParticleIndex, trackReport);
+ trackReport->imageTimestamp = imageTime;
+
+ /* clean up */
+ if (!success) {frcDispose(__FUNCTION__,cameraImage,histImage,NULL); return success;}
+
+ /* particle color statistics */
+ /* only if a color report requested */
+ if (colorReport != NULL)
+ {
+ /* first filter out the other particles */
+ ParticleFilterCriteria2 criteria;
+ ParticleFilterOptions* options = NULL;
+ Rect rect;
+ int numParticles;
+ success = frcParticleFilter(cameraImage, cameraImage, &criteria, 1, options,
+ rect, &numParticles);
+ if ( !success ) {
+ DPRINTF(LOG_INFO, "frcParticleFilter errorCode %i", GetLastVisionError());
+ }
+
+ /* histogram the original image using the thresholded image as a mask */
+ int numClasses = 10; //how many classes?
+ ColorHistogramReport* chrep = imaqColorHistogram2(histImage, numClasses, IMAQ_HSL,
+ NULL, cameraImage);
+ if (chrep == NULL) {
+ DPRINTF(LOG_INFO, "NULL Color Histogram");
+ errorCode = GetLastVisionError();
+ } else {
+ colorReport->particleHueMax = chrep->plane1.max;
+ colorReport->particleHueMin = chrep->plane1.min;
+ colorReport->particleHueMean = chrep->plane1.mean;
+ colorReport->particleSatMax = chrep->plane2.max;
+ colorReport->particleSatMin = chrep->plane2.min;
+ colorReport->particleSatMean = chrep->plane2.mean;
+ colorReport->particleLumMax = chrep->plane3.max;
+ colorReport->particleLumMin = chrep->plane3.min;
+ colorReport->particleLumMean = chrep->plane3.mean;
+ colorReport->numberParticlesFound = numParticles;
+ frcDispose(chrep);
+ }
+ }
+
+ /* clean up */
+ frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
+
+ return success;
+}
+
+
+/**
+ * Data functions for tracking
+ */
+
+
+/**
+ * @brief Get default HSL tracking parameters
+ * Note these parameters are not fully characterized at this point
+ * Get these default values and modify them as needed for your environment
+ * @param hue tasked color
+ * @param light saturation/luminance
+ */
+TrackingThreshold GetTrackingData(FrcHue hue, FrcLight light)
+{
+ TrackingThreshold trackingData;
+
+ //set saturation & luminance
+ switch (light) {
+ default:
+ case FLUORESCENT:
+ trackingData.saturation.minValue = 100;
+ trackingData.saturation.maxValue = 255;
+ trackingData.luminance.minValue = 40;
+ trackingData.luminance.maxValue = 255;
+ if (hue == GREEN) trackingData.luminance.minValue = 100;
+ if (hue == PINK) trackingData.saturation.minValue = 80;
+ if (hue == PINK) trackingData.luminance.minValue = 60;
+ if (hue == PINK) trackingData.luminance.maxValue = 155;
+ break;
+ case PASSIVE_LIGHT:
+ trackingData.saturation.minValue = 50;
+ trackingData.saturation.maxValue = 255;
+ trackingData.luminance.minValue = 20;
+ trackingData.luminance.maxValue = 255;
+ break;
+ case BRIGHT_LIGHT:
+ trackingData.saturation.minValue = 0;
+ trackingData.saturation.maxValue = 100;
+ trackingData.luminance.minValue = 100;
+ trackingData.luminance.maxValue = 255;
+ break;
+ case ACTIVE_LIGHT:
+ trackingData.saturation.minValue = 0;
+ trackingData.saturation.maxValue = 50;
+ trackingData.luminance.minValue = 150;
+ trackingData.luminance.maxValue = 255;
+ break;
+ case WHITE_LIGHT:
+ trackingData.saturation.minValue = 0;
+ trackingData.saturation.maxValue = 20;
+ trackingData.luminance.minValue = 200;
+ trackingData.luminance.maxValue = 255;
+ break;
+ }
+
+ //set hue
+ switch (hue){
+ default:
+ case WHITE:
+ strcpy (trackingData.name, "WHITE");
+ trackingData.hue.minValue = 0;
+ trackingData.hue.maxValue = 255;
+ break;
+ case ORANGE:
+ strcpy (trackingData.name, "ORANGE");
+ trackingData.hue.minValue = 5;
+ trackingData.hue.maxValue = 25;
+ break;
+ case YELLOW:
+ strcpy (trackingData.name, "YELLOW");
+ trackingData.hue.minValue = 30;
+ trackingData.hue.maxValue = 50;
+ break;
+ case GREEN:
+ strcpy (trackingData.name, "GREEN");
+ if (light == FLUORESCENT) {
+ trackingData.hue.minValue = 60;
+ trackingData.hue.maxValue = 110;
+ } else {
+ trackingData.hue.minValue = 90;
+ trackingData.hue.maxValue = 125;
+ }
+ break;
+ case BLUE:
+ strcpy (trackingData.name, "BLUE");
+ trackingData.hue.minValue = 140;
+ trackingData.hue.maxValue = 170;
+ break;
+ case PURPLE:
+ strcpy (trackingData.name, "PURPLE");
+ trackingData.hue.minValue = 180;
+ trackingData.hue.maxValue = 200;
+ break;
+ case PINK:
+ strcpy (trackingData.name, "PINK");
+ trackingData.hue.minValue = 210;
+ trackingData.hue.maxValue = 250;
+ break;
+ case RED:
+ strcpy (trackingData.name, "RED");
+ trackingData.hue.minValue = 240;
+ trackingData.hue.maxValue = 255;
+ break;
+ }
+ return(trackingData);
+}
+
+
+/**
+ * Print particle analysis report
+ * @param myReport Report to print
+ */
+void PrintReport(ParticleAnalysisReport* myReport)
+{
+ dprintf(LOG_INFO, "particle analysis:\n %s%i %s%i\n %s%lf\n %s%i %s%i\n %s%g %s%g\n %s%g\n %s%i %s%i\n %s%i %s%i\n",
+ "imageHeight = ", myReport->imageHeight,
+ "imageWidth = ", myReport->imageWidth,
+ "imageTimestamp = ", myReport->imageTimestamp,
+ "center_mass_x = ", myReport->center_mass_x,
+ "center_mass_y = ", myReport->center_mass_y,
+ "center_mass_x_normalized = ", myReport->center_mass_x_normalized,
+ "center_mass_y_normalized = ", myReport->center_mass_y_normalized,
+ "particleArea = ", myReport->particleArea,
+ "boundingRectangleTop = ", myReport->boundingRect.top,
+ "boundingRectangleLeft = ", myReport->boundingRect.left,
+ "boundingRectangleHeight = ", myReport->boundingRect.height,
+ "boundingRectangleWidth = ", myReport->boundingRect.width);
+
+ dprintf(LOG_INFO, "quality statistics: \n %s%g %s%g \n",
+ "particleToImagePercent = ", myReport->particleToImagePercent,
+ "particleQuality = ", myReport->particleQuality);
+}
+
+/**
+ * Print color report
+ * @param myReport Report to print
+ */
+void PrintReport(ColorReport* myReport)
+{
+ dprintf(LOG_INFO, "particle ranges for %i particles: ",
+ "numberParticlesFound = ", myReport->numberParticlesFound);
+ ;
+ dprintf(LOG_INFO, "\n %s%f %s%f %s%f\n %s%f %s%f %s%f\n %s%f %s%f %s%f\n -------",
+ "particleHueMax = ", myReport->particleHueMax,
+ "particleHueMin = ", myReport->particleHueMin,
+ "particleHueMean = ", myReport->particleHueMean,
+ "particleSatMax = ", myReport->particleSatMax,
+ "particleSatMin = ", myReport->particleSatMin,
+ "particleSatMean = ", myReport->particleSatMean,
+ "particleLumMax = ", myReport->particleLumMax,
+ "particleLumMin = ", myReport->particleLumMin,
+ "particleLumMean = ", myReport->particleLumMean);
+
+}
+
+/**
+ * Print color report
+ * @param myReport Report to print
+ */
+void PrintReport(TrackingThreshold* myReport)
+{
+ dprintf(LOG_INFO, "name of color: %s", myReport->name);
+
+ dprintf(LOG_INFO, "\n %s%i %s%i\n %s%i %s%i\n %s%i %s%i\n -------",
+ "hueMin = ", myReport->hue.minValue,
+ "hueMax = ", myReport->hue.maxValue,
+ "satMin = ", myReport->saturation.minValue,
+ "satMax = ", myReport->saturation.maxValue,
+ "lumMin = ", myReport->luminance.minValue,
+ "lumMax = ", myReport->luminance.maxValue );
+
+}
+
+
+