mavis.cpp

机器人程序

		// close unused object handles
		if(pHLines)
		{
			if(ctlData.ctlCode != LOCATE_H_LINES && ctlData.ctlCode != NEXT_H_LINE)
			{
				delete pHLines;
				pHLines = 0;
			}
		}
		if(pCalibrationHelper)
		{
			delete pCalibrationHelper;
			pCalibrationHelper = 0;
		}


		// handle request
		pLogger->writelnToLog("\nProcessing command-code %d", ctlData.ctlCode);
		switch(ctlData.ctlCode)
		{
			case LOOK_ONCE:
				lookOnce(&ctlData, &objLoc, &pMVObj);

				// pass result data thru shared memory
				sendResult( &objLoc, sizeof(ObjLoc_t) );
				break;


			case GET_NEXT_OBJ:
				getNextObj(&ctlData, &objLoc, &pMVObj);

				// pass result data thru shared memory
				sendResult( &objLoc, sizeof(ObjLoc_t) );
				break;


			case LOOK_UNTIL_SIGHTED:
				while(!bStop)
				{
					lookOnce(&ctlData, &objLoc, &pMVObj);

					// Was the object sighted?
					// If it was, return data and exit loop.
					if( objLoc.prob )
					{
						sendResult( &objLoc, sizeof(ObjLoc_t) );
						break;
					}

					showFrame();

					// check for a new ctl event
					if( WAIT_OBJECT_0 == WaitForSingleObject(hMavisCtlEvent, 0) ) break;
				}
				break;


			case LOCATE_H_LINES:
				// reset the lineList -- ?
				if( !pHLines ) pHLines = new HLines(this);
				pHLines->locateHLines(ctlData.d, ctlData.ordering, &HLineMetadata);

				// pass result data thru shared memory
				sendResult( &HLineMetadata, sizeof(HLineMetadata_t) );
				break;


			case NEXT_H_LINE:
			{
				NextHLine_t nextHLine;
				if( !pHLines ) pHLines = new HLines(this);
				nextHLine.nRemaining = pHLines->nextLine( &(nextHLine.hLine) );
				sendResult( &nextHLine, sizeof(NextHLine_t) );
				break;
			}


			case FWD_RAW_FRAMES:
				fwdRawFrames();
				break;


			case NEXT_FRAME:
				//nextFrame();
				pCurrentFrame = getNextFrame();
				break;

			case GET_FRAME_SIZE:
			{
				FrameSize_t fs;
				//nextFrame();
				pCurrentFrame = getNextFrame();
				fs.height   = getImgHeight();
				fs.width    = getImgWidth();
				fs.nBufSize = frameSize;
				fs.nPixels  = fs.height * fs.width;
				sendResult( &fs, sizeof(FrameSize_t) );
				break;
			}

			case GET_FRAME:
				//sendResult( nextFrame(), frameSize );
				pCurrentFrame = getNextFrame();
				sendResult( pCurrentFrame->getData(), frameSize );
				break;

			case SET_TILT_ANGLE:  // fall through - sends the new angle as its result
				pCamera->setTiltAngle(ctlData.dblValue);
			case GET_TILT_ANGLE:
			{
				double newAngle = pCamera->getTiltAngle();
				sendResult( &newAngle, sizeof(double) );
				break;
			}


			default:
				break;
		}

		// get elapsed time
		QueryPerformanceCounter(&counts);
		stopTime = counts.QuadPart;
		pLogger->writelnToLog("Elapsed time: %I64Ld ms", (stopTime - startTime)/countsPerMillisec);


		// display
		if(!bStop) showFrame();
	}

	// free resources
	if(pMVObj) pObjFactory->releaseMVObj(pMVObj);
}


//////////////////
//  stop()
//
void Mavis::stop()
{
	bStop = true;
	SetEvent(hMavisCtlEvent);
	pLogger->writelnToLog("\nStopping Mavis");
}


//////////////////
//  sendResult()
//
void Mavis::sendResult(void * pData, int nBytes)
{
	WaitForSingleObject(hResultMutex, INFINITE);
	memcpy( lpvResultAdd, pData, nBytes );
	ReleaseMutex(hResultMutex);
	SetEvent(hResultEvent);
}


//////////////////
//  fwdRawFrames()
//
void Mavis::fwdRawFrames()
{
	DWORD res;
	do
	{
		pCurrentFrame = getNextFrame();
		//nextFrame();
		showFrame();
		res = WaitForSingleObject(hMavisCtlEvent, 0);
	}
	while( WAIT_OBJECT_0 != res );

	// The call to WaitForSingleObject() will have reset
	// the event, so we need to set it before returning.
	SetEvent(hMavisCtlEvent);
}


