[Jderobot-admin] jderobot-r932 - trunk/src/components/playerserver

ahcorde en jderobot.org ahcorde en jderobot.org
Mar Jun 11 09:41:21 CEST 2013


Author: ahcorde
Date: 2013-06-11 09:40:21 +0200 (Tue, 11 Jun 2013)
New Revision: 932

Modified:
   trunk/src/components/playerserver/playerserver.cfg
   trunk/src/components/playerserver/playerserver4.cpp
Log:
[ahcorde] Modificado playerserver para diferencia entre laser real y simulado


Modified: trunk/src/components/playerserver/playerserver.cfg
===================================================================
--- trunk/src/components/playerserver/playerserver.cfg	2013-06-11 06:52:57 UTC (rev 931)
+++ trunk/src/components/playerserver/playerserver.cfg	2013-06-11 07:40:21 UTC (rev 932)
@@ -11,6 +11,7 @@
 PlayerServer.Initial_position.X=0 		#m
 PlayerServer.Initial_position.Y=0			#m
 PlayerServer.Initial_position.Theta=0	#rad
+PlayerServer.Hokuyo=1
 PlayerServer.Laser=0
 PlayerServer.Laser.Num_readings=180
 PlayerServer.Laser.Readings_per_degree=1

Modified: trunk/src/components/playerserver/playerserver4.cpp
===================================================================
--- trunk/src/components/playerserver/playerserver4.cpp	2013-06-11 06:52:57 UTC (rev 931)
+++ trunk/src/components/playerserver/playerserver4.cpp	2013-06-11 07:40:21 UTC (rev 932)
@@ -44,10 +44,7 @@
 #include <math.h>
 #include <libplayerc++/playerc++.h>
 
-#include <stdio.h> 
-#include <unistd.h> /* for fork */
-#include <sys/types.h> /* for pid_t */
-#include <sys/wait.h> /* for wait */
+#include <pthread.h>
 
 // Constants
 #define DEGTORAD 0.01745327
