axiscamera.cpp

good luck to everyone!

/********************************************************************************
*  Project   		: FIRST Motor Controller
*  File Name  		: AxisCamera.cpp        
*  Contributors 	: TD, ELF, JDG, SVK
*  Creation Date 	: July 29, 2008
*  Revision History	: Source code & revision history maintained at sourceforge.WPI.edu    
*  File Description	: Axis camera access for the FIRST Vision API
*      The camera task runs as an independent thread 
*/    
/*----------------------------------------------------------------------------*/
/*        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 "sockLib.h" 
#include "vxWorks.h" 

#include "errno.h"
#include "fioLib.h"
#include "hostLib.h" 
#include "inetLib.h" 
#include "signal.h"
#include "sigLib.h"		// for signal
#include <string>
#include "time.h"
#include "usrLib.h"

#include "AxisCamera.h" 
#include "BaeUtilities.h"
#include "FrcError.h"
#include "Task.h"
#include "Timer.h"
#include "Utility.h"
#include "VisionAPI.h" 

// To locally enable debug printing: set AxisCamera_debugFlag to a 1, to disable set to 0
int AxisCamera_debugFlag = 0;
#define DPRINTF if(AxisCamera_debugFlag)dprintf

/** @brief Camera data to be accessed globally */
struct {
	int readerPID; // Set to taskID for signaling
	int index; /* -1,0,1 */
	int	acquire; /* 0:STOP CAMERA; 1:START CAMERA */
	int cameraReady;  /* 0: CAMERA NOT INITIALIZED; 1: CAMERA INITIALIZED */
	int	decode; /* 0:disable decoding; 1:enable decoding to HSL Image */
	struct {
		//
		// To find the latest image timestamp, access:
		// globalCamera.data[globalCamera.index].timestamp
		//
		double timestamp;       // when image was taken
		char*	cameraImage;    // jpeg image string
		int cameraImageSize;    // image size
		Image* decodedImage;    // image decoded to NI Image object
		int decodedImageSize;   // size of decoded image
	}data[2];
	int cameraMetrics[CAM_NUM_METRICS];
}globalCamera;

/* run flag */
static short cont = 0;

/**
 * @brief Get the most recent camera image.
 * Supports IMAQ_IMAGE_RGB and IMAQ_IMAGE_HSL.
 * @param image Image to return to; image must have been first created using frcCreateImage. 
 * When you are done, use frcDispose.
 * @param timestamp Timestamp to return; will record the time at which the image was stored.
 * @param lastImageTimestamp Input - timestamp of last image; prevents serving of stale images
 * @return 0 is failure, 1 is success
 * @sa frcCreateImage(), frcDispose()
 */
int GetImageBlocking(Image* image, double *timestamp, double lastImageTimestamp)
{
	char funcName[]="GetImageBlocking";
	int success;
	double startTime = GetTime();
	
	while (1)
	{
		success = GetImage(image, timestamp);
		if (!success) return (success);
		
		if (*timestamp > lastImageTimestamp)
			return (1); // GOOD IMAGE RETURNED

		if (GetTime() > (startTime + MAX_BLOCKING_TIME_SEC))
		{
			imaqSetError(ERR_CAMERA_BLOCKING_TIMEOUT, funcName);
			globalCamera.cameraMetrics[CAM_BLOCKING_TIMEOUT]++;
			return (0); // NO IMAGE AVAILABLE WITHIN specified time
		}
		globalCamera.cameraMetrics[CAM_BLOCKING_COUNT]++;
		taskDelay (1);
	}
}

/**
 * @brief Verifies that the camera is initialized
 * @return 0 for failure, 1 for success
 */
