axiscamera.cpp

good luck to everyone!

	  if (( (int)(cameraAddr.sin_addr.s_addr = inet_addr (serverName) ) == ERROR) &&
		( (int)(cameraAddr.sin_addr.s_addr = hostGetByName (serverName) ) == ERROR)) 
	  {	
		  CameraCloseSocket("Failed to get IP, check hostname or IP", camSock);
		continue;
	  }
	  
	  DPRINTF (LOG_INFO, "Attempting to connect to camSock" ); 
	  if (connect (camSock, (struct sockaddr *) &cameraAddr, sockAddrSize) == ERROR) 	{
		imaqSetError(ERR_CAMERA_CONNECT_FAILED, funcName);
		CameraCloseSocket("Failed to connect to camera - check network", camSock);
		continue;
	  }	  

#if MEASURE_SOCKET_SETUP
	  socketEndTime = GetTime(); 
	  setupTime = socketEndTime - socketStartTime; 
	  printf("\n***socket setup time = %g\n", setupTime );
#endif	  
	  
	  globalCamera.cameraMetrics[CAM_SOCKET_OPEN]++;
	  break;
	} // end while (trying to connect to camera)

	DPRINTF (LOG_DEBUG, "writing GET request to camSock" ); 
	if (write (camSock, tempBuffer , strlen(tempBuffer) ) == ERROR) {
		return CameraCloseSocket("Failed to send GET request", camSock);
	}

	//DPRINTF (LOG_DEBUG, "reading header" ); 
	/* Find content-length, then read that many bytes */
	int counter = 2;
	char* contentString = "Content-Length: ";
	char* authorizeString = "200 OK";
	
#define MEASURE_TIME 0
#if MEASURE_TIME
	//timing parameters - only measure one at the time
	double loopStartTime = 0.0; // measuring speed of execution loop
	double loopEndTime = 0.0;
	double cameraStartTime = 0.0;
	double cameraEndTime = 0.0;
	double previousStartTime = 0.0;
	int performanceLoopCounter = 0;
	int maxCount = 30;