//////////////////
//  lookOnce()
//
void Mavis::lookOnce(CtlData_t * pCtlData, ObjLoc_t * pObjLoc, MVObj ** ppMVObj)
{
	static int      currObjId = -1;

	pLogger->writelnToLog("pCtlData->objId = %d", pCtlData->objId);

	// load object
	if(currObjId != pCtlData->objId)
	{
		currObjId = pCtlData->objId;
		if(*ppMVObj) pObjFactory->releaseMVObj(*ppMVObj);
		try {
			*ppMVObj = pObjFactory->getMVObj(currObjId, this);
		} catch (MavisErr & err) {
			pLogger->writelnToLog( err.getMsg() );
			*ppMVObj = 0;
			return;
		}
	}

	//todo: add an error value to the ObjLoc_t struct. Put error codes with
	// predefined text into maviscomm.h
	if( (*ppMVObj) )
		(*ppMVObj)->lookOnce(pObjLoc);
	else
	{
		pLogger->writelnToLog("Failed to create Object Searcher for objID=%d", pCtlData->objId);
	}
}


//////////////////
//  getNextObj()
//
void Mavis::getNextObj(CtlData_t * pCtlData, ObjLoc_t * pObjLoc, MVObj ** ppMVObj)
{

	pLogger->writelnToLog("pCtlData->objId = %d", pCtlData->objId);

	// load object
		try {
			*ppMVObj = pObjFactory->getMVObj(pCtlData->objId, this);			
		} catch (MavisErr & err) {
			pLogger->writelnToLog( err.getMsg() );
			*ppMVObj = 0;
			return;
		}

	if( (*ppMVObj) )
	{
		(*ppMVObj)->getNext(pObjLoc);
		pObjFactory->releaseMVObj(*ppMVObj);
	}
	else
	{
		pLogger->writelnToLog("Failed to create Object Searcher for objID=%d", pCtlData->objId);
	}
}


//////////////////
//  showFrame()
//
void Mavis::showFrame() {
	if(displayCB)
		(displayCB)(pCurrentFrame->getData(), frameSize);
}


//////////////////
//  getCalibrationHelper()
//
CalibrationHelper * Mavis::getCalibrationHelper()
{
	pCalibrationHelper = new CalibrationHelper(this);
	return pCalibrationHelper;
}


//////////////////
//  getCameraData()
//
void Mavis::getCameraData(CameraData_t * pCameraData)
{
	pCamera->getCameraData(pCameraData);
}


//////////////////
//  markLoc()
//
// todo: move this entire capability to MVImgUtils::Vis
//
void Mavis::markLoc(Pixel_t & px, int rgb, int h, int w)
{
	if( !displayCB ) return; // mark loc only if frame will be consumed

	int imgW = getImgWidth();
	int yMax = getImgHeight() -1;
	int xMax = imgW-1;

	int xlo = px.x - (w>>1);
	int xhi = 1 + px.x + (w>>1);
	int ylo = px.y - (h>>1);
	int yhi = 1 + px.y + (h>>1);

	// stay within image boundaries
	if(xlo < 0) xlo = 0;
	if(xhi > xMax) xhi = xMax;
	if(ylo < 0) ylo = 0;
	if(yhi > yMax) yhi = yMax;

	// draw a thick vertical bar
	for(int yc = px.y-1; yc<px.y+2; yc++)
		MVImgUtils::Vis::line(*pCurrentFrame, xlo, yc, xhi, yc, rgb);

	// draw a thick horizontal bar
	for(int xc = px.x-1; xc<px.x+2; xc++)
		MVImgUtils::Vis::line(*pCurrentFrame, xc, ylo, xc, yhi, rgb);
}


///////////////////////////////////////////////////////////////////////////////////////
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this
// license. If you do not agree to this license, do not download, install, copy or
// use the software.
//
//
//                        Mavis License Agreement
//
// Copyright (c) 2004-2005, Robin Hewitt (http://www.robin-hewitt.com).
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistributions of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistributions in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
// This software is provided "as is" and any express or implied warranties, including,
// but not limited to, the implied warranties of merchantability and fitness for a
// particular purpose are disclaimed. In no event shall the authors or contributors be
// liable for any direct, indirect, incidental, special, exemplary, or consequential
// damages (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused and on any
// theory of liability, whether in contract, strict liability, or tort (including
// negligence or otherwise) arising in any way out of the use of this software, even
// if advised of the possibility of such damage.
///////////////////////////////////////////////////////////////////////////////////////