[Jderobot-admin] jderobot-r1024 - in trunk/src/testing/components: . playerserver

eperdices en jderobot.org eperdices en jderobot.org
Mie Oct 9 16:00:01 CEST 2013


Author: eperdices
Date: 2013-10-09 15:59:01 +0200 (Wed, 09 Oct 2013)
New Revision: 1024

Added:
   trunk/src/testing/components/playerserver/
   trunk/src/testing/components/playerserver/CMakeLists.txt
   trunk/src/testing/components/playerserver/laser_real.cfg
   trunk/src/testing/components/playerserver/playerserver.cfg
   trunk/src/testing/components/playerserver/playerserver4.cpp
Log:
Added playerserver with Player 3.0.2


Added: trunk/src/testing/components/playerserver/CMakeLists.txt
===================================================================
--- trunk/src/testing/components/playerserver/CMakeLists.txt	                        (rev 0)
+++ trunk/src/testing/components/playerserver/CMakeLists.txt	2013-10-09 13:59:01 UTC (rev 1024)
@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 2.8)
+
+  SET( SOURCE_FILES playerserver4.cpp)
+	SET( LIBS_DIR /usr/local/lib/jderobot/)
+
+  include_directories(
+  /usr/local/include/jderobot/
+  /usr/local/include/player-3.0
+  #/usr/local/include/gearbox
+  )
+
+  set( CMAKE_CXX_FLAGS "-lplayerc -lplayerc++ -lplayercommon -lplayercore  -lIceUtil -lIce")
+
+  link_directories(/usr/local/lib)
+
+  add_executable (playerserver ${SOURCE_FILES})
+
+  TARGET_LINK_LIBRARIES(playerserver 
+		#${Gearbox_LIBRARIES}
+		${ZeroCIce_LIBRARIES}
+		${Player_LIBRARIES}
+		${LIBS_DIR}/libJderobotInterfaces.so
+		${LIBS_DIR}/libjderobotice.so
+		${LIBS_DIR}/libjderobotutil.so
+  )
+

Added: trunk/src/testing/components/playerserver/laser_real.cfg
===================================================================
--- trunk/src/testing/components/playerserver/laser_real.cfg	                        (rev 0)
+++ trunk/src/testing/components/playerserver/laser_real.cfg	2013-10-09 13:59:01 UTC (rev 1024)
@@ -0,0 +1,8 @@
+driver
+(
+	name "hokuyo_aist"
+	provides ["ranger:0"]
+	portopts "type=serial,device=/dev/ttyACM0,timeout=1,baud=19200"
+    min_angle -1.5707965 #PI/2
+    max_angle 1.5707965 #PI/2
+)

Added: trunk/src/testing/components/playerserver/playerserver.cfg
===================================================================
--- trunk/src/testing/components/playerserver/playerserver.cfg	                        (rev 0)
+++ trunk/src/testing/components/playerserver/playerserver.cfg	2013-10-09 13:59:01 UTC (rev 1024)
@@ -0,0 +1,24 @@
+#without registry
+PlayerServer.Endpoints=default -h localhost -p 9998
+#with registry
+#PlayerServer.Endpoints=default
+#PlayerServer.AdapterId=PlayerServer1
+#Ice.Default.Locator=IceGrid/Locator:tcp -h localhost -p 12000
+
+#Player server configuration
+PlayerServer.Hostname=localhost
+PlayerServer.Port=6665
+PlayerServer.Initial_position.X=0 		#m
+PlayerServer.Initial_position.Y=0			#m
+PlayerServer.Initial_position.Theta=0	#rad
+PlayerServer.Hokuyo=1
+PlayerServer.Laser=1
+PlayerServer.Laser.Num_readings=180
+PlayerServer.Laser.Readings_per_degree=1
+PlayerServer.Sonars=0
+PlayerServer.Encoders=0
+PlayerServer.Motors=0
+#PlayerServer.Bumpers=0
+#PlayerServer.PTMotors=0
+#PlayerServer.PTEncoders=0
+