int CameraInitialized()
{
	char funcName[]="CameraInitialized";
	int success = 0;
	/* check to see if camera is initialized */
	if (!globalCamera.cameraReady)  {
		imaqSetError(ERR_CAMERA_NOT_INITIALIZED, funcName);
		DPRINTF (LOG_DEBUG, "Camera request before camera is initialized");
		globalCamera.cameraMetrics[CAM_GETIMAGE_BEFORE_INIT]++;
		globalCamera.cameraMetrics[CAM_GETIMAGE_FAILURE]++;
		return success;
	}
	
	if (globalCamera.index == -1){
		imaqSetError(ERR_CAMERA_NO_BUFFER_AVAILABLE, funcName);
		DPRINTF (LOG_DEBUG, "No camera image available");
		globalCamera.cameraMetrics[CAM_GETIMAGE_BEFORE_AVAILABLE]++;
		globalCamera.cameraMetrics[CAM_GETIMAGE_FAILURE]++;
		return success;
	}
	return 1;
}

/**
 * @brief Gets the most recent camera image, as long as it is not stale.
 * Supported image types: IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL
 * @param image Image to return, must have first been created with frcCreateImage or imaqCreate. 
 * When you finish with the image, call frcDispose() to dispose of it.
 * @param timestamp Returned timestamp of when the image was taken from the camera
 * @return failure = 0, success = 1
 */
int GetImage(Image* image, double *timestamp)
{
	char funcName[]="GetImage";
	int success = 0;
	int readIndex;
	int	readCount = 10;
	double currentTime = time(NULL);
	double currentImageTimestamp;

	/* check to see if camera is initialized */
	
	if (!CameraInitialized()) {return success;}
	
	/* try readCount times to get an image */
	while (readCount) {
		readIndex = globalCamera.index;
		if (!imaqDuplicate(image, globalCamera.data[readIndex].decodedImage)) {
			int errorCode = GetLastVisionError(); 
			DPRINTF (LOG_DEBUG,"Error duplicating image= %i  %s ", errorCode, GetVisionErrorText(errorCode));			
		}
		// save the timestamp to check before returning
		currentImageTimestamp = globalCamera.data[readIndex].timestamp;
		
		// make sure this buffer is not being written to now
		if (readIndex == globalCamera.index) break;
		readCount--;
	}
	
	/* were we successful ? */
	if (readCount){
		success = 1;
		if (timestamp != NULL)
			  *timestamp = currentImageTimestamp; // Return image timestamp	  
	} else{
		globalCamera.cameraMetrics[CAM_GETIMAGE_FAILURE]++;
	}
	
	/* Ensure the buffered image is not too old - set this "stale time" above */
	if (currentTime > globalCamera.data[globalCamera.index].timestamp + CAMERA_IMAGE_STALE_TIME_SEC){
		DPRINTF (LOG_CRITICAL, "STALE camera image (THIS COULD BE A BAD IMAGE)");
		imaqSetError(ERR_CAMERA_STALE_IMAGE, funcName);
		globalCamera.cameraMetrics[CAM_STALE_IMAGE]++;
		globalCamera.cameraMetrics[CAM_GETIMAGE_FAILURE]++;
		success = 0;
	}
	globalCamera.cameraMetrics[CAM_GETIMAGE_SUCCESS]++;
	return success;
}

/**
 * @brief Method to get a raw image from the buffer
 * @param imageData returned image data
 * @param numBytes returned number of bytes in buffer
 * @param currentImageTimestamp returned buffer time of image data
 * @return 0 if failure; 1 if success
 */
int GetImageData(char** imageData, int* numBytes, double* currentImageTimestamp)
{
	int success = 0;
	int readIndex;
	int	readCount = 10;
	int cameraImageSize;
	char *cameraImageString;

	/* check to see if camera is initialized */
			
	if (!CameraInitialized()) {return success;}
			
	/* try readCount times to get an image */
	while (readCount) {
		readIndex = globalCamera.index;
		cameraImageSize = globalCamera.data[readIndex].cameraImageSize;
		//cameraImageString = (Image *) malloc(cameraImageSize);
		cameraImageString = new char[cameraImageSize];
		if (NULL == cameraImageString) {
					DPRINTF (LOG_DEBUG, "Unable to allocate cameraImage");
					globalCamera.cameraMetrics[CAM_GETIMAGE_FAILURE]++;
					return success;
		}
		memcpy (cameraImageString, globalCamera.data[readIndex].cameraImage, cameraImageSize);
		*currentImageTimestamp = globalCamera.data[readIndex].timestamp;
		// make sure this buffer is not being written to now
		if (readIndex == globalCamera.index) break;
		free (cameraImageString);
		readCount--;
	}
	if (readCount){
		*imageData = cameraImageString;
		*numBytes = cameraImageSize;
		return 1;
	}		
	return (OK);
}

