[Jderobot-admin] jderobot-r1056 - in trunk/src/stable/components/gazeboserver: . plugins plugins/nao plugins/pioneer

bmenendez en jderobot.org bmenendez en jderobot.org
Mar Oct 15 14:29:26 CEST 2013


Author: bmenendez
Date: 2013-10-15 14:29:25 +0200 (Tue, 15 Oct 2013)
New Revision: 1056

Added:
   trunk/src/stable/components/gazeboserver/CMakeLists.txt
   trunk/src/stable/components/gazeboserver/plugins/
   trunk/src/stable/components/gazeboserver/plugins/pioneer/
   trunk/src/stable/components/gazeboserver/plugins/pioneer/CMakeLists.txt
   trunk/src/stable/components/gazeboserver/plugins/pioneer/camera_dump.cc
   trunk/src/stable/components/gazeboserver/plugins/pioneer/encoders.cc
   trunk/src/stable/components/gazeboserver/plugins/pioneer/encoders.h
   trunk/src/stable/components/gazeboserver/plugins/pioneer/laser.cc
   trunk/src/stable/components/gazeboserver/plugins/pioneer/motors.cc
   trunk/src/stable/components/gazeboserver/plugins/pioneer/motors.h
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2Encoders.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2Motors.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_cam_sensor_left.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_cam_sensor_right.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_laser.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_pose3dencoders.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxEncoders.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxJde.world
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxMotors.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_cam_sensor_left.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_cam_sensor_right.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_laser.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_pose3dencoders.cfg
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pose3dencoders.cc
   trunk/src/stable/components/gazeboserver/plugins/pioneer/pose3dencoders.h
   trunk/src/stable/components/gazeboserver/plugins/pioneer/worlds/
Removed:
   trunk/src/stable/components/gazeboserver/CMakeLists.txt
   trunk/src/stable/components/gazeboserver/camera_dump.cc
   trunk/src/stable/components/gazeboserver/encoders.cc
   trunk/src/stable/components/gazeboserver/encoders.h
   trunk/src/stable/components/gazeboserver/laser.cc
   trunk/src/stable/components/gazeboserver/motors.cc
   trunk/src/stable/components/gazeboserver/motors.h
   trunk/src/stable/components/gazeboserver/pioneer2dx2Encoders.cfg
   trunk/src/stable/components/gazeboserver/pioneer2dx2Motors.cfg
   trunk/src/stable/components/gazeboserver/pioneer2dx2_cam_sensor_left.cfg
   trunk/src/stable/components/gazeboserver/pioneer2dx2_cam_sensor_right.cfg
   trunk/src/stable/components/gazeboserver/pioneer2dx2_laser.cfg
   trunk/src/stable/components/gazeboserver/pioneer2dx2_pose3dencoders.cfg
   trunk/src/stable/components/gazeboserver/pioneer2dxEncoders.cfg
   trunk/src/stable/components/gazeboserver/pioneer2dxJde.world
   trunk/src/stable/components/gazeboserver/pioneer2dxMotors.cfg
   trunk/src/stable/components/gazeboserver/pioneer2dx_cam_sensor_left.cfg
   trunk/src/stable/components/gazeboserver/pioneer2dx_cam_sensor_right.cfg
   trunk/src/stable/components/gazeboserver/pioneer2dx_laser.cfg
   trunk/src/stable/components/gazeboserver/pioneer2dx_pose3dencoders.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/
   trunk/src/stable/components/gazeboserver/pose3dencoders.cc
   trunk/src/stable/components/gazeboserver/pose3dencoders.h
   trunk/src/stable/components/gazeboserver/worlds/
Modified:
   trunk/src/stable/components/gazeboserver/plugins/nao/CMakeLists.txt
Log:
#74 Changed the gazeboserver structure


Deleted: trunk/src/stable/components/gazeboserver/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/gazeboserver/CMakeLists.txt	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/CMakeLists.txt	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,53 +0,0 @@
-if (GAZEBO_INCLUDE_DIRS)
-
-include_directories(
-	${GAZEBO_INCLUDE_DIRS}
-    ${INTERFACES_CPP_DIR}
-    ${LIBS_DIR}
-    ${CMAKE_CURRENT_SOURCE_DIR})
-
-link_directories(${GAZEBO_LIBRARY_DIRS})
-#boost_system 
-add_library(motors SHARED motors.cc)
-target_link_libraries(motors 
-	${GAZEBO_libraries} 
-	${ZeroCIce_LIBRARIES} 
-
-    colorspacesmm
-    JderobotInterfaces) 
-    
-add_library(encoders SHARED encoders.cc)
-target_link_libraries(encoders 
-	${GAZEBO_libraries} 
-
-	${ZeroCIce_LIBRARIES} 
-    colorspacesmm
-    JderobotInterfaces)    
-
-add_library(laser SHARED laser.cc)
-target_link_libraries(laser 
-	RayPlugin 
-	${GAZEBO_libraries} 
-	${ZeroCIce_LIBRARIES} 
-    colorspacesmm
-    JderobotInterfaces) 
-
-add_library(camera_dump SHARED camera_dump.cc)
-target_link_libraries(camera_dump ${GAZEBO_libraries} 
-	CameraPlugin 	
-	${GAZEBO_libraries} 
-	${ZeroCIce_LIBRARIES} 
-    colorspacesmm
-    JderobotInterfaces)
-    
-add_library(pose3dencoders SHARED pose3dencoders.cc)
-target_link_libraries(pose3dencoders ${GAZEBO_libraries} 	 	
-	${GAZEBO_libraries} 
-
-	${ZeroCIce_LIBRARIES} 
-    colorspacesmm
-    JderobotInterfaces)
-
-add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/pluginsGazebo ${CMAKE_CURRENT_SOURCE_DIR}/pluginsGazebo)
-
-ENDIF()

Added: trunk/src/stable/components/gazeboserver/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/gazeboserver/CMakeLists.txt	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/CMakeLists.txt	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,5 @@
+IF (GAZEBO_INCLUDE_DIRS)
+
+add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/plugins ${CMAKE_CURRENT_SOURCE_DIR}/plugins)
+
+ENDIF()

Deleted: trunk/src/stable/components/gazeboserver/camera_dump.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/camera_dump.cc	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/camera_dump.cc	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,280 +0,0 @@
-#include "gazebo.hh"
-#include "plugins/CameraPlugin.hh"
-#include "common/common.hh"
-#include "transport/transport.hh"
-
-#include <opencv2/core/core.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/highgui/highgui.hpp>
-
-// ICE utils includes
-#include <Ice/Ice.h>
-#include <IceUtil/IceUtil.h>
-
-#include <jderobot/camera.h>
-
-#include <visionlib/colorspaces/colorspacesmm.h>
-
-#include <iostream>
-
-#include <boost/algorithm/string.hpp>
-#include <boost/lexical_cast.hpp>
-
-void *myMain(void* v);
-
-namespace gazebo
-{   
-  class CameraDump : public CameraPlugin
-  { 
-    public: CameraDump() : CameraPlugin(),count(0) 
-	{
-		pthread_mutex_init (&mutex, NULL);
-		n = round(rand()*1000);
-	}
-
-    public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
-    {
-      // Don't forget to load the camera plugin
-      CameraPlugin::Load(_parent,_sdf);
-      std::cout << "Load: " <<n << " " <<  this->parentSensor->GetCamera()->GetName()<< std::endl;
-    } 
-
-    // Update the controller
-    public: void OnNewFrame(const unsigned char *_image, 
-        unsigned int _width, unsigned int _height, unsigned int _depth, 
-        const std::string &_format)
-    {
-      //std::cout << "OnNewFrame: " <<n << " " <<  this->parentSensor->GetCamera()->GetName()<< std::endl;
-		if(count==0){
-			std::vector<std::string> tokens;
-			nameCamera = this->parentSensor->GetCamera()->GetName();
-  			boost::split(tokens, nameCamera, boost::is_any_of("::"));
-  			boost::split(tokens, tokens[2], boost::is_any_of("("));
-			nameGlobal = tokens[0];
-			// El nombre del fichero de configuracion tiene que coincidir con el de la cámara en el .world y el .cfg 
-			nameCamera = std::string("--Ice.Config=" + tokens[0] + ".cfg"); 
-			
-			if (count == 0){
-				pthread_t thr_gui;
-				pthread_create(&thr_gui, NULL, &myMain, (void*)this);
-			}
-
-			image.create(_height, _width, CV_8UC3);
-			count++;
-		}
-		pthread_mutex_lock (&mutex);
-        memcpy((unsigned char *) image.data, &(_image[0]), _width*_height * 3);
-		
-		//cv::imshow(nameCamera, image);
-		//cv::waitKey(10);
-					
-		pthread_mutex_unlock (&mutex);
-		
-    }         
-
-    private: int count;
-    private: int n;
-    public: std::string nameCamera;
-    public: cv::Mat image;
-	public: pthread_mutex_t mutex;
-	public: std::string nameGlobal;
-  };
-
-  // Register this plugin with the simulator
-  GZ_REGISTER_SENSOR_PLUGIN(CameraDump)
-}
-
-class CameraI: virtual public jderobot::Camera {
-	public:
-		CameraI(std::string propertyPrefix, gazebo::CameraDump* camera)
-			   : prefix(propertyPrefix), cameraI(camera) {
-		
-			std::cout << "Constructor CameraI" << std::endl;
-
-			imageDescription = (new jderobot::ImageDescription());
-
-        	replyTask = new ReplyTask(this);
-		    replyTask->start(); // my own thread
-		  
-		}
-
-		std::string getName () {
-			return (cameraDescription->name);
-		}
-
-		std::string getRobotName () {
-			return "RobotName";
-		}
-
-		virtual ~CameraI() {
-
-
-		}
-
-		virtual jderobot::ImageDescriptionPtr getImageDescription(const Ice::Current& c){
-			return imageDescription;
-		}
-
-		virtual jderobot::CameraDescriptionPtr getCameraDescription(const Ice::Current& c){
-			return cameraDescription;
-		}
-
-		virtual Ice::Int setCameraDescription(const jderobot::CameraDescriptionPtr &description, const Ice::Current& c) {
-			return 0;
-		}
-
-		virtual void getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr& cb,const Ice::Current& c){
-			replyTask->pushJob(cb);
-		}
-
-		virtual std::string startCameraStreaming(const Ice::Current&){
-
-		}
-
-		virtual void stopCameraStreaming(const Ice::Current&) {
-
-		}
-
-	private:
-		class ReplyTask: public IceUtil::Thread {
-			public:
-				ReplyTask(CameraI* camera){
-                    mycamera = camera;
-
-				   	std::cout << "safeThread" << std::endl;
-				}
-
-				void pushJob(const jderobot::AMD_ImageProvider_getImageDataPtr& cb){
-					IceUtil::Mutex::Lock sync(requestsMutex);
-					requests.push_back(cb);
-				}
-
-				virtual void run(){
-					jderobot::ImageDataPtr reply(new jderobot::ImageData);
-					struct timeval a, b;
-					int cycle = 48;
-					long totalb,totala;
-					long diff;
-					
-					int count =0 ;
-
-					while(1){
-						
-						if(!mycamera->cameraI->image.data){
-							usleep(100);
-							continue;
-						}
-						if(count==0){
-							pthread_mutex_lock (&mycamera->cameraI->mutex);
-							mycamera->imageDescription->width = mycamera->cameraI->image.cols;
-							mycamera->imageDescription->height = mycamera->cameraI->image.rows;
-							mycamera->imageDescription->size = mycamera->cameraI->image.cols*mycamera->cameraI->image.rows*3;
-							pthread_mutex_unlock (&mycamera->cameraI->mutex);
-
-							mycamera->imageDescription->format = "RGB8";
-
-							reply->description = mycamera->imageDescription;
-							count++;
-						}
-
-						//std::cout << nameGlobal<< std::endl;
-									
-						gettimeofday(&a,NULL);
-						totala=a.tv_sec*1000000+a.tv_usec;
-
-
-						IceUtil::Time t = IceUtil::Time::now();
-						reply->timeStamp.seconds = (long)t.toSeconds();
-						reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
-          				
-          				pthread_mutex_lock (&mycamera->cameraI->mutex);
-					    reply->pixelData.resize(mycamera->cameraI->image.rows*mycamera->cameraI->image.cols*3);
-					    
-					    memcpy( &(reply->pixelData[0]), (unsigned char *) mycamera->cameraI->image.data, mycamera->cameraI->image.rows*mycamera->cameraI->image.cols*3);
-						pthread_mutex_unlock (&mycamera->cameraI->mutex);
-
-					   { //critical region start
-						   IceUtil::Mutex::Lock sync(requestsMutex);
-						   while(!requests.empty()) {
-							   jderobot::AMD_ImageProvider_getImageDataPtr cb = requests.front();
-							   requests.pop_front();
-							   cb->ice_response(reply);
-						   }
-					   } //critical region end
-                  
-						gettimeofday(&b,NULL);
-						totalb=b.tv_sec*1000000+b.tv_usec;
-
-						diff = (totalb-totala)/1000;
-						diff = cycle-diff;
-
-						//std::cout << "Gazeboserver takes " << diff << " ms " << mycamera->fileName << std::endl;
-
-						if(diff < 33)
-							diff = 33;
-
-
-						/*Sleep Algorithm*/
-						usleep(diff*1000);
-					}
-				}
-
-				CameraI* mycamera;
-				IceUtil::Mutex requestsMutex;
-				std::list<jderobot::AMD_ImageProvider_getImageDataPtr> requests;
-		};
-
-		typedef IceUtil::Handle<ReplyTask> ReplyTaskPtr;
-		std::string prefix;
-
-		colorspaces::Image::FormatPtr imageFmt;
-		jderobot::ImageDescriptionPtr imageDescription;
-		jderobot::CameraDescriptionPtr cameraDescription;
-		ReplyTaskPtr replyTask;
-		gazebo::CameraDump* cameraI;	
-		
-}; // end class CameraI
-
-
-void *myMain(void* v)
-{
-
-	gazebo::CameraDump* camera = (gazebo::CameraDump*)v;
-
-	char* name = (char*)camera->nameCamera.c_str();
-
-    Ice::CommunicatorPtr ic;
-    int argc = 1;
-
-    Ice::PropertiesPtr prop;
-	char* argv[] = {name};
-
-    try {
-        
-        ic = Ice::initialize(argc, argv);
-        prop = ic->getProperties();
-        
-        std::string Endpoints = prop->getProperty("CameraGazebo.Endpoints");
-        std::cout << "CameraGazebo "<< camera->nameGlobal <<" Endpoints > "  << Endpoints << std::endl;
-        
-        Ice::ObjectAdapterPtr adapter =
-        ic->createObjectAdapterWithEndpoints("CameraGazebo", Endpoints);
-		
-        Ice::ObjectPtr object = new CameraI(std::string("CameraGazebo"),  camera);
-        adapter->add(object, ic->stringToIdentity(camera->nameGlobal));
-        adapter->activate();
-        ic->waitForShutdown();
-    } catch (const Ice::Exception& e) {
-        std::cerr << e << std::endl;
-    } catch (const char* msg) {
-        std::cerr << msg << std::endl;
-    }
-    if (ic) {
-        try {
-            ic->destroy();
-        } catch (const Ice::Exception& e) {
-            std::cerr << e << std::endl;
-        }
-    }
-    
-}