#endif
	
	while (cont) {
#if MEASURE_TIME
		previousStartTime = loopStartTime;  // first time is bogus
		loopStartTime = GetTime(); 
#endif	
		// If camera has been turned OFF, jump to RETRY
		//if (globalCamera.acquire == 0) goto RETRY;
		
		/* Determine writer index */
		if (globalCamera.index == 0)
			writeIndex = 1;
		else
			writeIndex = 0;
		
		/* read header */
		//TODO: check for error in header, increment ERR_CAMERA_HEADER_ERROR
		char initialReadBuffer[DEFAULT_PACKET_SIZE] = "";
		char intermediateBuffer[1];
		char *trailingPtr = initialReadBuffer;
		int trailingCounter = 0;
		

#if MEASURE_TIME
		cameraStartTime = GetTime(); 
#endif	

		while (counter) {
			if (read (camSock, intermediateBuffer, 1) <= 0) {
				CameraCloseSocket("Failed to read image header", camSock);
				globalCamera.cameraMetrics[ERR_CAMERA_HEADER_ERROR]++;
				goto RETRY;
			}

			strncat(initialReadBuffer, intermediateBuffer, 1);
			if (NULL != strstr(trailingPtr, "\r\n\r\n")) {

				  if (!authorizeConfirmed){

					  if (strstr(initialReadBuffer, authorizeString))
					  {
						  authorizeConfirmed = 1;
						  /* set camera to initialized */
						  globalCamera.cameraReady = 1; 
					  }
					  else
					  {
						  CameraCloseSocket("Not authorized to connect to camera", camSock);
						  authorizeCount++;
				  goto RETRY;
					  }
				}
				--counter;
			}
			if (++trailingCounter >= 4) {
				trailingPtr++;
			}
		}
	
		counter = 1;
		char *contentLength = strstr(initialReadBuffer, contentString);
		if (contentLength == NULL) {
			globalCamera.cameraMetrics[ERR_CAMERA_HEADER_ERROR]++;
			CameraCloseSocket("No content-length token found in packet", camSock);
			goto RETRY;
		}
		/* get length of image content */
		contentLength = contentLength + strlen(contentString);
		globalCamera.data[writeIndex].cameraImageSize = atol (contentLength);
		
		if(globalCamera.data[writeIndex].cameraImage)
			free(globalCamera.data[writeIndex].cameraImage);
		//globalCamera.data[writeIndex].cameraImage = (Image *) malloc(globalCamera.data[writeIndex].cameraImageSize);
		globalCamera.data[writeIndex].cameraImage = (char*)malloc(globalCamera.data[writeIndex].cameraImageSize);
		if (NULL == globalCamera.data[writeIndex].cameraImage) {
			return CameraCloseSocket("Failed to allocate space for imageString", camSock);
		}
		globalCamera.cameraMetrics[CAM_BUFFERS_WRITTEN]++;
		
		//
		// This is a blocking camera read function, and will block if the camera
		// has been disconnected from the cRIO.  If however the camera is
		// POWERED OFF while connected to the cRIO, this function NEVER RETURNS
		//
		int bytesRead = fioRead (camSock, (char *)globalCamera.data[writeIndex].cameraImage,
				globalCamera.data[writeIndex].cameraImageSize);

#if MEASURE_TIME
		cameraEndTime = GetTime(); 
#endif	
		
		//DPRINTF (LOG_DEBUG, "Completed fioRead function - bytes read:%d", bytesRead);
		if (bytesRead <= 0) {
			CameraCloseSocket("Failed to read image data", camSock);
			goto RETRY;
		} else if (bytesRead != globalCamera.data[writeIndex].cameraImageSize){
			perror ("ERROR: Failed to read entire image: readLength does not match bytes read");
			globalCamera.cameraMetrics[CAM_BAD_IMAGE_SIZE]++;
		}
		// if decoding the JPEG to an HSL Image, do it here
		if (globalCamera.decode) {
			if(globalCamera.data[writeIndex].decodedImage)
				frcDispose(globalCamera.data[writeIndex].decodedImage);
			globalCamera.data[writeIndex].decodedImage = frcCreateImage(IMAQ_IMAGE_HSL);
			if (! Priv_ReadJPEGString_C(globalCamera.data[writeIndex].decodedImage, 
					(const unsigned char *)globalCamera.data[writeIndex].cameraImage, 
					globalCamera.data[writeIndex].cameraImageSize) ) {
				DPRINTF (LOG_DEBUG, "failure creating Image");			
			}
		}
		
		// TODO: React to partial image
		globalCamera.data[writeIndex].timestamp = GetTime();
		globalCamera.index = writeIndex;
		
		/* signal a listening task */
		if (globalCamera.readerPID) {
			if (taskKill (globalCamera.readerPID,SIGUSR1) == OK)
				DPRINTF (LOG_DEBUG, "SIGNALING PID= %i", globalCamera.readerPID);
			else
				globalCamera.cameraMetrics[CAM_PID_SIGNAL_ERR]++;
				DPRINTF (LOG_DEBUG, "ERROR SIGNALING PID= %i", globalCamera.readerPID);
		}

		globalCamera.cameraMetrics[CAM_NUM_IMAGE]++;	
		printCounter ++;
		if (printCounter == 1000) { 
			DPRINTF (LOG_DEBUG, "imageCounter = %i", globalCamera.cameraMetrics[CAM_NUM_IMAGE]); 
			printCounter=0; 
		}
		
		taskDelay(1);  
		
#if MEASURE_TIME
		loopEndTime = GetTime(); 
		performanceLoopCounter++;
		if (performanceLoopCounter <= maxCount) {
			DPRINTF (LOG_DEBUG, "%i DONE!!!: loop = ,%g,  camera = ,%g,  difference = ,%g, loopRate= ,%g,",
					performanceLoopCounter, loopEndTime-loopStartTime, cameraEndTime-cameraStartTime, 
					(loopEndTime-loopStartTime) - (cameraEndTime-cameraStartTime),
					loopStartTime-previousStartTime);						
		}
#endif	
	}  /* end while (cont) */

	/* clean up */
	close (camSock);
	cont = 0;
	DPRINTF (LOG_INFO, "\nJPEG SERVER ENDING  errorCode = %i\n", errorCode );
	
	return (OK);
}

/**
 * @brief Start signaling a task when new images are available
 * @param taskID number for task to get the signal
 */
void StartImageSignal(int taskId) // Start issuing a SIGUSR1 signal to the specified taskId
{	globalCamera.readerPID = taskId; }

