mavis.cpp

机器人程序

// Mavis.cpp - by Robin Hewitt, 2004-2005
// http://www.robinhewitt.com/mavis
// This is free software. See license at the bottom
// of this file for terms of use.
//

//////////////////////////////////////////////////////////////
// Implementations of the Mavis and MavisErr classes
//

#include "MVLib.h"
#include "mavistypes.h"
#include "maviscomm.h"
#include "Mavis.h"
#include "MavisErr.h"
#include "MVFrame.h"
#include "Params.h"
#include "Logger.h"
#include "objRec/MVObj.h"
#include "objRec/Homebase.h"
#include "camera/Camera.h"
#include "camera/CalibrationHelper.h"
#include "multiView/HLines.h"
#include <stdio.h>
#include <time.h>


//////////////////////////////////////////////////////////////
// class: Mavis
//

// initialize static member
int Mavis::instances = 0;

//////////////////
//  Constructor
//
Mavis::Mavis(MAVISCB cb)
{
	// makes sure all pointers are initialized to null, etc.
	defaultValues();


	// Mavis is a singleton
	if(instances)
		throw MavisErr( "Only one instance of Mavis is allowed." );
	else
		++instances;


	// Load the parameters. This needs to be done before the other
	// Mavis objects are initialized, so the parameters will be available
	// to these other objects.
	try { pParams = new Params(); }
	catch(MavisErr& e) { throw e; }


	// Start logging. This needs to be next so the remaining Mavis
	// objects can write log entries
	try { pLogger = new Logger(this); }
	catch(MavisErr& e) { throw e; }


	// Write a startup message to the logfile
	time_t tm;
	time(&tm);
	//pLogger->writelnToLog("----------------------------------------");
	pLogger->writelnToLog("Starting Mavis - %s", ctime(&tm) );


		//create a frame object
	pFrame = new MVFrame();

	// load the camera
	try { pCamera = new Camera(this); }
	catch(MavisErr& e) { throw e; }


	//create the control events for data sharing
	hMavisCtlEvent = CreateEvent(
		NULL,					//use default security attributes
		FALSE,                  //event will be auto reset
		FALSE,                  //initial state is non-signalled
		TEXT(CTL_EVENT_NAME)
	);
	hResultEvent = CreateEvent(
		NULL,					//use default security attributes
		FALSE,                  //event will be auto reset
		FALSE,                  //initial state is non-signalled
		TEXT(RESULT_EVENT_NAME)
	);

	if(NULL == hResultEvent || NULL==hMavisCtlEvent)
		throw MavisErr( "Failed to create Windows Event handles" );

	//create shared-memory areas for data passing
	int nResultBytes = sizeof(ObjLoc_t);
	if( sizeof(HLineMetadata_t) > nResultBytes )
		nResultBytes = sizeof(HLineMetadata_t);
	if( sizeof(NextHLine_t) > nResultBytes )
		nResultBytes = sizeof(NextHLine_t);
	if( sizeof(FrameSize_t) > nResultBytes )
		nResultBytes = sizeof(FrameSize_t);
	if( pFrame->getBufSize() > nResultBytes )
		nResultBytes = pFrame->getBufSize();
	pLogger->writelnToLog("shared-memory size: %d bytes", nResultBytes);
	hResultMem = CreateFileMapping(
		(HANDLE)0xFFFFFFFF,     //use system pagefile
		NULL,                   //use default security attributes
		PAGE_READWRITE,         //file-access setting
		0,                      //memory size - high-order 32 bytes
		nResultBytes,           //memory size - low-order 32 bytes
		TEXT(RESULT_MEM_NAME)   //inter-process name
	);
	hCtlMem = CreateFileMapping(
		(HANDLE)0xFFFFFFFF,     //use system pagefile
		NULL,                   //use default security attributes
		PAGE_READWRITE,         //file-access setting
		0,                      //memory size - high-order 32 bytes
		sizeof(CtlData_t),      //memory size - low-order 32 bytes
		TEXT(CTL_MEM_NAME)      //inter-process name
	);

	if(NULL == hResultMem || NULL == hCtlMem)
		throw MavisErr( "Can't open shared memory" );

	//get pointers to shared memory
	lpvResultAdd = MapViewOfFile(
		hResultMem,           //region to be mapped
		FILE_MAP_WRITE,       //access mode
		0,                    //high-order 32 bits of file offset
		0,                    //low-order 32 bits of file offset
		0                     //map the entire region
	);
	lpvCtlAdd = MapViewOfFile(
		hCtlMem,              //region to be mapped
		FILE_MAP_READ,        //access mode
		0,                    //high-order 32 bits of file offset
		0,                    //low-order 32 bits of file offset
		0                     //map the entire region
	);

	if(NULL == lpvResultAdd || NULL == lpvCtlAdd)
		throw MavisErr( "Can't map shared memory" );

	//create file-access mutex objects
	hResultMutex = CreateMutex(
		NULL,                      //use default security attributes
		FALSE,                     //don't take ownership
		TEXT(RESULT_MUTEX_NAME)    //inter-process name
	);
	hCtlMutex = CreateMutex(
		NULL,                      //use default security attributes
		FALSE,                     //don't take ownership
		TEXT(CTL_MUTEX_NAME)       //inter-process name
	);

	if(NULL == hResultMutex || NULL == hCtlMutex)
		throw MavisErr( "Can't create mutex objects" );


	//Store pointer to the callback function for frame display.
	displayCB = cb;


	//load one frame to initialize
	//nextFrame();
	pCurrentFrame = getNextFrame();
	showFrame();

	// Instantiate the object factory
	try { pObjFactory = new MVObjFactory(this); }
	catch(MavisErr& e) { throw e; }

	// Instantiate the ImgDB object
	string dataDir;
	const char * tmp = pParams->getStringValue("learnedObj", "dataDir");
	if(tmp)
		dataDir = tmp;
	else
		dataDir = "./data";  // default value
	try { pImgDB = new ImgDB(dataDir); }
	catch(MVErr& e) { throw MavisErr(e); }

	pLogger->writelnToLog("Mavis inititialization completed.");
}


