/*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;
}