/**
 * @brief Blocking call to get images for PC.
 * This should be called from a separate task to maintain camera read performance. 
 * It is intended to be used for sending raw (undecoded) image data to the PC.
 * @param imageData image data to return
 * @param numBytes number of bytes in buffer
 * @param timestamp timestamp of buffer returned
 * @param lastImageTimestamp buffer time of last image data sent to PC
 * @return 0 if failure; 1 if success
 */
int GetImageDataBlocking(char** imageData, int* numBytes, double* timestamp, double lastImageTimestamp)
{

	char funcName[]="GetImageDataBlocking";
	int success;
	double startTime = GetTime();
	
    *imageData = NULL;
	while (1)
	{
		success = GetImageData(imageData, numBytes, timestamp);
		if (!success) return (success);
		
		if (*timestamp > lastImageTimestamp)
			return (1); // GOOD IMAGE DATA RETURNED

        delete *imageData;
        *imageData = NULL;

		if (GetTime() > (startTime + MAX_BLOCKING_TIME_SEC))
		{
			imaqSetError(ERR_CAMERA_BLOCKING_TIMEOUT, funcName);
			return (0); // NO IMAGE AVAILABLE WITHIN specified time
		}
		globalCamera.cameraMetrics[CAM_BLOCKING_COUNT]++;
		taskDelay (1);
	}
}

/**
 * @brief Accessor for camera instrumentation data
 * @param the counter queried
 * @return the counter value
 */
int GetCameraMetric(FrcvCameraMetric metric)
{	return globalCamera.cameraMetrics[metric];  }

/**
 * @brief Close socket & report error
 * @param errstring String to print
 * @param socket Socket to close
 * @return error
 */
int CameraCloseSocket(char *errstring, int socket)
{
	DPRINTF (LOG_CRITICAL, "Closing socket - CAMERA ERROR: %s", errstring );
	close (socket);
	return (ERROR);
}


/**
 * @brief Reads one line from the TCP stream.
 * @param camSock         The socket.
 * @param buffer          A buffer with bufSize allocated for it. 
 *                        On return, bufSize-1 amount of data or data upto first line ending
 *                        whichever is smaller, null terminated.
 * @param bufSize         The size of buffer.
 * @param stripLineEnding If true, strips the line ending chacters from the buffer before return.
 * @return 0 if failure; 1 if success
*/
static int CameraReadLine(int camSock, char* buffer, int bufSize, bool stripLineEnding) {
	char funcName[]="CameraReadLine";
    // Need at least 3 bytes in the buffer to pull this off.
	if (bufSize < 3) {
		imaqSetError(ERR_CAMERA_FAILURE, funcName);
		return 0;
	}
    //  Reduce size by 1 to allow for null terminator.
    --bufSize;
    //  Read upto bufSize characters.
    for (int i=0;i < bufSize;++i, ++buffer) {
        //  Read one character.
        if (read (camSock, buffer, 1) <= 0) {
    		imaqSetError(ERR_CAMERA_FAILURE, funcName);
            return 0;
        }
        //  Line endings can be "\r\n" or just "\n". So always 
        //  look for a "\n". If you got just a "\n" and 
        //  stripLineEnding is false, then convert it into a \r\n
        //  because callers expect a \r\n.
        //  If the combination of the previous character and the current character
        //  is "\r\n", the line ending
        if (*buffer=='\n') {
            //  If asked to strip the line ending, then set the buffer to the previous 
            //  character.
            if (stripLineEnding) {
                if (i > 0 && *(buffer-1)=='\r') {
                    --buffer;
                }
            }
            else {
                //  If the previous character was not a '\r', 
                if (i == 0 || *(buffer-1)!='\r') {
                    //  Make the current character a '\r'.
                    *buffer = '\r';
                    //  If space permits, add back the '\n'.
                    if (i < bufSize-1) {
                        ++buffer;