//////////////////
//  defaultValues()
//
void Mavis::defaultValues()
{

	//set all default values
	pFrame        = 0;
	pCurrentFrame = 0;
	pParams       = 0;
	pLogger       = 0;
	displayCB     = 0;
	bStop         = true;

	pCalibrationHelper = 0;
	pCamera = 0;
	pObjFactory = 0;
	pImgDB = 0;

	hResultEvent   = 0;
	hResultMutex   = 0;
	hResultMem     = 0;
	lpvResultAdd   = 0;

	hMavisCtlEvent = 0;
	hCtlMutex      = 0;
	hCtlMem        = 0;
	lpvCtlAdd      = 0;
}


//////////////////
//  Destructor
//
Mavis::~Mavis()
{
	displayCB = NULL;
	stop();

	time_t tm;
	time(&tm);
	pLogger->writelnToLog("Exiting Mavis - %s", ctime(&tm) );


	// delete mavis program objects

	if(pFrame) delete pFrame;
	if(pObjFactory) delete pObjFactory;
	if(pImgDB) delete pImgDB;
	if(pCalibrationHelper) delete pCalibrationHelper;
	if(pCamera) delete pCamera;
	if(pLogger) delete pLogger;

	// The Params object saves the parameters to the ini file
	// when its destructor method is called. Since some of the
	// other objects may call it from their destructors to save
	// their final param values, the Params object should be
	// deleted last.
	if(pParams) delete pParams;


	//clean up data-sharing objects
	if(hMavisCtlEvent) CloseHandle(hMavisCtlEvent);
	if(hResultEvent)   CloseHandle(hResultEvent);

	if(hResultMutex)   CloseHandle(hResultMutex);
	if(hCtlMutex)      CloseHandle(hCtlMutex);

	if(lpvResultAdd)   UnmapViewOfFile(lpvResultAdd);
	if(hResultMem)     CloseHandle(hResultMem);
	if(lpvCtlAdd)      UnmapViewOfFile(lpvCtlAdd);
	if(hCtlMem)        CloseHandle(hCtlMem);


	defaultValues();

	--instances;
}


//////////////////
//  getImgWidth()
//
int Mavis::getImgWidth()
{ return pFrame->getWidth(); }


//////////////////
//  getImgHeight()
//
int Mavis::getImgHeight()
{ return pFrame->getHeight(); }


//////////////////
//  getNextFrame()
//
VideoFrame * Mavis::getNextFrame()
{
	pCurrentFrame = pFrame->nextFrame();
	frameSize = pCurrentFrame->getNBytes();
	return pCurrentFrame;
}


//////////////////
//  run()
//
void Mavis::run()
{
	ObjLoc_t         objLoc        = {0};
	HLineMetadata_t  HLineMetadata = {0};
	CtlData_t        ctlData       = {0};
	MVObj *          pMVObj        = NULL;
	HLines *         pHLines       = NULL;

	bStop = false;
	pLogger->writelnToLog("Running Mavis");

	// set up for performance timing
	LONGLONG        startTime;
	LONGLONG        stopTime;
	LONGLONG        countsPerMillisec;
	LARGE_INTEGER   counts;
	LARGE_INTEGER   freq;
	QueryPerformanceFrequency(&freq);
	countsPerMillisec = freq.QuadPart / 1000;


	// Enter command loop
	// Listen for commands and respond until bStop is set to true
	while(!bStop)
	{
		// wait for ctl event
		WaitForSingleObject(hMavisCtlEvent, INFINITE);
		WaitForSingleObject(hCtlMutex, INFINITE);

		// begin timing
		QueryPerformanceCounter(&counts);
		startTime = counts.QuadPart;


		// get the command data
		if(bStop)
			ctlData.ctlCode = PAUSE;
		else
		{
			memcpy( &ctlData, lpvCtlAdd, sizeof(CtlData_t) );
			ReleaseMutex(hCtlMutex);
		}