bool DrasculaEngine::animate(const char *animationFile, int FPS) {
int NFrames;
int cnt = 2;
Common::SeekableReadStream *stream = _archives.open(animationFile);
if (!stream) {
error("Animation file %s not found", animationFile);
}
NFrames = stream->readSint32LE();
showFrame(stream, true);
_system->delayMillis(1000 / FPS);
while (cnt < NFrames) {
showFrame(stream);
_system->delayMillis(1000 / FPS);
cnt++;
byte key = getScan();
if (key == Common::KEYCODE_ESCAPE)
term_int = 1;
if (key != 0)
break;
}
delete stream;
return ((term_int == 1) || (getScan() == Common::KEYCODE_ESCAPE) || shouldQuit());
}
void MainWindow::getScanFromDevice()
{
//QString savefileName = QFileDialog::getSaveFileName(this,"Save File", QString(QDir::homePath() + "/untitled.ppm"), ".ppm",0, QFileDialog::DontUseNativeDialog );
//QFile file(savefileName);
fileTransfer *fileT = new fileTransfer(scanSource);
if (pointcloud != NULL)
delete pointcloud;
pointcloud = new pointCloud;
ui->GLScanWidgetui->setCloud(pointcloud);
//Find the proper place to activate the savebutton
ui->pushButtonSaveScan->setEnabled(true);
connect(fileT, SIGNAL(addPointToCloud(float,float,float)), pointcloud, SLOT(addPoint(float,float,float)));
QThread* thread = new QThread;
//Worker* worker = new Worker();
fileT->moveToThread(thread);
//connect(scanner, SIGNAL(error(QString)), this, SLOT(errorString(QString)));
connect(thread, SIGNAL(started()), fileT, SLOT(getScan()));
connect(fileT, SIGNAL(finished()), thread, SLOT(quit()));
connect(fileT, SIGNAL(finished()), fileT, SLOT(deleteLater()));
connect(thread, SIGNAL(finished()), thread, SLOT(deleteLater()));
thread->start();
}
bool DrasculaEngine::confirmExit() {
byte key = 0;
color_abc(kColorRed);
updateRoom();
centerText(_textsys[1], 160, 87);
updateScreen();
delay(100);
while (!shouldQuit()) {
key = getScan();
if (key != 0)
break;
// This gives a better feedback to the user when he is asked to
// confirm whether he wants to quit. It now still updates the room and
// shows mouse cursor movement. Hopefully it will work in all
// locations of the game.
updateRoom();
color_abc(kColorRed);
centerText(_textsys[1], 160, 87);
updateScreen();
}
if (key == Common::KEYCODE_ESCAPE || shouldQuit()) {
stopMusic();
return false;
}
return true;
}
void DrasculaEngine::playFLI(const char *filefli, int vel) {
// Open file
globalSpeed = 1000 / vel;
FrameSSN = 0;
Common::SeekableReadStream *stream = _archives.open(filefli);
LastFrame = _system->getMillis();
while (playFrameSSN(stream) && (!term_int) && !shouldQuit()) {
if (getScan() == Common::KEYCODE_ESCAPE)
term_int = 1;
}
delete stream;
}
void DrasculaEngine::enterName() {
Common::KeyCode key;
flushKeyBuffer();
int v = 0, h = 0;
char select2[23];
strcpy(select2, " ");
for (;;) {
select2[v] = '-';
copyBackground(115, 14, 115, 14, 176, 9, bgSurface, screenSurface);
print_abc(select2, 117, 15);
updateScreen();
key = getScan();
if (key != 0) {
if (key >= 0 && key <= 0xFF && isalpha(key))
select2[v] = tolower(key);
else if ((key >= Common::KEYCODE_0 && key <= Common::KEYCODE_9) || key == Common::KEYCODE_SPACE)
select2[v] = key;
else if (key == Common::KEYCODE_ESCAPE)
break;
else if (key == Common::KEYCODE_RETURN) {
select2[v] = '\0';
h = 1;
break;
} else if (key == Common::KEYCODE_BACKSPACE)
select2[v] = '\0';
else
v--;
if (key == Common::KEYCODE_BACKSPACE)
v--;
else
v++;
}
if (v == 22)
v = 21;
else if (v == -1)
v = 0;
}
if (h == 1) {
strcpy(select, select2);
selectionMade = 1;
}
}
bool DrasculaEngine::confirmExit() {
byte key;
color_abc(kColorRed);
updateRoom();
centerText(_textsys[1], 160, 87);
updateScreen();
delay(100);
for (;;) {
key = getScan();
if (key != 0)
break;
}
if (key == Common::KEYCODE_ESCAPE) {
stopMusic();
return false;
}
return true;
}
void DrasculaEngine::volumeControls() {
if (_lang == kSpanish)
loadPic(95, tableSurface);
copyRect(1, 56, 73, 63, 177, 97, tableSurface, screenSurface);
updateScreen(73, 63, 73, 63, 177, 97, screenSurface);
setCursor(kCursorCrosshair);
showCursor();
while (!shouldQuit()) {
int masterVolume = CLIP((_mixer->getVolumeForSoundType(Audio::Mixer::kPlainSoundType) / 16), 0, 15);
int voiceVolume = CLIP((_mixer->getVolumeForSoundType(Audio::Mixer::kSpeechSoundType) / 16), 0, 15);
int musicVolume = CLIP((_mixer->getVolumeForSoundType(Audio::Mixer::kMusicSoundType) / 16), 0, 15);
int masterVolumeY = 72 + 61 - masterVolume * 4;
int voiceVolumeY = 72 + 61 - voiceVolume * 4;
int musicVolumeY = 72 + 61 - musicVolume * 4;
updateRoom();
copyRect(1, 56, 73, 63, 177, 97, tableSurface, screenSurface);
copyBackground(183, 56, 82, masterVolumeY, 39, 2 + masterVolume * 4, tableSurface, screenSurface);
copyBackground(183, 56, 138, voiceVolumeY, 39, 2 + voiceVolume * 4, tableSurface, screenSurface);
copyBackground(183, 56, 194, musicVolumeY, 39, 2 + musicVolume * 4, tableSurface, screenSurface);
updateScreen();
updateEvents();
// we're ignoring keypresses, so just empty the keyboard buffer
while (getScan())
;
if (rightMouseButton == 1) {
delay(100);
break;
}
if (leftMouseButton == 1) {
delay(100);
if (mouseX > 80 && mouseX < 121) {
updateVolume(Audio::Mixer::kPlainSoundType, masterVolumeY);
}
if (mouseX > 136 && mouseX < 178) {
updateVolume(Audio::Mixer::kSpeechSoundType, voiceVolumeY);
}
if (mouseX > 192 && mouseX < 233) {
updateVolume(Audio::Mixer::kMusicSoundType, musicVolumeY);
}
}
}
if (_lang == kSpanish)
loadPic(974, tableSurface);
selectVerb(kVerbNone);
updateEvents();
}
bool DrasculaEngine::runCurrentChapter() {
int n;
rightMouseButton = 0;
previousMusic = -1;
if (currentChapter != 2) {
int soc = 0;
for (n = 0; n < 6; n++) {
soc = soc + CHARACTER_WIDTH;
_frameX[n] = soc;
}
}
for (n = 1; n < ARRAYSIZE(inventoryObjects); n++)
inventoryObjects[n] = 0;
for (n = 0; n < NUM_FLAGS; n++)
flags[n] = 0;
if (currentChapter == 2) {
flags[16] = 1;
flags[17] = 1;
flags[27] = 1;
}
for (n = 1; n < 7; n++)
inventoryObjects[n] = n;
if (currentChapter == 1) {
pickObject(28);
if (loadedDifferentChapter == 0)
animation_1_1();
selectVerb(kVerbNone);
loadPic("2aux62.alg", drawSurface2);
trackProtagonist = 1;
objExit = 104;
if (loadedDifferentChapter != 0) {
if (!loadGame(saveName)) {
return true;
}
} else {
enterRoom(62);
curX = -20;
curY = 56;
gotoObject(65, 145);
}
} else if (currentChapter == 2) {
addObject(kItemPhone);
trackProtagonist = 3;
objExit = 162;
if (loadedDifferentChapter == 0)
enterRoom(14);
else {
if (!loadGame(saveName)) {
return true;
}
}
} else if (currentChapter == 3) {
addObject(kItemPhone);
addObject(kItemEarplugs);
addObject(kItemSickle);
addObject(kItemHandbag);
addObject(kItemCross);
addObject(kItemReefer);
addObject(kItemOneCoin);
flags[1] = 1;
trackProtagonist = 1;
objExit = 99;
if (loadedDifferentChapter == 0)
enterRoom(20);
else {
if (!loadGame(saveName)) {
return true;
}
}
// From here onwards the items have different IDs
} else if (currentChapter == 4) {
addObject(kItemPhone2);
addObject(kItemCross2);
addObject(kItemReefer2);
addObject(kItemOneCoin2);
objExit = 100;
if (loadedDifferentChapter == 0) {
enterRoom(21);
trackProtagonist = 0;
curX = 235;
curY = 164;
} else {
if (!loadGame(saveName)) {
return true;
}
}
} else if (currentChapter == 5) {
addObject(28);
addObject(7);
addObject(9);
addObject(11);
addObject(13);
addObject(14);
addObject(15);
addObject(17);
addObject(20);
trackProtagonist = 1;
objExit = 100;
if (loadedDifferentChapter == 0) {
enterRoom(45);
} else {
if (!loadGame(saveName)) {
return true;
}
}
} else if (currentChapter == 6) {
addObject(28);
addObject(9);
trackProtagonist = 1;
objExit = 104;
if (loadedDifferentChapter == 0) {
enterRoom(58);
animation_1_6();
} else {
if (!loadGame(saveName)) {
return true;
}
loadPic("auxdr.alg", drawSurface2);
}
}
showCursor();
while (!shouldQuit()) {
if (characterMoved == 0) {
stepX = STEP_X;
stepY = STEP_Y;
}
if (characterMoved == 0 && walkToObject == 1) {
trackProtagonist = trackFinal;
walkToObject = 0;
}
if (currentChapter == 2) {
// NOTE: the checks for room number 14 below are a hack used in the original
// game, and move the character to a place where his feet are not drawn above
// the pianist's head. Originally, walkToObject was not updated properly, which
// lead to an incorrect setting of the protagonist's tracking flag (above). This
// made the character start walking off screen, as his actual position was
// different than the displayed one
if (roomNumber == 3 && (curX == 279) && (curY + curHeight == 101)) {
gotoObject(178, 121);
gotoObject(169, 135);
} else if (roomNumber == 14 && (curX == 214) && (curY + curHeight == 121)) {
walkToObject = 1;
gotoObject(190, 130);
} else if (roomNumber == 14 && (curX == 246) && (curY + curHeight == 112)) {
walkToObject = 1;
gotoObject(190, 130);
}
}
moveCursor();
updateScreen();
if (currentChapter == 2) {
if (musicStatus() == 0 && roomMusic != 0)
playMusic(roomMusic);
} else {
if (musicStatus() == 0)
playMusic(roomMusic);
}
delay(25);
#ifndef _WIN32_WCE
// FIXME
// This and the following #ifndefs disable the excess updateEvents() calls *within* the game loop.
// Events such as keypresses or mouse clicks are dropped on the ground with no processing
// by these calls. They are properly handled by the implicit call through getScan() below.
// It is not a good practice to not process events and indeed this created problems with synthesized
// events in the wince port.
updateEvents();
#endif
if (!_menuScreen && takeObject == 1)
checkObjects();
#ifdef _WIN32_WCE
if (rightMouseButton)
if (_menuScreen) {
#else
if (rightMouseButton == 1 && _menuScreen) {
#endif
rightMouseButton = 0;
delay(100);
if (currentChapter == 2) {
loadPic(menuBackground, cursorSurface);
loadPic(menuBackground, backSurface);
} else {
loadPic(99, cursorSurface);
loadPic(99, backSurface);
}
setPalette((byte *)&gamePalette);
_menuScreen = false;
#ifndef _WIN32_WCE
// FIXME: This call here is in hope that it will catch the rightmouseup event so the
// next if block won't be executed. This too is not a good coding practice. I've recoded it
// with a mutual exclusive if block for the menu. I would commit this properly but I cannot test
// for other (see Desktop) ports right now.
updateEvents();
#endif
#ifdef _WIN32_WCE
} else {
#else
}
// Do not show the inventory screen in chapter 5, if the right mouse button is clicked
// while the plug (object 16) is held
// Fixes bug #2059621 - "DRASCULA: Plug bug"
if (rightMouseButton == 1 && !_menuScreen &&
!(currentChapter == 5 && pickedObject == 16)) {
#endif
rightMouseButton = 0;
delay(100);
characterMoved = 0;
if (trackProtagonist == 2)
trackProtagonist = 1;
if (currentChapter == 4) {
loadPic("icons2.alg", backSurface);
loadPic("icons2.alg", cursorSurface);
} else if (currentChapter == 5) {
loadPic("icons3.alg", backSurface);
loadPic("icons3.alg", cursorSurface);
} else if (currentChapter == 6) {
loadPic("iconsp.alg", backSurface);
loadPic("iconsp.alg", cursorSurface);
} else {
loadPic("icons.alg", backSurface);
loadPic("icons.alg", cursorSurface);
}
_menuScreen = true;
#ifndef _WIN32_WCE
updateEvents();
#endif
selectVerb(kVerbNone);
}
if (leftMouseButton == 1 && _menuBar) {
delay(100);
selectVerbFromBar();
} else if (leftMouseButton == 1 && takeObject == 0) {
delay(100);
if (verify1())
return true;
} else if (leftMouseButton == 1 && takeObject == 1) {
if (verify2())
return true;
}
_menuBar = (mouseY < 24 && !_menuScreen) ? true : false;
Common::KeyCode key = getScan();
if (key == Common::KEYCODE_F1 && !_menuScreen) {
selectVerb(kVerbLook);
} else if (key == Common::KEYCODE_F2 && !_menuScreen) {
selectVerb(kVerbPick);
} else if (key == Common::KEYCODE_F3 && !_menuScreen) {
selectVerb(kVerbOpen);
} else if (key == Common::KEYCODE_F4 && !_menuScreen) {
selectVerb(kVerbClose);
} else if (key == Common::KEYCODE_F5 && !_menuScreen) {
selectVerb(kVerbTalk);
} else if (key == Common::KEYCODE_F6 && !_menuScreen) {
selectVerb(kVerbMove);
} else if (key == Common::KEYCODE_F9) {
volumeControls();
} else if (key == Common::KEYCODE_F10) {
if (!saveLoadScreen())
return true;
} else if (key == Common::KEYCODE_F8) {
selectVerb(kVerbNone);
} else if (key == Common::KEYCODE_v) {
_subtitlesDisabled = true;
ConfMan.setBool("subtitles", !_subtitlesDisabled);
print_abc(_textsys[2], 96, 86);
updateScreen();
delay(1410);
} else if (key == Common::KEYCODE_t) {
_subtitlesDisabled = false;
ConfMan.setBool("subtitles", !_subtitlesDisabled);
print_abc(_textsys[3], 94, 86);
updateScreen();
delay(1460);
} else if (key == Common::KEYCODE_ESCAPE) {
if (!confirmExit())
return false;
} else if (key == Common::KEYCODE_TILDE || key == Common::KEYCODE_BACKQUOTE) {
_console->attach();
_console->onFrame();
} else if (currentChapter == 6 && key == Common::KEYCODE_0 && roomNumber == 61) {
loadPic("alcbar.alg", bgSurface, 255);
}
if (leftMouseButton != 0 || rightMouseButton != 0 || key != 0)
if (currentChapter != 3)
framesWithoutAction = 0;
if (framesWithoutAction == 15000) {
screenSaver();
if (currentChapter != 3)
framesWithoutAction = 0;
}
if (currentChapter != 3)
framesWithoutAction++;
}
return false;
}
bool DrasculaEngine::verify1() {
int l;
if (_menuScreen)
removeObject();
else {
for (l = 0; l < numRoomObjs; l++) {
if (mouseX >= x1[l] && mouseY >= y1[l]
&& mouseX <= x2[l] && mouseY <= y2[l] && doBreak == 0) {
if (exitRoom(l))
return true;
if (doBreak == 1)
break;
}
}
if (mouseX > curX && mouseY > curY
&& mouseX < curX + curWidth && mouseY < curY + curHeight)
doBreak = 1;
for (l = 0; l < numRoomObjs; l++) {
if (mouseX > x1[l] && mouseY > y1[l]
&& mouseX < x2[l] && mouseY < y2[l] && doBreak == 0) {
roomX = roomObjX[l];
roomY = roomObjY[l];
trackFinal = trackObj[l];
doBreak = 1;
walkToObject = 1;
startWalking();
}
}
if (doBreak == 0) {
roomX = CLIP(mouseX, floorX1, floorX2);
roomY = CLIP(mouseY, floorY1 + feetHeight, floorY2);
startWalking();
}
doBreak = 0;
}
return false;
}
SEXP findmzROI(SEXP mz, SEXP intensity, SEXP scanindex, SEXP mzrange, SEXP scanrange, SEXP lastscan, SEXP dev, SEXP minEntries, SEXP prefilter, SEXP noise) {
double *pmz, *pintensity, mzrangeFrom,mzrangeTo;
int i,*pscanindex, scanrangeFrom, scanrangeTo, ctScan, nmz, lastScan, inoise;
int scerr = 0; // count of peak insertion errors, due to missing/bad centroidisation
int perc, lp = -1;
SEXP peaklist,entrylist,list_names,vmz,vmzmin,vmzmax,vscmin,vscmax,vlength,vintensity;
pmz = REAL(mz);
nmz = GET_LENGTH(mz);
pintensity = REAL(intensity);
pscanindex = INTEGER(scanindex);
lastScan = INTEGER(lastscan)[0];
inoise = INTEGER(noise)[0];
pickOptions.dev = REAL(dev)[0];
pickOptions.minEntries = INTEGER(minEntries)[0];
pickOptions.minimumIntValues=INTEGER(prefilter)[0];
pickOptions.minimumInt=INTEGER(prefilter)[1];
mzrangeFrom = REAL(mzrange)[0];
mzrangeTo = REAL(mzrange)[1];
scanrangeFrom = INTEGER(scanrange)[0];
scanrangeTo = INTEGER(scanrange)[1];
struct mzROIStruct * mzROI = (struct mzROIStruct *) calloc(ROI_INIT_LENGTH, sizeof(struct mzROIStruct));
if (mzROI == NULL)
error("findmzROI/calloc: buffer memory could not be allocated ! (%d bytes)\n",ROI_INIT_LENGTH * sizeof(struct mzROIStruct) );
struct mzROIStruct * mzval = (struct mzROIStruct *) calloc(MZVAL_INIT_LENGTH, sizeof(struct mzROIStruct));
if (mzval == NULL)
error("findmzROI/calloc: buffer memory could not be allocated ! (%d bytes)\n",MZVAL_INIT_LENGTH * sizeof(struct mzROIStruct) );
mzLength.mzvalTotal = MZVAL_INIT_LENGTH;
mzLength.mzROITotal = ROI_INIT_LENGTH;
mzLength.mzval = 0;
mzLength.mzROI = 0;
struct scanBuf * scanbuf = &scbuf;
scanbuf->thisScan = NULL;
scanbuf->nextScan = NULL;
scanbuf->thisScanLength = 0;
scanbuf->nextScanLength = 0;
char *names[N_NAMES] = {"mz", "mzmin", "mzmax", "scmin", "scmax", "length", "intensity"};
PROTECT(list_names = allocVector(STRSXP, N_NAMES));
for(i = 0; i < N_NAMES; i++)
SET_STRING_ELT(list_names, i, mkChar(names[i]));
Rprintf(" %% finished: ");
for (ctScan=scanrangeFrom;ctScan<=scanrangeTo;ctScan++)
{
perc = (int) (ctScan* 100)/scanrangeTo;
if ((perc % 10) == 0 && (perc != lp))
{
Rprintf("%d ",perc);
lp = perc;
}
scanbuf=getScan(ctScan, pmz, pintensity, pscanindex,nmz,lastScan, scanbuf);
if (scanbuf->thisScanLength > 0)
{
#ifdef DEBUG
Rprintf("ScanLength %d ",scanbuf->thisScanLength);
Rprintf("Scan Nr. %d of %d (%d %%) %d peaks -- working at %d m/z ROI's, %d ROI's completed.\n", ctScan, scanrangeTo, (int)100.0*ctScan/scanrangeTo,scanbuf->thisScanLength,mzLength.mzval,mzLength.mzROI);
#endif
int p;
double fMass,lastMass=-1;
double fInten;
for (p=0;p < scanbuf->thisScanLength;p++)
{
fMass = scanbuf->thisScan[p].mz;
fInten = scanbuf->thisScan[p].intensity;
if (fMass < lastMass)
error("m/z sort assumption violated ! (scan %d, p %d, current %2.4f (I=%2.2f), last %2.4f) \n",ctScan,p,fMass,fInten,lastMass);
lastMass = fMass;
if (fInten > inoise)
mzval=insertpeak(fMass, fInten, scanbuf, ctScan, scanrangeTo, mzval, &mzLength, &pickOptions);
}
}
mzROI=cleanup(ctScan,mzROI,mzval,&mzLength,&scerr,&pickOptions);
R_FlushConsole();
} //for ctScan
mzROI=cleanup(ctScan+1,mzROI,mzval,&mzLength,&scerr,&pickOptions);
PROTECT(peaklist = allocVector(VECSXP, mzLength.mzROI));
int total = 0;
for (i=0;i<mzLength.mzROI;i++) {
PROTECT(entrylist = allocVector(VECSXP, N_NAMES));
PROTECT(vmz = NEW_NUMERIC(1));
PROTECT(vmzmin = NEW_NUMERIC(1));
PROTECT(vmzmax = NEW_NUMERIC(1));
PROTECT(vscmin = NEW_INTEGER(1));
PROTECT(vscmax = NEW_INTEGER(1));
PROTECT(vlength = NEW_INTEGER(1));
PROTECT(vintensity = NEW_INTEGER(1));
NUMERIC_POINTER(vmz)[0] = mzROI[i].mz;
NUMERIC_POINTER(vmzmin)[0] = mzROI[i].mzmin;
NUMERIC_POINTER(vmzmax)[0] = mzROI[i].mzmax;
INTEGER_POINTER(vscmin)[0] = mzROI[i].scmin;
INTEGER_POINTER(vscmax)[0] = mzROI[i].scmax;
INTEGER_POINTER(vlength)[0] = mzROI[i].length;
INTEGER_POINTER(vintensity)[0] = mzROI[i].intensity;
SET_VECTOR_ELT(entrylist, 0, vmz);
SET_VECTOR_ELT(entrylist, 1, vmzmin);
SET_VECTOR_ELT(entrylist, 2, vmzmax);
SET_VECTOR_ELT(entrylist, 3, vscmin);
SET_VECTOR_ELT(entrylist, 4, vscmax);
SET_VECTOR_ELT(entrylist, 5, vlength);
SET_VECTOR_ELT(entrylist, 6, vintensity);
setAttrib(entrylist, R_NamesSymbol, list_names); //attaching the vector names
SET_VECTOR_ELT(peaklist, i, entrylist);
UNPROTECT(N_NAMES + 1); //entrylist + values
total++;
}
if (scerr > 0) Rprintf("Warning: There were %d peak data insertion problems. \n Please try lowering the \"ppm\" parameter.\n", scerr);
Rprintf("\n %d m/z ROI's.\n", total);
UNPROTECT(2); // peaklist,list_names
// free(ptpeakbuf);
if (scanbuf->thisScan != NULL)
free(scanbuf->thisScan);
if (scanbuf->thisScan != NULL)
free(scanbuf->nextScan);
free(mzval);
free(mzROI);
return(peaklist);
}
int ICP_Test(int argc, char **argv){
if(argc != 3){
fprintf(stderr,"Usage: %s las.log scan.out\n", argv[0]);
return -1;
}
FileLaserSource las(argv[1],8.0);
FILE *fscans = fopen(argv[2],"w");
if(!las.isOk()){
fprintf(stderr,"Failed to open: %s\n", argv[1]);
return -1;
}else{
printf("%%Opened: %s \n", argv[1]);
}
ICPScanMatch icp;
LaserScan *scan = NULL;
LaserScan *prevScan = NULL;
double odo[3] = {0,0,0};
double step[3] = {0,0,0};
int iScan = 0;
while(las.nextScan() && iScan < 200){
scan = getScan(las);
scan->computeCentroid();
if(prevScan != NULL){
int np = icp.match(*scan, *prevScan, 0.01, 0.999, step, odo);
double ca = cos(odo[2]);
double sa = sin(odo[2]);
odo[0] = step[0];
odo[1] = step[1];
odo[2] = step[2];
printf("%+10.3f %+10.3f %+10.8f \t\t %+10.3f %+10.3f %+10.8f %d\n"
, odo[0], odo[1], odo[2]
, step[0], step[1], step[2], np);
fflush(stdout);
#if 1
ca = cos(odo[2]);
sa = sin(odo[2]);
unsigned int i;
for(i = 0; i < scan->n; ++i){
double x,y;
x = scan->x[i]*ca - scan->y[i]*sa + odo[0];
y = scan->x[i]*sa + scan->y[i]*ca + odo[1];
fprintf(fscans,"%.3e %.3e %.3e %.3e %d %d\n", x,y, scan->x[i],scan->y[i], np, scan->n);
}
#endif
iScan += 1;
delete scan;
}else{
prevScan = scan;
prevScan->buildTree();
}
}
if(prevScan != NULL) delete prevScan;
fclose(fscans);
return 0;
}
/**
Handle incomming command
Must return true if the function is handled -
otherwise the client will get a 'failed' reply */
virtual bool handleCommand(UServerInMsg * msg, void * extra)
{ // handle command(s) send to this plug-in
// check for parameters - one parameter is tested for - 'help'
// the help value is ignored, e.g. if help="bark", then
// the value "bark" will be in the 'helpValue' string.
bool ask4help;
const int MVL = 50;
char val[MVL];
int camDevice = -1;
bool debug = true; // default is debug on
bool result = true;
ULaserData * data;
ULaserDevice * lasDev; // pointer to laser device
ask4help = msg->tag.getAttValue("help", val, MVL);
if (not ask4help)
{ // get all other parameters
msg->tag.getAttValueInt("device", &camDevice);
//gotImg = msg->tag.getAttValueInt("img", &imgPoolNum);
msg->tag.getAttValueBool("debug", &debug, true);
//msg->tag.getAttValueBool("smrcl", &smrcl, true);
//msg->tag.getAttValueBool("blue", &gotBlue, true);
}
// ask4help = false, if no 'help' option were available.
if (ask4help)
{ // create the reply in XML-like (html - like) format
printf("test3\n");
sendHelpStart("B2");
sendText("--- available B2 options\n");
sendText("device=X Use this camera - for position and parameters\n");
sendText("img=X Get image from image pool - else take new image\n");
sendText("blue Try find a blue ball (def is red)\n");
sendText("debug=false More images and print on server console (def=true)\n");
sendText("smrcl Format the reply for MRC (<vision vis1=\"x.x\" vis2=\"y.y\" .../>\n");
sendText("v2 use version 2 code (edge extraction)");
sendText("help This message\n");
sendHelpDone();
sendInfo("done");
result = true;
}
// Handle all other commands
else
{
// Print out data
if(msg->tag.getAttValue("dataprint", NULL, 0))
{
ULaserDevice * lasDev; // laser device
data = getScan(msg, (ULaserData*)extra, false, &lasDev);
if (data->isValid())
{
printf("MAX Scan: %d\n",data->getRangeCnt());
int i;
for(i=0;i<data->getRangeCnt();i=i+1){
if(data->getRangeMeter(i)>=1) printf("ID: %d \t%f \t%f \n",i,data->getRangeMeter(i),data->getAngleDeg(i));
}
}
else
{
sendWarning(msg, "No scandata available");
}
}
// test stop condition
else if(msg->tag.getAttValue("test", NULL, 0))
{
ULaserDevice * lasDev; // laser device
data = getScan(msg, (ULaserData*)extra, false, &lasDev);
if (data->isValid())
{
printf("MAX Scan: %d\n",data->getRangeCnt());
// loop that run for all messurements
int i;
for(i = 0; i < data->getRangeCnt(); i = i+1)
{
// Filter out the angles of interrest
if((data->getAngleDeg(i) > -30) && (data->getAngleDeg(i) < 30))
{
printf("ID: %d \t%f \t%f \n",i,data->getRangeMeter(i),data->getAngleDeg(i));
}
}
}
}
// MRC
else if(msg->tag.getAttValue("mrc", NULL, 0))
{
ULaserDevice * lasDev; // laser device
data = getScan(msg, (ULaserData*)extra, false, &lasDev);
int objectDetected = 0;
if (data->isValid())
{
// loop that run for all messurements
int i;
for(i = 0; i < data->getRangeCnt(); i = i+1)
{
// Filter out the angles of interrest
if((data->getAngleDeg(i) >= -30) && (data->getAngleDeg(i) <= 30))
{
// 0.25 stop if obstacel is closer than 0.25m
// remove samples that are closer than 0.06m due to datasheet presision.
// Remove samples that are out of index (0).
if((data->getRangeMeter(i)<0.25) && (data->getRangeMeter(i) > 0.06)){
objectDetected = 1;
//printf("ID: %d \t%f \t%f \n",i,data->getRangeMeter(i),data->getAngleDeg(i));
}
}
}
if(objectDetected == 1){
sendText("Object in front of SMR!\n");
sendMsg("<laser l1=\"1\"/>\n");
}
else{
sendMsg("<laser l1=\"0\"/>\n");
}
}
else{
sendWarning(msg, "No scandata available!");
}
}
else{
result = false;
}
} // end of handle all other commands
return result;
}
/**
Handle incomming command
(intended for command separation)
Must return true if the function is handled -
otherwise the client will get a failed - reply */
virtual bool handleCommand(UServerInMsg * msg, void * extra)
{ // handle a plugin command
const int MRL = 500;
char reply[MRL];
bool ask4help;
const int MVL = 30;
char value[MVL];
ULaserData * data;
//
// check for parameters - one parameter is tested for - 'help'
ask4help = msg->tag.getAttValue("help", value, MVL);
if (ask4help)
{ // create the reply in XML-like (html - like) format
sendHelpStart(msg, "inbox");
sendText("--- available INBOX options\n");
sendText("add=name x1=A y1=B x2=C y2=D");
sendText(" add named box with limits A,B,C,D in robot coordinates\n");
sendText("detect Detect for objects in box\n");
sendText("poly=S make also result polygons in -1=none, 0=odo (default), 1=UPM, 2=Map coordinates\n");
sendText("silent send no (less) reply\n");
sendText("fake=F Fake some data 1=random, 2-4 a fake corridor\n");
sendText("device=N Laser device to use (see: SCANGET help)\n");
sendText("see also: SCANGET and SCANSET\n");
sendHelpDone();
}
else
{ // do some action and send a reply
int polySys = -2;
bool silent;
msg->tag.getAttInteger("poly", &polySys, 0);
// test if silent option - i.e. no reply needed
msg->tag.getAttBool("silent", &silent);
// test add option
if (msg->tag.getAttValue("add", value, MVL))
{ // get all needed parameters
double x1, x2, y1, y2;
bool gotX1, gotX2, gotY1, gotY2;
gotX1 = msg->tag.getAttDouble("x1", &x1);
gotX2 = msg->tag.getAttDouble("x2", &x2);
gotY1 = msg->tag.getAttDouble("y1", &y1);
gotY2 = msg->tag.getAttDouble("y2", &y2);
if (gotX1 and gotX2 and gotY1 and gotY2)
{
UNamedBox * bx;
bx = boxes.addBox(value, x1, y1, x2, y2);
if (not silent)
{
if (bx != NULL)
sendInfo("box added");
else
sendWarning("failed to add box");
}
if (polySys != -1 and bx != NULL)
{
snprintf(reply, MRL, "%s_Box", bx->varName->getString());
sendToPolyPlugin(reply, &bx->cornerPolygon, 0);
}
}
else
{
if (not silent)
sendInfo("failed to add box, not enough parameters");
}
}
// test detect option
if (msg->tag.getAttValue("detect", NULL, 0))
{
ULaserDevice * lasDev; // laser device
data = getScan(msg, (ULaserData*)extra, false, &lasDev);
//
if (data->isValid())
{ // make analysis for closest measurement
int i;
// double minAngle = 0.0; // degrees
i = boxes.doDetect(data, polySys, lasDev, polySys);
if (not silent and boxes.boxesCnt > 1)
{
snprintf(reply, MRL, "detections in %d boxes", i);
sendInfo(reply);
}
if (not silent)
{ // send reply to client
UNamedBox * nb;
snprintf(reply, MRL, "boxCnt=\"%d\"", boxes.boxesCnt);
sendStartTag(reply);
for (int i= 0; i < boxes.boxesCnt; i++)
{
nb = &boxes.boxes[i];
snprintf(reply, MRL, "<box idx=\"%d\" name=\"%s\" detectCnt=\"%d\"/>\n",
i, nb->varName->getString(), nb->varDetections->getInt());
sendMsg(reply);
}
sendEndTag();
}
if (polySys != -1)
{ // send polygons to polygon plugin
UNamedBox * bx;
const int MSL = 20;
char s[MSL];
for (int i = 0; i < boxes.boxesCnt; i++)
{
bx = &boxes.boxes[i];
snprintf(s, MSL, "%s_Box", bx->varName->getString());
sendToPolyPlugin(bx->varName->getString(), &bx->poly, 0);
sendToPolyPlugin(s, &bx->cornerPolygon, 0);
}
}
}
else
sendWarning(msg, "No scandata available");
}
}
// return true if the function is handled with a positive result
return true;
}
