Code cleanup, improved communication between pheromone and whycon sys…

…tems

Localization/src/camera/CCamera.cpp

@@ -123,8 +123,8 @@ int CCamera::init(const char *deviceName,int *wi,int *he,bool saveI)
 	if (cameraType == CT_VIDEOLOADER){
 		initLut();
 		aviFile = AVI_open_input_file(deviceName,1);
-		width = AVI_video_width(aviFile);
-		height = AVI_video_height(aviFile);
+		*wi = width = AVI_video_width(aviFile);
+		*he = height = AVI_video_height(aviFile);
 		printf("AVI file opened, video dimensions are %i %i\n",width,height);
 		aviBuffer1 = (char*) malloc(4*width*height);
 		aviBuffer2 = (unsigned char*) malloc(4*width*height);

Localization/src/imageproc/CPositionServer.cpp

@@ -79,8 +79,7 @@ void* serverLoop(void* serv)
 		STrackedObject o;
 		for (int i=0;i<server->numObjects;i++){
 			o=server->object[i];
-			//sprintf(buffer,"%s020-000-021-%03i,%.3f,%.3f,%.3f,%.3f\n",buffer,i,o.x,o.y,o.z,o.error);//Ubisense server emulation
-			sprintf(buffer,"%s%03i %.3f %.3f %.3f\n",buffer,i,o.x,o.y,o.yaw*180/M_PI);//Ubisense server emulation
+			sprintf(buffer,"%s%03i %.3f %.3f %.3f\n",buffer,i,o.x,o.y,o.yaw*180/M_PI);
 		}
 		if (send(socket,buffer,strlen(buffer),MSG_NOSIGNAL) != (int)strlen(buffer)) {
 			connected = false;
@@ -114,8 +113,6 @@ int CPositionServer::updatePosition(STrackedObject o,int num)
 	return 0;
 }
 
-
-
 int CPositionServer::closeConnection(int socket)
 {
 	close(socket);

Localization/src/imageproc/CTransformation.cpp

@@ -343,23 +343,23 @@ void CTransformation::loadCalibration(const char *str)
 		char errStr[1000];
 		char dumStr[1000];
 		sprintf(errStr,"Transformation: error reading coordinate system transformation file %s\n",str);
-		if (fscanf(file,"Dimensions %f %f\n",&gDimX,&gDimY)!=2) fprintf(stderr,errStr);
+		if (fscanf(file,"Dimensions %f %f\n",&gDimX,&gDimY)!=2) fprintf(stderr,"%s",errStr);
 		int dum = 0;
 		for (int k = 0;k<4;k++){
-			if (fscanf(file,"3D_calibration %i\n",&dum)!=1) fprintf(stderr,errStr);
+			if (fscanf(file,"3D_calibration %i\n",&dum)!=1) fprintf(stderr,"%s",errStr);
 			for (int i = 0;i<3;i++){
 				for (int j = 0;j<3;j++){
-					if (fscanf(file,"%f ",&D3transform[k].simlar[i][j])!=1) fprintf(stderr,errStr);
+					if (fscanf(file,"%f ",&D3transform[k].simlar[i][j])!=1) fprintf(stderr,"%s",errStr);
 				}
-				if (fscanf(file,"\n")!=0) fprintf(stderr,errStr);
+				if (fscanf(file,"\n")!=0) fprintf(stderr,"%s",errStr);
 			}
-			if (fscanf(file,"Offset %f %f %f\n",&D3transform[k].orig.x,&D3transform[k].orig.y,&D3transform[k].orig.z)!=3)fprintf(stderr,errStr);
+			if (fscanf(file,"Offset %f %f %f\n",&D3transform[k].orig.x,&D3transform[k].orig.y,&D3transform[k].orig.z)!=3)fprintf(stderr,"%s",errStr);
 		}
-		if (fscanf(file,"%s\n",dumStr)!=1) fprintf(stderr,errStr);
+		if (fscanf(file,"%s\n",dumStr)!=1) fprintf(stderr,"%s",errStr);
 		for (int i = 0;i<9;i++){
-			if (fscanf(file,"%f ",&hom[i])!=1)fprintf(stderr,errStr);
+			if (fscanf(file,"%f ",&hom[i])!=1)fprintf(stderr,"%s",errStr);
 			if (i%3 == 2){
-				if (fscanf(file,"\n")!=0) fprintf(stderr,errStr);
+				if (fscanf(file,"\n")!=0) fprintf(stderr,"%s",errStr);
 			}
 		}
 		fclose(file);

Pheromone/src/common/CPheroField.cpp

@@ -18,7 +18,12 @@ CPheroField::~CPheroField()
 	free(data);
 }
 
-void CPheroField::addTo(int x, int y,int id,int num)
+void CPheroField::clear()
+{
+	memset(data,0,size*sizeof(float));
+}
+
+void CPheroField::addTo(int x, int y,int id,int num,int radius)
 {
 	id = id%MAX_ID;
 	int dx = x-lastX[id];
@@ -43,7 +48,7 @@ void CPheroField::addTo(int x, int y,int id,int num)
 		float f = (float)dy/(float)dx;
 		for (int ix = sx;ix<=ex;ix++)
 		{
-			add(ix,iy,id,num);
+			add(ix,iy,id,num,radius);
 			iy+=f;
 		}
 	}else{
@@ -59,7 +64,7 @@ void CPheroField::addTo(int x, int y,int id,int num)
 		float f = (float)dx/(float)dy;
 		for (int iy = sy;iy<=ey;iy++)
 		{
-			add(ix,iy,id,num);
+			add(ix,iy,id,num,radius);
 			ix+=f;
 		}
 
@@ -70,10 +75,15 @@ void CPheroField::addTo(int x, int y,int id,int num)
 }
 
 
-void CPheroField::add(int x, int y,int id,int num)
+float CPheroField::get(int x, int y)
+{
+	if (x > 0 && y >0 && x<width && y<height) return data[x+y*width];
+	return -1;
+}
+
+void CPheroField::add(int x, int y,int id,int num,int radius)
 {
 	id = id%MAX_ID;
-	int radius = 25;
 	int pos = 0;
 	int iix,iiy;
 	for (int ix = -radius;ix<radius+1;ix++){
@@ -90,12 +100,13 @@ void CPheroField::add(int x, int y,int id,int num)
 	lastY[id] = y;
 }
 
-void CPheroField::remove(int x, int y,float an,int num)
+void CPheroField::remove(int x, int y,int lx,int ly,int num)
 {
-	int radius = 30;
 	int pos = 0;
 	int iix,iiy;
-	for (float r=25;r<31;r+=0.01)
+	float an = atan2(y-ly,x-lx);
+	float distance = sqrt((x-lx)*(x-lx)+(y-ly)*(y-ly)); 
+	for (float r=25;r<35+distance;r+=0.1)
 	{
 		for (float a=an-M_PI/2;a<an+M_PI/2;a+=0.01)
 		{

Pheromone/src/common/CPheroField.h

@@ -20,9 +20,11 @@ class CPheroField
 
   void addCalibration();
   void recompute(float decay,float diffuse = 0);
-  void add(int x, int y,int id,int num);
-  void remove(int x, int y,float a,int num);
-  void addTo(int x, int y,int id,int num);
+  void add(int x, int y,int id,int num,int radius);
+  void remove(int x, int y,int lx,int ly,int num);
+  void addTo(int x, int y,int id,int num,int radius = 25);
+  float get(int x, int y);
+  void clear();
 
   int width;
   int height;

Pheromone/src/common/CPositionClient.cpp

@@ -2,9 +2,16 @@
 
 #define NETWORK_BLOCK MSG_WAITALL
 
-CPositionClient::CPositionClient()
+CPositionClient::CPositionClient(int numBots,float diam)
 {
 	module = LOG_MODULE_CLIENT;
+	for (int i =0 ; i < MAX_POSITIONS;i++) vArray[i] = false;
+	timer.reset();
+	timer.start();
+	calibrated = true;
+	numDetected = -1;
+	numSearched = numBots;
+	robotDiameter = diam;
 }
 
 CPositionClient::~CPositionClient()
@@ -30,6 +37,21 @@ int CPositionClient::init(const char *ip,const char* port)
 	return result;
 }
 
+void CPositionClient::resetTime()
+{
+	timer.reset();
+	timer.start();
+	for (int i =0 ; i < MAX_POSITIONS;i++) vArray[i] = false;
+}
+
+void CPositionClient::calibrate(float fieldLength,float fieldWidth,float camHeight,float robotHeight)
+{
+	if (calibrated == true){
+		//poslat naky mrdky wajkonu 
+		calibrated = false;
+	}
+}
+
 int CPositionClient::checkForData()
 {
 	float x,y,phi;
@@ -46,13 +68,20 @@ int CPositionClient::checkForData()
 			token = strtok(data, "\n");
 			while( token != NULL ) 
 			{
-				sscanf(token,"%i %f %f %f \n",&id,&x,&y,&phi);
-				printf("%i %f %f %f \n",id,x,y,phi);
-				token = strtok(NULL, "\n");
-				if (id >=0 && id < MAX_POSITIONS){
-					xArray[id] = x;
-					yArray[id] = y;
+				if (strncmp(token,"Robot",5)==0){
+					sscanf(token,"Robot %i %f %f %f \n",&id,&x,&y,&phi);
+					if (id >=0 && id < MAX_POSITIONS){
+						xArray[id] = x;
+						yArray[id] = y;
+						pArray[id] = phi/180*M_PI;
+						vArray[id] = true;
+						tArray[id] = timer.getTime()/1000000.0;
+					}
 				}
+				if (strncmp(token,"Params",6)==0) sscanf(token,"Params %i %f \n",&numSearched,&robotDiameter);
+				if (strncmp(token,"Detected",6)==0) sscanf(token,"Detected %i \n",&numDetected);
+				if (strncmp(token,"Calibrated",11)==0) calibrated = true;
+				token = strtok(NULL, "\n");
 			}
 		}
 	}
@@ -72,3 +101,19 @@ float CPositionClient::getY(int i)
 	if (i >= 0 && i < MAX_POSITIONS) return yArray[i];
 	return 0;
 }
+
+float CPositionClient::getPhi(int i)
+{
+	if (i >= 0 && i < MAX_POSITIONS) return pArray[i];
+	return 0;
+}
+
+bool CPositionClient::exists(int i)
+{
+	return vArray[i];
+}
+
+float CPositionClient::getTime(int i)
+{
+	return tArray[i];
+}

Pheromone/src/common/CPositionClient.h

@@ -8,11 +8,13 @@
 #include <netinet/in.h>
 #include <arpa/inet.h>
 #include <netdb.h>
+#include <math.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <stdlib.h>
 #include <string.h>
 #include "CDump.h" 
+#include "CTimer.h" 
 
 
 /**
@@ -21,18 +23,34 @@
 class CPositionClient
 {
 public:
-  CPositionClient();
+  CPositionClient(int numBots,float diam);
   ~CPositionClient();
 
   int init(const char *ip,const char* port);
+  void calibrate(float fieldLength,float fieldWidth,float camHeight,float robotHeight);
+
   int checkForData();
   float getX(int i);
   float getY(int i);
+  float getTime(int i);
+  float getPhi(int i);
+  bool exists(int i);
+  void resetTime();
+
+  int numSearched;
+  float robotDiameter;
+  int numDetected;
+  bool calibrated;
 
 private:
+  float tArray[MAX_POSITIONS];
   float xArray[MAX_POSITIONS];
   float yArray[MAX_POSITIONS];
+  float pArray[MAX_POSITIONS];
+  bool vArray[MAX_POSITIONS];
+
 
+  CTimer timer;
   int mySocket;
   TLogModule module;
 };