mavisclient.cpp

机器人程序

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

//////////////////////////////////////////////////////////////
// A dll for communicating with Mavis. Uses the API in
// maviscomm.h.
//
// The API, including exported function interfaces, is in
// maviscomm.h

// special project settings:
//    C/C++, category=preprocessor, added ../../include to the additional
//    include directories.
//
//    added post-build command: copy Debug\mavisclient.dll ..\..\bin

#define __MAVISCLIENT_IMPL
#define USE_OPENCVINTERFACE

#include <windows.h>
#include <stdio.h>
#include "MVLib.h"
#include "maviscomm.h"


// internal name - to kill any waiting threads when
// the dll is unloaded
#define INTERRUPT_EVENT_NAME  "mavis_client_interrupt_event"


// internal methods
BOOL linkToMavis();
void detachFromMavis();
int  sendCtl();
int  getObjLoc(ObjLoc_t * pObjLoc, long waitTime);
void getResult(void * pData, int nBytes);


// file-level data
static CtlData_t   ctlData;
static FrameSize_t g_frameSize = {0};

static HANDLE  hResultEvent      = NULL;
static HANDLE  hResultMutex      = NULL;
static HANDLE  hResultMem        = NULL;
static LPVOID  resultAdd         = NULL;

static HANDLE  hMavisCtlEvent = NULL;
static HANDLE  hCtlMutex      = NULL;
static HANDLE  hCtlMem        = NULL;
static LPVOID  CtlAdd         = NULL;

static HANDLE  hInterruptEvent = NULL;


//set up a shared variable to track number of
//attached instances
#pragma data_seg(".shared")
	int instCount = 0;
#pragma data_seg()
#pragma comment(linker, "/SECTION:.shared,RWS")



BOOL WINAPI DllMain(
	HINSTANCE hinstDLL,    // DLL module handle
    DWORD fdwReason,       // reason called
    LPVOID lpvReserved     // reserved
) {

	switch (fdwReason)
	{

		case DLL_PROCESS_ATTACH:
			if(instCount)
			{
				fprintf(stderr, "Mavis already in use - dll initialization aborted\n");
				return FALSE;
			}
			else
			{
				++instCount;
				return linkToMavis();
			}

		case DLL_THREAD_ATTACH:
			break;

		case DLL_THREAD_DETACH:
			break;

		case DLL_PROCESS_DETACH:
			detachFromMavis();
			--instCount;
			break;

		default:
			break;
	}

	return TRUE;
	UNREFERENCED_PARAMETER(hinstDLL);
	UNREFERENCED_PARAMETER(lpvReserved);
}

