/*process mouse and keyboard events coming from the GUI*/ void processEvents() { keys = SDL_GetKeyState(&keyNumber); SDL_ShowCursor(leftMousePressed||rightMousePressed); //mouse events - allows to use the mouse to draw pheromone track while (SDL_PollEvent(&event)){ if (event.type == SDL_MOUSEBUTTONDOWN){ if (event.button.button == SDL_BUTTON_LEFT)leftMousePressed = true; if (event.button.button == SDL_BUTTON_RIGHT) rightMousePressed = true; } if (event.type == SDL_MOUSEBUTTONUP){ if (event.button.button == SDL_BUTTON_RIGHT)rightMousePressed = false; if (event.button.button == SDL_BUTTON_LEFT)leftMousePressed = false; } if (leftMousePressed) pherofield[0]->add(event.motion.x,event.motion.y,0,pheroStrength,35); if (rightMousePressed) pherofield[1]->addTo(event.motion.x,event.motion.y,1,pheroStrength,35); } //terminate if (keys[SDLK_ESCAPE]) stop = true; if ((keys[SDLK_LCTRL] || keys[SDLK_RCTRL]) && keys[SDLK_c]) stop = true; //press space to start the experiment if (keys[SDLK_SPACE] && lastKeys[SDLK_SPACE] == false && calibration == false){ placement = false; globalTimer.reset(); globalTimer.start(); client->resetTime(); } //clear pheromone fields if (keys[SDLK_c]){ pherofield[0]->clear(); pherofield[1]->clear(); pherofield[2]->clear(); } //generate new random positions to start if (keys[SDLK_p] && lastKeys[SDLK_p] == false) randomPlacement(); if (keys[SDLK_c] && lastKeys[SDLK_c] == false) calibration = true; //save an image if (keys[SDLK_s] && lastKeys[SDLK_s] == false) image->saveBmp(); memcpy(lastKeys,keys,keyNumber); }
int main(int argc, char *argv[]) /* Process command line. */ { optionInit(&argc, argv, optionSpecs); if (argc != 3) usage(); trials = optionInt("trials", 0); seed = optionInt("seed", 0); shoulder = optionInt("shoulder", 100); neighbor = optionVal("neighbor", NULL); bedOutFile = optionVal("bed", NULL); zeroBedOutFile = optionVal("zeroBed", NULL); distOut = optionVal("distOut", NULL); shoulderBedOutFile = optionVal("shoulderBed", NULL); verbosity = optionInt("verbose", 1); upstreamOnly = optionExists("upstreamOnly"); if (! upstreamOnly) downstreamOnly = optionExists("downstreamOnly"); verboseSetLevel(verbosity); verbose(2,"bounding elements file: %s\n", argv[1]); verbose(2,"placed items file: %s\n", argv[2]); if (neighbor) verbose(2,"nearest neighbor file: %s\n", neighbor); if (upstreamOnly) verbose(2,"nearest neighbor measured only to upstream element\n"); if (downstreamOnly) verbose(2,"nearest neighbor measured only to downstream element\n"); if (!upstreamOnly && !downstreamOnly) verbose(2,"nearest neighbor measured either to upstream or downstream element\n"); if (bedOutFile) verbose(2,"last alignment of trials output to bed file: %s\n", bedOutFile); if (zeroBedOutFile) verbose(2,"zero distance items to bed file: %s\n", zeroBedOutFile); if (distOut) verbose(2,"distances written to bedGraph file: %s\n", distOut); if (shoulderBedOutFile) verbose(2,"shoulder distance items to bed file: %s\n", shoulderBedOutFile); verbose(2,"number of trials: %d\n", trials); verbose(2,"seed value for drand48(): %d\n", seed); verbose(2,"shoulder value: %d\n", shoulder); randomPlacement(argv[1], argv[2]); verbose(3,"at exit of main()\n"); return(0); }
int main(int argc,char* argv[]) { //register ctrl+c handler signal (SIGINT,ctrl_c_handler); //initialize the logging system if (initializeLogging()==false) return -1; //auto-detect screen resolution (in case of a single display) and initialize the GUI gui = new CGui(&imageWidth,&imageHeight,dualMonitor); image = new CRawImage(imageWidth,imageHeight); //read number of robots and pheromone half-life from the command line numBots = atoi(argv[2]); float evaporation = atof(argv[1]); float diffusion = 0; float influence = 1.0; /*initialize the pheromone fields * pheromone field 0 simulates a longer-decay pheromone that the other robots follow * pheromone field 1 is released by the leader if it gets too close to arena boundaries causing the leader to avoid them - this pheromone decays quickly * pheromone field 2 is released by the leader to supress pheromone field 0 (this avoids the leader to detect pheromone 0 by its sensors) *evaporation defines pheromone's half-life, diffusion its spreading over time and strength determines how the pheromone influences the LCD-displayed image for details, see the chapter 2 of paper Arvin, Krajnik, Turgut, Yue: "CosPhi: Artificial Pheromone System for Robotic Swarms Research", IROS 2015*/ pherofield[0] = new CPheroField(imageWidth,imageHeight,evaporation,diffusion,influence); pherofield[1] = new CPheroField(imageWidth,imageHeight,0.1,0,1); pherofield[2] = new CPheroField(imageWidth,imageHeight,0.1,0,-5); /*connect to the localization system*/ client = new CPositionClient(); client->init(whyconIP,"6666"); image->getSaveNumber(); randomPlacement(); globalTimer.pause(); CTimer performanceTimer; performanceTimer.start(); while (stop == false){ //get the latest data from localization system and check if the calibration finished stop = (globalTimer.getTime()/1000000>experimentTime); /*PHEROMONE DECAY*/ pherofield[0]->recompute(); //main pheromone half-life (user-settable, usually long) pherofield[1]->recompute(); //collision avoidance pheromone with quick decay pherofield[2]->recompute(); //suppression pheromone with quick decay client->checkForData(); /*PHEROMONE INJECTION*/ if (calibration==false && placement==false) { int leader = 0; /*PHEROMONE 1 - released by the leading robot*/ for (int i = 0;i<numBots;i++) { if (client->getID(i) == leaderID){ pherofield[0]->addTo(client->getX(i)*imageWidth/arenaLength,client->getY(i)*imageHeight/arenaWidth,i,pheroStrength); leader = i; } } /*cause the leading robot to release pheromone 1 that is used for obstacle avoidance and 2 that temporarily suppresses pheromone 0*/ float dist = 0.030; //distance of the pheromone release relatively to the leader (controls pheromones 1 and 2 only) float addPhi = 0; //angle of the pheromone release relatively to the leader (controls pheromones 1 and 2 only) float phi = client->getPhi(leader); /*is the leader close to the arena edge ?*/ if ((client->getX(leader)<avoidDistance && cos(phi)<0) || (client->getX(leader)>arenaLength-avoidDistance && cos(phi) > 0 )|| (client->getY(leader)<avoidDistance && sin(phi)<0) || (client->getY(leader)>arenaWidth-avoidDistance && sin(phi)>0)) { /*leader is close to the arena edge -> release pheromone 1 that causes the robot to turn away */ pherofield[1]->addTo((client->getX(leader)+dist*cos(phi+addPhi))*imageWidth/arenaLength,(client->getY(leader)+dist*sin(phi+addPhi))*imageHeight/arenaWidth,0,pheroStrength,35); }else{ /*leader is not close to the arena edge -> release pheromone 2 suporessed pheromone 0, so that the leader does not pick it's own pheromone */ pherofield[2]->addTo((client->getX(leader)+dist*cos(phi+addPhi))*imageWidth/arenaLength,(client->getY(leader)+dist*sin(phi+addPhi))*imageHeight/arenaWidth,0,pheroStrength,45); } /*save positions for later analysis*/ logRobotPositions(); } //convert the pheromone field to grayscale image image->combinePheromones(pherofield,3,0); //the last value determines the color channel - 0 is for grayscale, 1 is red etc. gui->drawImage(image); //experiment preparation phase 2: draw initial and real robot positions initRadius = robotDiameter/arenaLength*imageWidth/2; //calculate robot radius in pixels, so that it matches the real robot dimensions for (int i = 0;i<numBots && placement;i++) { gui->displayInitialPositions(initX[i],initY[i],initA[i],initBrightness,initRadius+10); if (client->exists(i) && calibration == false) gui->displayRobot(client->getX(i)*imageWidth/arenaLength,client->getY(i)*imageHeight/arenaWidth,client->getPhi(i),0,initRadius+10); } /*this chunk of code is used to determine lag*/ /*float t = globalTimer.getTime()/1000000.0; for (int i = 0;i<numBots;i++){ if (client->exists(i)){ gui->displayRobot(client->getX(i)*imageWidth/arenaLength,client->getY(i)*imageHeight/arenaWidth,client->getPhi(i),0,initRadius+10); float dx = fabs(100+50*t-client->getX(i)*imageWidth/arenaLength); float dy = fabs(imageHeight/2-client->getY(i)*imageHeight/arenaWidth); printf("Distance: %.3f Lag: %f \n",sqrt(dx*dx+dy*dy),sqrt(dx*dx+dy*dy)/50); } } gui->displayPattern(100+t*50,imageHeight/2,initRadius);*/ //experiment preparation phase 1: draw calibration, contact WhyCon to calibrate and draw initial robot positions if (calibration){ int calibRadius = initRadius/(cameraHeight-robotHeight)*cameraHeight; //slightly enlarge to compensate for the higher distance from the camera gui->displayCalibrationInfo(cameraHeight,client->numSearched,client->numDetected,calibRadius,performanceTimer.getTime()/1000); client->calibrate(numBots,arenaLength,arenaWidth,cameraHeight,robotDiameter,robotHeight); client->checkForData(); }else if (placement){ gui->displayPlacementInfo(client->numSearched,client->numDetected); } calibration = client->calibrated==false; //update GUI etc gui->update(); processEvents(); printf("GUI refresh: %i ms, updates %i frame delay %.0f ms\n",performanceTimer.getTime()/1000,client->updates,(performanceTimer.getRealTime()-client->frameTime)/1000.0); performanceTimer.reset(); } fclose(robotPositionLog); if (globalTimer.getTime()/1000000<experimentTime) { remove(logFileName); printf("EXPERIMENT TERMINATED MANUALLY\n"); }else{ printf("EXPERIMENT FINISHED SUCESSFULLY, results saved to %s.\n",logFileName); } for (int i = 0;i<3;i++) delete pherofield[i]; delete client; delete image; delete gui; return 0; }