playerdispatcher.cpp

Particle filtering implementation and application to people tracking.

#include <libplayerc++/playerc++.h>
#include <iostream>
#include <vector>

#include <utils/commandline.h>
#include <utils/sigquit_handler.h>
#include <utils/polarpoint.h>
#include <gnuplotdisplay/gnuplot.h>

#include "marker.h"

using std::cerr;
using std::endl;
using std::cout;

using namespace Gnuplot;

int main(int argc, char** argv)
{
	std::string sHost = "localhost";
	int iPort = 6665,
			iIndex = 0;

	CMD_PARSE_BEGIN(1,argc)
	{
		parseString("-h",sHost);
		parseInt("-i",iPort);
	}
	CMD_PARSE_END

	try
	{
		PlayerCc::PlayerClient client(sHost, iPort);
		PlayerCc::BlobfinderProxy bfp(&client, iIndex);

		INITSIGINT
		while (SIGNAL_RUN)
		{
			client.Read();
			int bcount = bfp.GetCount();

			std::vector<Marker> markerVector;
			Marker m;
			gp_init();
			for (int i=0; i<bcount; ++i)
			{
				playerc_blobfinder_blob_t blob = bfp[i];
				switch (blob.id)
				{
					case 0:
					{
						m.position.x = 0;
						m.position.y = 1950;
						markerVector.push_back(m);
					}
					break;
					case 1:
					{
						m.position.x = 0;
						m.position.y = 1950;
						markerVector.push_back(m);
					}
					break;
					case 2:
					{
						m.position.x = 0;
						m.position.y = 1950;
						markerVector.push_back(m);
					}
					break;
					case 3:
					{
						m.position.x = 0;
						m.position.y = 1950;
						markerVector.push_back(m);
					}
					break;
					case 4:
					{
						m.position.x = 0;
						m.position.y = 1950;
						markerVector.push_back(m);
					}
					break;
					case 5:
					{
						m.position.x = 0;
						m.position.y = 1950;
						markerVector.push_back(m);
					}
					break;
				}
				cout << bfp << endl;
			}
			gp_display();
		}
		cerr << "Terminating " << argv[0] << endl;
	}
	catch (PlayerCc::PlayerError e)
	{
		std::cerr << e << std::endl;
		return -1;
	}
	return 1;
}