@@ -142,28 +139,46 @@
 			prefix(propertyPrefix),context(context),laserData(new jderobot::LaserData()) {
 				Ice::PropertiesPtr prop = context.properties();
 
+                pthread_mutex_init(&mutex, NULL);
+
 				// Create a client object
 				cout << "Connecting to Player Server at '" << playerhost << ":" << playerport << "..." << endl;
 
 				robot = new PlayerCc::PlayerClient(playerhost.c_str(), playerport);
-				playerLaser = new PlayerCc::RangerProxy(robot, 1);
+            	playerLaser = new PlayerCc::RangerProxy(robot, 0);
 
 				/*Get laser resolution*/
 				laser_num_readings = prop->getPropertyAsIntWithDefault("PlayerServer.Laser.Num_readings", NUM_LASER_DEF);
 				laser_resolution = (double) prop->getPropertyAsIntWithDefault("PlayerServer.Laser.Readings_per_degree", RESOLUTION_LASER_DEF);
+				hokuyo = (double) prop->getPropertyAsIntWithDefault("PlayerServer.Hokuyo", 0);
 
 				laserData->numLaser = laser_num_readings;
 				laserData->distanceData.resize(sizeof(int)*laserData->numLaser);
+                cout << "Constructor end" << endl;
+                
 			}
 
 			virtual ~LaserI(){};
 
 			virtual jderobot::LaserDataPtr getLaserData(const Ice::Current&) {
-				return laserData;
+		        pthread_mutex_lock(&mutex);
+        		jderobot::LaserDataPtr laserData_send(new jderobot::LaserData());
+
+laserData_send->numLaser = 180;
+        		laserData_send->distanceData.resize(180);
+        		//std::cout << laserData->numLaser <<" "<<  laserData_send->distanceData.size()  << std::endl;
+				for(int i = 0; i < 180; i++ ) {
+	        		//std::cout << i  << std::endl;
+				    laserData_send->distanceData[i]=laserData->distanceData[i];
+				}
+				
+		        pthread_mutex_unlock(&mutex);
+				return laserData_send;
 			};
 
 			virtual void update() {
 
+		        pthread_mutex_lock(&mutex);
 				robot->Read();
 
 				int i;
@@ -171,26 +186,51 @@
 				double player_resolution;
 				double jump;
 				int offset;
+				
+				if(hokuyo==1){
+				    offset=0;//((playerLaser->GetRangeCount()/jump)-laser_num_readings)/2;
+				    i=180;
+				    j=offset;
+				
+				    //std::cout << "playerLaser->GetRangeCount(): "<< playerLaser->GetRangeCount()  << std::endl;
+				    //std::cout << "jump "<< jump  << std::endl;
 
-				//Now we must transform player resolution into user defined resolution
-				player_resolution=(double)1.0/(0.5);
+                    jump = 2.85;
 
-				jump = player_resolution / laser_resolution;
+				    //Update laser values//
+				    while(j<playerLaser->GetRangeCount() && i>0){
+				        if(playerLaser->GetRange((int)j)<0.01){
+					        laserData->distanceData[i]=(int)(5*1000);
+				        }else{
+					        laserData->distanceData[i]=(int)(playerLaser->GetRange((int)j)*1000);
+				        }
+					    j=j+jump;
+					    i--;
+				    }
+				}else{
+				    //Now we must transform player resolution into user defined resolution
+				    player_resolution=(double)1.0/(0.5);
 
-				offset=((playerLaser->GetRangeCount()/jump)-laser_num_readings)/2;
-				i=0;
-				j=offset;
+				    jump = player_resolution / laser_resolution;
 
-				//Update laser values//
-				while(j<playerLaser->GetRangeCount() && i<laser_num_readings){
-					laserData->distanceData[i]=(int)(playerLaser->GetRange((int)j)*1000);
-					j=j+jump;
-					i++;
+				    offset=((playerLaser->GetRangeCount()/jump)-laser_num_readings)/2;
+				    i=0;
+				    j=offset;
+
+				    //Update laser values//
+				    while(j<playerLaser->GetRangeCount() && i<laser_num_readings){
+					    laserData->distanceData[i]=(int)(playerLaser->GetRange((int)j)*1000);
+					    j=j+jump;
+					    i++;
+				    }
 				}
+		        pthread_mutex_unlock(&mutex);
+				//std::cout << "nReading: "  << i << std::endl;
 
 			};
 
 		private:
+	        pthread_mutex_t mutex;
 			std::string prefix;
 			jderobotice::Context context;
 			jderobot::LaserDataPtr laserData;
@@ -200,6 +240,7 @@
 
 			int laser_num_readings;
 			double laser_resolution;
+			int hokuyo;
 	};
 
   class EncodersI: virtual public jderobot::Encoders {
@@ -224,7 +265,9 @@
 			virtual jderobot::EncodersDataPtr getEncodersData(const Ice::Current&){
 				return encodersData;
 			};
-
+	        virtual void setEncodersData(const jderobot::EncodersDataPtr&  encodersData,
+						     const Ice::Current&) {
+		     };
 			virtual void update() {
 				float robotx, roboty, robottheta;
 
@@ -570,27 +613,7 @@
 
 int main(int argc, char** argv) 
 {
-	//playerserver::Component component;
-	//jderobotice::Application app(component);
-	//return app.jderobotMain(argc,argv);
-	
-  int pid, status; 
-  
-	if(argc>2){ 
-		if(fork()){ 
-			sleep(2);
-			playerserver::Component component;
-			jderobotice::Application app(component);
-			return app.jderobotMain(argc,argv);
-		}else{ 
-			printf("%s\n", argv[2]); 
-			char* argvs = argv[2];
-			static char *argv[]={"player", argvs,NULL};
-			execv("/usr/local/bin/player",argv);
-			printf("Error\n"); 
-			exit(-1);
-		} 	
-	}else{
-		printf(" %d Usage: ./playerserver <--Ice.Config=fileConfig.cfg> <playerConfigFile>", argc);
-	}
+	playerserver::Component component;
+	jderobotice::Application app(component);
+	return app.jderobotMain(argc,argv);
 }



More information about the Jderobot-admin mailing list