Deleted: trunk/src/stable/components/gazeboserver/encoders.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/encoders.cc	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/encoders.cc	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,145 +0,0 @@
-#include "encoders.h"
-
-namespace gazebo {
-
-    void *encodersICE(void* v);
-
-    GZ_REGISTER_MODEL_PLUGIN(Encoders)
-
-    Encoders::Encoders() {
-        pthread_mutex_init(&mutex, NULL);
-        pthread_mutex_init(&mutexEncoders, NULL);
-        count = 0;
-        std::cout << "constructor Encoders" << std::endl;
-    }
-
-    void Encoders::Load(physics::ModelPtr _parent, sdf::ElementPtr) {
-
-        model = _parent;
-        this->updateConnection = event::Events::ConnectWorldUpdateStart(
-                boost::bind(&Encoders::OnUpdate, this));
-    }
-    
-    physics::ModelPtr Encoders::getModel()
-    {
-    	return model;
-    }
-
-    void Encoders::OnUpdate() {
-
-        if (count == 0) {
-            count++;
-            std::string name = this->model->GetName();
-            std::cout << "GetName() encoders: " << name << std::endl;
-            nameEncoders = std::string("--Ice.Config=" + name + "Encoders.cfg");
-            pthread_t thr_gui;
-            pthread_create(&thr_gui, NULL, &encodersICE, (void*) this);
-        }
-
-        position = model->GetWorldPose();
-        this->initial_q = position.rot;
-
-        math::Vector3 initial_rpy = initial_q.GetAsEuler();
-
-        double degrees = initial_rpy.z * 180.0 / M_PI;
-        if (degrees < 0) {
-            degrees = 360 + degrees;
-        }
-
-
-        pthread_mutex_lock(&mutex);
-        robotEncoders.x = position.pos.x;
-        robotEncoders.y = position.pos.y;
-        robotEncoders.theta = degrees;
-        pthread_mutex_unlock(&mutex);
-
-    }
-
-    class EncodersI : virtual public jderobot::Encoders {
-    public:
-
-        EncodersI(gazebo::Encoders* pose) : encodersData(new jderobot::EncodersData()) {
-            this->pose = pose;
-        }
-
-        virtual ~EncodersI() {
-        };
-        
-        virtual void setEncodersData(const jderobot::EncodersDataPtr&  encodersData,
-        						     const Ice::Current&) {
-             math::Pose position = this->pose->getModel()->GetWorldPose();
-             
-             position.Set(encodersData->robotx,
-             			  encodersData->roboty,
-             			  encodersData->robotcos,
-             			  0,
-             			  0,
-             			  encodersData->robottheta);
-        	this->pose->getModel()->SetWorldPose(position); 
-			//this->getModel();
-        }
-
-
-        virtual jderobot::EncodersDataPtr getEncodersData(const Ice::Current&) {
-            pthread_mutex_lock(&pose->mutex);
-
-            //std::cout << "theta: " << pose->robotEncoders.theta << std::endl;
-
-            encodersData->robotx = pose->robotEncoders.x * 1000;
-            encodersData->roboty = pose->robotEncoders.y * 1000;
-            encodersData->robottheta = pose->robotEncoders.theta;
-            encodersData->robotcos = cos(pose->robotEncoders.theta);
-            encodersData->robotsin = sin(pose->robotEncoders.theta);
-
-            pthread_mutex_unlock(&pose->mutex);
-
-            return encodersData;
-        };
-
-    public:
-        gazebo::Encoders* pose;
-    private:
-        jderobot::EncodersDataPtr encodersData;
-    };
-
-    void *encodersICE(void* v) {
-        gazebo::Encoders* base = (gazebo::Encoders*)v;
-        char* name = (char*) base->nameEncoders.c_str();
-        Ice::CommunicatorPtr ic;
-        int argc = 1;
-        Ice::PropertiesPtr prop;
-        char* argv[] = {name};
-
-        try {
-
-            ic = Ice::initialize(argc, argv);
-
-
-            prop = ic->getProperties();
-            std::string Endpoints = prop->getProperty("Encoders.Endpoints");
-            std::cout << "Encoders Endpoints > " << Endpoints << std::endl;
-
-            Ice::ObjectAdapterPtr adapter =
-                    ic->createObjectAdapterWithEndpoints("Encoders", Endpoints);
-            Ice::ObjectPtr object = new EncodersI(base);
-
-            adapter->add(object, ic->stringToIdentity("Encoders"));
-
-            adapter->activate();
-            ic->waitForShutdown();
-        } catch (const Ice::Exception& e) {
-            std::cerr << e << std::endl;
-        } catch (const char* msg) {
-            std::cerr << msg << std::endl;
-        }
-        if (ic) {
-            try {
-                ic->destroy();
-            } catch (const Ice::Exception& e) {
-                std::cerr << e << std::endl;
-            }
-        }
-
-    }
-
-}

Deleted: trunk/src/stable/components/gazeboserver/encoders.h
===================================================================
--- trunk/src/stable/components/gazeboserver/encoders.h	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/encoders.h	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,57 +0,0 @@
-#include <boost/bind.hpp>
-#include "gazebo.hh"
-#include "physics/physics.hh"
-#include "common/common.hh"
-#include "transport/transport.hh"
-
-
-// ICE utils includes
-#include <Ice/Ice.h>
-#include <IceUtil/IceUtil.h>
-
-
-// JDErobot general ice component includes
-#include <jderobot/motors.h>
-#include <jderobot/encoders.h>
-
-#ifndef ENCODERS_H
-#define	ENCODERS_H
-
-namespace gazebo {
-    
-    
-    class Encoders : public ModelPlugin {
-    public:
-        
-        Encoders();
-        
-        virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
-        physics::ModelPtr getModel();
-        
-        pthread_mutex_t mutex;
-        pthread_mutex_t mutexEncoders;
-        int count;
-        struct encoders_t {
-            float x;
-            float y;
-            float theta;
-            float cos;
-            float sin;
-        };
-        math::Quaternion initial_q;
-        encoders_t robotEncoders;
-        std::string nameEncoders;
-        
-    private:
-        
-        void OnUpdate();
-        physics::ModelPtr model;
-        math::Pose position;
-        event::ConnectionPtr updateConnection;
-        
-        
-    };
-    
-}
-
-#endif	/* POSE_H */

Deleted: trunk/src/stable/components/gazeboserver/laser.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/laser.cc	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/laser.cc	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,146 +0,0 @@
-#include <boost/bind.hpp>
-#include "gazebo.hh"
-#include "physics/physics.hh"
-#include "physics/MultiRayShape.hh"
-#include "common/common.hh"
-#include "transport/transport.hh"
-
-#include "plugins/RayPlugin.hh"
-
-#include <boost/algorithm/string.hpp>
-
-
-#include <iostream>
-
-// ICE utils includes
-#include <Ice/Ice.h>
-#include <IceUtil/IceUtil.h>
-
-#include <jderobot/laser.h>
-
-void *mainLaser(void* v);
-
-namespace gazebo
-{     
-    class LaserDump : public RayPlugin
-  	{ 
-  	
-		public: LaserDump() : RayPlugin()
-		{
-			std::cout << "LaserDump Constructor" <<std::endl;
-			count = 0;
-			pthread_mutex_init (&mutex, NULL);	
-		}
-		
-	    public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
-		{
-		  // Don't forget to load the camera plugin
-		  RayPlugin::Load(_parent,_sdf);
-		  this->parentSensor =  boost::shared_dynamic_cast<sensors::RaySensor>(_parent);
-		  
-		} 
-
-		// Update the controller
-		public: void OnNewLaserScans()
-		{
-			if(count == 0){
-				count++;
-				std::string name = this->parentSensor->GetParentName();
-				std::cout <<" laser: " << name  << std::endl;
-				
-				std::vector<std::string> strs;
-				boost::split(strs, name, boost::is_any_of("::"));
-
-    			nameLaser = std::string("--Ice.Config=" +  strs[0] + "_laser.cfg");
-				pthread_t thr_gui;
-				pthread_create(&thr_gui, NULL, &mainLaser, (void*)this);
-			}
-		
-			physics::MultiRayShapePtr laser = this->parentSensor->GetLaserShape();
-
-			pthread_mutex_lock (&mutex); 
-			laserValues.resize(laser->GetSampleCount ());
-			for (int i = 0; i< laser->GetSampleCount (); i++){
-				laserValues[i] = laser->GetRange(i);
-			}
-			pthread_mutex_unlock (&mutex); 
-		}
-		sensors::RaySensorPtr parentSensor;
-		int count;
-		std::string nameLaser;
-		std::vector<float> laserValues;
-		pthread_mutex_t mutex;
-  	};
-
-  // Register this plugins with the simulator
-  GZ_REGISTER_SENSOR_PLUGIN(LaserDump)
-}
-
-class LaserI: virtual public jderobot::Laser {
-	public:
-		LaserI (gazebo::LaserDump* laser) 
-		{
-			this->laser = laser;
-		}
-
-		virtual ~LaserI(){};
-
-		virtual jderobot::LaserDataPtr getLaserData(const Ice::Current&) {
-		    jderobot::LaserDataPtr laserData (new jderobot::LaserData());
-			pthread_mutex_lock (&laser->mutex); 
-			laserData->numLaser = laser->laserValues.size();
-			laserData->distanceData.resize(sizeof(int)*laserData->numLaser);
-			
-			//Update laser values
-			for(int i = 0 ; i < laserData->numLaser; i++){
-			   laserData->distanceData[i] = laser->laserValues[i]*1000;
-			}
-			pthread_mutex_unlock (&laser->mutex); 
-			return laserData;
-		};
-
-	private:
-		int laser_num_readings;
-		gazebo::LaserDump* laser;
-};
-
-void *mainLaser(void* v) 
-{
-
-	gazebo::LaserDump* laser = (gazebo::LaserDump*)v;
-	
-	char* name = (char*)laser->nameLaser.c_str();
-
-    Ice::CommunicatorPtr ic;
-    int argc = 1;
-
-    Ice::PropertiesPtr prop;
-	char* argv[] = {name};
-    try {
-        
-        ic = Ice::initialize(argc, argv);
-        prop = ic->getProperties();
-        
-        std::string Endpoints = prop->getProperty("Laser.Endpoints");
-        std::cout << "Laser Endpoints > " << Endpoints << std::endl;
-        
-        Ice::ObjectAdapterPtr adapter =
-            ic->createObjectAdapterWithEndpoints("Laser", Endpoints);
-        Ice::ObjectPtr object = new LaserI(laser);
-        adapter->add(object, ic->stringToIdentity("Laser"));
-        adapter->activate();
-        ic->waitForShutdown();
-    } catch (const Ice::Exception& e) {
-        std::cerr << e << std::endl;
-    } catch (const char* msg) {
-        std::cerr << msg << std::endl;
-    }
-    if (ic) {
-        try {
-            ic->destroy();
-        } catch (const Ice::Exception& e) {
-            std::cerr << e << std::endl;
-        }
-    }
-
-}