BOOL linkToMavis()
{

	//open event handles
	hResultEvent = OpenEvent(
		SYNCHRONIZE|EVENT_MODIFY_STATE,
		FALSE,
		TEXT(RESULT_EVENT_NAME)
	);
	hMavisCtlEvent = OpenEvent(
		SYNCHRONIZE|EVENT_MODIFY_STATE,
		FALSE,
		TEXT(CTL_EVENT_NAME)
	);

	if(NULL == hResultEvent || NULL==hMavisCtlEvent) goto error;


	//open mutex handles
	hResultMutex = OpenMutex(
		SYNCHRONIZE|MUTEX_MODIFY_STATE,
		FALSE,
		RESULT_MUTEX_NAME
	);
	hCtlMutex = OpenMutex(
		SYNCHRONIZE|MUTEX_MODIFY_STATE,
		FALSE,
		CTL_MUTEX_NAME
	);


	//open and map shared memory
	hResultMem = OpenFileMapping(
		FILE_MAP_READ,
		FALSE,
		TEXT(RESULT_MEM_NAME)
	);
	resultAdd = MapViewOfFile(
		hResultMem,           //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==hResultMem || NULL==resultAdd) goto error;

	hCtlMem = OpenFileMapping(
		FILE_MAP_WRITE,
		FALSE,
		TEXT(CTL_MEM_NAME)
	);
	CtlAdd = MapViewOfFile(
		hCtlMem,              //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
	);
	if(NULL==hCtlMem || NULL==CtlAdd) goto error;


	// create the internal interrupt event
	hInterruptEvent = CreateEvent(
		NULL,					//use default security attributes
		FALSE,                  //event will be auto reset
		FALSE,                  //initial state is non-signalled
		TEXT(INTERRUPT_EVENT_NAME)
	);
	if(NULL==hInterruptEvent) goto error;

	return TRUE;

error:
	fprintf(stderr, "Can't connect to Mavis - dll initialization aborted\n");
	detachFromMavis();

	fprintf(stderr, "Failed to initialize %s\n",
		(!hResultEvent)?    "hResultEvent" :
		(!hMavisCtlEvent)?  "hMavisCtlEvent" :
		(!hResultMutex)?    "hResultMutex" :
		(!hCtlMutex)?       "hCtlMutex" :
		(!hResultMem)?      "hResultMem" :
		(!resultAdd)?       "resultAdd" :
		(!hCtlMem)?         "hCtlMem" :
		(!CtlAdd)?          "CtlAdd" :
		(!hInterruptEvent)? "hInterruptEvent" :
		"-- unknown initialization failure");
	return FALSE;
}

void detachFromMavis()
{
	//signal an internal interrupt in case
	//an attached thread is waiting
	SetEvent(hInterruptEvent);


	//close event handles
	if(hResultEvent)    CloseHandle(hResultEvent);
	if(hMavisCtlEvent)  CloseHandle(hMavisCtlEvent);
	if(hInterruptEvent) CloseHandle(hInterruptEvent);

	hResultEvent = hMavisCtlEvent = hInterruptEvent = NULL;


	//close mutex handles
	if(hResultMutex) CloseHandle(hResultMutex);
	if(hCtlMutex)    CloseHandle(hCtlMutex);

	hResultMutex = hCtlMutex = NULL;


	//release shared memory resources
	if(resultAdd)  UnmapViewOfFile(resultAdd);
	if(hResultMem) CloseHandle(hResultMem);
	hResultMem = resultAdd = NULL;

	if(CtlAdd)  UnmapViewOfFile(CtlAdd);
	if(hCtlMem) CloseHandle(hCtlMem);
	hCtlMem = CtlAdd = NULL;

	return;
}


int lookOnce(int objId, ObjLoc_t * pObjLoc)
{
	int ret = 0;

	//check for valid object id
	//if(objId < 3 || objId > 5) return -1; //todo: validate objId from database

	//write control data, use mutex to synchronize access
	ctlData.objId = objId;
	ctlData.ctlCode = LOOK_ONCE;
	sendCtl();

	//get result
	ret = getObjLoc(pObjLoc, INFINITE);

	return ret;
}



int getNextObj(int objId, ObjLoc_t * pObjLoc)
{
	int ret = 0;

	//write control data, use mutex to synchronize access
	ctlData.objId = objId;
	ctlData.ctlCode = GET_NEXT_OBJ;
	sendCtl();

	//get result
	ret = getObjLoc(pObjLoc, INFINITE);

	return ret;
}


int startLookUntilSighted(int objId, ObjLoc_t * pObjLoc, DWORD millisec)
{
	int ret = 0;
	HANDLE lpHandleArray[2];
	DWORD waitTerm;

	memset(pObjLoc, 0, sizeof(ObjLoc_t));

	lpHandleArray[0] = hResultEvent;
	lpHandleArray[1] = hInterruptEvent;

	//write control data, use mutex to synchronize access
	ctlData.objId = objId;
	ctlData.ctlCode = LOOK_UNTIL_SIGHTED;
	sendCtl();


	waitTerm = WaitForMultipleObjects(
	  2,              // number of handles in the handle array
	  lpHandleArray,  // pointer to the object-handle array
	  FALSE,          // wait type is to wait for any (but not for all)
	  millisec        // time-out interval in milliseconds
	);

	if(WAIT_OBJECT_0 == waitTerm)
	{
		// The first event, hResultEvent, was signalled,
		// so get the result data.
		WaitForSingleObject(hResultMutex, INFINITE);
		memcpy( pObjLoc, resultAdd, sizeof(ObjLoc_t) );
		ReleaseMutex(hResultMutex);
	}


	return ret;
}

int stopLookUntilSighted()
{

	int ret = 0;

	// Send Pause (null) command to Mavis
	ctlData.objId = INVALID_OBJ_ID;
	ctlData.ctlCode = PAUSE;
	sendCtl();

	SetEvent(hInterruptEvent);

	return ret;
}

int startRawFeed()
{
	// send command to Mavis
	ctlData.ctlCode = FWD_RAW_FRAMES;
	sendCtl();

	return 0;
}

int stopRawFeed()
{
	// Send Pause (null) command to Mavis
	ctlData.ctlCode = PAUSE;
	sendCtl();

	return 0;
}

int nextFrame()
{
	// Send NextFrame command to Mavis
	ctlData.ctlCode = NEXT_FRAME;
	sendCtl();

	return 0;
}

int sendCtl()
{

	// write control data, using mutex to synchronize access
	WaitForSingleObject(hCtlMutex, INFINITE);
	memcpy( CtlAdd, &ctlData, sizeof(CtlData_t) );
	ReleaseMutex(hCtlMutex);

	SetEvent(hMavisCtlEvent);

	return 0;
}

int getObjLoc(ObjLoc_t * pObjLoc, long waitTime)
{
	//wait up to waitTime for a new resultset to become avail
	if( WAIT_TIMEOUT == WaitForSingleObject(hResultEvent, waitTime) )
		return -1; //wait timed out, so return with unchanged data

	//update data, use mutex to synchronize access
	WaitForSingleObject(hResultMutex, INFINITE);
	memcpy( pObjLoc, resultAdd, sizeof(ObjLoc_t) );
	ReleaseMutex(hResultMutex);

	return 0;
}



// Functions for locating horizontal lines in 3D
//
int locateHLines(int d, HLineOrder_t ordering, HLineMetadata_t * pHLineMetadata)
{
	//write control data, use mutex to synchronize access
	ctlData.d = d;
	ctlData.ordering = ordering;
	ctlData.ctlCode = LOCATE_H_LINES;
	sendCtl();

	//wait for the result
	if( WAIT_TIMEOUT == WaitForSingleObject(hResultEvent, INFINITE) )
		return -1; //wait timed out, so return with unchanged data

	//get result
	WaitForSingleObject(hResultMutex, INFINITE);
	memcpy( pHLineMetadata, resultAdd, sizeof(HLineMetadata_t) );
	ReleaseMutex(hResultMutex);

	return 0;
}


int nextLine(HLine_t * pHLine)
{
	NextHLine_t nextHLine;

	//write control data, use mutex to synchronize access
	ctlData.ctlCode = NEXT_H_LINE;
	sendCtl();

	//wait for the result
	if( WAIT_TIMEOUT == WaitForSingleObject(hResultEvent, INFINITE) )
		return -1; //wait timed out, so return with unchanged data

	//get result
	WaitForSingleObject(hResultMutex, INFINITE);
	memcpy( &nextHLine, resultAdd, sizeof(NextHLine_t) );
	ReleaseMutex(hResultMutex);

	// copy mavis data to output
	memcpy( pHLine, &(nextHLine.hLine), sizeof(HLine_t) );

	return nextHLine.nRemaining;
}


// Frame-forwarding functions
getFrameSize(FrameSize_t * pFrameSize)
{
	// send the command
	ctlData.ctlCode = GET_FRAME_SIZE;
	sendCtl();
	getResult((void *)pFrameSize, sizeof(FrameSize_t));

	// store this to use when retrieving frame buffer
	g_frameSize = *pFrameSize;

	return 0;
}

int getFrame(BYTE * buf, int nBytes)
{
	if( !g_frameSize.nBufSize )
		getFrameSize( &g_frameSize );

	if(nBytes < g_frameSize.nBufSize)
		return -1;  // oops, buffer's too small
	else
	{
		// send the command
		ctlData.ctlCode = GET_FRAME;
		sendCtl();
		getResult(buf, g_frameSize.nBufSize);
		return 0;
	}
}


CvSize getIplImageSize()
{
	if( !g_frameSize.nBufSize )
		getFrameSize( &g_frameSize );

	CvSize size;
	size.width  = g_frameSize.width;
	size.height = g_frameSize.height;

	return size;
}


//int getIplImageData(IplImage * pImg, BYTE * imageData)
int getIplImageData(IplImage * pImg)
{
	if( !g_frameSize.nBufSize )
		getFrameSize( &g_frameSize );

	if(pImg->width     != g_frameSize.width ||
	   pImg->height    != g_frameSize.height ||
	   pImg->nChannels != 3 ||
	   pImg->depth     != IPL_DEPTH_8U)
	   { return -1; }

	BYTE * buf = new BYTE[g_frameSize.nBufSize];
	getFrame(buf, g_frameSize.nBufSize);

	pImg->origin = 1;     // origin at lower left (windows bitmap style)
	pImg->dataOrder = 0;  // interleaved data

	BYTE * pFrameData = buf;
	BYTE * imageData = (BYTE *)pImg->imageData;
	int widthStepFrame = 3*g_frameSize.width;
	int widthStepIpl = pImg->widthStep;

	cout << "widthStepIpl = " << widthStepIpl << endl;
	cout << "g_frameSize.nBufSize = " << g_frameSize.nBufSize << endl;

	for(int i=0; i<g_frameSize.height; i++)
	{
		memcpy(imageData, pFrameData, widthStepFrame);
		imageData += widthStepIpl;
		pFrameData += widthStepFrame;
	}

	if(buf) delete[] buf;

	return 0;
}


int getTiltAngle(double * pAngle)
{
	ctlData.ctlCode = GET_TILT_ANGLE;
	sendCtl();
	getResult(pAngle, sizeof(double));
	return 0;
}


int setTiltAngle(double angle)
{
	double newAngle;
	ctlData.ctlCode = SET_TILT_ANGLE;
	ctlData.dblValue = angle;
	sendCtl();
	getResult(&newAngle, sizeof(double));
	return (newAngle != angle);
}


void getResult(void * pData, int nBytes)
{
	WaitForSingleObject(hResultEvent, INFINITE);  // wait for mavis to signal
	WaitForSingleObject(hResultMutex, INFINITE);  // capture mutex
	memcpy( pData, resultAdd, nBytes );           // read from shared memeory
	ReleaseMutex(hResultMutex);                   // release the mutex
}


///////////////////////////////////////////////////////////////////////////////////////
// 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.
///////////////////////////////////////////////////////////////////////////////////////