void Roadway1()
{
pc.Read();
pp.SetSpeed(0.2, 0);
while(1)
{
pc.Read();
if ((sp[MID] < 0.6) || (sp[MID_LEFT] < 0.6) || (sp[MID_RIGHT] < 0.6))
{
PrintObjectLocationByPosition(pp.GetXPos(), pp.GetYPos(), pp.GetYaw(), 2, sp[2]);
// TURN
float Angle;
if (sp [LEFT] > sp[RIGHT])
{
Angle = dtor(30);
}
else
{
Angle = dtor(-30);
}
pp.SetSpeed(0, Angle);
while ((sp[MID] < 0.6) || (sp[MID_LEFT] < 0.6) || (sp[MID_RIGHT] < 0.6))
pc.Read();
pp.SetSpeed(0.2, 0);
}
}
}
void MoveDistance(PlayerClient pc, Position2dProxy& pp, double Meters)
{
pc.Read();
int StartPos = pp.GetXPos();
pp.SetSpeed(SPEED, 0);
while ((pp.GetXPos() - StartPos) < 1)
{
pc.Read();
}
pp.SetSpeed(0, 0);
}
int main (int argc, char *argv[])
{
ClientProxy* deviceProxy;
bool boolValue;
int32_t argsIndex = 0, intValue;
double dblValue;
char *strValue;
if ((argsIndex = GetOptions (argc, argv)) < 0)
exit (1);
PlayerClient client (host, port, PLAYERC_TRANSPORT_TCP);
int code = client.LookupCode(device);
switch (code)
{
case PLAYER_ACTARRAY_CODE:
deviceProxy = (ClientProxy*) new ActArrayProxy (&client, devIndex);
break;
case PLAYER_AUDIO_CODE:
deviceProxy = (ClientProxy*) new AudioProxy (&client, devIndex);
break;
case PLAYER_AIO_CODE:
deviceProxy = (ClientProxy*) new AioProxy (&client, devIndex);
break;
case PLAYER_BLOBFINDER_CODE:
deviceProxy = (ClientProxy*) new BlobfinderProxy (&client, devIndex);
break;
case PLAYER_BUMPER_CODE:
deviceProxy = (ClientProxy*) new BumperProxy (&client, devIndex);
break;
case PLAYER_CAMERA_CODE:
deviceProxy = (ClientProxy*) new CameraProxy (&client, devIndex);
break;
case PLAYER_DIO_CODE:
deviceProxy = (ClientProxy*) new DioProxy (&client, devIndex);
break;
case PLAYER_FIDUCIAL_CODE:
deviceProxy = (ClientProxy*) new FiducialProxy (&client, devIndex);
break;
case PLAYER_GRAPHICS2D_CODE:
deviceProxy = (ClientProxy*) new Graphics2dProxy (&client, devIndex);
break;
case PLAYER_GRAPHICS3D_CODE:
deviceProxy = (ClientProxy*) new Graphics3dProxy (&client, devIndex);
break;
case PLAYER_GRIPPER_CODE:
deviceProxy = (ClientProxy*) new GripperProxy (&client, devIndex);
break;
case PLAYER_IMU_CODE:
deviceProxy = (ClientProxy*) new ImuProxy (&client, devIndex);
break;
case PLAYER_IR_CODE:
deviceProxy = (ClientProxy*) new IrProxy (&client, devIndex);
break;
case PLAYER_LASER_CODE:
deviceProxy = (ClientProxy*) new LaserProxy (&client, devIndex);
break;
case PLAYER_LIMB_CODE:
deviceProxy = (ClientProxy*) new LimbProxy (&client, devIndex);
break;
case PLAYER_LOCALIZE_CODE:
deviceProxy = (ClientProxy*) new LocalizeProxy (&client, devIndex);
break;
case PLAYER_LOG_CODE:
deviceProxy = (ClientProxy*) new LogProxy (&client, devIndex);
break;
case PLAYER_MAP_CODE:
deviceProxy = (ClientProxy*) new MapProxy (&client, devIndex);
break;
case PLAYER_OPAQUE_CODE:
deviceProxy = (OpaqueProxy*) new OpaqueProxy (&client, devIndex);
break;
case PLAYER_PLANNER_CODE:
deviceProxy = (ClientProxy*) new PlannerProxy (&client, devIndex);
break;
case PLAYER_POSITION1D_CODE:
deviceProxy = (ClientProxy*) new Position1dProxy (&client, devIndex);
break;
case PLAYER_POSITION2D_CODE:
deviceProxy = (ClientProxy*) new Position2dProxy (&client, devIndex);
break;
case PLAYER_POSITION3D_CODE:
deviceProxy = (ClientProxy*) new Position3dProxy (&client, devIndex);
break;
case PLAYER_POWER_CODE:
deviceProxy = (ClientProxy*) new PowerProxy (&client, devIndex);
break;
case PLAYER_PTZ_CODE:
deviceProxy = (ClientProxy*) new PtzProxy (&client, devIndex);
break;
case PLAYER_SIMULATION_CODE:
deviceProxy = (ClientProxy*) new SimulationProxy (&client, devIndex);
break;
case PLAYER_SONAR_CODE:
deviceProxy = (ClientProxy*) new SonarProxy (&client, devIndex);
break;
case PLAYER_SPEECH_CODE:
deviceProxy = (ClientProxy*) new SpeechProxy (&client, devIndex);
break;
case PLAYER_VECTORMAP_CODE:
deviceProxy = (ClientProxy*) new VectorMapProxy (&client, devIndex);
break;
default:
cout << "Unknown interface " << device << endl;
exit (-1);
}
if (strncmp (argv[argsIndex], "getbool", 7) == 0)
{
if (deviceProxy->GetBoolProp (argv[argsIndex + 1], &boolValue) >= 0)
cout << "Property " << argv[argsIndex + 1] << " = " << boolValue << endl;
}
else if (strncmp (argv[argsIndex], "getint", 6) == 0)
{
if (deviceProxy->GetIntProp (argv[argsIndex + 1], &intValue) >= 0)
cout << "Property " << argv[argsIndex + 1] << " = " << intValue << endl;
}
else if (strncmp (argv[argsIndex], "getdbl", 6) == 0)
{
if (deviceProxy->GetDblProp (argv[argsIndex + 1], &dblValue) >= 0)
cout << "Property " << argv[argsIndex + 1] << " = " << dblValue << endl;
}
else if (strncmp (argv[argsIndex], "getstr", 6) == 0)
{
if (deviceProxy->GetStrProp (argv[argsIndex + 1], &strValue) >= 0)
{
cout << "Property " << argv[argsIndex + 1] << " = " << strValue << endl;
free (strValue);
}
}
else if (strncmp (argv[argsIndex], "setbool", 7) == 0)
{
if (strncmp (argv[argsIndex + 2], "true", 4) == 0)
deviceProxy->SetBoolProp (argv[argsIndex + 1], true);
else
deviceProxy->SetBoolProp (argv[argsIndex + 1], false);
}
else if (strncmp (argv[argsIndex], "setint", 6) == 0)
{
deviceProxy->SetIntProp (argv[argsIndex + 1], atoi (argv[argsIndex + 2]));
}
else if (strncmp (argv[argsIndex], "setdbl", 6) == 0)
{
deviceProxy->SetDblProp (argv[argsIndex + 1], atof (argv[argsIndex + 2]));
}
else if (strncmp (argv[argsIndex], "setstr", 6) == 0)
{
deviceProxy->SetStrProp (argv[argsIndex + 1], argv[argsIndex + 2]);
}
return 0;
}
int main(int argc, char **argv) {
PlayerClient *robot = nullptr;
int at = 0;
stringstream portConverter(argv[2]);
int simPort;
portConverter >> simPort;
while (robot == nullptr) {
try {
robot = new PlayerClient("localhost", simPort);
} catch (PlayerError &ex) {
robot = nullptr;
sleep(1);
}
if(at >= 20){
return 0;
}
at++;
}
Position2dProxy pp(robot);
LaserProxy lp(robot);
ifstream targetData("target.txt");
double rtx = 0;
double rty = 0;
double timeLimit = 60;
targetData >> rtx >> rty;
targetData.close();
cout << "Target: " << rtx << ", " << rty << endl;
RobotDescriptor descriptor;
Robot r(&pp, &lp, &descriptor);
stringstream fileName;
fileName << "robots/desc" << argv[1];
ifstream read;
read.open("stats.txt");
stringstream fileLockName;
fileLockName << "lock." << argv[1];
ifstream fileLockCheck(fileLockName.str());
if(fileLockCheck.good()){
fileLockCheck.close();
return 0;
}
fileLockCheck.close();
bool simulated = false;
do {
string id;
double score;
int stall;
double duration;
read >> id >> score >> stall >> duration;
if (!id.length()) {
break;
}
stringstream fileName2;
fileName2 << "robots/desc" << id;
if (fileName == fileName2) {
simulated = true;
break;
}
} while (!read.eof());
if(simulated){
return 0;
}
cout << "Creating LOCK" << endl;
ofstream fileLock(fileLockName.str());
fileLock << endl;
fileLock.close();
cout << "READING FROM FILE: " << fileName.str() << endl;
ifstream inputFile;
inputFile.open(fileName.str());
descriptor.loadFromFile(inputFile, &r);
inputFile.close();
//printDescriptor(cout, descriptor);
cout << "*** DESCRIPTOR READ. STARTING SIMULATION. ***" << endl;
auto begin = chrono::high_resolution_clock::now();
auto lastSample = chrono::high_resolution_clock::now();
double points = 0;
bool stall = false;
vector<double> pl;
double sampleRate = 1;
while (true) {
robot->Read();
r.update();
if (pp.GetStall()) {
stall = true;
break;
}
auto now = chrono::high_resolution_clock::now();
double elapsed = (chrono::duration_cast<chrono::duration<double> >(
now - begin)).count();
if ((chrono::duration_cast<chrono::duration<double> >(now - lastSample)).count()
>= sampleRate) {
lastSample = now;
double dist = pow(pp.GetXPos() - rtx, 2)
+ pow(pp.GetYPos() - rty, 2);
pl.push_back(dist);
}
if (elapsed > timeLimit) {
break;
}
}
sort(pl.begin(), pl.end());
int samples = 0;
for (unsigned int i = 0; (i < 10 && i < pl.size()); i++) {
points += pl[i];
samples++;
}
points /= samples;
double duration = (chrono::duration_cast<chrono::duration<double> >(
chrono::high_resolution_clock::now() - begin)).count();
cout << "Acabou o tempo! Pontos: " << points << endl;
cout << "Bateu: " << stall << endl;
cout << "Duration: " << duration << endl;
ofstream stats;
stats.open("stats.txt", ios::app);
stats << argv[1] << " " << points << " " << (stall ? "1" : "0") << " " << duration
<< endl;
stats.close();
remove(fileLockName.str().c_str());
return 0;
}