Added: trunk/src/testing/components/playerserver/playerserver4.cpp
===================================================================
--- trunk/src/testing/components/playerserver/playerserver4.cpp	                        (rev 0)
+++ trunk/src/testing/components/playerserver/playerserver4.cpp	2013-10-09 13:59:01 UTC (rev 1024)
@@ -0,0 +1,619 @@
+/*
+ *
+ *  Copyright (C) 1997-2010 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Julio Vega  <julio.vega en urjc.es>
+ *						Eduardo Perdices  <eperdices en gsyc.es>
+ *
+ */
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobotice/component.h>
+#include <jderobotice/application.h>
+#include <tr1/memory>
+#include <list>
+#include <sstream>
+#include <jderobotice/exceptions.h>
+
+// JDErobot specific ice component includes
+#include <jderobot/motors.h>
+#include <jderobot/laser.h>
+#include <jderobot/encoders.h>
+#include <jderobot/ptencoders.h>
+#include <jderobot/ptmotors.h>
+#include <jderobot/sonars.h>
+
+// Library includes
+#include <math.h>
+#include <libplayerc++/playerc++.h>
+
+#include <pthread.h>
+
+// Constants
+#define DEGTORAD 0.01745327
+#define RADTODEG 57.29582790
+#define MIN_PAN 40.0
+#define MAX_PAN 160.0
+#define MIN_TILT 40.0
+#define MAX_TILT 120.0
+#define PLAYERSERVER_2PI 6.28318531
+#define ITERATION_TIME 75 //ms
+
+/*Laser configuration*/
+#define NUM_LASER_DEF 180
+#define RESOLUTION_LASER_DEF 1
+
+/*Sonar configuration*/
+#define NUM_SONARS_DEF 16
+
+using namespace std;
+
+// Global variables
+string playerhost;
+int playerport;
+
+namespace playerserver {
+	class MotorsI: virtual public jderobot::Motors {
+		public:
+			MotorsI (std::string& propertyPrefix, const jderobotice::Context& context):
+			prefix(propertyPrefix),context(context) {
+
+				Ice::PropertiesPtr prop = context.properties();
+
+				robot = new PlayerCc::PlayerClient(playerhost.c_str(), playerport);
+				playerPose = new PlayerCc::Position2dProxy(robot, 0);
+
+				// Create a client object
+				cout << "Connecting to Player Server at '" << playerhost << ":" << playerport << "..." << endl;
+
+				// Start the robot
+				player_v = 0.0;
+				player_w = 0.0;
+				playerPose->SetSpeed(player_v, player_w);
+			}
+
+			virtual ~MotorsI(){};
+
+			virtual float getV (const Ice::Current&) {
+
+				return player_v;
+			};
+
+			virtual float getW (const Ice::Current&) {
+				return player_w;
+			};
+
+			virtual Ice::Int setV (Ice::Float v, const Ice::Current&) {
+				player_v = v;
+
+				return 0;
+			};
+
+			virtual Ice::Int setW (Ice::Float w, const Ice::Current&) {
+				player_w = w;
+
+				return 0;
+			};
+
+			virtual float getL(const Ice::Current&) {
+				return (float)0.0;
+			};
+
+			virtual  Ice::Int setL(Ice::Float w, const Ice::Current&) {
+				return 0;
+			};
+
+			virtual void update() {
+				// Update robot vel
+				playerPose->SetSpeed(player_v, player_w);				
+			};
+		private:
+
+			std::string prefix;
+			jderobotice::Context context;
+			PlayerCc::PlayerClient *robot;
+			PlayerCc::Position2dProxy *playerPose;
+			float player_v, player_w;
+	};
+
+	class LaserI: virtual public jderobot::Laser {
+		public:
+			LaserI (std::string& propertyPrefix, const jderobotice::Context& context):
+			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, 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&) {
+		        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;
+				double j;
+				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;
+
+                    jump = 2.85;
+
+				    //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);
+
+				    jump = player_resolution / laser_resolution;
+
+				    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;
+
+			PlayerCc::PlayerClient *robot;
+			PlayerCc::RangerProxy *playerLaser;
+
+			int laser_num_readings;
+			double laser_resolution;
+			int hokuyo;
+	};
+
+  class EncodersI: virtual public jderobot::Encoders {
+		public:
+			EncodersI (std::string& propertyPrefix, const jderobotice::Context& context):
+			prefix(propertyPrefix),context(context),encodersData(new jderobot::EncodersData()) {
+				Ice::PropertiesPtr prop = context.properties();
+
+				robot = new PlayerCc::PlayerClient(playerhost.c_str(), playerport);
+				playerPose = new PlayerCc::Position2dProxy(robot, 0);
+
+				// Create a client object
+				cout << "Connecting to Player Server at '" << playerhost << ":" << playerport << "..." << endl;
+
+				correcting_x = prop->getPropertyAsIntWithDefault("PlayerServer.Initial_position.X", 0); /* m */
+				correcting_y = prop->getPropertyAsIntWithDefault("PlayerServer.Initial_position.Y", 0); /* m */
+				correcting_theta = prop->getPropertyAsIntWithDefault("PlayerServer.Initial_position.Theta", 0); /* rad */
+			}
+
+			virtual ~EncodersI(){};
+
+			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;
+
+				// Read data from the server
+				robot->Read();
+
+
+				robotx = (playerPose->GetXPos())*(float)cos(correcting_theta) - (playerPose->GetYPos())*(float)sin(correcting_theta) + correcting_x;
+				roboty = (playerPose->GetYPos())*(float)cos(correcting_theta) + (playerPose->GetXPos())*(float)sin(correcting_theta) + correcting_y;
+
+				robottheta = (playerPose->GetYaw()) + correcting_theta;
+
+				if (robottheta<=0) 
+					robottheta = robottheta + PLAYERSERVER_2PI;
+				else if (robottheta > PLAYERSERVER_2PI) 
+					robottheta = robottheta - PLAYERSERVER_2PI;
+
+				encodersData->robotx = robotx*100;
+				encodersData->roboty = roboty*100;
+				encodersData->robottheta = robottheta;
+				encodersData->robotcos = cos(robottheta);
+				encodersData->robotsin = sin(robottheta);
+			};
+
+		private:
+			std::string prefix;
+			jderobotice::Context context;
+			jderobot::EncodersDataPtr encodersData;
+
+			PlayerCc::PlayerClient *robot;
+			PlayerCc::Position2dProxy *playerPose;
+
+			float correcting_x; /* m */
+			float correcting_y; /* m */
+			float correcting_theta; /* rad */
+	};
+
+
+	class PTMotorsI: virtual public jderobot::PTMotors {
+		public:
+			PTMotorsI(std::string& propertyPrefix, const jderobotice::Context& context):
+			prefix(propertyPrefix),context(context) {
+				Ice::PropertiesPtr prop = context.properties();
+
+				playerserver_id=0;
+				playerclient_id=0;
+
+				// Create a client object
+				cout << "Connecting to Player Server at '" << playerhost << ":" << playerport << "..." << endl;
+				playerclient = playerc_client_create (NULL, playerhost.c_str(), playerport);
+
+				// Connect to the server
+				if (playerc_client_connect(playerclient) != 0)	{
+					cout << "ERROR: Connecting to Player Server" << endl;
+				}
+
+				// Getting player server pantilt
+				player_ptz = playerc_ptz_create (playerclient, 0);
+
+				if (playerc_ptz_subscribe (player_ptz, PLAYER_OPEN_MODE) != 0) {
+					player_ptz = NULL;
+					cout << "ERROR: Getting Player PTZ" << endl;
+				}
+			}
+
+			virtual ~PTMotorsI() {};
+
+			virtual Ice::Int setPTMotorsData(const jderobot::PTMotorsDataPtr & data, const Ice::Current&){
+				// Read data from the server
+				if(playerc_client_read(playerclient) == NULL) {
+					cout << "ERROR: Reading from Player Server" << endl;
+				}
+
+				//player_ptz_lock (player_ptz, 1);
+				player_ptz->pan = data->longitude;
+				if (data->longitude > MAX_PAN*DEGTORAD)
+				player_ptz->pan = MAX_PAN*DEGTORAD;
+				else if (data->longitude < MIN_PAN*DEGTORAD)
+				player_ptz->pan = MIN_PAN*DEGTORAD;
+
+				player_ptz->tilt = data->latitude;
+				if (data->latitude > MAX_TILT*DEGTORAD)
+				player_ptz->tilt = MAX_TILT*DEGTORAD;
+				else if (data->latitude < MIN_TILT*DEGTORAD)
+				player_ptz->tilt = MIN_TILT*DEGTORAD;
+				//player_ptz_unlock (player_ptz);
+
+				return 0;
+			};
+
+			std::string prefix;
+			jderobotice::Context context;
+			jderobot::PTMotorsDataPtr ptMotorsData;
+
+			playerc_client_t *playerclient;
+			int playerserver_id;
+			int playerclient_id;
+			playerc_ptz_t *player_ptz;
+	};
+
+	class PTEncodersI: virtual public jderobot::PTEncoders {
+		public:
+			PTEncodersI(std::string& propertyPrefix, const jderobotice::Context& context):
+			prefix(propertyPrefix),context(context)	{
+				Ice::PropertiesPtr prop = context.properties();
+
+				playerserver_id=0;
+				playerclient_id=0;
+
+				// Create a client object
+				cout << "Connecting to Player Server at '" << playerhost << ":" << playerport << "..." << endl;
+				playerclient = playerc_client_create (NULL, playerhost.c_str(), playerport);
+
+				// Connect to the server
+				if (playerc_client_connect(playerclient) != 0)	{
+					cout << "ERROR: Connecting to Player Server" << endl;
+				}
+
+				// Getting player server pantilt
+				player_ptz = playerc_ptz_create (playerclient, 0);
+
+				if (playerc_ptz_subscribe (player_ptz, PLAYER_OPEN_MODE) != 0) {
+					player_ptz = NULL;
+					cout << "ERROR: Getting Player PTZ" << endl;
+				}
+			}
+
+			virtual ~PTEncodersI(){};
+
+			virtual jderobot::PTEncodersDataPtr getPTEncodersData(const Ice::Current&){
+				// Read data from the server
+				if(playerc_client_read(playerclient) == NULL) {
+					cout << "ERROR: Reading from Player Server" << endl;
+				}
+
+				//player_ptz_lock (player_ptz, 1);
+				ptEncodersData->panAngle = -1 * player_ptz->pan;
+				ptEncodersData->tiltAngle = -1 * player_ptz->tilt;
+				//player_ptz_unlock (player_ptz);
+
+				return ptEncodersData; 
+			};
+
+
+			std::string prefix;
+			jderobotice::Context context;
+			jderobot::PTEncodersDataPtr ptEncodersData;
+
+			playerc_client_t *playerclient;
+			int playerserver_id;
+			int playerclient_id;
+			playerc_ptz_t *player_ptz;
+	};
+
+	class SonarsI: virtual public jderobot::Sonars {
+		public:
+			SonarsI(std::string& propertyPrefix, const jderobotice::Context& context):
+			prefix(propertyPrefix),context(context),sonarsData(new jderobot::SonarsData()) {
+								Ice::PropertiesPtr prop = context.properties();
+
+				// Create a client object
+				cout << "Connecting to Player Server at '" << playerhost << ":" << playerport << "..." << endl;
+
+				robot = new PlayerCc::PlayerClient(playerhost.c_str(), playerport);
+				playerSonar = new PlayerCc::RangerProxy(robot, 0);
+
+				sonarsData->numSonars = playerSonar->GetRangeCount();
+				sonarsData->us.resize(sonarsData->numSonars);
+			}
+
+			virtual ~SonarsI() {};
+
+			virtual jderobot::SonarsDataPtr getSonarsData (const Ice::Current&) {
+				return sonarsData; 
+			};
+
+			virtual void update() {
+				// Read data from the server	
+				robot->Read();
+				
+				int num_sonars = playerSonar->GetRangeCount();;
+
+				sonarsData->numSonars = playerSonar->GetRangeCount();
+				sonarsData->us.resize(playerSonar->GetRangeCount());
+				
+				//Update sonars values//
+				for (int i = 0; i < num_sonars; i++) {
+					sonarsData->us[i] = (int) playerSonar->GetRange(i)*1000;
+				}
+
+				sonarsData->numSonars = num_sonars;
+			}
+
+			std::string prefix;
+			jderobotice::Context context;
+			jderobot::SonarsDataPtr sonarsData;
+			
+			PlayerCc::PlayerClient *robot;
+			PlayerCc::RangerProxy *playerSonar;
+	};
+
+	class Component: public jderobotice::Component {
+		public:
+			Component():jderobotice::Component("PlayerServer"),
+			motors1(0), laser1(0), encoders1(0), ptmotors1(0), ptencoders1(0), sonars1(0) {}
+
+			virtual void start() {		
+				int avMotors, avLaser, avEncoders;
+				int avPTMotors, avPTEncoders, avSonars;
+
+				Ice::PropertiesPtr prop = context().properties();
+								
+				playerhost = prop->getPropertyWithDefault(context().tag() + ".Hostname","localhost");
+				playerport = prop->getPropertyAsIntWithDefault(context().tag() + ".Port",6665);
+
+				//Motors
+				avMotors = prop->getPropertyAsInt(context().tag() + ".Motors");
+				if (avMotors == 1) {
+					cout << "DEBUG: Entramos en motors" << endl;
+					std::string objPrefix2(context().tag() + ".Motors");
+					std::string motorsName = "motors1";
+					context().tracer().info("Creating motors1 " + motorsName);
+					motors1 = new MotorsI(objPrefix2,context());
+					context().createInterfaceWithString(motors1,motorsName);
+				}
+
+
+				//Laser
+				avLaser = prop->getPropertyAsInt(context().tag() + ".Laser");
+				if (avLaser == 1) {
+					cout << "DEBUG: Entramos en laser" << endl;
+					std::string objPrefix3(context().tag() + ".Laser");
+					std::string laserName = "laser1";
+					context().tracer().info("Creating laser1 " + laserName);
+					laser1 = new LaserI(objPrefix3,context());
+					context().createInterfaceWithString(laser1,laserName);
+				}
+				cout << "DEBUG: Salidos de laser" << endl;
+
+				//Encoders
+				avEncoders = prop->getPropertyAsInt(context().tag() + ".Encoders");
+				if (avEncoders == 1) {
+					cout << "DEBUG: Entramos en encoders" << endl;
+					std::string objPrefix4(context().tag() + ".Encoders");
+					std::string encodersName = "encoders1";
+					context().tracer().info("Creating encoders1 " + encodersName);
+					encoders1 = new EncodersI(objPrefix4,context());
+					context().createInterfaceWithString(encoders1,encodersName);
+				}
+
+				//PTMotors
+				/*avPTMotors = prop->getPropertyAsInt(context().tag() + ".PTMotors");
+				if (avPTMotors == 1) {
+					cout << "DEBUG: Entramos en ptmotors" << endl;
+					std::string objPrefix5(context().tag() + ".PTMotors");
+					std::string ptmotorsName = "ptmotors1";
+					context().tracer().info("Creating ptmotors1 " + ptmotorsName);
+					ptmotors1 = new PTMotorsI(objPrefix5,context());
+					context().createInterfaceWithString(ptmotors1,ptmotorsName);
+				}*/
+
+				//PTEncoders
+				/*avPTEncoders = prop->getPropertyAsInt(context().tag() + ".PTEncoders");
+				if (avPTEncoders == 1) {
+					cout << "DEBUG: Entramos en ptencoders" << endl;
+					std::string objPrefix6(context().tag() + ".PTEncoders");
+					std::string ptencodersName = "ptencoders1";
+					context().tracer().info("Creating ptencoders1 " + ptencodersName);
+					ptencoders1 = new PTEncodersI(objPrefix6,context());
+					context().createInterfaceWithString(ptencoders1,ptencodersName);
+				}*/
+
+				//Sonars
+				avSonars = prop->getPropertyAsInt(context().tag() + ".Sonars");
+				if (avSonars == 1) {
+					cout << "DEBUG: Entramos en sonars" << endl;
+					std::string objPrefix7(context().tag() + ".Sonars");
+					std::string sonarsName = "sonars1";
+					context().tracer().info("Creating sonars1 " + sonarsName);
+					sonars1 = new SonarsI(objPrefix7,context());
+					context().createInterfaceWithString(sonars1,sonarsName);
+				}
+				
+				//Get subclasses
+				MotorsI * motors = NULL;
+				LaserI * laser = NULL;
+				EncodersI * encoders = NULL;
+				PTMotorsI * ptmotors = NULL;
+				PTEncodersI * ptencoders = NULL;
+				SonarsI * sonars = NULL;
+
+				if(avMotors)
+					motors = dynamic_cast<MotorsI*>(&(*motors1));
+				if(avLaser)
+				    laser = dynamic_cast<LaserI*>(&(*laser1));
+				if(avEncoders)
+					encoders = dynamic_cast<EncodersI*>(&(*encoders1));
+				//if(avPTMotors)
+					//ptmotors = dynamic_cast<PTMotorsI*>(&(*ptmotors1));
+				//if(avPTEncoders)
+				//	ptencoders = dynamic_cast<PTEncodersI*>(&(*ptencoders1));			
+				if(avSonars)
+					sonars = dynamic_cast<SonarsI*>(&(*sonars1));		
+					
+				//Update values
+				while(true) {
+					if(avMotors && motors)
+						  motors->update();
+
+					if(avLaser && laser)    
+						  laser->update();
+
+					if(avEncoders && encoders)
+						  encoders->update();
+
+					//if(avPTMotors && ptmotors)
+						  //ptmotors->update();
+
+					//if(avPTEncoders && ptencoders)
+						//  ptencoders->update();
+
+					if(avSonars && sonars)
+						  sonars->update();
+                    
+					usleep(ITERATION_TIME*1000);
+				}
+			}
+
+			virtual ~Component() {}
+
+			private:
+				Ice::ObjectPtr motors1;
+				Ice::ObjectPtr laser1;
+				Ice::ObjectPtr encoders1;
+				Ice::ObjectPtr ptmotors1;
+				Ice::ObjectPtr ptencoders1;
+				Ice::ObjectPtr sonars1;
+	};
+}
+
+#include <signal.h>
+#include <stdio.h>
+
+int main(int argc, char** argv) 
+{
+	playerserver::Component component;
+	jderobotice::Application app(component);
+	return app.jderobotMain(argc,argv);
+}



More information about the Jderobot-admin mailing list