Deleted: trunk/src/stable/components/gazeboserver/motors.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/motors.cc	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/motors.cc	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,238 +0,0 @@
-#include "motors.h"
-
-enum {
-    RIGHT,
-    LEFT
-};
-
-
-
-namespace gazebo {
-
-    //GLOBAL DATA
-    
-
-    void *motorsICE(void* v);
-
-    GZ_REGISTER_MODEL_PLUGIN(Motors)
-
-    Motors::Motors() {
-        pthread_mutex_init(&mutex, NULL);
-        pthread_mutex_init(&mutexMotor, NULL);
-        count = 0;
-        std::cout << "constructor Motors" << std::endl;
-        this->wheelSpeed[LEFT] = this->wheelSpeed[RIGHT] = 0;
-    }
-
-    void Motors::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
-        // Get a pointer to the model
-        this->model = _model;
-
-        this->node = transport::NodePtr(new transport::Node());
-        this->node->Init(this->model->GetWorld()->GetName());
-
-        //this->velSub = this->node->Subscribe(std::string("~/") + this->model->GetName() + "/vel_cmd", &Motors::OnVelMsg, this);
-        if (!_sdf->HasElement("left_joint"))
-            gzerr << "Motors plugin missing <left_joint> element\n";
-        if (!_sdf->HasElement("right_joint"))
-            gzerr << "DiffDrive plugin missing <right_joint> element\n";
-
-        this->leftJoint = _model->GetJoint(
-                _sdf->GetElement("left_joint")->GetValueString());
-        this->rightJoint = _model->GetJoint(
-                _sdf->GetElement("right_joint")->GetValueString());
-
-        if (_sdf->HasElement("torque"))
-            this->torque = _sdf->GetElement("torque")->GetValueDouble();
-        else {
-            gzwarn << "No torque value set for the DiffDrive plugin.\n";
-            this->torque = 5.0;
-        }
-        if (!this->leftJoint)
-            gzerr << "Unable to find left joint["
-                << _sdf->GetElement("left_joint")->GetValueString() << "]\n";
-        if (!this->rightJoint)
-            gzerr << "Unable to find right joint["
-                << _sdf->GetElement("right_joint")->GetValueString() << "]\n";
-
-
-
-        // Listen to the update event. This event is broadcast every
-        // simulation iteration.
-        this->updateConnection = event::Events::ConnectWorldUpdateStart(
-                boost::bind(&Motors::OnUpdate, this));
-    }
-
-    // Called by the world update start event
-
-    void Motors::Init() {
-        this->wheelSeparation = this->leftJoint->GetAnchor(0).Distance(this->rightJoint->GetAnchor(0));
-        std::cout << "Wheel Separation:" << this->wheelSeparation << std::endl;
-        physics::EntityPtr parent = boost::shared_dynamic_cast<physics::Entity > (this->leftJoint->GetChild());
-
-        math::Box bb = parent->GetBoundingBox();
-
-        this->wheelRadius = bb.GetSize().GetMax() * 0.5;
-        std::cout << "Wheel Diameter:" << this->wheelRadius * 2 << std::endl;
-    }
-
-    /*
-    void Motors::OnVelMsg(ConstPosePtr& _msg) {
-
-        double vr, va;
-
-        vr = _msg->position().x() + 5;
-        std::cout << "vr:" << vr << std::endl;
-        va = msgs::Convert(_msg->orientation()).GetAsEuler().z + 3;
-
-        this->wheelSpeed[LEFT] = vr + va * this->wheelSeparation / 2.0;
-        this->wheelSpeed[RIGHT] = vr - va * this->wheelSeparation / 2.0;
-
-
-
-    }
-    */
-    
-    void Motors::OnUpdate() {
-
-        if (count == 0) {
-        
-	        robotMotors.v = 0;
-	        robotMotors.w = 0;
-        
-            count++;
-            std::string name = this->model->GetName();
-            std::cout << "motors name " << name << std::endl;
-            nameMotors = std::string("--Ice.Config=" + name +"Motors.cfg");
-            pthread_t thr_gui;
-            pthread_create(&thr_gui, NULL, &motorsICE, (void*) this);
-        }
-
-        double vr, va; //vr -> velocidad lineal; va -> velocidad angular
-
-        pthread_mutex_lock(&mutex);
-
-        vr = robotMotors.v/100;
-
-        va = robotMotors.w/10;
-
-        pthread_mutex_unlock(&mutex);
-
-
-        this->wheelSpeed[LEFT] = vr + va * this->wheelSeparation / 2.0;
-        this->wheelSpeed[RIGHT] = vr - va * this->wheelSeparation / 2.0;
-
-        double leftVelDesired = (this->wheelSpeed[LEFT] / this->wheelRadius);
-        double rightVelDesired = (this->wheelSpeed[RIGHT] / this->wheelRadius);
-/*
-		std::cout << "leftVelDesired " << leftVelDesired << std::endl;
-		std::cout << "rightVelDesired " << rightVelDesired << std::endl;
-		std::cout << "torque " << torque << std::endl;
-*/
-        this->leftJoint->SetVelocity(0, leftVelDesired);
-        this->rightJoint->SetVelocity(0, rightVelDesired);
-
-        this->leftJoint->SetMaxForce(0, this->torque);
-        this->rightJoint->SetMaxForce(0, this->torque);
-    }
-
-    class MotorsI : virtual public jderobot::Motors {
-    public:
-
-        MotorsI(gazebo::Motors* pose) {
-            this->pose = pose;
-        }
-
-        virtual ~MotorsI() {
-        };
-
-        virtual float getV(const Ice::Current&) {
-
-            float v_return;
-            pthread_mutex_lock(&pose->mutexMotor);
-            //v_return = pose->vel;
-            v_return = pose->robotMotors.v;
-            pthread_mutex_unlock(&pose->mutexMotor);
-            return v_return;
-        };
-
-        virtual float getW(const Ice::Current&) {
-            float w_return;
-            pthread_mutex_lock(&pose->mutexMotor);
-            //w_return = pose->w;
-            w_return = pose->robotMotors.w;
-            pthread_mutex_unlock(&pose->mutexMotor);
-            return w_return;
-        };
-
-        virtual float getL(const Ice::Current&) {
-            return 0.;
-        };
-
-        virtual Ice::Int setV(Ice::Float v, const Ice::Current&) {
-            pthread_mutex_lock(&pose->mutexMotor);
-            //pose->vel = v;
-            pose->robotMotors.v = v;
-            pthread_mutex_unlock(&pose->mutexMotor);
-            return 0;
-        };
-
-        virtual Ice::Int setW(Ice::Float _w, const Ice::Current&) {
-            pthread_mutex_lock(&pose->mutexMotor);
-            //pose->w = _w;
-            pose->robotMotors.w = -_w;
-            pthread_mutex_unlock(&pose->mutexMotor);
-            return 0;
-        };
-
-        virtual Ice::Int setL(Ice::Float l, const Ice::Current&) {
-            return 0;
-        };
-
-    public:
-        gazebo::Motors* pose;
-    }; // end class MotorsI
-
-
-    void *motorsICE(void* v) {
-
-        gazebo::Motors* base = (gazebo::Motors*)v;
-
-        Ice::CommunicatorPtr ic;
-        int argc = 1;
-        char* name = (char*) base->nameMotors.c_str();
-        Ice::PropertiesPtr prop;
-        char* argv[] = {name};
-
-        try {
-
-            ic = Ice::initialize(argc, argv);
-
-
-            prop = ic->getProperties();
-            std::string Endpoints = prop->getProperty("Motors.Endpoints");
-            std::cout << "Motors Endpoints > " << Endpoints << std::endl;
-
-            Ice::ObjectAdapterPtr adapter =
-                    ic->createObjectAdapterWithEndpoints("Motors", Endpoints);
-            Ice::ObjectPtr object = new MotorsI(base);
-            
-            adapter->add(object, ic->stringToIdentity("Motors"));
-            
-            adapter->activate();
-            ic->waitForShutdown();
-        } catch (const Ice::Exception& e) {
-            std::cerr << e << std::endl;
-        } catch (const char* msg) {
-            std::cerr << msg << std::endl;
-        }
-        if (ic) {
-            try {
-                ic->destroy();
-            } catch (const Ice::Exception& e) {
-                std::cerr << e << std::endl;
-            }
-        }
-
-    }
-}

Deleted: trunk/src/stable/components/gazeboserver/motors.h
===================================================================
--- trunk/src/stable/components/gazeboserver/motors.h	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/motors.h	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,73 +0,0 @@
-#include <boost/bind.hpp>
-#include "gazebo.hh"
-#include "physics/physics.hh"
-#include "common/common.hh"
-#include "transport/transport.hh"
-
-#include "plugins/RayPlugin.hh"
-
-// ICE utils includes
-#include <Ice/Ice.h>
-#include <IceUtil/IceUtil.h>
-
-
-// JDErobot general ice component includes
-#include <jderobot/motors.h>
-
-
-#ifndef MOTORS_H
-#define	MOTORS_H
-
-namespace gazebo {
-
-    class Motors : public ModelPlugin {
-    public:
-
-        Motors();
-        virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
-        virtual void Init();
-
-        std::string nameMotors;
-        float vel, w;
-        pthread_mutex_t mutex;
-        pthread_mutex_t mutexMotor;
-
-        struct motor_t {
-            float v;
-            float w;
-            float l;
-        };
-
-
-        motor_t robotMotors;
-
-    private:
-        void OnUpdate();
-        //void OnVelMsg(ConstPosePtr &_msg);
-        transport::NodePtr node;
-        transport::SubscriberPtr velSub;
-
-        physics::ModelPtr model;
-        physics::JointPtr leftJoint, rightJoint;
-        event::ConnectionPtr updateConnection;
-        double wheelSpeed[2];
-        double torque;
-        double wheelSeparation;
-        double wheelRadius;
-        common::Time prevUpdateTime;
-
-        physics::LinkPtr link, leftWheelLink, rightWheelLink;
-        double sum;
-
-        int count;
-
-        //event::ConnectionPtr updateConnection; Alex
-        //physics::ModelPtr model; Alex
-    };
-}
-
-
-
-
-#endif	/* POSE_H */
-

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dx2Encoders.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dx2Encoders.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dx2Encoders.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,3 +0,0 @@
-#without registry
-Encoders.Endpoints=default -h localhost -p 8997
-

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dx2Motors.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dx2Motors.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dx2Motors.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,3 +0,0 @@
-#without registry
-Motors.Endpoints=default -h localhost -p 8999
-

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dx2_cam_sensor_left.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dx2_cam_sensor_left.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dx2_cam_sensor_left.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,8 +0,0 @@
-#without registry
-CameraGazebo.Endpoints=default -h localhost -p 8995
-
-#camera 1
-CameraGazebo.Camera.0.Name=cameraA
-CameraGazebo.Camera.0.ImageWidth=320
-CameraGazebo.Camera.0.ImageHeight=240
-CameraGazebo.Camera.0.Format=RGB8

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dx2_cam_sensor_right.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dx2_cam_sensor_right.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dx2_cam_sensor_right.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,8 +0,0 @@
-#without registry
-CameraGazebo.Endpoints=default -h localhost -p 8994
-
-#camera 1
-CameraGazebo.Camera.0.Name=cameraA
-CameraGazebo.Camera.0.ImageWidth=320
-CameraGazebo.Camera.0.ImageHeight=240
-CameraGazebo.Camera.0.Format=RGB8

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dx2_laser.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dx2_laser.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dx2_laser.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,2 +0,0 @@
-#without registry
-Laser.Endpoints=default -h localhost -p 8998

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dx2_pose3dencoders.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dx2_pose3dencoders.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dx2_pose3dencoders.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,5 +0,0 @@
-#without registry
-Pose3DEncoders1.Endpoints=default -h localhost -p 8993
-Pose3DEncoders2.Endpoints=default -h localhost -p 8992
-Pose3DMotors1.Endpoints=default -h localhost -p 8991
-Pose3DMotors2.Endpoints=default -h localhost -p 8990

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dxEncoders.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dxEncoders.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dxEncoders.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,3 +0,0 @@
-#without registry
-Encoders.Endpoints=default -h localhost -p 9997
-

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dxJde.world
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dxJde.world	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dxJde.world	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,32 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.3">
-  <world name="default">
-    <!-- Ground -->
-    <include>
-      <uri>model://ground_plane</uri>
-    </include>
-    
-    <!-- A global light source -->
-    <include>
-      <uri>model://sun</uri>
-    </include>
-
-    <!-- Pioneer2dx model -->
-    <include>
-      <uri>model://pioneer2dxJde</uri>
-      <name>pioneer2dx</name>
-    </include>
-    
-        <!-- Pioneer2dx model
-    <include>
-<!--      <uri>model://pioneer2dxJde</uri>
-      <name>pioneer2dx2</name>
-      <pose>2 0 0 0 0 0</pose>
-    </include> -->
-    
-<!--    <include>
-      <uri>model://simpleWorld</uri>
-    </include> -->
-
-  </world>
-</sdf>

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dxMotors.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dxMotors.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dxMotors.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,3 +0,0 @@
-#without registry
-Motors.Endpoints=default -h localhost -p 9999
-

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dx_cam_sensor_left.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dx_cam_sensor_left.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dx_cam_sensor_left.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,8 +0,0 @@
-#without registry
-CameraGazebo.Endpoints=default -h localhost -p 9995
-
-#camera 1
-CameraGazebo.Camera.0.Name=cameraA
-CameraGazebo.Camera.0.ImageWidth=320
-CameraGazebo.Camera.0.ImageHeight=240
-CameraGazebo.Camera.0.Format=RGB8

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dx_cam_sensor_right.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dx_cam_sensor_right.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dx_cam_sensor_right.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,8 +0,0 @@
-#without registry
-CameraGazebo.Endpoints=default -h localhost -p 9994
-
-#camera 1
-CameraGazebo.Camera.0.Name=cameraA
-CameraGazebo.Camera.0.ImageWidth=320
-CameraGazebo.Camera.0.ImageHeight=240
-CameraGazebo.Camera.0.Format=RGB8

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dx_laser.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dx_laser.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dx_laser.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,2 +0,0 @@
-#without registry
-Laser.Endpoints=default -h localhost -p 9996

Deleted: trunk/src/stable/components/gazeboserver/pioneer2dx_pose3dencoders.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pioneer2dx_pose3dencoders.cfg	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pioneer2dx_pose3dencoders.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,5 +0,0 @@
-#without registry
-Pose3DEncoders1.Endpoints=default -h localhost -p 9993
-Pose3DEncoders2.Endpoints=default -h localhost -p 9992
-Pose3DMotors1.Endpoints=default -h localhost -p 9991
-Pose3DMotors2.Endpoints=default -h localhost -p 9990

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/CMakeLists.txt	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/CMakeLists.txt	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,5 +1,3 @@
-if (GAZEBO_INCLUDE_DIRS)
-
 include_directories(
 	${GAZEBO_INCLUDE_DIRS}
     ${INTERFACES_CPP_DIR}
@@ -51,5 +49,3 @@
 	${GAZEBO_libraries} 
 	${ZeroCIce_LIBRARIES}
 )
