Example #1
0
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);
		}
	}
}
Example #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);

}
Example #3
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;
}
Example #4
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;
}