/**
 * @brief Start serving images
 */
void StartImageAcquisition()
{	
	globalCamera.cameraMetrics[CAM_STARTS]++;  
	globalCamera.acquire = 1; 
	DPRINTF(LOG_DEBUG, "starting acquisition");
}


/**
 * @brief Stop serving images
 */
void StopImageAcquisition()
{	globalCamera.cameraMetrics[CAM_STOPS]++;  globalCamera.acquire = 0; }


/**
 * @brief This is the routine that is run when the task is spawned
 * It initializes the camera with the image settings passed in, and
 * starts image acquisition.
 * @param frames Frames per second 
 * @param compression Camera image compression 
 * @param resolution Camera image size 
 * @param rotation Camera image rotation 
 */
static int initCamera(int frames, int compression, ImageSize resolution, ImageRotation rotation) 
{
	//SetDebugFlag ( DEBUG_SCREEN_AND_FILE  ) ;
	
	DPRINTF(LOG_DEBUG, "\n+++++ camera task starting: rotation = %i", (int)rotation);
	int errorCode;

	/* Initialize globalCamera area 
	 * Set decode to 1 - always want to decode images for processing 
	 * If ONLY sending images to the dashboard, you could set it to 0 */
	bzero ((char *)&globalCamera, sizeof(globalCamera));
	globalCamera.index = -1;
	globalCamera.decode = 1;
	
	/* allow writing to vxWorks target */
	Priv_SetWriteFileAllowed(1); 
	
	/* start acquisition immediately */
	StartImageAcquisition();
	
	/*  cameraJPEGServer runs until camera is stopped */
	DPRINTF (LOG_DEBUG, "calling cameraJPEGServer" ); 
	errorCode = cameraJPEGServer(frames, compression, resolution, rotation);	
	DPRINTF (LOG_INFO, "errorCode from cameraJPEGServer = %i\n", errorCode ); 
	return (OK);
}

Task g_axisCameraTask("Camera", (FUNCPTR)initCamera);

/**
 * @brief Start the camera task
 * @param frames Frames per second 
 * @param compression Camera image compression 
 * @param resolution Camera image size 
 * @param rotation Camera image rotation (ROT_0 or ROT_180)
 * @return TaskID of camera task, or -1 if error.
 */
int StartCameraTask()
{
	return StartCameraTask(10, 0, k160x120, ROT_0);
}
int StartCameraTask(int frames, int compression, ImageSize resolution, ImageRotation rotation)
{
	char funcName[]="startCameraTask";
	DPRINTF(LOG_DEBUG, "starting camera");

	int cameraTaskID = 0;

	//range check
	if (frames < 1) frames = 1;
	else if (frames > 30) frames = 30;
	if (compression < 0) compression = 0;		
	else if (compression > 100) compression = 100;

	// stop any prior copy of running task
	StopCameraTask(); 

	// spawn camera task
	bool started = g_axisCameraTask.Start(frames, compression, resolution, rotation);
	cameraTaskID = g_axisCameraTask.GetID();
	DPRINTF(LOG_DEBUG, "spawned task id %i", cameraTaskID);

	if (!started)	{
		DPRINTF(LOG_DEBUG, "camera task failed to start");
		imaqSetError(ERR_CAMERA_TASK_SPAWN_FAILED, funcName);
		return -1;
	}
	return cameraTaskID;
}

/**
 * @brief Stops the camera task
 * @return TaskID of camera task killed, or -1 if none was running.
 */
int StopCameraTask()
{
    std::string taskName("FRC_Camera");    
	// check for prior copy of running task
	int oldTaskID = taskNameToId(const_cast<char*>(taskName.c_str()));
	if(oldTaskID != ERROR) { taskDelete(oldTaskID);  }
	return oldTaskID;
}

#if 0
/* if you want to run this task by itself to debug  
 * enable this code and make RunProgram the entry point 
 */
extern "C"
{
void RunProgram();
int AxisCamera_StartupLibraryInit();
}
/** * @brief Start point of the program */
void RunProgram()
{	StartCameraTask();}

/** * @brief This is the main program that is run by the debugger or the robot on boot. */
int AxisCamera_StartupLibraryInit()
	{		RunProgram();		return 0;	}

#endif