-
-ENDIF()

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/CMakeLists.txt (from rev 1055, trunk/src/stable/components/gazeboserver/CMakeLists.txt)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/CMakeLists.txt	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/CMakeLists.txt	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,47 @@
+include_directories(
+	${GAZEBO_INCLUDE_DIRS}
+    ${INTERFACES_CPP_DIR}
+    ${LIBS_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR})
+
+link_directories(${GAZEBO_LIBRARY_DIRS})
+#boost_system 
+add_library(motors SHARED motors.cc)
+target_link_libraries(motors 
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES} 
+
+    colorspacesmm
+    JderobotInterfaces) 
+    
+add_library(encoders SHARED encoders.cc)
+target_link_libraries(encoders 
+	${GAZEBO_libraries} 
+
+	${ZeroCIce_LIBRARIES} 
+    colorspacesmm
+    JderobotInterfaces)    
+
+add_library(laser SHARED laser.cc)
+target_link_libraries(laser 
+	RayPlugin 
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES} 
+    colorspacesmm
+    JderobotInterfaces) 
+
+add_library(camera_dump SHARED camera_dump.cc)
+target_link_libraries(camera_dump ${GAZEBO_libraries} 
+	CameraPlugin 	
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES} 
+    colorspacesmm
+    JderobotInterfaces)
+    
+add_library(pose3dencoders SHARED pose3dencoders.cc)
+target_link_libraries(pose3dencoders ${GAZEBO_libraries} 	 	
+	${GAZEBO_libraries} 
+
+	${ZeroCIce_LIBRARIES} 
+    colorspacesmm
+    JderobotInterfaces)

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/camera_dump.cc (from rev 1055, trunk/src/stable/components/gazeboserver/camera_dump.cc)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/camera_dump.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/camera_dump.cc	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,280 @@
+#include "gazebo.hh"
+#include "plugins/CameraPlugin.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+#include <jderobot/camera.h>
+
+#include <visionlib/colorspaces/colorspacesmm.h>
+
+#include <iostream>
+
+#include <boost/algorithm/string.hpp>
+#include <boost/lexical_cast.hpp>
+
+void *myMain(void* v);
+
+namespace gazebo
+{   
+  class CameraDump : public CameraPlugin
+  { 
+    public: CameraDump() : CameraPlugin(),count(0) 
+	{
+		pthread_mutex_init (&mutex, NULL);
+		n = round(rand()*1000);
+	}
+
+    public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
+    {
+      // Don't forget to load the camera plugin
+      CameraPlugin::Load(_parent,_sdf);
+      std::cout << "Load: " <<n << " " <<  this->parentSensor->GetCamera()->GetName()<< std::endl;
+    } 
+
+    // Update the controller
+    public: void OnNewFrame(const unsigned char *_image, 
+        unsigned int _width, unsigned int _height, unsigned int _depth, 
+        const std::string &_format)
+    {
+      //std::cout << "OnNewFrame: " <<n << " " <<  this->parentSensor->GetCamera()->GetName()<< std::endl;
+		if(count==0){
+			std::vector<std::string> tokens;
+			nameCamera = this->parentSensor->GetCamera()->GetName();
+  			boost::split(tokens, nameCamera, boost::is_any_of("::"));
+  			boost::split(tokens, tokens[2], boost::is_any_of("("));
+			nameGlobal = tokens[0];
+			// El nombre del fichero de configuracion tiene que coincidir con el de la cámara en el .world y el .cfg 
+			nameCamera = std::string("--Ice.Config=" + tokens[0] + ".cfg"); 
+			
+			if (count == 0){
+				pthread_t thr_gui;
+				pthread_create(&thr_gui, NULL, &myMain, (void*)this);
+			}
+
+			image.create(_height, _width, CV_8UC3);
+			count++;
+		}
+		pthread_mutex_lock (&mutex);
+        memcpy((unsigned char *) image.data, &(_image[0]), _width*_height * 3);
+		
+		//cv::imshow(nameCamera, image);
+		//cv::waitKey(10);
+					
+		pthread_mutex_unlock (&mutex);
+		
+    }         
+
+    private: int count;
+    private: int n;
+    public: std::string nameCamera;
+    public: cv::Mat image;
+	public: pthread_mutex_t mutex;
+	public: std::string nameGlobal;
+  };
+
+  // Register this plugin with the simulator
+  GZ_REGISTER_SENSOR_PLUGIN(CameraDump)
+}
+
+class CameraI: virtual public jderobot::Camera {
+	public:
+		CameraI(std::string propertyPrefix, gazebo::CameraDump* camera)
+			   : prefix(propertyPrefix), cameraI(camera) {
+		
+			std::cout << "Constructor CameraI" << std::endl;
+
+			imageDescription = (new jderobot::ImageDescription());
+
+        	replyTask = new ReplyTask(this);
+		    replyTask->start(); // my own thread
+		  
+		}
+
+		std::string getName () {
+			return (cameraDescription->name);
+		}
+
+		std::string getRobotName () {
+			return "RobotName";
+		}
+
+		virtual ~CameraI() {
+
+
+		}
+
+		virtual jderobot::ImageDescriptionPtr getImageDescription(const Ice::Current& c){
+			return imageDescription;
+		}
+
+		virtual jderobot::CameraDescriptionPtr getCameraDescription(const Ice::Current& c){
+			return cameraDescription;
+		}
+
+		virtual Ice::Int setCameraDescription(const jderobot::CameraDescriptionPtr &description, const Ice::Current& c) {
+			return 0;
+		}
+
+		virtual void getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr& cb,const Ice::Current& c){
+			replyTask->pushJob(cb);
+		}
+
+		virtual std::string startCameraStreaming(const Ice::Current&){
+
+		}
+
+		virtual void stopCameraStreaming(const Ice::Current&) {
+
+		}
+
+	private:
+		class ReplyTask: public IceUtil::Thread {
+			public:
+				ReplyTask(CameraI* camera){
+                    mycamera = camera;
+
+				   	std::cout << "safeThread" << std::endl;
+				}
+
+				void pushJob(const jderobot::AMD_ImageProvider_getImageDataPtr& cb){
+					IceUtil::Mutex::Lock sync(requestsMutex);
+					requests.push_back(cb);
+				}
+
+				virtual void run(){
+					jderobot::ImageDataPtr reply(new jderobot::ImageData);
+					struct timeval a, b;
+					int cycle = 48;
+					long totalb,totala;
+					long diff;
+					
+					int count =0 ;
+
+					while(1){
+						
+						if(!mycamera->cameraI->image.data){
+							usleep(100);
+							continue;
+						}
+						if(count==0){
+							pthread_mutex_lock (&mycamera->cameraI->mutex);
+							mycamera->imageDescription->width = mycamera->cameraI->image.cols;
+							mycamera->imageDescription->height = mycamera->cameraI->image.rows;
+							mycamera->imageDescription->size = mycamera->cameraI->image.cols*mycamera->cameraI->image.rows*3;
+							pthread_mutex_unlock (&mycamera->cameraI->mutex);
+
+							mycamera->imageDescription->format = "RGB8";
+
+							reply->description = mycamera->imageDescription;
+							count++;
+						}
+
+						//std::cout << nameGlobal<< std::endl;
+									
+						gettimeofday(&a,NULL);
+						totala=a.tv_sec*1000000+a.tv_usec;
+
+
+						IceUtil::Time t = IceUtil::Time::now();
+						reply->timeStamp.seconds = (long)t.toSeconds();
+						reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
+          				
+          				pthread_mutex_lock (&mycamera->cameraI->mutex);
+					    reply->pixelData.resize(mycamera->cameraI->image.rows*mycamera->cameraI->image.cols*3);
+					    
+					    memcpy( &(reply->pixelData[0]), (unsigned char *) mycamera->cameraI->image.data, mycamera->cameraI->image.rows*mycamera->cameraI->image.cols*3);
+						pthread_mutex_unlock (&mycamera->cameraI->mutex);
+
+					   { //critical region start
+						   IceUtil::Mutex::Lock sync(requestsMutex);
+						   while(!requests.empty()) {
+							   jderobot::AMD_ImageProvider_getImageDataPtr cb = requests.front();
+							   requests.pop_front();
+							   cb->ice_response(reply);
+						   }
+					   } //critical region end
+                  
+						gettimeofday(&b,NULL);
+						totalb=b.tv_sec*1000000+b.tv_usec;
+
+						diff = (totalb-totala)/1000;
+						diff = cycle-diff;
+
+						//std::cout << "Gazeboserver takes " << diff << " ms " << mycamera->fileName << std::endl;
+
+						if(diff < 33)
+							diff = 33;
+
+
+						/*Sleep Algorithm*/
+						usleep(diff*1000);
+					}
+				}
+
+				CameraI* mycamera;
+				IceUtil::Mutex requestsMutex;
+				std::list<jderobot::AMD_ImageProvider_getImageDataPtr> requests;
+		};
+
+		typedef IceUtil::Handle<ReplyTask> ReplyTaskPtr;
+		std::string prefix;
+
+		colorspaces::Image::FormatPtr imageFmt;
+		jderobot::ImageDescriptionPtr imageDescription;
+		jderobot::CameraDescriptionPtr cameraDescription;
+		ReplyTaskPtr replyTask;
+		gazebo::CameraDump* cameraI;	
+		
+}; // end class CameraI
+
+
+void *myMain(void* v)
+{
+
+	gazebo::CameraDump* camera = (gazebo::CameraDump*)v;
+
+	char* name = (char*)camera->nameCamera.c_str();
+
+    Ice::CommunicatorPtr ic;
+    int argc = 1;
+
+    Ice::PropertiesPtr prop;
+	char* argv[] = {name};
+
+    try {
+        
+        ic = Ice::initialize(argc, argv);
+        prop = ic->getProperties();
+        
+        std::string Endpoints = prop->getProperty("CameraGazebo.Endpoints");
+        std::cout << "CameraGazebo "<< camera->nameGlobal <<" Endpoints > "  << Endpoints << std::endl;
+        
+        Ice::ObjectAdapterPtr adapter =
+        ic->createObjectAdapterWithEndpoints("CameraGazebo", Endpoints);
+		
+        Ice::ObjectPtr object = new CameraI(std::string("CameraGazebo"),  camera);
+        adapter->add(object, ic->stringToIdentity(camera->nameGlobal));
+        adapter->activate();
+        ic->waitForShutdown();
+    } catch (const Ice::Exception& e) {
+        std::cerr << e << std::endl;
+    } catch (const char* msg) {
+        std::cerr << msg << std::endl;
+    }
+    if (ic) {
+        try {
+            ic->destroy();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        }
+    }
+    
+}

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/encoders.cc (from rev 1055, trunk/src/stable/components/gazeboserver/encoders.cc)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/encoders.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/encoders.cc	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,145 @@
+#include "encoders.h"
+
+namespace gazebo {
+
+    void *encodersICE(void* v);
+
+    GZ_REGISTER_MODEL_PLUGIN(Encoders)
+
+    Encoders::Encoders() {
+        pthread_mutex_init(&mutex, NULL);
+        pthread_mutex_init(&mutexEncoders, NULL);
+        count = 0;
+        std::cout << "constructor Encoders" << std::endl;
+    }
+
+    void Encoders::Load(physics::ModelPtr _parent, sdf::ElementPtr) {
+
+        model = _parent;
+        this->updateConnection = event::Events::ConnectWorldUpdateStart(
+                boost::bind(&Encoders::OnUpdate, this));
+    }
+    
+    physics::ModelPtr Encoders::getModel()
+    {
+    	return model;
+    }
+
+    void Encoders::OnUpdate() {
+
+        if (count == 0) {
+            count++;
+            std::string name = this->model->GetName();
+            std::cout << "GetName() encoders: " << name << std::endl;
+            nameEncoders = std::string("--Ice.Config=" + name + "Encoders.cfg");
+            pthread_t thr_gui;
+            pthread_create(&thr_gui, NULL, &encodersICE, (void*) this);
+        }
+
+        position = model->GetWorldPose();
+        this->initial_q = position.rot;
+
+        math::Vector3 initial_rpy = initial_q.GetAsEuler();
+
+        double degrees = initial_rpy.z * 180.0 / M_PI;
+        if (degrees < 0) {
+            degrees = 360 + degrees;
+        }
+
+
+        pthread_mutex_lock(&mutex);
+        robotEncoders.x = position.pos.x;
+        robotEncoders.y = position.pos.y;
+        robotEncoders.theta = degrees;
+        pthread_mutex_unlock(&mutex);
+
+    }
+
+    class EncodersI : virtual public jderobot::Encoders {
+    public:
+
+        EncodersI(gazebo::Encoders* pose) : encodersData(new jderobot::EncodersData()) {
+            this->pose = pose;
+        }
+
+        virtual ~EncodersI() {
+        };
+        
+        virtual void setEncodersData(const jderobot::EncodersDataPtr&  encodersData,
+        						     const Ice::Current&) {
+             math::Pose position = this->pose->getModel()->GetWorldPose();
+             
+             position.Set(encodersData->robotx,
+             			  encodersData->roboty,
+             			  encodersData->robotcos,
+             			  0,
+             			  0,
+             			  encodersData->robottheta);
+        	this->pose->getModel()->SetWorldPose(position); 
+			//this->getModel();
+        }
+
+
+        virtual jderobot::EncodersDataPtr getEncodersData(const Ice::Current&) {
+            pthread_mutex_lock(&pose->mutex);
+
+            //std::cout << "theta: " << pose->robotEncoders.theta << std::endl;
+
+            encodersData->robotx = pose->robotEncoders.x * 1000;
+            encodersData->roboty = pose->robotEncoders.y * 1000;
+            encodersData->robottheta = pose->robotEncoders.theta;
+            encodersData->robotcos = cos(pose->robotEncoders.theta);
+            encodersData->robotsin = sin(pose->robotEncoders.theta);
+
+            pthread_mutex_unlock(&pose->mutex);
+
+            return encodersData;
+        };
+
+    public:
+        gazebo::Encoders* pose;
+    private:
+        jderobot::EncodersDataPtr encodersData;
+    };
+
+    void *encodersICE(void* v) {
+        gazebo::Encoders* base = (gazebo::Encoders*)v;
+        char* name = (char*) base->nameEncoders.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+
+            ic = Ice::initialize(argc, argv);
+
+
+            prop = ic->getProperties();
+            std::string Endpoints = prop->getProperty("Encoders.Endpoints");
+            std::cout << "Encoders Endpoints > " << Endpoints << std::endl;
+
+            Ice::ObjectAdapterPtr adapter =
+                    ic->createObjectAdapterWithEndpoints("Encoders", Endpoints);
+            Ice::ObjectPtr object = new EncodersI(base);
+
+            adapter->add(object, ic->stringToIdentity("Encoders"));
+
+            adapter->activate();
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+
+    }
+
+}

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/encoders.h (from rev 1055, trunk/src/stable/components/gazeboserver/encoders.h)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/encoders.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/encoders.h	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,57 @@
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+
+// JDErobot general ice component includes
+#include <jderobot/motors.h>
+#include <jderobot/encoders.h>
+
+#ifndef ENCODERS_H
+#define	ENCODERS_H
+
+namespace gazebo {
+    
+    
+    class Encoders : public ModelPlugin {
+    public:
+        
+        Encoders();
+        
+        virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
+        physics::ModelPtr getModel();
+        
+        pthread_mutex_t mutex;
+        pthread_mutex_t mutexEncoders;
+        int count;
+        struct encoders_t {
+            float x;
+            float y;
+            float theta;
+            float cos;
+            float sin;
+        };
+        math::Quaternion initial_q;
+        encoders_t robotEncoders;
+        std::string nameEncoders;
+        
+    private:
+        
+        void OnUpdate();
+        physics::ModelPtr model;
+        math::Pose position;
+        event::ConnectionPtr updateConnection;
+        
+        
+    };
+    
+}
+
+#endif	/* POSE_H */

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/laser.cc (from rev 1055, trunk/src/stable/components/gazeboserver/laser.cc)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/laser.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/laser.cc	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,146 @@
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "physics/MultiRayShape.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+#include "plugins/RayPlugin.hh"
+
+#include <boost/algorithm/string.hpp>
+
+
+#include <iostream>
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+#include <jderobot/laser.h>
+
+void *mainLaser(void* v);
+
+namespace gazebo
+{     
+    class LaserDump : public RayPlugin
+  	{ 
+  	
+		public: LaserDump() : RayPlugin()
+		{
+			std::cout << "LaserDump Constructor" <<std::endl;
+			count = 0;
+			pthread_mutex_init (&mutex, NULL);	
+		}
+		
+	    public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
+		{
+		  // Don't forget to load the camera plugin
+		  RayPlugin::Load(_parent,_sdf);
+		  this->parentSensor =  boost::shared_dynamic_cast<sensors::RaySensor>(_parent);
+		  
+		} 
+
+		// Update the controller
+		public: void OnNewLaserScans()
+		{
+			if(count == 0){
+				count++;
+				std::string name = this->parentSensor->GetParentName();
+				std::cout <<" laser: " << name  << std::endl;
+				
+				std::vector<std::string> strs;
+				boost::split(strs, name, boost::is_any_of("::"));
+
+    			nameLaser = std::string("--Ice.Config=" +  strs[0] + "_laser.cfg");
+				pthread_t thr_gui;
+				pthread_create(&thr_gui, NULL, &mainLaser, (void*)this);
+			}
+		
+			physics::MultiRayShapePtr laser = this->parentSensor->GetLaserShape();
+
+			pthread_mutex_lock (&mutex); 
+			laserValues.resize(laser->GetSampleCount ());
+			for (int i = 0; i< laser->GetSampleCount (); i++){
+				laserValues[i] = laser->GetRange(i);
+			}
+			pthread_mutex_unlock (&mutex); 
+		}
+		sensors::RaySensorPtr parentSensor;
+		int count;
+		std::string nameLaser;
+		std::vector<float> laserValues;
+		pthread_mutex_t mutex;
+  	};
+
+  // Register this plugins with the simulator
+  GZ_REGISTER_SENSOR_PLUGIN(LaserDump)
+}
+
+class LaserI: virtual public jderobot::Laser {
+	public:
+		LaserI (gazebo::LaserDump* laser) 
+		{
+			this->laser = laser;
+		}
+
+		virtual ~LaserI(){};
+
+		virtual jderobot::LaserDataPtr getLaserData(const Ice::Current&) {
+		    jderobot::LaserDataPtr laserData (new jderobot::LaserData());
+			pthread_mutex_lock (&laser->mutex); 
+			laserData->numLaser = laser->laserValues.size();
+			laserData->distanceData.resize(sizeof(int)*laserData->numLaser);
+			
+			//Update laser values
+			for(int i = 0 ; i < laserData->numLaser; i++){
+			   laserData->distanceData[i] = laser->laserValues[i]*1000;
+			}
+			pthread_mutex_unlock (&laser->mutex); 
+			return laserData;
+		};
+
+	private:
+		int laser_num_readings;
+		gazebo::LaserDump* laser;
+};
+
+void *mainLaser(void* v) 
+{
+
+	gazebo::LaserDump* laser = (gazebo::LaserDump*)v;
+	
+	char* name = (char*)laser->nameLaser.c_str();
+
+    Ice::CommunicatorPtr ic;
+    int argc = 1;
+
+    Ice::PropertiesPtr prop;
+	char* argv[] = {name};
+    try {
+        
+        ic = Ice::initialize(argc, argv);
+        prop = ic->getProperties();
+        
+        std::string Endpoints = prop->getProperty("Laser.Endpoints");
+        std::cout << "Laser Endpoints > " << Endpoints << std::endl;
+        
+        Ice::ObjectAdapterPtr adapter =
+            ic->createObjectAdapterWithEndpoints("Laser", Endpoints);
+        Ice::ObjectPtr object = new LaserI(laser);
+        adapter->add(object, ic->stringToIdentity("Laser"));
+        adapter->activate();
+        ic->waitForShutdown();
+    } catch (const Ice::Exception& e) {
+        std::cerr << e << std::endl;
+    } catch (const char* msg) {
+        std::cerr << msg << std::endl;
+    }
+    if (ic) {
+        try {
+            ic->destroy();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        }
+    }
+
+}

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/motors.cc (from rev 1055, trunk/src/stable/components/gazeboserver/motors.cc)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/motors.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/motors.cc	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,238 @@
+#include "motors.h"
+
+enum {
+    RIGHT,
+    LEFT
+};
+
+
+
+namespace gazebo {
+
+    //GLOBAL DATA
+    
+
+    void *motorsICE(void* v);
+
+    GZ_REGISTER_MODEL_PLUGIN(Motors)
+
+    Motors::Motors() {
+        pthread_mutex_init(&mutex, NULL);
+        pthread_mutex_init(&mutexMotor, NULL);
+        count = 0;
+        std::cout << "constructor Motors" << std::endl;
+        this->wheelSpeed[LEFT] = this->wheelSpeed[RIGHT] = 0;
+    }
+
+    void Motors::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
+        // Get a pointer to the model
+        this->model = _model;
+
+        this->node = transport::NodePtr(new transport::Node());
+        this->node->Init(this->model->GetWorld()->GetName());
+
+        //this->velSub = this->node->Subscribe(std::string("~/") + this->model->GetName() + "/vel_cmd", &Motors::OnVelMsg, this);
+        if (!_sdf->HasElement("left_joint"))
+            gzerr << "Motors plugin missing <left_joint> element\n";
+        if (!_sdf->HasElement("right_joint"))
+            gzerr << "DiffDrive plugin missing <right_joint> element\n";
+
+        this->leftJoint = _model->GetJoint(
+                _sdf->GetElement("left_joint")->GetValueString());
+        this->rightJoint = _model->GetJoint(
+                _sdf->GetElement("right_joint")->GetValueString());
+
+        if (_sdf->HasElement("torque"))
+            this->torque = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            this->torque = 5.0;
+        }
+        if (!this->leftJoint)
+            gzerr << "Unable to find left joint["
+                << _sdf->GetElement("left_joint")->GetValueString() << "]\n";
+        if (!this->rightJoint)
+            gzerr << "Unable to find right joint["
+                << _sdf->GetElement("right_joint")->GetValueString() << "]\n";
+
+
+
+        // Listen to the update event. This event is broadcast every
+        // simulation iteration.
+        this->updateConnection = event::Events::ConnectWorldUpdateStart(
+                boost::bind(&Motors::OnUpdate, this));
+    }
+
+    // Called by the world update start event
+
+    void Motors::Init() {
+        this->wheelSeparation = this->leftJoint->GetAnchor(0).Distance(this->rightJoint->GetAnchor(0));
+        std::cout << "Wheel Separation:" << this->wheelSeparation << std::endl;
+        physics::EntityPtr parent = boost::shared_dynamic_cast<physics::Entity > (this->leftJoint->GetChild());
+
+        math::Box bb = parent->GetBoundingBox();
+
+        this->wheelRadius = bb.GetSize().GetMax() * 0.5;
+        std::cout << "Wheel Diameter:" << this->wheelRadius * 2 << std::endl;
+    }
+
+    /*
+    void Motors::OnVelMsg(ConstPosePtr& _msg) {
+
+        double vr, va;
+
+        vr = _msg->position().x() + 5;
+        std::cout << "vr:" << vr << std::endl;
+        va = msgs::Convert(_msg->orientation()).GetAsEuler().z + 3;
+
+        this->wheelSpeed[LEFT] = vr + va * this->wheelSeparation / 2.0;
+        this->wheelSpeed[RIGHT] = vr - va * this->wheelSeparation / 2.0;
+
+
+
+    }
+    */
+    
+    void Motors::OnUpdate() {
+
+        if (count == 0) {
+        
+	        robotMotors.v = 0;
+	        robotMotors.w = 0;
+        
+            count++;
+            std::string name = this->model->GetName();
+            std::cout << "motors name " << name << std::endl;
+            nameMotors = std::string("--Ice.Config=" + name +"Motors.cfg");
+            pthread_t thr_gui;
+            pthread_create(&thr_gui, NULL, &motorsICE, (void*) this);
+        }
+
+        double vr, va; //vr -> velocidad lineal; va -> velocidad angular
+
+        pthread_mutex_lock(&mutex);
+
+        vr = robotMotors.v/100;
+
+        va = robotMotors.w/10;
+
+        pthread_mutex_unlock(&mutex);
+
+
+        this->wheelSpeed[LEFT] = vr + va * this->wheelSeparation / 2.0;
+        this->wheelSpeed[RIGHT] = vr - va * this->wheelSeparation / 2.0;
+
+        double leftVelDesired = (this->wheelSpeed[LEFT] / this->wheelRadius);
+        double rightVelDesired = (this->wheelSpeed[RIGHT] / this->wheelRadius);
+/*
+		std::cout << "leftVelDesired " << leftVelDesired << std::endl;
+		std::cout << "rightVelDesired " << rightVelDesired << std::endl;
+		std::cout << "torque " << torque << std::endl;
+*/
+        this->leftJoint->SetVelocity(0, leftVelDesired);
+        this->rightJoint->SetVelocity(0, rightVelDesired);
+
+        this->leftJoint->SetMaxForce(0, this->torque);
+        this->rightJoint->SetMaxForce(0, this->torque);
+    }
+
+    class MotorsI : virtual public jderobot::Motors {
+    public:
+
+        MotorsI(gazebo::Motors* pose) {
+            this->pose = pose;
+        }
+
+        virtual ~MotorsI() {
+        };
+
+        virtual float getV(const Ice::Current&) {
+
+            float v_return;
+            pthread_mutex_lock(&pose->mutexMotor);
+            //v_return = pose->vel;
+            v_return = pose->robotMotors.v;
+            pthread_mutex_unlock(&pose->mutexMotor);
+            return v_return;
+        };
+
+        virtual float getW(const Ice::Current&) {
+            float w_return;
+            pthread_mutex_lock(&pose->mutexMotor);
+            //w_return = pose->w;
+            w_return = pose->robotMotors.w;
+            pthread_mutex_unlock(&pose->mutexMotor);
+            return w_return;
+        };
+
+        virtual float getL(const Ice::Current&) {
+            return 0.;
+        };
+
+        virtual Ice::Int setV(Ice::Float v, const Ice::Current&) {
+            pthread_mutex_lock(&pose->mutexMotor);
+            //pose->vel = v;
+            pose->robotMotors.v = v;
+            pthread_mutex_unlock(&pose->mutexMotor);
+            return 0;
+        };
+
+        virtual Ice::Int setW(Ice::Float _w, const Ice::Current&) {
+            pthread_mutex_lock(&pose->mutexMotor);
+            //pose->w = _w;
+            pose->robotMotors.w = -_w;
+            pthread_mutex_unlock(&pose->mutexMotor);
+            return 0;
+        };
+
+        virtual Ice::Int setL(Ice::Float l, const Ice::Current&) {
+            return 0;
+        };
+
+    public:
+        gazebo::Motors* pose;
+    }; // end class MotorsI
+
+
+    void *motorsICE(void* v) {
+
+        gazebo::Motors* base = (gazebo::Motors*)v;
+
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        char* name = (char*) base->nameMotors.c_str();
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+
+            ic = Ice::initialize(argc, argv);
+
+
+            prop = ic->getProperties();
+            std::string Endpoints = prop->getProperty("Motors.Endpoints");
+            std::cout << "Motors Endpoints > " << Endpoints << std::endl;
+
+            Ice::ObjectAdapterPtr adapter =
+                    ic->createObjectAdapterWithEndpoints("Motors", Endpoints);
+            Ice::ObjectPtr object = new MotorsI(base);
+            
+            adapter->add(object, ic->stringToIdentity("Motors"));
+            
+            adapter->activate();
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+
+    }
+}

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/motors.h (from rev 1055, trunk/src/stable/components/gazeboserver/motors.h)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/motors.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/motors.h	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,73 @@
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+#include "plugins/RayPlugin.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+
+// JDErobot general ice component includes
+#include <jderobot/motors.h>
+
+
+#ifndef MOTORS_H
+#define	MOTORS_H
+
+namespace gazebo {
+
+    class Motors : public ModelPlugin {
+    public:
+
+        Motors();
+        virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+        virtual void Init();
+
+        std::string nameMotors;
+        float vel, w;
+        pthread_mutex_t mutex;
+        pthread_mutex_t mutexMotor;
+
+        struct motor_t {
+            float v;
+            float w;
+            float l;
+        };
+
+
+        motor_t robotMotors;
+
+    private:
+        void OnUpdate();
+        //void OnVelMsg(ConstPosePtr &_msg);
+        transport::NodePtr node;
+        transport::SubscriberPtr velSub;
+
+        physics::ModelPtr model;
+        physics::JointPtr leftJoint, rightJoint;
+        event::ConnectionPtr updateConnection;
+        double wheelSpeed[2];
+        double torque;
+        double wheelSeparation;
+        double wheelRadius;
+        common::Time prevUpdateTime;
+
+        physics::LinkPtr link, leftWheelLink, rightWheelLink;
+        double sum;
+
+        int count;
+
+        //event::ConnectionPtr updateConnection; Alex
+        //physics::ModelPtr model; Alex
+    };
+}
+
+
+
+
+#endif	/* POSE_H */
+

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2Encoders.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dx2Encoders.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2Encoders.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2Encoders.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,3 @@
+#without registry
+Encoders.Endpoints=default -h localhost -p 8997
+

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2Motors.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dx2Motors.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2Motors.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2Motors.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,3 @@
+#without registry
+Motors.Endpoints=default -h localhost -p 8999
+

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_cam_sensor_left.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dx2_cam_sensor_left.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_cam_sensor_left.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_cam_sensor_left.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,8 @@
+#without registry
+CameraGazebo.Endpoints=default -h localhost -p 8995
+
+#camera 1
+CameraGazebo.Camera.0.Name=cameraA
+CameraGazebo.Camera.0.ImageWidth=320
+CameraGazebo.Camera.0.ImageHeight=240
+CameraGazebo.Camera.0.Format=RGB8

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_cam_sensor_right.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dx2_cam_sensor_right.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_cam_sensor_right.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_cam_sensor_right.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,8 @@
+#without registry
+CameraGazebo.Endpoints=default -h localhost -p 8994
+
+#camera 1
+CameraGazebo.Camera.0.Name=cameraA
+CameraGazebo.Camera.0.ImageWidth=320
+CameraGazebo.Camera.0.ImageHeight=240
+CameraGazebo.Camera.0.Format=RGB8

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_laser.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dx2_laser.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_laser.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_laser.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,2 @@
+#without registry
+Laser.Endpoints=default -h localhost -p 8998

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_pose3dencoders.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dx2_pose3dencoders.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_pose3dencoders.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx2_pose3dencoders.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,5 @@
+#without registry
+Pose3DEncoders1.Endpoints=default -h localhost -p 8993
+Pose3DEncoders2.Endpoints=default -h localhost -p 8992
+Pose3DMotors1.Endpoints=default -h localhost -p 8991
+Pose3DMotors2.Endpoints=default -h localhost -p 8990

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxEncoders.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dxEncoders.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxEncoders.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxEncoders.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,3 @@
+#without registry
+Encoders.Endpoints=default -h localhost -p 9997
+

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxJde.world (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dxJde.world)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxJde.world	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxJde.world	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,32 @@
+<?xml version="1.0" ?>
+<sdf version="1.3">
+  <world name="default">
+    <!-- Ground -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+    
+    <!-- A global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- Pioneer2dx model -->
+    <include>
+      <uri>model://pioneer2dxJde</uri>
+      <name>pioneer2dx</name>
+    </include>
+    
+        <!-- Pioneer2dx model
+    <include>
+<!--      <uri>model://pioneer2dxJde</uri>
+      <name>pioneer2dx2</name>
+      <pose>2 0 0 0 0 0</pose>
+    </include> -->
+    
+<!--    <include>
+      <uri>model://simpleWorld</uri>
+    </include> -->
+
+  </world>
+</sdf>

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxMotors.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dxMotors.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxMotors.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dxMotors.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,3 @@
+#without registry
+Motors.Endpoints=default -h localhost -p 9999
+

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_cam_sensor_left.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dx_cam_sensor_left.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_cam_sensor_left.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_cam_sensor_left.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,8 @@
+#without registry
+CameraGazebo.Endpoints=default -h localhost -p 9995
+
+#camera 1
+CameraGazebo.Camera.0.Name=cameraA
+CameraGazebo.Camera.0.ImageWidth=320
+CameraGazebo.Camera.0.ImageHeight=240
+CameraGazebo.Camera.0.Format=RGB8

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_cam_sensor_right.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dx_cam_sensor_right.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_cam_sensor_right.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_cam_sensor_right.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,8 @@
+#without registry
+CameraGazebo.Endpoints=default -h localhost -p 9994
+
+#camera 1
+CameraGazebo.Camera.0.Name=cameraA
+CameraGazebo.Camera.0.ImageWidth=320
+CameraGazebo.Camera.0.ImageHeight=240
+CameraGazebo.Camera.0.Format=RGB8

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_laser.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dx_laser.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_laser.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_laser.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,2 @@
+#without registry
+Laser.Endpoints=default -h localhost -p 9996

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_pose3dencoders.cfg (from rev 1055, trunk/src/stable/components/gazeboserver/pioneer2dx_pose3dencoders.cfg)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_pose3dencoders.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pioneer2dx_pose3dencoders.cfg	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,5 @@
+#without registry
+Pose3DEncoders1.Endpoints=default -h localhost -p 9993
+Pose3DEncoders2.Endpoints=default -h localhost -p 9992
+Pose3DMotors1.Endpoints=default -h localhost -p 9991
+Pose3DMotors2.Endpoints=default -h localhost -p 9990

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pose3dencoders.cc (from rev 1055, trunk/src/stable/components/gazeboserver/pose3dencoders.cc)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pose3dencoders.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pose3dencoders.cc	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,496 @@
+#include "pose3dencoders.h"
+//#include "pose3dmotors.h"
+
+
+
+namespace gazebo {
+
+    void *pose3dencodersICE(void* v);
+
+    GZ_REGISTER_MODEL_PLUGIN(Pose3DEncoders)
+
+    Pose3DEncoders::Pose3DEncoders() {
+        pthread_mutex_init(&mutex, NULL);
+        pthread_mutex_init(&mutexPose3DEncoders, NULL);
+        count = 0;
+        std::cout << "constructor pose3dencoders" << std::endl;
+    }
+
+    void Pose3DEncoders::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
+
+        this->model = _model;
+
+        // LOAD CAMERA LEFT        
+        if (!_sdf->HasElement("left_joint_pose3dencoders_pan"))
+            gzerr << "pose3dencoders plugin missing <left_joint_pose3dencoders_pan> element\n";
+        if (!_sdf->HasElement("left_joint_pose3dencoders_tilt"))
+            gzerr << "pose3dencoders plugin missing <left_joint_pose3dencoders_tilt> element\n";
+
+        this->cameraLeft.joint_pose3dencoders_pan = this->model->GetJoint("pan_joint_left");
+        this->cameraLeft.joint_pose3dencoders_tilt = this->model->GetJoint("tilt_joint_left");
+
+        if (!this->cameraLeft.joint_pose3dencoders_pan)
+            gzerr << "Unable to find left joint pose3dencoders_pan["
+                << _sdf->GetElement("left_joint_pose3dencoders_pan")->GetValueString() << "]\n";
+        if (!this->cameraLeft.joint_pose3dencoders_tilt)
+            gzerr << "Unable to find left joint pose3dencoders_tilt["
+                << _sdf->GetElement("left_joint_pose3dencoders_tilt")->GetValueString() << "]\n";
+        this->cameraLeft.camera_link_pan = this->model->GetLink("camera_column_body_left");
+        this->cameraLeft.camera_link_tilt = this->model->GetLink("camera_top_body_left");
+
+        //LOAD CAMERA RIGHT
+        if (!_sdf->HasElement("right_joint_pose3dencoders_pan"))
+            gzerr << "Motors plugin missing <right_joint_pose3dencoders_pan> element\n";
+        if (!_sdf->HasElement("right_joint_pose3dencoders_tilt"))
+            gzerr << "Motors plugin missing <right_joint_pose3dencoders_tilt> element\n";
+
+
+        this->cameraRight.joint_pose3dencoders_pan = this->model->GetJoint("pan_joint_right");
+        this->cameraRight.joint_pose3dencoders_tilt = this->model->GetJoint("tilt_joint_right");
+
+
+        if (!this->cameraRight.joint_pose3dencoders_pan)
+            gzerr << "Unable to find right joint pose3dencoders_pan["
+                << _sdf->GetElement("right_joint_pose3dencoders_pan")->GetValueString() << "]\n";
+        if (!this->cameraRight.joint_pose3dencoders_tilt)
+            gzerr << "Unable to find right joint pose3dencoders_tilt["
+                << _sdf->GetElement("right_joint_pose3dencoders_tilt")->GetValueString() << "]\n";
+        this->cameraRight.camera_link_pan = this->model->GetLink("camera_column_body_right");
+        this->cameraRight.camera_link_tilt = this->model->GetLink("camera_top_body_right");
+
+
+        //LOAD TORQUE        
+        if (_sdf->HasElement("torque"))
+            this->torque = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            this->torque = 5.0;
+        }
+
+        //LOAD POSE3DMOTORS
+
+        this->updateConnection = event::Events::ConnectWorldUpdateStart(
+                boost::bind(&Pose3DEncoders::OnUpdate, this));
+
+
+    }
+
+    void Pose3DEncoders::Init() {
+
+
+
+    }
+
+    void Pose3DEncoders::OnUpdate() {
+
+        if (count == 0) {
+            count++;
+
+            std::string name = this->model->GetName();
+
+            namePose3DEncoders = std::string("--Ice.Config=" + name + "_pose3dencoders.cfg");
+            pthread_t thr_gui;
+            pthread_create(&thr_gui, NULL, &pose3dencodersICE, (void*) this);
+        }
+
+        //          ----------ENCODERS----------
+        //GET pose3dencoders data from left_camera (PAN&TILT)
+        this->cameraLeft.encoder.pan = this->cameraLeft.camera_link_pan->GetRelativePose().rot.GetAsEuler().z * 180.0 / M_PI;
+        if (this->cameraLeft.encoder.pan > 0) {
+            this->cameraLeft.encoder.pan = 180 - this->cameraLeft.encoder.pan;
+        }
+        if (this->cameraLeft.encoder.pan < 0) {
+            this->cameraLeft.encoder.pan = -(180 + this->cameraLeft.encoder.pan);
+        }
+
+        //std::cout << this->cameraLeft.encoder.pan << std::endl;
+        this->cameraLeft.encoder.tilt = this->cameraLeft.camera_link_tilt->GetRelativePose().rot.GetAsEuler().y * 180.0 / M_PI;
+        //std::cout << this->cameraLeft.encoder.tilt << std::endl;
+
+
+        //GET pose3dencoders data from left_camera (PAN&TILT)
+
+        this->cameraRight.encoder.pan = this->cameraRight.camera_link_pan->GetRelativePose().rot.GetAsEuler().z * 180.0 / M_PI;
+        if (this->cameraRight.encoder.pan > 0) {
+            this->cameraRight.encoder.pan = 180 - this->cameraRight.encoder.pan;
+        }
+        if (this->cameraRight.encoder.pan < 0) {
+            this->cameraRight.encoder.pan = -(180 + this->cameraRight.encoder.pan);
+        }
+
+        //std::cout << this->cameraRight.pan << std::endl;
+        this->cameraRight.encoder.tilt = this->cameraRight.camera_link_tilt->GetRelativePose().rot.GetAsEuler().y * 180.0 / M_PI;
+        //std::cout << this->cameraRight.encoder.tilt << std::endl;
+
+        double setPanRight = -50;
+        double setPanLeft = -50;
+        double setTiltRight = -10;
+        double setTiltLeft = -10;
+        //          ----------MOTORS----------
+
+        
+        
+        if (this->cameraLeft.motor.pan >= 0) {
+            if (this->cameraLeft.encoder.pan < this->cameraLeft.motor.pan) {
+                this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, -0.1);
+                this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, 0);
+                this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+            }
+        } else {
+            if (this->cameraLeft.encoder.pan > this->cameraLeft.motor.pan) {
+                this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, 0.1);
+                this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, 0);
+                this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+            }            
+        }
+        if (this->cameraRight.motor.pan >= 0) {
+            if (this->cameraRight.encoder.pan < this->cameraRight.motor.pan) {
+                this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, -0.1);
+                this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, 0);
+                this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+            }
+        } else {
+            if (this->cameraRight.encoder.pan > this->cameraRight.motor.pan) {
+                this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, 0.1);
+                this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, 0);
+                this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
+            }            
+        }
+
+        if (this->cameraLeft.motor.tilt >= 0) {
+            if (this->cameraLeft.encoder.tilt < this->cameraLeft.motor.tilt) {
+                this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, -0.1);
+                this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, 0);
+                this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+            }
+        } else {
+            if (this->cameraLeft.encoder.tilt > this->cameraLeft.motor.tilt) {
+                this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, 0.1);
+                this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, 0);
+                this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+            }            
+        }
+        if (this->cameraRight.motor.tilt >= 0) {
+            if (this->cameraRight.encoder.tilt < this->cameraRight.motor.tilt) {
+                this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, -0.1);
+                this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, 0);
+                this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+            }
+        } else {
+            if (this->cameraRight.encoder.tilt > this->cameraRight.motor.tilt) {
+                this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, 0.1);
+                this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, 0);
+                this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
+            }            
+        }        
+        
+
+
+
+
+    }
+
+    class Pose3DEncodersI : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncodersI(gazebo::Pose3DEncoders* pose) : pose3DEncodersData(new jderobot::Pose3DEncodersData()) {
+
+            this->pose = pose;
+
+        }
+
+        virtual ~Pose3DEncodersI() {
+
+        }
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData(const Ice::Current&) {
+
+            pthread_mutex_lock(&pose->mutex);
+            pose3DEncodersData->x = 0;
+            pose3DEncodersData->y = 0;
+            pose3DEncodersData->z = 0;
+            pose3DEncodersData->pan = pose->cameraLeft.encoder.pan;
+            pose3DEncodersData->tilt = pose->cameraLeft.encoder.tilt;
+            pose3DEncodersData->clock = 0;
+            pose3DEncodersData->roll = 0;
+            pthread_mutex_unlock(&pose->mutex);
+
+            return pose3DEncodersData;
+
+        }
+
+        gazebo::Pose3DEncoders* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+
+    };
+
+    class Pose3DEncodersII : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncodersII(gazebo::Pose3DEncoders* pose) : pose3DEncodersData(new jderobot::Pose3DEncodersData()) {
+
+            this->pose = pose;
+
+        }
+
+        virtual ~Pose3DEncodersII() {
+
+        }
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData(const Ice::Current&) {
+
+            pthread_mutex_lock(&pose->mutex);
+            pose3DEncodersData->x = 0;
+            pose3DEncodersData->y = 0;
+            pose3DEncodersData->z = 0;
+            pose3DEncodersData->pan = pose->cameraRight.encoder.pan;
+            pose3DEncodersData->tilt = pose->cameraRight.encoder.tilt;
+            pose3DEncodersData->clock = 0;
+            pose3DEncodersData->roll = 0;
+            pthread_mutex_unlock(&pose->mutex);
+
+            return pose3DEncodersData;
+
+        }
+
+        gazebo::Pose3DEncoders* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+
+    };
+    
+    class Pose3DMotorsI : virtual public jderobot::Pose3DMotors {
+        
+    public:
+        
+        Pose3DMotorsI(gazebo::Pose3DEncoders* pose) : pose3DMotorsData(new jderobot::Pose3DMotorsData()) {
+            
+            this->pose = pose;
+            
+        }
+        
+        virtual ~Pose3DMotorsI(){
+            
+        }
+        
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData(const Ice::Current&){
+            
+            pthread_mutex_lock(&pose->mutex);
+            pose3DMotorsData->x = 0;
+            pose3DMotorsData->y = 0;
+            pose3DMotorsData->z = 0;
+            pose3DMotorsData->pan = pose->cameraLeft.encoder.pan;
+            pose3DMotorsData->tilt = pose->cameraLeft.encoder.tilt;
+            pose3DMotorsData->roll = 0;
+            pose3DMotorsData->panSpeed = 0;
+            pose3DMotorsData->tiltSpeed = 0;
+            pthread_mutex_unlock(&pose->mutex);
+
+            return pose3DMotorsData;
+            
+            
+        }
+        
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams(const Ice::Current&) {
+            pthread_mutex_lock(&pose->mutex);
+            pose3DMotorsParams->maxPan = 0;
+            pose3DMotorsParams->minPan = 0;
+            pose3DMotorsParams->maxTilt = 0;
+            pose3DMotorsParams->minTilt = 0;
+            pose3DMotorsParams->maxPanSpeed = 0;
+            pose3DMotorsParams->maxTiltSpeed = 0;
+            pthread_mutex_unlock(&pose->mutex);
+
+            return pose3DMotorsParams;
+        }
+        
+        virtual Ice::Int setPose3DMotorsData(const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current&) {
+            pthread_mutex_lock(&pose->mutex);
+            pose->cameraLeft.motor.x = data->x;
+            pose->cameraLeft.motor.y = data->y;
+            pose->cameraLeft.motor.z = data->z;
+            pose->cameraLeft.motor.pan = data->pan;
+            pose->cameraLeft.motor.tilt = data->tilt;
+            pose->cameraLeft.motor.roll = data->roll;
+            pose->cameraLeft.motor.panSpeed = data->panSpeed;
+            pose->cameraLeft.motor.tiltSpeed = data->tiltSpeed;            
+            pthread_mutex_unlock(&pose->mutex);
+
+        }        
+        
+        gazebo::Pose3DEncoders* pose;
+        
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+        
+                
+    };
+
+    class Pose3DMotorsII : virtual public jderobot::Pose3DMotors {
+        
+    public:
+        
+        Pose3DMotorsII(gazebo::Pose3DEncoders* pose) : pose3DMotorsData(new jderobot::Pose3DMotorsData()) {
+            
+            this->pose = pose;
+            
+        }
+        
+        virtual ~Pose3DMotorsII(){
+            
+        }
+        
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData(const Ice::Current&){
+            
+            pthread_mutex_lock(&pose->mutex);
+            pose3DMotorsData->x = 0;
+            pose3DMotorsData->y = 0;
+            pose3DMotorsData->z = 0;
+            pose3DMotorsData->pan = pose->cameraRight.encoder.pan;
+            pose3DMotorsData->tilt = pose->cameraRight.encoder.tilt;
+            pose3DMotorsData->roll = 0;
+            pose3DMotorsData->panSpeed = 0;
+            pose3DMotorsData->tiltSpeed = 0;
+            pthread_mutex_unlock(&pose->mutex);
+
+            return pose3DMotorsData;
+            
+            
+        }
+        
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams(const Ice::Current&) {
+            pthread_mutex_lock(&pose->mutex);
+            pose3DMotorsParams->maxPan = 0;
+            pose3DMotorsParams->minPan = 0;
+            pose3DMotorsParams->maxTilt = 0;
+            pose3DMotorsParams->minTilt = 0;
+            pose3DMotorsParams->maxPanSpeed = 0;
+            pose3DMotorsParams->maxTiltSpeed = 0;
+            pthread_mutex_unlock(&pose->mutex);
+
+            return pose3DMotorsParams;
+        }
+        
+        virtual Ice::Int setPose3DMotorsData(const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current&) {
+            pthread_mutex_lock(&pose->mutex);
+            pose->cameraRight.motor.x = data->x;
+            pose->cameraRight.motor.y = data->y;
+            pose->cameraRight.motor.z = data->z;
+            pose->cameraRight.motor.pan = data->pan;
+            pose->cameraRight.motor.tilt = data->tilt;
+            pose->cameraRight.motor.roll = data->roll;
+            pose->cameraRight.motor.panSpeed = data->panSpeed;
+            pose->cameraRight.motor.tiltSpeed = data->tiltSpeed;            
+            pthread_mutex_unlock(&pose->mutex);
+
+        }        
+        
+        gazebo::Pose3DEncoders* pose;
+        
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+        
+                
+    };
+    
+    
+    void *pose3dencodersICE(void* v) {
+
+        gazebo::Pose3DEncoders* base = (gazebo::Pose3DEncoders*)v;
+        char* name = (char*) base->namePose3DEncoders.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+
+            ic = Ice::initialize(argc, argv);
+
+
+            prop = ic->getProperties();
+            std::string Endpoints1 = prop->getProperty("Pose3DEncoders1.Endpoints");
+            std::cout << "Pose3DEncoders1 Endpoints > " << Endpoints1 << std::endl;
+            std::string Endpoints2 = prop->getProperty("Pose3DEncoders2.Endpoints");
+            std::cout << "Pose3DEncoders2 Endpoints > " << Endpoints2 << std::endl;
+            std::string Endpoints3 = prop->getProperty("Pose3DMotors1.Endpoints");
+            std::cout << "Pose3DMotors1 Endpoints > " << Endpoints3 << std::endl;
+            std::string Endpoints4 = prop->getProperty("Pose3DMotors2.Endpoints");
+            std::cout << "Pose3DMotors2 Endpoints > " << Endpoints4 << std::endl;
+
+
+            Ice::ObjectAdapterPtr adapter1 =
+                    ic->createObjectAdapterWithEndpoints("Pose3DEncoders1", Endpoints1);
+            Ice::ObjectAdapterPtr adapter2 =
+                    ic->createObjectAdapterWithEndpoints("Pose3DEncoders2", Endpoints2);
+            Ice::ObjectAdapterPtr adapter3 =
+                    ic->createObjectAdapterWithEndpoints("Pose3DMotors1", Endpoints3);
+            Ice::ObjectAdapterPtr adapter4 =
+                    ic->createObjectAdapterWithEndpoints("Pose3DMotors2", Endpoints4);
+
+
+
+            Ice::ObjectPtr object1 = new Pose3DEncodersI(base);
+            Ice::ObjectPtr object2 = new Pose3DEncodersII(base);
+            Ice::ObjectPtr object3 = new Pose3DMotorsI(base);
+            Ice::ObjectPtr object4 = new Pose3DMotorsII(base);
+
+
+
+            adapter1->add(object1, ic->stringToIdentity("Pose3DEncoders1"));
+            adapter2->add(object2, ic->stringToIdentity("Pose3DEncoders2"));
+            adapter3->add(object3, ic->stringToIdentity("Pose3DMotors1"));
+            adapter4->add(object4, ic->stringToIdentity("Pose3DMotors2"));
+
+            adapter1->activate();
+            adapter2->activate();
+            adapter3->activate();
+            adapter4->activate();
+            
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+
+    }
+
+
+}

