[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