Copied: trunk/src/stable/components/gazeboserver/plugins/pioneer/pose3dencoders.h (from rev 1055, trunk/src/stable/components/gazeboserver/pose3dencoders.h)
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/pioneer/pose3dencoders.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/pioneer/pose3dencoders.h	2013-10-15 12:29:25 UTC (rev 1056)
@@ -0,0 +1,82 @@
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#ifndef POSE3DENCODERS_H
+#define POSE3DENCODERS_H
+
+namespace gazebo {
+
+    class Pose3DEncoders : public ModelPlugin {
+    public:
+
+        Pose3DEncoders();
+        virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+        virtual void Init();
+
+        std::string namePose3DEncoders;
+        pthread_mutex_t mutex;
+        pthread_mutex_t mutexPose3DEncoders;
+
+        struct pose3dencoders_t {
+            float x;
+            float y;
+            float z;
+            float pan;
+            float tilt;
+            float roll;
+            int clock;
+
+        };
+        
+        struct pose3dmotors_t{
+            float x;
+            float y;
+            float z;
+            float pan;
+            float tilt;
+            float roll;
+            float panSpeed;
+            float tiltSpeed;
+            float maxPan;
+            float minPan;
+            float maxTilt;
+            float minTilt;
+            float maxPanSpeed;
+            float maxTiltSpeed;
+        };
+
+        struct camera_t {
+            physics::JointPtr joint_pose3dencoders_tilt, joint_pose3dencoders_pan;
+            physics::LinkPtr camera_link_pan, camera_link_tilt;
+            pose3dmotors_t motor;
+            pose3dencoders_t encoder;            
+        };
+
+        camera_t cameraLeft;
+        camera_t cameraRight;
+
+    private:
+        
+        double torque;
+        void OnUpdate();
+        physics::ModelPtr model;
+        event::ConnectionPtr updateConnection;
+        int count;
+
+    };
+
+
+}
+
+#endif

Deleted: trunk/src/stable/components/gazeboserver/pose3dencoders.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pose3dencoders.cc	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pose3dencoders.cc	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,496 +0,0 @@
-#include "pose3dencoders.h"
-//#include "pose3dmotors.h"
-
-
-
-namespace gazebo {
-
-    void *pose3dencodersICE(void* v);
-
-    GZ_REGISTER_MODEL_PLUGIN(Pose3DEncoders)
-
-    Pose3DEncoders::Pose3DEncoders() {
-        pthread_mutex_init(&mutex, NULL);
-        pthread_mutex_init(&mutexPose3DEncoders, NULL);
-        count = 0;
-        std::cout << "constructor pose3dencoders" << std::endl;
-    }
-
-    void Pose3DEncoders::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
-
-        this->model = _model;
-
-        // LOAD CAMERA LEFT        
-        if (!_sdf->HasElement("left_joint_pose3dencoders_pan"))
-            gzerr << "pose3dencoders plugin missing <left_joint_pose3dencoders_pan> element\n";
-        if (!_sdf->HasElement("left_joint_pose3dencoders_tilt"))
-            gzerr << "pose3dencoders plugin missing <left_joint_pose3dencoders_tilt> element\n";
-
-        this->cameraLeft.joint_pose3dencoders_pan = this->model->GetJoint("pan_joint_left");
-        this->cameraLeft.joint_pose3dencoders_tilt = this->model->GetJoint("tilt_joint_left");
-
-        if (!this->cameraLeft.joint_pose3dencoders_pan)
-            gzerr << "Unable to find left joint pose3dencoders_pan["
-                << _sdf->GetElement("left_joint_pose3dencoders_pan")->GetValueString() << "]\n";
-        if (!this->cameraLeft.joint_pose3dencoders_tilt)
-            gzerr << "Unable to find left joint pose3dencoders_tilt["
-                << _sdf->GetElement("left_joint_pose3dencoders_tilt")->GetValueString() << "]\n";
-        this->cameraLeft.camera_link_pan = this->model->GetLink("camera_column_body_left");
-        this->cameraLeft.camera_link_tilt = this->model->GetLink("camera_top_body_left");
-
-        //LOAD CAMERA RIGHT
-        if (!_sdf->HasElement("right_joint_pose3dencoders_pan"))
-            gzerr << "Motors plugin missing <right_joint_pose3dencoders_pan> element\n";
-        if (!_sdf->HasElement("right_joint_pose3dencoders_tilt"))
-            gzerr << "Motors plugin missing <right_joint_pose3dencoders_tilt> element\n";
-
-
-        this->cameraRight.joint_pose3dencoders_pan = this->model->GetJoint("pan_joint_right");
-        this->cameraRight.joint_pose3dencoders_tilt = this->model->GetJoint("tilt_joint_right");
-
-
-        if (!this->cameraRight.joint_pose3dencoders_pan)
-            gzerr << "Unable to find right joint pose3dencoders_pan["
-                << _sdf->GetElement("right_joint_pose3dencoders_pan")->GetValueString() << "]\n";
-        if (!this->cameraRight.joint_pose3dencoders_tilt)
-            gzerr << "Unable to find right joint pose3dencoders_tilt["
-                << _sdf->GetElement("right_joint_pose3dencoders_tilt")->GetValueString() << "]\n";
-        this->cameraRight.camera_link_pan = this->model->GetLink("camera_column_body_right");
-        this->cameraRight.camera_link_tilt = this->model->GetLink("camera_top_body_right");
-
-
-        //LOAD TORQUE        
-        if (_sdf->HasElement("torque"))
-            this->torque = _sdf->GetElement("torque")->GetValueDouble();
-        else {
-            gzwarn << "No torque value set for the DiffDrive plugin.\n";
-            this->torque = 5.0;
-        }
-
-        //LOAD POSE3DMOTORS
-
-        this->updateConnection = event::Events::ConnectWorldUpdateStart(
-                boost::bind(&Pose3DEncoders::OnUpdate, this));
-
-
-    }
-
-    void Pose3DEncoders::Init() {
-
-
-
-    }
-
-    void Pose3DEncoders::OnUpdate() {
-
-        if (count == 0) {
-            count++;
-
-            std::string name = this->model->GetName();
-
-            namePose3DEncoders = std::string("--Ice.Config=" + name + "_pose3dencoders.cfg");
-            pthread_t thr_gui;
-            pthread_create(&thr_gui, NULL, &pose3dencodersICE, (void*) this);
-        }
-
-        //          ----------ENCODERS----------
-        //GET pose3dencoders data from left_camera (PAN&TILT)
-        this->cameraLeft.encoder.pan = this->cameraLeft.camera_link_pan->GetRelativePose().rot.GetAsEuler().z * 180.0 / M_PI;
-        if (this->cameraLeft.encoder.pan > 0) {
-            this->cameraLeft.encoder.pan = 180 - this->cameraLeft.encoder.pan;
-        }
-        if (this->cameraLeft.encoder.pan < 0) {
-            this->cameraLeft.encoder.pan = -(180 + this->cameraLeft.encoder.pan);
-        }
-
-        //std::cout << this->cameraLeft.encoder.pan << std::endl;
-        this->cameraLeft.encoder.tilt = this->cameraLeft.camera_link_tilt->GetRelativePose().rot.GetAsEuler().y * 180.0 / M_PI;
-        //std::cout << this->cameraLeft.encoder.tilt << std::endl;
-
-
-        //GET pose3dencoders data from left_camera (PAN&TILT)
-
-        this->cameraRight.encoder.pan = this->cameraRight.camera_link_pan->GetRelativePose().rot.GetAsEuler().z * 180.0 / M_PI;
-        if (this->cameraRight.encoder.pan > 0) {
-            this->cameraRight.encoder.pan = 180 - this->cameraRight.encoder.pan;
-        }
-        if (this->cameraRight.encoder.pan < 0) {
-            this->cameraRight.encoder.pan = -(180 + this->cameraRight.encoder.pan);
-        }
-
-        //std::cout << this->cameraRight.pan << std::endl;
-        this->cameraRight.encoder.tilt = this->cameraRight.camera_link_tilt->GetRelativePose().rot.GetAsEuler().y * 180.0 / M_PI;
-        //std::cout << this->cameraRight.encoder.tilt << std::endl;
-
-        double setPanRight = -50;
-        double setPanLeft = -50;
-        double setTiltRight = -10;
-        double setTiltLeft = -10;
-        //          ----------MOTORS----------
-
-        
-        
-        if (this->cameraLeft.motor.pan >= 0) {
-            if (this->cameraLeft.encoder.pan < this->cameraLeft.motor.pan) {
-                this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, -0.1);
-                this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
-                //std::cout << "AQUI" << std::endl;
-            } else {
-                this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, 0);
-                this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
-            }
-        } else {
-            if (this->cameraLeft.encoder.pan > this->cameraLeft.motor.pan) {
-                this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, 0.1);
-                this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
-                //std::cout << "AQUI" << std::endl;
-            } else {
-                this->cameraLeft.joint_pose3dencoders_pan->SetVelocity(0, 0);
-                this->cameraLeft.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
-            }            
-        }
-        if (this->cameraRight.motor.pan >= 0) {
-            if (this->cameraRight.encoder.pan < this->cameraRight.motor.pan) {
-                this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, -0.1);
-                this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
-                //std::cout << "AQUI" << std::endl;
-            } else {
-                this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, 0);
-                this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
-            }
-        } else {
-            if (this->cameraRight.encoder.pan > this->cameraRight.motor.pan) {
-                this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, 0.1);
-                this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
-                //std::cout << "AQUI" << std::endl;
-            } else {
-                this->cameraRight.joint_pose3dencoders_pan->SetVelocity(0, 0);
-                this->cameraRight.joint_pose3dencoders_pan->SetMaxForce(0, this->torque);
-            }            
-        }
-
-        if (this->cameraLeft.motor.tilt >= 0) {
-            if (this->cameraLeft.encoder.tilt < this->cameraLeft.motor.tilt) {
-                this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, -0.1);
-                this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
-                //std::cout << "AQUI" << std::endl;
-            } else {
-                this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, 0);
-                this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
-            }
-        } else {
-            if (this->cameraLeft.encoder.tilt > this->cameraLeft.motor.tilt) {
-                this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, 0.1);
-                this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
-                //std::cout << "AQUI" << std::endl;
-            } else {
-                this->cameraLeft.joint_pose3dencoders_tilt->SetVelocity(0, 0);
-                this->cameraLeft.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
-            }            
-        }
-        if (this->cameraRight.motor.tilt >= 0) {
-            if (this->cameraRight.encoder.tilt < this->cameraRight.motor.tilt) {
-                this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, -0.1);
-                this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
-                //std::cout << "AQUI" << std::endl;
-            } else {
-                this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, 0);
-                this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
-            }
-        } else {
-            if (this->cameraRight.encoder.tilt > this->cameraRight.motor.tilt) {
-                this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, 0.1);
-                this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
-                //std::cout << "AQUI" << std::endl;
-            } else {
-                this->cameraRight.joint_pose3dencoders_tilt->SetVelocity(0, 0);
-                this->cameraRight.joint_pose3dencoders_tilt->SetMaxForce(0, this->torque);
-            }            
-        }        
-        
-
-
-
-
-    }
-
-    class Pose3DEncodersI : virtual public jderobot::Pose3DEncoders {
-    public:
-
-        Pose3DEncodersI(gazebo::Pose3DEncoders* pose) : pose3DEncodersData(new jderobot::Pose3DEncodersData()) {
-
-            this->pose = pose;
-
-        }
-
-        virtual ~Pose3DEncodersI() {
-
-        }
-
-        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData(const Ice::Current&) {
-
-            pthread_mutex_lock(&pose->mutex);
-            pose3DEncodersData->x = 0;
-            pose3DEncodersData->y = 0;
-            pose3DEncodersData->z = 0;
-            pose3DEncodersData->pan = pose->cameraLeft.encoder.pan;
-            pose3DEncodersData->tilt = pose->cameraLeft.encoder.tilt;
-            pose3DEncodersData->clock = 0;
-            pose3DEncodersData->roll = 0;
-            pthread_mutex_unlock(&pose->mutex);
-
-            return pose3DEncodersData;
-
-        }
-
-        gazebo::Pose3DEncoders* pose;
-
-    private:
-        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
-
-    };
-
-    class Pose3DEncodersII : virtual public jderobot::Pose3DEncoders {
-    public:
-
-        Pose3DEncodersII(gazebo::Pose3DEncoders* pose) : pose3DEncodersData(new jderobot::Pose3DEncodersData()) {
-
-            this->pose = pose;
-
-        }
-
-        virtual ~Pose3DEncodersII() {
-
-        }
-
-        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData(const Ice::Current&) {
-
-            pthread_mutex_lock(&pose->mutex);
-            pose3DEncodersData->x = 0;
-            pose3DEncodersData->y = 0;
-            pose3DEncodersData->z = 0;
-            pose3DEncodersData->pan = pose->cameraRight.encoder.pan;
-            pose3DEncodersData->tilt = pose->cameraRight.encoder.tilt;
-            pose3DEncodersData->clock = 0;
-            pose3DEncodersData->roll = 0;
-            pthread_mutex_unlock(&pose->mutex);
-
-            return pose3DEncodersData;
-
-        }
-
-        gazebo::Pose3DEncoders* pose;
-
-    private:
-        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
-
-    };
-    
-    class Pose3DMotorsI : virtual public jderobot::Pose3DMotors {
-        
-    public:
-        
-        Pose3DMotorsI(gazebo::Pose3DEncoders* pose) : pose3DMotorsData(new jderobot::Pose3DMotorsData()) {
-            
-            this->pose = pose;
-            
-        }
-        
-        virtual ~Pose3DMotorsI(){
-            
-        }
-        
-        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData(const Ice::Current&){
-            
-            pthread_mutex_lock(&pose->mutex);
-            pose3DMotorsData->x = 0;
-            pose3DMotorsData->y = 0;
-            pose3DMotorsData->z = 0;
-            pose3DMotorsData->pan = pose->cameraLeft.encoder.pan;
-            pose3DMotorsData->tilt = pose->cameraLeft.encoder.tilt;
-            pose3DMotorsData->roll = 0;
-            pose3DMotorsData->panSpeed = 0;
-            pose3DMotorsData->tiltSpeed = 0;
-            pthread_mutex_unlock(&pose->mutex);
-
-            return pose3DMotorsData;
-            
-            
-        }
-        
-        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams(const Ice::Current&) {
-            pthread_mutex_lock(&pose->mutex);
-            pose3DMotorsParams->maxPan = 0;
-            pose3DMotorsParams->minPan = 0;
-            pose3DMotorsParams->maxTilt = 0;
-            pose3DMotorsParams->minTilt = 0;
-            pose3DMotorsParams->maxPanSpeed = 0;
-            pose3DMotorsParams->maxTiltSpeed = 0;
-            pthread_mutex_unlock(&pose->mutex);
-
-            return pose3DMotorsParams;
-        }
-        
-        virtual Ice::Int setPose3DMotorsData(const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current&) {
-            pthread_mutex_lock(&pose->mutex);
-            pose->cameraLeft.motor.x = data->x;
-            pose->cameraLeft.motor.y = data->y;
-            pose->cameraLeft.motor.z = data->z;
-            pose->cameraLeft.motor.pan = data->pan;
-            pose->cameraLeft.motor.tilt = data->tilt;
-            pose->cameraLeft.motor.roll = data->roll;
-            pose->cameraLeft.motor.panSpeed = data->panSpeed;
-            pose->cameraLeft.motor.tiltSpeed = data->tiltSpeed;            
-            pthread_mutex_unlock(&pose->mutex);
-
-        }        
-        
-        gazebo::Pose3DEncoders* pose;
-        
-    private:
-        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
-        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
-        
-                
-    };
-
-    class Pose3DMotorsII : virtual public jderobot::Pose3DMotors {
-        
-    public:
-        
-        Pose3DMotorsII(gazebo::Pose3DEncoders* pose) : pose3DMotorsData(new jderobot::Pose3DMotorsData()) {
-            
-            this->pose = pose;
-            
-        }
-        
-        virtual ~Pose3DMotorsII(){
-            
-        }
-        
-        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData(const Ice::Current&){
-            
-            pthread_mutex_lock(&pose->mutex);
-            pose3DMotorsData->x = 0;
-            pose3DMotorsData->y = 0;
-            pose3DMotorsData->z = 0;
-            pose3DMotorsData->pan = pose->cameraRight.encoder.pan;
-            pose3DMotorsData->tilt = pose->cameraRight.encoder.tilt;
-            pose3DMotorsData->roll = 0;
-            pose3DMotorsData->panSpeed = 0;
-            pose3DMotorsData->tiltSpeed = 0;
-            pthread_mutex_unlock(&pose->mutex);
-
-            return pose3DMotorsData;
-            
-            
-        }
-        
-        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams(const Ice::Current&) {
-            pthread_mutex_lock(&pose->mutex);
-            pose3DMotorsParams->maxPan = 0;
-            pose3DMotorsParams->minPan = 0;
-            pose3DMotorsParams->maxTilt = 0;
-            pose3DMotorsParams->minTilt = 0;
-            pose3DMotorsParams->maxPanSpeed = 0;
-            pose3DMotorsParams->maxTiltSpeed = 0;
-            pthread_mutex_unlock(&pose->mutex);
-
-            return pose3DMotorsParams;
-        }
-        
-        virtual Ice::Int setPose3DMotorsData(const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current&) {
-            pthread_mutex_lock(&pose->mutex);
-            pose->cameraRight.motor.x = data->x;
-            pose->cameraRight.motor.y = data->y;
-            pose->cameraRight.motor.z = data->z;
-            pose->cameraRight.motor.pan = data->pan;
-            pose->cameraRight.motor.tilt = data->tilt;
-            pose->cameraRight.motor.roll = data->roll;
-            pose->cameraRight.motor.panSpeed = data->panSpeed;
-            pose->cameraRight.motor.tiltSpeed = data->tiltSpeed;            
-            pthread_mutex_unlock(&pose->mutex);
-
-        }        
-        
-        gazebo::Pose3DEncoders* pose;
-        
-    private:
-        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
-        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
-        
-                
-    };
-    
-    
-    void *pose3dencodersICE(void* v) {
-
-        gazebo::Pose3DEncoders* base = (gazebo::Pose3DEncoders*)v;
-        char* name = (char*) base->namePose3DEncoders.c_str();
-        Ice::CommunicatorPtr ic;
-        int argc = 1;
-        Ice::PropertiesPtr prop;
-        char* argv[] = {name};
-
-        try {
-
-            ic = Ice::initialize(argc, argv);
-
-
-            prop = ic->getProperties();
-            std::string Endpoints1 = prop->getProperty("Pose3DEncoders1.Endpoints");
-            std::cout << "Pose3DEncoders1 Endpoints > " << Endpoints1 << std::endl;
-            std::string Endpoints2 = prop->getProperty("Pose3DEncoders2.Endpoints");
-            std::cout << "Pose3DEncoders2 Endpoints > " << Endpoints2 << std::endl;
-            std::string Endpoints3 = prop->getProperty("Pose3DMotors1.Endpoints");
-            std::cout << "Pose3DMotors1 Endpoints > " << Endpoints3 << std::endl;
-            std::string Endpoints4 = prop->getProperty("Pose3DMotors2.Endpoints");
-            std::cout << "Pose3DMotors2 Endpoints > " << Endpoints4 << std::endl;
-
-
-            Ice::ObjectAdapterPtr adapter1 =
-                    ic->createObjectAdapterWithEndpoints("Pose3DEncoders1", Endpoints1);
-            Ice::ObjectAdapterPtr adapter2 =
-                    ic->createObjectAdapterWithEndpoints("Pose3DEncoders2", Endpoints2);
-            Ice::ObjectAdapterPtr adapter3 =
-                    ic->createObjectAdapterWithEndpoints("Pose3DMotors1", Endpoints3);
-            Ice::ObjectAdapterPtr adapter4 =
-                    ic->createObjectAdapterWithEndpoints("Pose3DMotors2", Endpoints4);
-
-
-
-            Ice::ObjectPtr object1 = new Pose3DEncodersI(base);
-            Ice::ObjectPtr object2 = new Pose3DEncodersII(base);
-            Ice::ObjectPtr object3 = new Pose3DMotorsI(base);
-            Ice::ObjectPtr object4 = new Pose3DMotorsII(base);
-
-
-
-            adapter1->add(object1, ic->stringToIdentity("Pose3DEncoders1"));
-            adapter2->add(object2, ic->stringToIdentity("Pose3DEncoders2"));
-            adapter3->add(object3, ic->stringToIdentity("Pose3DMotors1"));
-            adapter4->add(object4, ic->stringToIdentity("Pose3DMotors2"));
-
-            adapter1->activate();
-            adapter2->activate();
-            adapter3->activate();
-            adapter4->activate();
-            
-            ic->waitForShutdown();
-        } catch (const Ice::Exception& e) {
-            std::cerr << e << std::endl;
-        } catch (const char* msg) {
-            std::cerr << msg << std::endl;
-        }
-        if (ic) {
-            try {
-                ic->destroy();
-            } catch (const Ice::Exception& e) {
-                std::cerr << e << std::endl;
-            }
-        }
-
-    }
-
-
-}

Deleted: trunk/src/stable/components/gazeboserver/pose3dencoders.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pose3dencoders.h	2013-10-15 10:30:32 UTC (rev 1055)
+++ trunk/src/stable/components/gazeboserver/pose3dencoders.h	2013-10-15 12:29:25 UTC (rev 1056)
@@ -1,82 +0,0 @@
-#include <boost/bind.hpp>
-#include "gazebo.hh"
-#include "physics/physics.hh"
-#include "common/common.hh"
-#include "transport/transport.hh"
-// ICE utils includes
-#include <Ice/Ice.h>
-#include <IceUtil/IceUtil.h>
-
-
-// JDErobot general ice component includes
-#include <jderobot/pose3dencoders.h>
-#include <jderobot/pose3dmotors.h>
-
-#ifndef POSE3DENCODERS_H
-#define POSE3DENCODERS_H
-
-namespace gazebo {
-
-    class Pose3DEncoders : public ModelPlugin {
-    public:
-
-        Pose3DEncoders();
-        virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
-        virtual void Init();
-
-        std::string namePose3DEncoders;
-        pthread_mutex_t mutex;
-        pthread_mutex_t mutexPose3DEncoders;
-
-        struct pose3dencoders_t {
-            float x;
-            float y;
-            float z;
-            float pan;
-            float tilt;
-            float roll;
-            int clock;
-
-        };
-        
-        struct pose3dmotors_t{
-            float x;
-            float y;
-            float z;
-            float pan;
-            float tilt;
-            float roll;
-            float panSpeed;
-            float tiltSpeed;
-            float maxPan;
-            float minPan;
-            float maxTilt;
-            float minTilt;
-            float maxPanSpeed;
-            float maxTiltSpeed;
-        };
-
-        struct camera_t {
-            physics::JointPtr joint_pose3dencoders_tilt, joint_pose3dencoders_pan;
-            physics::LinkPtr camera_link_pan, camera_link_tilt;
-            pose3dmotors_t motor;
-            pose3dencoders_t encoder;            
-        };
-
-        camera_t cameraLeft;
-        camera_t cameraRight;
-
-    private:
-        
-        double torque;
-        void OnUpdate();
-        physics::ModelPtr model;
-        event::ConnectionPtr updateConnection;
-        int count;
-
-    };
-
-
-}
-
-#endif



More information about the Jderobot-admin mailing list