[Jderobot-admin] jderobot-r1049 - in trunk/src/stable/components/gazeboserver/pluginsGazebo: . nao nao/build
bmenendez en jderobot.org
bmenendez en jderobot.org
Lun Oct 14 19:47:38 CEST 2013
Author: bmenendez
Date: 2013-10-14 19:47:38 +0200 (Mon, 14 Oct 2013)
New Revision: 1049
Added:
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/CMakeLists.txt
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/CMakeLists.txt
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/clean.sh
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_bottom.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_top.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/camera_dump.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/common.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/naoworld.world
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.h
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cfg
trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.h
Log:
#69 Uploaded simulated Nao. Neck, shoulders, elbows and cameras works fine.
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/CMakeLists.txt (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/CMakeLists.txt 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,55 @@
+if (GAZEBO_INCLUDE_DIRS)
+
+include_directories(
+ ${GAZEBO_INCLUDE_DIRS}
+ ${INTERFACES_CPP_DIR}
+ ${LIBS_DIR}
+ ${CMAKE_CURRENT_SOURCE_DIR})
+
+link_directories(${GAZEBO_LIBRARY_DIRS})
+
+add_library(camera_nao SHARED camera_dump.cc)
+target_link_libraries(camera_nao ${GAZEBO_libraries}
+ CameraPlugin
+ JderobotInterfaces
+ colorspacesmm
+ ${GAZEBO_libraries}
+ ${ZeroCIce_LIBRARIES}
+)
+
+add_library(poseneck SHARED poseneck.cc)
+target_link_libraries(poseneck ${GAZEBO_libraries}
+ JderobotInterfaces
+ ${GAZEBO_libraries}
+ ${ZeroCIce_LIBRARIES}
+)
+
+add_library(poserightshoulder SHARED poserightshoulder.cc)
+target_link_libraries(poserightshoulder ${GAZEBO_libraries}
+ JderobotInterfaces
+ ${GAZEBO_libraries}
+ ${ZeroCIce_LIBRARIES}
+)
+
+add_library(poseleftshoulder SHARED poseleftshoulder.cc)
+target_link_libraries(poseleftshoulder ${GAZEBO_libraries}
+ JderobotInterfaces
+ ${GAZEBO_libraries}
+ ${ZeroCIce_LIBRARIES}
+)
+
+add_library(poserightelbow SHARED poserightelbow.cc)
+target_link_libraries(poserightelbow ${GAZEBO_libraries}
+ JderobotInterfaces
+ ${GAZEBO_libraries}
+ ${ZeroCIce_LIBRARIES}
+)
+
+add_library(poseleftelbow SHARED poseleftelbow.cc)
+target_link_libraries(poseleftelbow ${GAZEBO_libraries}
+ JderobotInterfaces
+ ${GAZEBO_libraries}
+ ${ZeroCIce_LIBRARIES}
+)
+
+ENDIF()
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,266 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "ballencoder.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+ GZ_REGISTER_MODEL_PLUGIN(BallGreen)
+
+ BallGreen::BallGreen () {
+ pthread_mutex_init(&mutex_ballgreenencoders, NULL);
+ pthread_mutex_init(&mutex_ballgreenmotors, NULL);
+ this->count = 0;
+ this->cycle = 50;
+ this->cfgfile_ballgreen = std::string("--Ice.Config=ballgreen.cfg");
+
+ std::cout << "Constructor BallGreen" << std::endl;
+ }
+
+ void BallGreen::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ this->ballgreen.joint_pan = _model->GetJoint("jball_vertical");
+ this->ballgreen.joint_tilt = _model->GetJoint("wmp2");
+
+ this->stiffness = 40.0;
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&BallGreen::OnUpdate, this));
+ }
+
+ void BallGreen::Init () {}
+
+ void BallGreen::OnUpdate () {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (this->count == 0) {
+ this->count++;
+ pthread_t thr_ice;
+ pthread_create(&thr_ice, NULL, &thread_BallGreenICE, (void*) this);
+
+ this->ballgreen.motorsdata.pan = 0.0;
+ this->ballgreen.motorsdata.tilt = 0.0;
+ }
+ // ----------ENCODERS----------
+ //GET pose3dencoders data from the ball (PAN&TILT)
+ this->ballgreen.encoders.pan = - this->ballgreen.joint_pan->GetAngle(0).Radian();
+ this->ballgreen.encoders.tilt = - this->ballgreen.joint_tilt->GetAngle(0).Radian();
+
+ // ----------MOTORS----------
+ if (this->ballgreen.motorsdata.pan > 0) {
+ if (this->ballgreen.encoders.pan < this->ballgreen.motorsdata.pan) {
+ this->ballgreen.joint_pan->SetVelocity(0, -0.3);
+ this->ballgreen.joint_pan->SetMaxForce(0, this->stiffness);
+ } else if (this->ballgreen.encoders.pan > this->ballgreen.motorsdata.pan) {
+ this->ballgreen.joint_pan->SetVelocity(0, 0.3);
+ this->ballgreen.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ } else if (this->ballgreen.motorsdata.pan < 0) {
+ if (this->ballgreen.encoders.pan > this->ballgreen.motorsdata.pan) {
+ this->ballgreen.joint_pan->SetVelocity(0, 0.3);
+ this->ballgreen.joint_pan->SetMaxForce(0, this->stiffness);
+ } else if (this->ballgreen.encoders.pan < this->ballgreen.motorsdata.pan) {
+ this->ballgreen.joint_pan->SetVelocity(0, -0.3);
+ this->ballgreen.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+
+ if (this->ballgreen.motorsdata.tilt > 0) {
+ if (this->ballgreen.encoders.tilt < this->ballgreen.motorsdata.tilt) {
+ this->ballgreen.joint_tilt->SetVelocity(0, -0.3);
+ this->ballgreen.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else if (this->ballgreen.encoders.tilt > this->ballgreen.motorsdata.tilt) {
+ this->ballgreen.joint_tilt->SetVelocity(0, 0.3);
+ this->ballgreen.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ } else if (this->ballgreen.motorsdata.tilt < 0) {
+ if (this->ballgreen.encoders.tilt > this->ballgreen.motorsdata.tilt) {
+ this->ballgreen.joint_tilt->SetVelocity(0, 0.3);
+ this->ballgreen.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else if (this->ballgreen.encoders.tilt < this->ballgreen.motorsdata.tilt) {
+ this->ballgreen.joint_tilt->SetVelocity(0, -0.3);
+ this->ballgreen.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseGreenBall* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_greenballencoders);
+
+ pose3DEncodersData->x = pose->ballgreen.encoders.x;
+ pose3DEncodersData->y = pose->ballgreen.encoders.y;
+ pose3DEncodersData->z = pose->ballgreen.encoders.z;
+ pose3DEncodersData->pan = pose->ballgreen.encoders.pan;
+ pose3DEncodersData->tilt = pose->ballgreen.encoders.tilt;
+ pose3DEncodersData->roll = pose->ballgreen.encoders.roll;
+ pose3DEncodersData->clock = pose->ballgreen.encoders.clock;
+ pose3DEncodersData->maxPan = pose->ballgreen.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->ballgreen.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->ballgreen.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->ballgreen.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_greenballencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseNeck* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseGreenBall* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_greenballmotors);
+
+ pose3DMotorsData->x = pose->ballgreen.motorsdata.x;
+ pose3DMotorsData->y = pose->ballgreen.motorsdata.y;
+ pose3DMotorsData->z = pose->ballgreen.motorsdata.z;
+ pose3DMotorsData->pan = pose->ballgreen.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->ballgreen.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->ballgreen.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->ballgreen.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->ballgreen.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_greenballmotors);
+
+ return pose3DMotorsData;
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_greenballmotors);
+
+ pose3DMotorsParams->maxPan = pose->ballgreen.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->ballgreen.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->ballgreen.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->ballgreen.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->ballgreen.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->ballgreen.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_greenballmotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_greenballmotors);
+
+ pose->ballgreen.motorsdata.x = data->x;
+ pose->ballgreen.motorsdata.y = data->y;
+ pose->ballgreen.motorsdata.z = data->z;
+ pose->ballgreen.motorsdata.pan = data->pan;
+ pose->ballgreen.motorsdata.tilt = data->tilt;
+ pose->ballgreen.motorsdata.roll = data->roll;
+ pose->ballgreen.motorsdata.panSpeed = data->panSpeed;
+ pose->ballgreen.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_greenballmotors);
+ }
+
+ gazebo::PoseGreenBall* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_BallGreenICE ( void* v ) {
+
+ gazebo::BallGreen* ballgreen = (gazebo::BallGreen*)v;
+ char* name = (char*) ballgreen->cfgfile_ballgreen.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 EndpointsEncoders = prop->getProperty("BallGreenEncoders.Endpoints");
+ std::cout << "BallGreenEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("BallGreenMotors.Endpoints");
+ std::cout << "BallGreenMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterBallGreenEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterBallGreenMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(ballgreen);
+ Ice::ObjectPtr motors = new Pose3DMotors(ballgreen);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("BallGreenEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("BallGreenMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->activate();
+
+ ic->waitForShutdown();
+ } catch (const Ice::Exception& e) {
+ std::cerr << e << std::endl;
+ } catch (const char* msg) {
+ std::cerr << msg << std::endl;
+ }
+ if (icB) {
+ try {
+ icB->destroy();
+ } catch (const Ice::Exception& e) {
+ std::cerr << e << std::endl;
+ }
+ }
+ }
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+BallGreenEncoders.Endpoints=default -h localhost -p 9294
+BallGreenMotors.Endpoints=default -h localhost -p 9295
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef BALLGREEN_H
+#define BALLGREEN_H
+
+namespace gazebo {
+
+ void* thread_BallGreenICE ( void* v );
+
+ class BallGreen : public ModelPlugin {
+ public:
+
+ BallGreen ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_ballgreen;
+ pthread_mutex_t mutex_ballgreenencoders;
+ pthread_mutex_t mutex_ballgreenmotors;
+
+ struct ballgreen_t {
+ physics::JointPtr joint_tilt, joint_pan;
+ physics::LinkPtr link_pan, link_tilt;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ ballgreen_t ballgreen;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // BALLGREEN_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,266 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "ballencoder.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+ GZ_REGISTER_MODEL_PLUGIN(BallRed)
+
+ BallRed::BallRed () {
+ pthread_mutex_init(&mutex_ballredencoders, NULL);
+ pthread_mutex_init(&mutex_ballredmotors, NULL);
+ this->count = 0;
+ this->cycle = 50;
+ this->cfgfile_ballred = std::string("--Ice.Config=ballred.cfg");
+
+ std::cout << "Constructor BallRed" << std::endl;
+ }
+
+ void BallRed::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ this->ballred.joint_pan = _model->GetJoint("jball_vertical");
+ this->ballred.joint_tilt = _model->GetJoint("wmp2");
+
+ this->stiffness = 40.0;
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&BallRed::OnUpdate, this));
+ }
+
+ void BallRed::Init () {}
+
+ void BallRed::OnUpdate () {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (this->count == 0) {
+ this->count++;
+ pthread_t thr_ice;
+ pthread_create(&thr_ice, NULL, &thread_BallRedICE, (void*) this);
+
+ this->ballred.motorsdata.pan = 0.0;
+ this->ballred.motorsdata.tilt = 0.0;
+ }
+ // ----------ENCODERS----------
+ //GET pose3dencoders data from the ball (PAN&TILT)
+ this->ballred.encoders.pan = - this->ballred.joint_pan->GetAngle(0).Radian();
+ this->ballred.encoders.tilt = - this->ballred.joint_tilt->GetAngle(0).Radian();
+
+ // ----------MOTORS----------
+ if (this->ballred.motorsdata.pan > 0) {
+ if (this->ballred.encoders.pan < this->ballred.motorsdata.pan) {
+ this->ballred.joint_pan->SetVelocity(0, -0.3);
+ this->ballred.joint_pan->SetMaxForce(0, this->stiffness);
+ } else if (this->ballred.encoders.pan > this->ballred.motorsdata.pan) {
+ this->ballred.joint_pan->SetVelocity(0, 0.3);
+ this->ballred.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ } else if (this->ballred.motorsdata.pan < 0) {
+ if (this->ballred.encoders.pan > this->ballred.motorsdata.pan) {
+ this->ballred.joint_pan->SetVelocity(0, 0.3);
+ this->ballred.joint_pan->SetMaxForce(0, this->stiffness);
+ } else if (this->ballred.encoders.pan < this->ballred.motorsdata.pan) {
+ this->ballred.joint_pan->SetVelocity(0, -0.3);
+ this->ballred.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+
+ if (this->ballred.motorsdata.tilt > 0) {
+ if (this->ballred.encoders.tilt < this->ballred.motorsdata.tilt) {
+ this->ballred.joint_tilt->SetVelocity(0, -0.3);
+ this->ballred.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else if (this->ballred.encoders.tilt > this->ballred.motorsdata.tilt) {
+ this->ballred.joint_tilt->SetVelocity(0, 0.3);
+ this->ballred.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ } else if (this->ballred.motorsdata.tilt < 0) {
+ if (this->ballred.encoders.tilt > this->ballred.motorsdata.tilt) {
+ this->ballred.joint_tilt->SetVelocity(0, 0.3);
+ this->ballred.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else if (this->ballred.encoders.tilt < this->ballred.motorsdata.tilt) {
+ this->ballred.joint_tilt->SetVelocity(0, -0.3);
+ this->ballred.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseRedBall* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_redballencoders);
+
+ pose3DEncodersData->x = pose->ballred.encoders.x;
+ pose3DEncodersData->y = pose->ballred.encoders.y;
+ pose3DEncodersData->z = pose->ballred.encoders.z;
+ pose3DEncodersData->pan = pose->ballred.encoders.pan;
+ pose3DEncodersData->tilt = pose->ballred.encoders.tilt;
+ pose3DEncodersData->roll = pose->ballred.encoders.roll;
+ pose3DEncodersData->clock = pose->ballred.encoders.clock;
+ pose3DEncodersData->maxPan = pose->ballred.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->ballred.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->ballred.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->ballred.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_redballencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseNeck* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseRedBall* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_redballmotors);
+
+ pose3DMotorsData->x = pose->ballred.motorsdata.x;
+ pose3DMotorsData->y = pose->ballred.motorsdata.y;
+ pose3DMotorsData->z = pose->ballred.motorsdata.z;
+ pose3DMotorsData->pan = pose->ballred.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->ballred.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->ballred.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->ballred.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->ballred.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_redballmotors);
+
+ return pose3DMotorsData;
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_redballmotors);
+
+ pose3DMotorsParams->maxPan = pose->ballred.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->ballred.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->ballred.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->ballred.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->ballred.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->ballred.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_redballmotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_redballmotors);
+
+ pose->ballred.motorsdata.x = data->x;
+ pose->ballred.motorsdata.y = data->y;
+ pose->ballred.motorsdata.z = data->z;
+ pose->ballred.motorsdata.pan = data->pan;
+ pose->ballred.motorsdata.tilt = data->tilt;
+ pose->ballred.motorsdata.roll = data->roll;
+ pose->ballred.motorsdata.panSpeed = data->panSpeed;
+ pose->ballred.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_redballmotors);
+ }
+
+ gazebo::PoseRedBall* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_BallRedICE ( void* v ) {
+
+ gazebo::BallRed* ballred = (gazebo::BallRed*)v;
+ char* name = (char*) ballred->cfgfile_ballred.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 EndpointsEncoders = prop->getProperty("BallRedEncoders.Endpoints");
+ std::cout << "BallRedEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("BallRedMotors.Endpoints");
+ std::cout << "BallRedMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterBallRedEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterBallRedMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(ballred);
+ Ice::ObjectPtr motors = new Pose3DMotors(ballred);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("BallRedEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("BallRedMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->activate();
+
+ ic->waitForShutdown();
+ } catch (const Ice::Exception& e) {
+ std::cerr << e << std::endl;
+ } catch (const char* msg) {
+ std::cerr << msg << std::endl;
+ }
+ if (icB) {
+ try {
+ icB->destroy();
+ } catch (const Ice::Exception& e) {
+ std::cerr << e << std::endl;
+ }
+ }
+ }
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+BallRedEncoders.Endpoints=default -h localhost -p 9194
+BallRedMotors.Endpoints=default -h localhost -p 9195
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef BALLRED_H
+#define BALLRED_H
+
+namespace gazebo {
+
+ void* thread_BallRedICE ( void* v );
+
+ class BallRed : public ModelPlugin {
+ public:
+
+ BallRed ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_ballred;
+ pthread_mutex_t mutex_ballredencoders;
+ pthread_mutex_t mutex_ballredmotors;
+
+ struct ballred_t {
+ physics::JointPtr joint_tilt, joint_pan;
+ physics::LinkPtr link_pan, link_tilt;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ ballred_t ballred;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // BALLRED_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/CMakeLists.txt (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/CMakeLists.txt 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+include (FindPkgConfig)
+if (PKG_CONFIG_FOUND)
+ pkg_check_modules(GAZEBO gazebo)
+ message(${GAZEBO_INCLUDE_DIRS})
+endif()
+
+SET( INTERFACES_CPP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../interfaces/cpp) # Directorio con las interfaces ICE en C++
+SET( LIBS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../libs) # Directorio donde se encuentran las librerias propias de jderobot
+SET( SLICE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../interfaces/slice) # Directorio donde se encuentran las interfaces ICE
+SET( LIBS_NEEDED jderobotice colorspaces jderobotutil) # Librerias de las que depende el componente
+SET( DEPS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../../Deps) # Directorio donde se encuentran las dependencias
+
+include(FindPkgConfig)
+
+include(${DEPS_DIR}/gearbox/CMakeLists.txt)
+include(${DEPS_DIR}/ice/CMakeLists.txt)
+include(${DEPS_DIR}/opencv/CMakeLists.txt)
+
+add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/../../../.. ${CMAKE_CURRENT_SOURCE_DIR}/../../../..)
+add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/.. ${CMAKE_CURRENT_SOURCE_DIR}/..)
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/clean.sh
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/clean.sh (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/clean.sh 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+make clean
+rm ../cmake_install.cmake
+rm -r CMakeFiles
+rm -r ../CMakeFiles
+mkdir ../temp
+mv CMakeLists.txt ../temp
+mv clean.sh ../temp
+mv cmake_uninstall.cmake.in ../temp
+rm -r *
+mv ../temp/CMakeLists.txt .
+mv ../temp/clean.sh .
+mv ../temp/cmake_uninstall.cmake.in .
+rm -r ../temp
+cd ../../../../
+./clean_repository
Property changes on: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/clean.sh
___________________________________________________________________
Added: svn:executable
+ *
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_bottom.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_bottom.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_bottom.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,8 @@
+#without registry
+CameraGazebo.Endpoints=default -h 0.0.0.0 -p 9991
+
+#camera 1
+CameraGazebo.Camera.0.Name=cameraA
+CameraGazebo.Camera.0.ImageWidth=320
+CameraGazebo.Camera.0.ImageHeight=240
+CameraGazebo.Camera.0.Format=RGB8
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_top.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_top.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_top.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,8 @@
+#without registry
+CameraGazebo.Endpoints=default -h 0.0.0.0 -p 9990
+
+#camera 1
+CameraGazebo.Camera.0.Name=cameraA
+CameraGazebo.Camera.0.ImageWidth=320
+CameraGazebo.Camera.0.ImageHeight=240
+CameraGazebo.Camera.0.Format=RGB8
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/camera_dump.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/camera_dump.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/camera_dump.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -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;
+ }
+ }
+
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/common.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/common.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/common.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#ifndef NAOCOMMON_H
+#define NAOCOMMON_H
+
+struct encoders_t {
+ float x;
+ float y;
+ float z;
+ float pan;
+ float tilt;
+ float roll;
+ int clock;
+ float maxPan;
+ float maxTilt;
+ float minPan;
+ float minTilt;
+};
+
+struct motorsdata_t {
+ float x;
+ float y;
+ float z;
+ float pan;
+ float tilt;
+ float roll;
+ float panSpeed;
+ float tiltSpeed;
+};
+
+struct motorsparams_t {
+ float maxPan;
+ float minPan;
+ float maxTilt;
+ float minTilt;
+ float maxPanSpeed;
+ float maxTiltSpeed;
+};
+
+#endif // NAOCOMMON_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/naoworld.world
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/naoworld.world (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/naoworld.world 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,32 @@
+<?xml version="1.0" ?>
+<sdf version="1.3">
+ <world name="default">
+ <!-- Ground -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <!-- My robots -->
+ <include>
+ <uri>model://nao_robot</uri>
+ <pose>0.08 0 0 0 0 3.14</pose>
+ </include>
+
+ <!-- A global light source -->
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <!-- A ball -->
+<!-- <include>
+ <uri>model://pelota</uri>
+ <pose>0 -2 0 0 0 0</pose>
+ </include>
+
+ <include>
+ <uri>model://pelota_verde</uri>
+ <pose>-0.5 -2.3 0 0 0 0</pose>
+ </include>
+-->
+ </world>
+</sdf>
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,295 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poseleftankle.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+ GZ_REGISTER_MODEL_PLUGIN(Pose3DEncodersLeftAnkle)
+
+ PoseLeftAnkle::PoseLeftAnkle () {
+ pthread_mutex_init(&mutex_lefankleencoders, NULL);
+ pthread_mutex_init(&mutex_lefanklemotors, NULL);
+ this->count = 0;
+ this->cycle = 50;
+ this->cfgfile_leftankle = std::string("--Ice.Config=poseleftankle.cfg");
+
+ this->leftankle.motorsparams.maxPan = 1.57;
+ this->leftankle.motorsparams.minPan = -1.57;
+ this->leftankle.motorsparams.maxTilt = 0.5;
+ this->leftankle.motorsparams.minTilt = -0.5;
+
+ std::cout << "Constructor PoseLeftAnkle" << std::endl;
+ }
+
+ void PoseLeftAnkle::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ // LOAD CAMERA LEFT
+ if (!_sdf->HasElement("joint_pose3dencodersleftankle_pitch"))
+ gzerr << "pose3dencodersleftankle plugin missing <joint_pose3dencodersleftankle_pitch> element\n";
+ if (!_sdf->HasElement("joint_pose3dencodersleftankle_roll"))
+ gzerr << "pose3dencodersleftankle plugin missing <joint_pose3dencodersleftankle_roll> element\n";
+
+ this->leftankle.joint_roll = this->model->GetJoint("lankle_roll");
+ this->leftankle.joint_tilt = this->model->GetJoint("lankle_pitch");
+
+ if (!this->leftankle.joint_roll)
+ gzerr << "Unable to find joint_pose3dencodersleftankle_roll["
+ << _sdf->GetElement("joint_pose3dencodersleftankle_roll")->GetValueString() << "]\n";
+ if (!this->leftankle.joint_tilt)
+ gzerr << "Unable to find joint_pose3dencodersleftankle_pitch["
+ << _sdf->GetElement("joint_pose3dencodersleftankle_pitch")->GetValueString() << "]\n";
+
+ this->leftankle.link_roll = this->model->GetLink("left_foot");
+ this->leftankle.link_tilt = this->model->GetLink("leftankle_pitch");
+
+ //LOAD TORQUE
+ if (_sdf->HasElement("torque"))
+ this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+ else {
+ gzwarn << "No torque value set for the DiffDrive plugin.\n";
+ this->stiffness = 5.0;
+ }
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&PoseLeftAnkle::OnUpdate, this));
+ }
+
+ void PoseLeftAnkle::Init () {}
+
+ void PoseLeftAnkle::OnUpdate () {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (count == 0) {
+ count++;
+ pthread_t thr_ice;
+ pthread_create(&thr_ice, NULL, &thread_LeftAnkleICE, (void*) this);
+
+ this->leftankle.encoders.roll = 0;
+ this->leftankle.encoders.tilt = 0;
+ } else {
+ // ----------ENCODERS----------
+ //GET pose3dencoders data from the left ankle (PAN&TILT)
+ this->leftankle.encoders.roll = this->leftankle.link_roll->GetRelativePose().rot.GetAsEuler().y;
+ this->leftankle.encoders.tilt = this->leftankle.link_tilt->GetRelativePose().rot.GetAsEuler().x;
+ }
+
+ // ----------MOTORS----------
+ if (this->leftankle.motorsdata.roll >= 0) {
+ if (this->leftankle.encoders.roll < this->leftankle.motorsdata.roll) {
+ this->leftankle.joint_roll->SetVelocity(0, -0.1);
+ this->leftankle.joint_roll->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->leftankle.joint_roll->SetVelocity(0, 0.1);
+ this->leftankle.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->leftankle.encoders.roll > this->leftankle.motorsdata.roll) {
+ this->leftankle.joint_roll->SetVelocity(0, 0.1);
+ this->leftankle.joint_roll->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->leftankle.joint_roll->SetVelocity(0, -0.1);
+ this->leftankle.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ if (this->leftankle.motorsdata.tilt >= 0) {
+ if (this->leftankle.encoders.tilt < this->leftankle.motorsdata.tilt) {
+ this->leftankle.joint_tilt->SetVelocity(0, -0.1);
+ this->leftankle.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->leftankle.joint_tilt->SetVelocity(0, 0.1);
+ this->leftankle.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->leftankle.encoders.tilt > this->leftankle.motorsdata.tilt) {
+ this->leftankle.joint_tilt->SetVelocity(0, 0.1);
+ this->leftankle.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->leftankle.joint_tilt->SetVelocity(0, -0.1);
+ this->leftankle.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseLeftAnkle* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftankleencoders);
+
+ pose3DEncodersData->x = pose->leftankle.encoders.x;
+ pose3DEncodersData->y = pose->leftankle.encoders.y;
+ pose3DEncodersData->z = pose->leftankle.encoders.z;
+ pose3DEncodersData->pan = pose->leftankle.encoders.pan;
+ pose3DEncodersData->tilt = pose->leftankle.encoders.tilt;
+ pose3DEncodersData->roll = pose->leftankle.encoders.roll;
+ pose3DEncodersData->clock = pose->leftankle.encoders.clock;
+ pose3DEncodersData->maxPan = pose->leftankle.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->leftankle.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->leftankle.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->leftankle.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_leftankleencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseLeftAnkle* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseLeftAnkle* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftanklemotors);
+
+ pose3DMotorsData->x = pose->leftankle.motorsdata.x;
+ pose3DMotorsData->y = pose->leftankle.motorsdata.y;
+ pose3DMotorsData->z = pose->leftankle.motorsdata.z;
+ pose3DMotorsData->pan = pose->leftankle.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->leftankle.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->leftankle.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->leftankle.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->leftankle.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_leftanklemotors);
+
+ return pose3DMotorsData;
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftanklemotors);
+
+ pose3DMotorsParams->maxPan = pose->leftankle.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->leftankle.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->leftankle.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->leftankle.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->leftankle.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->leftankle.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_leftanklemotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftanklemotors);
+
+ pose->leftankle.motorsdata.x = data->x;
+ pose->leftankle.motorsdata.y = data->y;
+ pose->leftankle.motorsdata.z = data->z;
+ pose->leftankle.motorsdata.pan = data->pan;
+ pose->leftankle.motorsdata.tilt = data->tilt;
+ pose->leftankle.motorsdata.roll = data->roll;
+ pose->leftankle.motorsdata.panSpeed = data->panSpeed;
+ pose->leftankle.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_leftanklemotors);
+ }
+
+ gazebo::PoseLeftAnkle* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_LeftAnkleICE ( void* v ) {
+
+ gazebo::PoseLeftAnkle* leftankle = (gazebo::PoseLeftAnkle*)v;
+ char* name = (char*) leftankle->cfgfile_leftankle.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 EndpointsEncoders = prop->getProperty("PoseLeftAnkleEncoders.Endpoints");
+ std::cout << "PoseLeftAnkleEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("PoseLeftAnkleMotors.Endpoints");
+ std::cout << "PoseLeftAnkleMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterLeftAnkleEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterLeftAnkleMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(leftankle);
+ Ice::ObjectPtr motors = new Pose3DMotors(leftankle);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("LeftAnkleEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("LeftAnkleMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->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;
+ }
+ }
+ }
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseLeftAnkleEncoders.Endpoints=default -h localhost -p 11996
+PoseLeftAnkleMotors.Endpoints=default -h localhost -p 11997
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef POSELEFTANKLE_H
+#define POSELEFTANKLE_H
+
+namespace gazebo {
+
+ void* thread_LeftAnkleICE ( void* v );
+
+ class PoseLeftAnkle : public ModelPlugin {
+ public:
+
+ PoseLeftAnkle ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_lefankle;
+ pthread_mutex_t mutex_lefankleencoders;
+ pthread_mutex_t mutex_lefanklemotors;
+
+ struct lefankle_t {
+ physics::JointPtr joint_tilt, joint_pan;
+ physics::LinkPtr link_pan, link_tilt;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ lefankle_t lefankle;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // POSELEFTANKLE_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,296 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poseleftelbow.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+
+ GZ_REGISTER_MODEL_PLUGIN(PoseLeftElbow)
+
+ PoseLeftElbow::PoseLeftElbow () {
+ pthread_mutex_init(&this->mutex_leftelbowencoders, NULL);
+ pthread_mutex_init(&this->mutex_leftelbowmotors, NULL);
+ this->count = 0;
+ this->cycle = 50;
+ this->cfgfile_leftelbow = std::string("--Ice.Config=poseleftelbow.cfg");
+
+ this->leftelbow.motorsparams.maxPan = 1.57;
+ this->leftelbow.motorsparams.minPan = -1.57;
+ this->leftelbow.motorsparams.maxTilt = 0.5;
+ this->leftelbow.motorsparams.minTilt = -0.5;
+
+ std::cout << "Constructor PoseLeftElbow" << std::endl;
+ }
+
+ void PoseLeftElbow::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ // LOAD CAMERA LEFT
+ if (!_sdf->HasElement("joint_pose3dencodersleftelbow_yaw"))
+ gzerr << "pose3dencodersleftelbow plugin missing <joint_pose3dencodersleftelbow_yaw> element\n";
+ if (!_sdf->HasElement("joint_pose3dencodersleftelbow_roll"))
+ gzerr << "pose3dencodersleftelbow plugin missing <joint_pose3dencodersleftelbow_roll> element\n";
+
+ this->leftelbow.joint_pan = _model->GetJoint("lelbow_yaw");
+ this->leftelbow.joint_roll = _model->GetJoint("lelbow_roll");
+
+ if (!this->leftelbow.joint_pan)
+ gzerr << "Unable to find joint_pose3dencodersleftelbow_yaw["
+ << _sdf->GetElement("joint_pose3dencodersleftelbow_yaw")->GetValueString() << "]\n";
+ if (!this->leftelbow.joint_roll)
+ gzerr << "Unable to find joint_pose3dencodersleftelbow_roll["
+ << _sdf->GetElement("joint_pose3dencodersleftelbow_roll")->GetValueString() << "]\n";
+
+ this->leftelbow.link_pan = _model->GetLink("leftelbow_yaw");
+ this->leftelbow.link_roll = _model->GetLink("left_lower_arm");
+
+ //LOAD TORQUE
+ if (_sdf->HasElement("torque"))
+ this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+ else {
+ gzwarn << "No torque value set for the DiffDrive plugin.\n";
+ this->stiffness = 5.0;
+ }
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&PoseLeftElbow::OnUpdate, this));
+ }
+
+ void PoseLeftElbow::Init () {}
+
+ void PoseLeftElbow::OnUpdate () {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (this->count == 0) {
+ this->count++;
+ pthread_t thr_ice;
+ pthread_create(&thr_ice, NULL, &thread_LeftElbowICE, (void*) this);
+ }
+
+ // ----------ENCODERS----------
+ //GET pose3dencoders data from the left elbow (PAN&TILT)
+// this->leftelbow.encoder.pan = this->leftelbow.leftelbow_link_pan->GetRelativePose().rot.GetAsEuler().z;
+// this->leftelbow.encoder.roll = this->leftelbow.leftelbow_link_roll->GetRelativePose().rot.GetAsEuler().y;
+
+ this->leftelbow.encoders.pan = - this->leftelbow.joint_pan->GetAngle(0).Radian();
+ this->leftelbow.encoders.roll = - this->leftelbow.joint_roll->GetAngle(0).Radian();
+
+ // ----------MOTORS----------
+ if (this->leftelbow.motorsdata.pan >= 0) {
+ if (this->leftelbow.encoders.pan < this->leftelbow.motorsdata.pan) {
+ this->leftelbow.joint_pan->SetVelocity(0, -0.1);
+ this->leftelbow.joint_pan->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->leftelbow.joint_pan->SetVelocity(0, 0.1);
+ this->leftelbow.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->leftelbow.encoders.pan > this->leftelbow.motorsdata.pan) {
+ this->leftelbow.joint_pan->SetVelocity(0, 0.1);
+ this->leftelbow.joint_pan->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->leftelbow.joint_pan->SetVelocity(0, -0.1);
+ this->leftelbow.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ if (this->leftelbow.motorsdata.roll >= 0) {
+ if (this->leftelbow.encoders.roll < this->leftelbow.motorsdata.roll) {
+ this->leftelbow.joint_roll->SetVelocity(0, -0.1);
+ this->leftelbow.joint_roll->SetMaxForce(0, this->stiffness);
+ } else {
+ this->leftelbow.joint_roll->SetVelocity(0, 0.1);
+ this->leftelbow.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->leftelbow.encoders.roll > this->leftelbow.motorsdata.roll) {
+ this->leftelbow.joint_roll->SetVelocity(0, 0.1);
+ this->leftelbow.joint_roll->SetMaxForce(0, this->stiffness);
+ } else {
+ this->leftelbow.joint_roll->SetVelocity(0, -0.1);
+ this->leftelbow.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseLeftElbow* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftelbowencoders);
+
+ pose3DEncodersData->x = pose->leftelbow.encoders.x;
+ pose3DEncodersData->y = pose->leftelbow.encoders.y;
+ pose3DEncodersData->z = pose->leftelbow.encoders.z;
+ pose3DEncodersData->pan = pose->leftelbow.encoders.pan;
+ pose3DEncodersData->tilt = pose->leftelbow.encoders.tilt;
+ pose3DEncodersData->roll = pose->leftelbow.encoders.roll;
+ pose3DEncodersData->clock = pose->leftelbow.encoders.clock;
+ pose3DEncodersData->maxPan = pose->leftelbow.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->leftelbow.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->leftelbow.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->leftelbow.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_leftelbowencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseLeftElbow* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseLeftElbow* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftelbowmotors);
+
+ pose3DMotorsData->x = pose->leftelbow.motorsdata.x;
+ pose3DMotorsData->y = pose->leftelbow.motorsdata.y;
+ pose3DMotorsData->z = pose->leftelbow.motorsdata.z;
+ pose3DMotorsData->pan = pose->leftelbow.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->leftelbow.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->leftelbow.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->leftelbow.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->leftelbow.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_leftelbowmotors);
+
+ return pose3DMotorsData;
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftelbowmotors);
+
+ pose3DMotorsParams->maxPan = pose->leftelbow.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->leftelbow.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->leftelbow.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->leftelbow.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->leftelbow.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->leftelbow.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_leftelbowmotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftelbowmotors);
+
+ pose->leftelbow.motorsdata.x = data->x;
+ pose->leftelbow.motorsdata.y = data->y;
+ pose->leftelbow.motorsdata.z = data->z;
+ pose->leftelbow.motorsdata.pan = data->pan;
+ pose->leftelbow.motorsdata.tilt = data->tilt;
+ pose->leftelbow.motorsdata.roll = data->roll;
+ pose->leftelbow.motorsdata.panSpeed = data->panSpeed;
+ pose->leftelbow.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_leftelbowmotors);
+ }
+
+ gazebo::PoseLeftElbow* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_LeftElbowICE ( void* v ) {
+
+ gazebo::PoseLeftElbow* leftelbow = (gazebo::PoseLeftElbow*)v;
+ char* name = (char*) leftelbow->cfgfile_leftelbow.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 EndpointsEncoders = prop->getProperty("PoseLeftElbowEncoders.Endpoints");
+ std::cout << "PoseNeckEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("PoseLeftElbowMotors.Endpoints");
+ std::cout << "PoseNeckMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterLeftElbowEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterLeftElbowMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(leftelbow);
+ Ice::ObjectPtr motors = new Pose3DMotors(leftelbow);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("LeftElbowEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("LeftElbowMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->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;
+ }
+ }
+ }
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseLeftElbowEncoders.Endpoints=default -h localhost -p 5998
+PoseLeftElbowMotors.Endpoints=default -h localhost -p 5999
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef POSELEFTELBOW_H
+#define POSELEFTELBOW_H
+
+namespace gazebo {
+
+ void* thread_LeftElbowICE ( void* v );
+
+ class PoseLeftElbow : public ModelPlugin {
+ public:
+
+ PoseLeftElbow ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_leftelbow;
+ pthread_mutex_t mutex_leftelbowencoders;
+ pthread_mutex_t mutex_leftelbowmotors;
+
+ struct leftelbow_t {
+ physics::JointPtr joint_pan, joint_roll;
+ physics::LinkPtr link_pan, link_roll;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ leftelbow_t leftelbow;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // POSELEFTELBOW_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,295 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poseleftshoulder.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+ GZ_REGISTER_MODEL_PLUGIN(PoseLeftShoulder)
+
+ PoseLeftShoulder::PoseLeftShoulder () {
+ pthread_mutex_init(&this->mutex_leftshoulderencoders, NULL);
+ pthread_mutex_init(&this->mutex_leftshouldermotors, NULL);
+ this->count = 0;
+ this->cycle = 50;
+ this->cfgfile_leftshoulder = std::string("--Ice.Config=poseleftshoulder.cfg");
+
+ this->leftshoulder.motorsparams.maxPan = 1.57;
+ this->leftshoulder.motorsparams.minPan = -1.57;
+ this->leftshoulder.motorsparams.maxTilt = 0.5;
+ this->leftshoulder.motorsparams.minTilt = -0.5;
+
+ std::cout << "Constructor PoseLeftShoulder" << std::endl;
+ }
+
+ void PoseLeftShoulder::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ // LOAD CAMERA LEFT
+ if (!_sdf->HasElement("joint_pose3dencodersleftshoulder_pitch"))
+ gzerr << "pose3dencodersleftshoulder plugin missing <joint_pose3dencodersleftshoulder_pitch> element\n";
+ if (!_sdf->HasElement("joint_pose3dencodersleftshoulder_roll"))
+ gzerr << "pose3dencodersleftshoulder plugin missing <joint_pose3dencodersleftshoulder_roll> element\n";
+
+ this->leftshoulder.joint_roll = _model->GetJoint("lshoulder_roll");
+ this->leftshoulder.joint_tilt = _model->GetJoint("lshoulder_pitch");
+
+ if (!this->leftshoulder.joint_roll)
+ gzerr << "Unable to find joint_pose3dencodersleftshoulder_roll["
+ << _sdf->GetElement("joint_pose3dencodersleftshoulder_roll")->GetValueString() << "]\n";
+ if (!this->leftshoulder.joint_tilt)
+ gzerr << "Unable to find joint_pose3dencodersleftshoulder_pitch["
+ << _sdf->GetElement("joint_pose3dencodersleftshoulder_pitch")->GetValueString() << "]\n";
+
+ this->leftshoulder.link_roll = _model->GetLink("left_upper_arm");
+ this->leftshoulder.link_tilt = _model->GetLink("leftshoulder_pitch");
+
+ //LOAD TORQUE
+ if (_sdf->HasElement("torque"))
+ this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+ else {
+ gzwarn << "No torque value set for left shoulder.\n";
+ this->stiffness = 5.0;
+ }
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&PoseLeftShoulder::OnUpdate, this));
+ }
+
+ void PoseLeftShoulder::Init () {}
+
+ void PoseLeftShoulder::OnUpdate() {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (this->count == 0) {
+ this->count++;
+ pthread_t thr_gui;
+ pthread_create(&thr_gui, NULL, &thread_LeftShoulderICE, (void*) this);
+ }
+
+ // ----------ENCODERS----------
+ //GET pose3dencoders data from the left shoulder (PAN&TILT)
+// this->leftshoulder.encoder.roll = this->leftshoulder.leftshoulder_link_roll->GetRelativePose().rot.GetAsEuler().y;
+// this->leftshoulder.encoder.tilt = this->leftshoulder.leftshoulder_link_tilt->GetRelativePose().rot.GetAsEuler().x;
+
+ this->leftshoulder.encoders.tilt = - this->leftshoulder.joint_tilt->GetAngle(0).Radian();
+ this->leftshoulder.encoders.roll = - this->leftshoulder.joint_roll->GetAngle(0).Radian();
+
+ // ----------MOTORS----------
+ if (this->leftshoulder.motorsdata.roll >= 0) {
+ if (this->leftshoulder.encoders.roll < this->leftshoulder.motorsdata.roll) {
+ this->leftshoulder.joint_roll->SetVelocity(0, -0.1);
+ this->leftshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->leftshoulder.joint_roll->SetVelocity(0, 0.1);
+ this->leftshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->leftshoulder.encoders.roll > this->leftshoulder.motorsdata.roll) {
+ this->leftshoulder.joint_roll->SetVelocity(0, 0.1);
+ this->leftshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->leftshoulder.joint_roll->SetVelocity(0, -0.1);
+ this->leftshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ if (this->leftshoulder.motorsdata.tilt >= 0) {
+ if (this->leftshoulder.encoders.tilt < this->leftshoulder.motorsdata.tilt) {
+ this->leftshoulder.joint_tilt->SetVelocity(0, -0.1);
+ this->leftshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->leftshoulder.joint_tilt->SetVelocity(0, 0.1);
+ this->leftshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->leftshoulder.encoders.tilt > this->leftshoulder.motorsdata.tilt) {
+ this->leftshoulder.joint_tilt->SetVelocity(0, 0.1);
+ this->leftshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->leftshoulder.joint_tilt->SetVelocity(0, -0.1);
+ this->leftshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseLeftShoulder* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftshoulderencoders);
+
+ pose3DEncodersData->x = pose->leftshoulder.encoders.x;
+ pose3DEncodersData->y = pose->leftshoulder.encoders.y;
+ pose3DEncodersData->z = pose->leftshoulder.encoders.z;
+ pose3DEncodersData->pan = pose->leftshoulder.encoders.pan;
+ pose3DEncodersData->tilt = pose->leftshoulder.encoders.tilt;
+ pose3DEncodersData->roll = pose->leftshoulder.encoders.roll;
+ pose3DEncodersData->clock = pose->leftshoulder.encoders.clock;
+ pose3DEncodersData->maxPan = pose->leftshoulder.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->leftshoulder.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->leftshoulder.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->leftshoulder.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_leftshoulderencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseLeftShoulder* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseLeftShoulder* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftshouldermotors);
+
+ pose3DMotorsData->x = pose->leftshoulder.motorsdata.x;
+ pose3DMotorsData->y = pose->leftshoulder.motorsdata.y;
+ pose3DMotorsData->z = pose->leftshoulder.motorsdata.z;
+ pose3DMotorsData->pan = pose->leftshoulder.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->leftshoulder.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->leftshoulder.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->leftshoulder.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->leftshoulder.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_leftshouldermotors);
+
+ return pose3DMotorsData;
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftshouldermotors);
+
+ pose3DMotorsParams->maxPan = pose->leftshoulder.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->leftshoulder.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->leftshoulder.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->leftshoulder.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->leftshoulder.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->leftshoulder.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_leftshouldermotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_leftshouldermotors);
+
+ pose->leftshoulder.motorsdata.x = data->x;
+ pose->leftshoulder.motorsdata.y = data->y;
+ pose->leftshoulder.motorsdata.z = data->z;
+ pose->leftshoulder.motorsdata.pan = data->pan;
+ pose->leftshoulder.motorsdata.tilt = data->tilt;
+ pose->leftshoulder.motorsdata.roll = data->roll;
+ pose->leftshoulder.motorsdata.panSpeed = data->panSpeed;
+ pose->leftshoulder.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_leftshouldermotors);
+ }
+
+ gazebo::PoseLeftShoulder* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_LeftShoulderICE ( void* v ) {
+
+ gazebo::PoseLeftShoulder* leftshoulder = (gazebo::PoseLeftShoulder*)v;
+ char* name = (char*) leftshoulder->cfgfile_leftshoulder.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 EndpointsEncoders = prop->getProperty("PoseLeftShoulderEncoders.Endpoints");
+ std::cout << "PoseLeftShoulderEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("PoseLeftShoulderMotors.Endpoints");
+ std::cout << "PoseLeftShoulderMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterLeftShoulderEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterLeftShoulderMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(leftshoulder);
+ Ice::ObjectPtr motors = new Pose3DMotors(leftshoulder);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("LeftShoulderEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("LeftShoulderMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->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;
+ }
+ }
+ }
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseLeftShoulderEncoders.Endpoints=default -h localhost -p 7996
+PoseLeftShoulderMotors.Endpoints=default -h localhost -p 7997
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef LEFTSHOULDER_H
+#define LEFTSHOULDER_H
+
+namespace gazebo {
+
+ void* thread_LeftShoulderICE ( void* v );
+
+ class PoseLeftShoulder : public ModelPlugin {
+ public:
+
+ PoseLeftShoulder ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_leftshoulder;
+ pthread_mutex_t mutex_leftshoulderencoders;
+ pthread_mutex_t mutex_leftshouldermotors;
+
+ struct leftshoulder_t {
+ physics::JointPtr joint_tilt, joint_roll;
+ physics::LinkPtr link_tilt, link_roll;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ leftshoulder_t leftshoulder;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // LEFTSHOULDER_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,295 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poseneck.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+ GZ_REGISTER_MODEL_PLUGIN(PoseNeck)
+
+ PoseNeck::PoseNeck () {
+ pthread_mutex_init(&this->mutex_neckencoders, NULL);
+ pthread_mutex_init(&this->mutex_neckmotors, NULL);
+ this->count = 0;
+ this->cycle = 50;
+ this->cfgfile_neck = std::string("--Ice.Config=poseneck.cfg");
+
+ this->neck.motorsparams.maxPan = 1.57;
+ this->neck.motorsparams.minPan = -1.57;
+ this->neck.motorsparams.maxTilt = 0.5;
+ this->neck.motorsparams.minTilt = -0.5;
+
+ std::cout << "Constructor PoseNeck" << std::endl;
+ }
+
+ void PoseNeck::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ // LOAD CAMERA LEFT
+ if (!_sdf->HasElement("bottom_joint_pose3dencodersneck_pan"))
+ gzerr << "pose3dencodersneck plugin missing <bottom_joint_pose3dencodersneck_pan> element\n";
+ if (!_sdf->HasElement("top_joint_pose3dencodersneck_tilt"))
+ gzerr << "pose3dencodersneck plugin missing <top_joint_pose3dencodersneck_tilt> element\n";
+
+ this->neck.joint_pan = _model->GetJoint("neck_pan");
+ this->neck.joint_tilt = _model->GetJoint("neck_tilt");
+
+ if (!this->neck.joint_pan)
+ gzerr << "Unable to find bottom_joint_pose3dencodersneck_pan["
+ << _sdf->GetElement("bottom_joint_pose3dencodersneck_pan")->GetValueString() << "]\n";
+ if (!this->neck.joint_tilt)
+ gzerr << "Unable to find top_joint_pose3dencodersneck_tilt["
+ << _sdf->GetElement("top_joint_pose3dencodersneck_tilt")->GetValueString() << "]\n";
+
+ this->neck.link_pan = _model->GetLink("head_pan");
+ this->neck.link_tilt = _model->GetLink("head_tilt");
+
+ //LOAD TORQUE
+ if (_sdf->HasElement("torque"))
+ this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+ else {
+ gzwarn << "No torque value set for the DiffDrive plugin.\n";
+ this->stiffness = 5.0;
+ }
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&PoseNeck::OnUpdate, this));
+
+ }
+
+ void PoseNeck::Init () {}
+
+ void PoseNeck::OnUpdate () {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (this->count == 0) {
+ this->count++;
+ pthread_t thr_ice;
+ pthread_create(&thr_ice, NULL, &thread_NeckICE, (void*) this);
+ }
+
+ // ----------ENCODERS----------
+ //GET pose3dencoders data from the neck (PAN&TILT)
+ this->neck.encoders.pan = this->neck.link_pan->GetRelativePose().rot.GetAsEuler().z;
+ this->neck.encoders.tilt = this->neck.link_tilt->GetRelativePose().rot.GetAsEuler().x;
+
+ // ----------MOTORS----------
+ if (this->neck.motorsdata.pan >= 0) {
+ if (this->neck.encoders.pan < this->neck.motorsdata.pan) {
+ this->neck.joint_pan->SetVelocity(0, -0.1);
+ this->neck.joint_pan->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->neck.joint_pan->SetVelocity(0, 0.1);
+ this->neck.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->neck.encoders.pan > this->neck.motorsdata.pan) {
+ this->neck.joint_pan->SetVelocity(0, 0.1);
+ this->neck.joint_pan->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->neck.joint_pan->SetVelocity(0, -0.1);
+ this->neck.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ if (this->neck.motorsdata.tilt >= 0) {
+ if (this->neck.encoders.tilt < this->neck.motorsdata.tilt) {
+ this->neck.joint_tilt->SetVelocity(0, -0.1);
+ this->neck.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->neck.joint_tilt->SetVelocity(0, 0.1);
+ this->neck.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->neck.encoders.tilt > this->neck.motorsdata.tilt) {
+ this->neck.joint_tilt->SetVelocity(0, 0.1);
+ this->neck.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->neck.joint_tilt->SetVelocity(0, -0.1);
+ this->neck.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseNeck* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_neckencoders);
+
+ pose3DEncodersData->x = pose->neck.encoders.x;
+ pose3DEncodersData->y = pose->neck.encoders.y;
+ pose3DEncodersData->z = pose->neck.encoders.z;
+ pose3DEncodersData->pan = pose->neck.encoders.pan;
+ pose3DEncodersData->tilt = pose->neck.encoders.tilt;
+ pose3DEncodersData->roll = pose->neck.encoders.roll;
+ pose3DEncodersData->clock = pose->neck.encoders.clock;
+ pose3DEncodersData->maxPan = pose->neck.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->neck.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->neck.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->neck.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_neckencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseNeck* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseNeck* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_neckmotors);
+
+ pose3DMotorsData->x = pose->neck.motorsdata.x;
+ pose3DMotorsData->y = pose->neck.motorsdata.y;
+ pose3DMotorsData->z = pose->neck.motorsdata.z;
+ pose3DMotorsData->pan = pose->neck.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->neck.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->neck.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->neck.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->neck.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_neckmotors);
+
+ return pose3DMotorsData;
+
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_neckmotors);
+
+ pose3DMotorsParams->maxPan = pose->neck.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->neck.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->neck.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->neck.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->neck.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->neck.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_neckmotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_neckmotors);
+
+ pose->neck.motorsdata.x = data->x;
+ pose->neck.motorsdata.y = data->y;
+ pose->neck.motorsdata.z = data->z;
+ pose->neck.motorsdata.pan = data->pan;
+ pose->neck.motorsdata.tilt = data->tilt;
+ pose->neck.motorsdata.roll = data->roll;
+ pose->neck.motorsdata.panSpeed = data->panSpeed;
+ pose->neck.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_neckmotors);
+
+ }
+
+ gazebo::PoseNeck* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_NeckICE ( void* v ) {
+
+ gazebo::PoseNeck* neck = (gazebo::PoseNeck*)v;
+ char* name = (char*) neck->cfgfile_neck.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 EndpointsEncoders = prop->getProperty("PoseNeckEncoders.Endpoints");
+ std::cout << "PoseNeckEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("PoseNeckMotors.Endpoints");
+ std::cout << "PoseNeckMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterNeckEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterNeckMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(neck);
+ Ice::ObjectPtr motors = new Pose3DMotors(neck);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("NeckEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("NeckMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->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;
+ }
+ }
+ }
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseNeckMotors.Endpoints=default -h localhost -p 9994
+PoseNeckEncoders.Endpoints=default -h localhost -p 9995
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef POSENECK_H
+#define POSENECK_H
+
+namespace gazebo {
+
+ void* thread_NeckICE ( void* v );
+
+ class PoseNeck : public ModelPlugin {
+ public:
+
+ PoseNeck ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_neck;
+ pthread_mutex_t mutex_neckencoders;
+ pthread_mutex_t mutex_neckmotors;
+
+ struct neck_t {
+ physics::JointPtr joint_tilt, joint_pan;
+ physics::LinkPtr link_pan, link_tilt;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ neck_t neck;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // POSENECK_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,259 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poseneckspeed.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+ GZ_REGISTER_MODEL_PLUGIN(PoseNeckSpeed)
+
+ PoseNeckSpeed::PoseNeckSpeed () {
+ pthread_mutex_init(&this->mutex_neckspeedencoders, NULL);
+ pthread_mutex_init(&this->mutex_neckspeedmotors, NULL);
+ this->count = 0;
+ this->cycle = 50;
+ this->cfgfile_neckspeed = std::string("--Ice.Config=poseneckspeed.cfg");
+
+ this->neckspeed.motorsparams.maxPan = 1.57;
+ this->neckspeed.motorsparams.minPan = -1.57;
+ this->neckspeed.motorsparams.maxTilt = 0.5;
+ this->neckspeed.motorsparams.minTilt = -0.5;
+
+ std::cout << "Constructor PoseNeckSpeed" << std::endl;
+ }
+
+ void PoseNeckSpeed::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ // LOAD CAMERA LEFT
+ if (!_sdf->HasElement("bottom_joint_pose3dencodersneckvel_pan"))
+ gzerr << "pose3dencodersneck plugin missing <bottom_joint_pose3dencodersneckvel_pan> element\n";
+ if (!_sdf->HasElement("top_joint_pose3dencodersneckvel_tilt"))
+ gzerr << "pose3dencodersneck plugin missing <top_joint_pose3dencodersneckvel_tilt> element\n";
+
+ this->neckspeed.joint_pan = _model->GetJoint("neck_pan");
+ this->neckspeed.joint_tilt = _model->GetJoint("neck_tilt");
+
+ if (!this->neckspeed.joint_pan)
+ gzerr << "Unable to find bottom_joint_pose3dencodersneckvel_pan["
+ << _sdf->GetElement("bottom_joint_pose3dencodersneckvel_pan")->GetValueString() << "]\n";
+ if (!this->neckspeed.joint_tilt)
+ gzerr << "Unable to find top_joint_pose3dencodersneckvel_tilt["
+ << _sdf->GetElement("top_joint_pose3dencodersneckvel_tilt")->GetValueString() << "]\n";
+
+ this->neckspeed.link_pan = _model->GetLink("head_pan");
+ this->neckspeed.link_tilt = _model->GetLink("head_tilt");
+
+ //LOAD TORQUE
+ if (_sdf->HasElement("torque"))
+ this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+ else {
+ gzwarn << "No torque value set for the neck speed.\n";
+ this->stiffness = 5.0;
+ }
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&PoseNeckSpeed::OnUpdate, this));
+ }
+
+ void PoseNeckSpeed::Init () {}
+
+ void PoseNeckSpeed::OnUpdate () {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (this->count == 0) {
+ this->count++;
+ pthread_t thr_ice;
+ pthread_create(&thr_ice, NULL, &thread_NeckSpeedICE, (void*) this);
+ }
+
+ count++;
+ if ((count % 2) == 0){
+ this->neckspeed.joint_pan->SetVelocity(0, this->neckspeed.motorsdata.panSpeed);
+ this->neckspeed.joint_pan->SetMaxForce(0, this->stiffness);
+ } else {
+ this->neckspeed.joint_tilt->SetVelocity(0, this->neckspeed.motorsdata.panSpeed);
+ this->neckspeed.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseNeck* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_neckspeedencoders);
+
+ pose3DEncodersData->x = pose->neckspeed.encoders.x;
+ pose3DEncodersData->y = pose->neckspeed.encoders.y;
+ pose3DEncodersData->z = pose->neckspeed.encoders.z;
+ pose3DEncodersData->pan = pose->neckspeed.encoders.pan;
+ pose3DEncodersData->tilt = pose->neckspeed.encoders.tilt;
+ pose3DEncodersData->roll = pose->neckspeed.encoders.roll;
+ pose3DEncodersData->clock = pose->neckspeed.encoders.clock;
+ pose3DEncodersData->maxPan = pose->neckspeed.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->neckspeed.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->neckspeed.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->neckspeed.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_neckspeedencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseNeck* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseNeck* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_neckspeedmotors);
+
+ pose3DMotorsData->x = pose->neckspeed.motorsdata.x;
+ pose3DMotorsData->y = pose->neckspeed.motorsdata.y;
+ pose3DMotorsData->z = pose->neckspeed.motorsdata.z;
+ pose3DMotorsData->pan = pose->neckspeed.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->neckspeed.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->neckspeed.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->neckspeed.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->neckspeed.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_neckspeedmotors);
+
+ return pose3DMotorsData;
+
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_neckspeedmotors);
+
+ pose3DMotorsParams->maxPan = pose->neckspeed.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->neckspeed.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->neckspeed.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->neckspeed.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->neckspeed.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->neckspeed.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_neckspeedmotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_neckspeedmotors);
+
+ pose->neckspeed.motorsdata.x = data->x;
+ pose->neckspeed.motorsdata.y = data->y;
+ pose->neckspeed.motorsdata.z = data->z;
+ pose->neckspeed.motorsdata.pan = data->pan;
+ pose->neckspeed.motorsdata.tilt = data->tilt;
+ pose->neckspeed.motorsdata.roll = data->roll;
+ pose->neckspeed.motorsdata.panSpeed = data->panSpeed;
+ pose->neckspeed.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_neckspeedmotors);
+
+ }
+
+ gazebo::PoseNeck* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_NeckSpeedICE ( void* v ) {
+
+ gazebo::PoseNeckSpeed* neckspeed = (gazebo::PoseNeckSpeed*)v;
+ char* name = (char*) neckspeed->cfgfile_neckspeed.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 EndpointsEncoders = prop->getProperty("PoseNeckSpeedEncoders.Endpoints");
+ std::cout << "PoseNeckSpeedEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("PoseNeckSpeedMotors.Endpoints");
+ std::cout << "PoseNeckSpeedMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterNeckSpeedEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterNeckSpeedMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(neckspeed);
+ Ice::ObjectPtr motors = new Pose3DMotors(neckspeed);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("NeckSpeedEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("NeckSpeedMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->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;
+ }
+ }
+ }
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseNeckSpeedMotors.Endpoints=default -h localhost -p 12994
+PoseNeckSpeedEncoders.Endpoints=default -h localhost -p 12995
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef NECKSPEED_H
+#define NECKSPEED_H
+
+namespace gazebo {
+
+ void* thread_NeckSpeedICE ( void* v );
+
+ class PoseNeckSpeed : public ModelPlugin {
+ public:
+
+ PoseNeckSpeed ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_neckspeed;
+ pthread_mutex_t mutex_neckspeedencoders;
+ pthread_mutex_t mutex_neckspeedmotors;
+
+ struct neckspeed_t {
+ physics::JointPtr joint_tilt, joint_pan;
+ physics::LinkPtr link_pan, link_tilt;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ neckspeed_t neckspeed;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // NECKSPEED_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,295 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poserightankle.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+ GZ_REGISTER_MODEL_PLUGIN(PoseRightAnkle)
+
+ PoseRightAnkle::PoseRightAnkle () {
+ pthread_mutex_init(&this->mutex_rightankleencoders, NULL);
+ pthread_mutex_init(&this->mutex_rightanklemotors, NULL);
+ this->count = 0;
+ this->cycle = 50;
+ this->cfgfile_rightshoulder = std::string("--Ice.Config=poserightankle.cfg");
+
+ this->rightankle.motorsparams.maxPan = 1.57;
+ this->rightankle.motorsparams.minPan = -1.57;
+ this->rightankle.motorsparams.maxTilt = 0.5;
+ this->rightankle.motorsparams.minTilt = -0.5;
+
+ std::cout << "Constructor PoseRightAnkle" << std::endl;
+ }
+
+ void PoseRightAnkle::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ // LOAD CAMERA LEFT
+ if (!_sdf->HasElement("joint_pose3dencodersrightankle_pitch"))
+ gzerr << "pose3dencodersrightankle plugin missing <joint_pose3dencodersrightankle_pitch> element\n";
+ if (!_sdf->HasElement("joint_pose3dencodersrightankle_roll"))
+ gzerr << "pose3dencodersrightankle plugin missing <joint_pose3dencodersrightankle_roll> element\n";
+
+ this->rightankle.joint_roll = _model->GetJoint("rankle_roll");
+ this->rightankle.joint_tilt = _model->GetJoint("rankle_pitch");
+
+ if (!this->rightankle.joint_pose3dencoders_roll)
+ gzerr << "Unable to find joint_pose3dencodersrightankle_roll["
+ << _sdf->GetElement("joint_pose3dencodersrightankle_roll")->GetValueString() << "]\n";
+ if (!this->rightankle.joint_pose3dencoders_tilt)
+ gzerr << "Unable to find joint_pose3dencodersrightankle_pitch["
+ << _sdf->GetElement("joint_pose3dencodersrightankle_pitch")->GetValueString() << "]\n";
+
+ this->rightankle.link_roll = _model->GetLink("right_foot");
+ this->rightankle.link_tilt = _model->GetLink("rightankle_pitch");
+
+ //LOAD TORQUE
+ if (_sdf->HasElement("torque"))
+ this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+ else {
+ gzwarn << "No torque value set for the DiffDrive plugin.\n";
+ this->stiffness = 5.0;
+ }
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&PoseRightAnkle::OnUpdate, this));
+ }
+
+ void PoseRightAnkle::Init () {}
+
+ void PoseRightAnkle::OnUpdate () {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (this->count == 0) {
+ this->count++;
+ pthread_t thr_ice;
+ pthread_create(&thr_ice, NULL, &thread_RightAnkleICE, (void*) this);
+
+ this->rightankle.encoders.roll = 0;
+ this->rightankle.encoders.tilt = 0;
+ } else {
+ // ----------ENCODERS----------
+ //GET pose3dencoders data from the right ankle (PAN&TILT)
+ this->rightankle.encoders.roll = this->rightankle.link_roll->GetRelativePose().rot.GetAsEuler().y;
+ this->rightankle.encoders.tilt = this->rightankle.link_tilt->GetRelativePose().rot.GetAsEuler().x;
+ }
+
+ // ----------MOTORS----------
+ if (this->rightankle.motorsdata.roll >= 0) {
+ if (this->rightankle.encoders.roll < this->rightankle.motorsdata.roll) {
+ this->rightankle.joint_roll->SetVelocity(0, -0.1);
+ this->rightankle.joint_roll->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->rightankle.joint_roll->SetVelocity(0, 0.1);
+ this->rightankle.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->rightankle.encoders.roll > this->rightankle.motorsdata.roll) {
+ this->rightankle.joint_roll->SetVelocity(0, 0.1);
+ this->rightankle.joint_roll->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->rightankle.joint_roll->SetVelocity(0, -0.1);
+ this->rightankle.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ if (this->rightankle.motorsdata.tilt >= 0) {
+ if (this->rightankle.encoders.tilt < this->rightankle.motorsdata.tilt) {
+ this->rightankle.joint_tilt->SetVelocity(0, -0.1);
+ this->rightankle.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->rightankle.joint_tilt->SetVelocity(0, 0.1);
+ this->rightankle.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->rightankle.encoders.tilt > this->rightankle.motorsdata.tilt) {
+ this->rightankle.joint_tilt->SetVelocity(0, 0.1);
+ this->rightankle.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->rightankle.joint_tilt->SetVelocity(0, -0.1);
+ this->rightankle.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseRightAnkle* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightankleencoders);
+
+ pose3DEncodersData->x = pose->rightankle.encoders.x;
+ pose3DEncodersData->y = pose->rightankle.encoders.y;
+ pose3DEncodersData->z = pose->rightankle.encoders.z;
+ pose3DEncodersData->pan = pose->rightankle.encoders.pan;
+ pose3DEncodersData->tilt = pose->rightankle.encoders.tilt;
+ pose3DEncodersData->roll = pose->rightankle.encoders.roll;
+ pose3DEncodersData->clock = pose->rightankle.encoders.clock;
+ pose3DEncodersData->maxPan = pose->rightankle.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->rightankle.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->rightankle.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->rightankle.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_rightankleencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseRightAnkle* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseRightAnkle* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightanklemotors);
+
+ pose3DMotorsData->x = pose->rightankle.motorsdata.x;
+ pose3DMotorsData->y = pose->rightankle.motorsdata.y;
+ pose3DMotorsData->z = pose->rightankle.motorsdata.z;
+ pose3DMotorsData->pan = pose->rightankle.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->rightankle.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->rightankle.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->rightankle.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->rightankle.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightanklemotors);
+
+ return pose3DMotorsData;
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightanklemotors);
+
+ pose3DMotorsParams->maxPan = pose->rightankle.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->rightankle.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->rightankle.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->rightankle.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->rightankle.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->rightankle.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightanklemotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightanklemotors);
+
+ pose->rightankle.motorsdata.x = data->x;
+ pose->rightankle.motorsdata.y = data->y;
+ pose->rightankle.motorsdata.z = data->z;
+ pose->rightankle.motorsdata.pan = data->pan;
+ pose->rightankle.motorsdata.tilt = data->tilt;
+ pose->rightankle.motorsdata.roll = data->roll;
+ pose->rightankle.motorsdata.panSpeed = data->panSpeed;
+ pose->rightankle.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightanklemotors);
+ }
+
+ gazebo::PoseRightAnkle* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_RightAnkleICE ( void* v ) {
+
+ gazebo::PoseRightAnkle* rightankle = (gazebo::PoseRightAnkle*)v;
+ char* name = (char*) rightankle->cfgfile_rightankle.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 EndpointsEncoders = prop->getProperty("PoseRightAnkleEncoders.Endpoints");
+ std::cout << "PoseRightAnkleEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("PoseRightAnkleMotors.Endpoints");
+ std::cout << "PoseRightAnkleMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterRightAnkleEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterRightAnkleMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(rightankle);
+ Ice::ObjectPtr motors = new Pose3DMotors(rightankle);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("RightAnkleEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("RightAnkleMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->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;
+ }
+ }
+ }
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseRightAnkleEncoders.Endpoints=default -h localhost -p 10996
+PoseRightAnkleMotors.Endpoints=default -h localhost -p 10997
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef RIGHTANKLE_H
+#define RIGHTANKLE_H
+
+namespace gazebo {
+
+ void* thread_RightAnkleICE ( void* v );
+
+ class PoseRightAnkle : public ModelPlugin {
+ public:
+
+ PoseRightAnkle ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_rightankle;
+ pthread_mutex_t mutex_rightankleencoders;
+ pthread_mutex_t mutex_rightanklemotors;
+
+ struct rightankle_t {
+ physics::JointPtr joint_tilt, joint_pan;
+ physics::LinkPtr link_pan, link_tilt;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ rightankle_t rightankle;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // RIGHTANKLE_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,295 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poserightelbow.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+ GZ_REGISTER_MODEL_PLUGIN(PoseRightElbow)
+
+ PoseRightElbow::PoseRightElbow () {
+ pthread_mutex_init(&this->mutex_rightelbowencoders, NULL);
+ pthread_mutex_init(&this->mutex_rightelbowmotors, NULL);
+ this->count = 0;
+ this->cycle = 50;
+ this->cfgfile_rightelbow = std::string("--Ice.Config=poserightelbow.cfg");
+
+ this->rightelbow.motorsparams.maxPan = 1.57;
+ this->rightelbow.motorsparams.minPan = -1.57;
+ this->rightelbow.motorsparams.maxTilt = 0.5;
+ this->rightelbow.motorsparams.minTilt = -0.5;
+
+ std::cout << "Constructor PoseRightElbow" << std::endl;
+ }
+
+ void PoseRightElbow::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ // LOAD CAMERA LEFT
+ if (!_sdf->HasElement("joint_pose3dencodersrightelbow_yaw"))
+ gzerr << "pose3dencodersrightelbow plugin missing <joint_pose3dencodersrightelbow_yaw> element\n";
+ if (!_sdf->HasElement("joint_pose3dencodersrightelbow_roll"))
+ gzerr << "pose3dencodersrightelbow plugin missing <joint_pose3dencodersrightelbow_roll> element\n";
+
+ this->rightelbow.joint_pan = _model->GetJoint("relbow_yaw");
+ this->rightelbow.joint_roll = _model->GetJoint("relbow_roll");
+
+ if (!this->rightelbow.joint_pan)
+ gzerr << "Unable to find joint_pose3dencodersrightelbow_yaw["
+ << _sdf->GetElement("joint_pose3dencodersrightelbow_yaw")->GetValueString() << "]\n";
+ if (!this->rightelbow.joint_roll)
+ gzerr << "Unable to find joint_pose3dencodersrightelbow_roll["
+ << _sdf->GetElement("joint_pose3dencodersrightelbow_roll")->GetValueString() << "]\n";
+
+ this->rightelbow.link_pan = _model->GetLink("rightelbow_yaw");
+ this->rightelbow.link_roll = _model->GetLink("right_lower_arm");
+
+ //LOAD TORQUE
+ if (_sdf->HasElement("torque"))
+ this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+ else {
+ gzwarn << "No torque value set for the rigth elbow.\n";
+ this->stiffness = 5.0;
+ }
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&PoseRightElbow::OnUpdate, this));
+ }
+
+ void PoseRightElbow::Init () {}
+
+ void PoseRightElbow::OnUpdate () {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (this->count == 0) {
+ this->count++;
+ pthread_t thr_ice;
+ pthread_create(&thr_ice, NULL, &thread_RightElbowICE, (void*) this);
+ }
+
+ // ----------ENCODERS----------
+ //GET pose3dencoders data from the right elbow (PAN&TILT)
+// this->rightelbow.encoder.pan = this->rightelbow.rightelbow_link_pan->GetRelativePose().rot.GetAsEuler().z;
+// this->rightelbow.encoder.roll = this->rightelbow.rightelbow_link_roll->GetRelativePose().rot.GetAsEuler().y;
+
+ this->rightelbow.encoders.pan = - this->rightelbow.joint_pan->GetAngle(0).Radian();
+ this->rightelbow.encoders.roll = - this->rightelbow.joint_roll->GetAngle(0).Radian();
+
+ // ----------MOTORS----------
+ if (this->rightelbow.motorsdata.pan >= 0) {
+ if (this->rightelbow.encoders.pan < this->rightelbow.motorsdata.pan) {
+ this->rightelbow.joint_pan->SetVelocity(0, -0.1);
+ this->rightelbow.joint_pan->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->rightelbow.joint_pan->SetVelocity(0, 0.1);
+ this->rightelbow.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->rightelbow.encoders.pan > this->rightelbow.motorsdata.pan) {
+ this->rightelbow.joint_pan->SetVelocity(0, 0.1);
+ this->rightelbow.joint_pan->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->rightelbow.joint_pan->SetVelocity(0, -0.1);
+ this->rightelbow.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ if (this->rightelbow.motorsdata.roll >= 0) {
+ if (this->rightelbow.encoders.roll < this->rightelbow.motorsdata.roll) {
+ this->rightelbow.joint_roll->SetVelocity(0, -0.1);
+ this->rightelbow.joint_roll->SetMaxForce(0, this->stiffness);
+ } else {
+ this->rightelbow.joint_roll->SetVelocity(0, 0.1);
+ this->rightelbow.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->rightelbow.encoders.roll > this->rightelbow.motorsdata.roll) {
+ this->rightelbow.joint_roll->SetVelocity(0, 0.1);
+ this->rightelbow.joint_roll->SetMaxForce(0, this->stiffness);
+ } else {
+ this->rightelbow.joint_roll->SetVelocity(0, -0.1);
+ this->rightelbow.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseRightElbow* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightelbowencoders);
+
+ pose3DEncodersData->x = pose->rightelbow.encoders.x;
+ pose3DEncodersData->y = pose->rightelbow.encoders.y;
+ pose3DEncodersData->z = pose->rightelbow.encoders.z;
+ pose3DEncodersData->pan = pose->rightelbow.encoders.pan;
+ pose3DEncodersData->tilt = pose->rightelbow.encoders.tilt;
+ pose3DEncodersData->roll = pose->rightelbow.encoders.roll;
+ pose3DEncodersData->clock = pose->rightelbow.encoders.clock;
+ pose3DEncodersData->maxPan = pose->rightelbow.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->rightelbow.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->rightelbow.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->rightelbow.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_rightelbowencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseRightElbow* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseRightElbow* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightelbowmotors);
+
+ pose3DMotorsData->x = pose->rightelbow.motorsdata.x;
+ pose3DMotorsData->y = pose->rightelbow.motorsdata.y;
+ pose3DMotorsData->z = pose->rightelbow.motorsdata.z;
+ pose3DMotorsData->pan = pose->rightelbow.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->rightelbow.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->rightelbow.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->rightelbow.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->rightelbow.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightelbowmotors);
+
+ return pose3DMotorsData;
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightelbowmotors);
+
+ pose3DMotorsParams->maxPan = pose->rightelbow.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->rightelbow.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->rightelbow.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->rightelbow.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->rightelbow.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->rightelbow.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightelbowmotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightelbowmotors);
+
+ pose->rightelbow.motorsdata.x = data->x;
+ pose->rightelbow.motorsdata.y = data->y;
+ pose->rightelbow.motorsdata.z = data->z;
+ pose->rightelbow.motorsdata.pan = data->pan;
+ pose->rightelbow.motorsdata.tilt = data->tilt;
+ pose->rightelbow.motorsdata.roll = data->roll;
+ pose->rightelbow.motorsdata.panSpeed = data->panSpeed;
+ pose->rightelbow.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightelbowmotors);
+ }
+
+ gazebo::PoseRightElbow* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_RightElbowICE ( void* v ) {
+
+ gazebo::PoseRightElbow* rightelbow = (gazebo::PoseRightElbow*)v;
+ char* name = (char*) rightelbow->cfgfile_rightelbow.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 EndpointsEncoders = prop->getProperty("PoseRightElbowEncoders.Endpoints");
+ std::cout << "PoseRightElbowEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("PoseRightElbowMotors.Endpoints");
+ std::cout << "PoseRightElbowMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterRightElbowEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterRightElbowMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(rightelbow);
+ Ice::ObjectPtr motors = new Pose3DMotors(rightelbow);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("RightElbowEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("RightElbowMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->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;
+ }
+ }
+ }
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseRightElbowEncoders.Endpoints=default -h localhost -p 9998
+PoseRightElbowMotors.Endpoints=default -h localhost -p 9999
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef POSERIGHTELBOW_H
+#define POSERIGHTELBOW_H
+
+namespace gazebo {
+
+ void* thread_RightElbowICE ( void* v );
+
+ class PoseRightElbow : public ModelPlugin {
+ public:
+
+ PoseRightElbow ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_rightelbow;
+ pthread_mutex_t mutex_rightelbowencoders;
+ pthread_mutex_t mutex_rightelbowmotors;
+
+ struct rightelbow_t {
+ physics::JointPtr joint_pan, joint_roll;
+ physics::LinkPtr link_pan, link_roll;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ rightelbow_t rightelbow;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // POSERIGHTELBOW_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,315 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poserighthip.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+ GZ_REGISTER_MODEL_PLUGIN(PoseRightHip)
+
+ PoseRightHip::PoseRightHip () {
+ pthread_mutex_init(&this->mutex_righthipencoders, NULL);
+ pthread_mutex_init(&this->mutex_righthipmotors, NULL);
+ this->countH = 0;
+ this->cycle = 50;
+ this->cfgfile_righthip = std::string("--Ice.Config=poserighthip.cfg");
+
+ this->righthip.motorsparams.maxPan = 1.57;
+ this->righthip.motorsparams.minPan = -1.57;
+ this->righthip.motorsparams.maxTilt = 0.5;
+ this->righthip.motorsparams.minTilt = -0.5;
+
+ std::cout << "Constructor PoseRightHip" << std::endl;
+ }
+
+ void PoseRightHip::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ // LOAD CAMERA LEFT
+ if (!_sdf->HasElement("joint_pose3dencodersrhip_yawpitch"))
+ gzerr << "pose3dencodership plugin missing <joint_pose3dencodersrhip_yawpitch> element\n";
+ if (!_sdf->HasElement("joint_pose3dencodersrhip_roll"))
+ gzerr << "pose3dencodership plugin missing <joint_pose3dencodersrhip_roll> element\n";
+ if (!_sdf->HasElement("joint_pose3dencodersrhip_pitch"))
+ gzerr << "pose3dencodership plugin missing <joint_pose3dencodersrhip_pitch> element\n";
+
+ this->righthip.joint_pan = _model->GetJoint("rhip_yawpitch");
+ this->righthip.joint_tilt = _model->GetJoint("rhip_pitch");
+ this->righthip.joint_roll = _model->GetJoint("rhip_roll");
+
+ if (!this->righthip.joint_pan)
+ gzerr << "Unable to find joint_pose3dencodership_pan["
+ << _sdf->GetElement("joint_pose3dencodership_pan")->GetValueString() << "]\n";
+ if (!this->righthip.joint_tilt)
+ gzerr << "Unable to find joint_pose3dencodership_tilt["
+ << _sdf->GetElement("joint_pose3dencodership_tilt")->GetValueString() << "]\n";
+ if (!this->righthip.joint_roll)
+ gzerr << "Unable to find joint_pose3dencodership_roll["
+ << _sdf->GetElement("joint_pose3dencodership_roll")->GetValueString() << "]\n";
+
+ this->righthip.link_pan = _model->GetLink("righthip_yawpitch");
+ this->righthip.link_tilt = _model->GetLink("right_thigh");
+ this->righthip.link_roll = _model->GetLink("righthip_roll");
+
+ //LOAD TORQUE
+ if (_sdf->HasElement("torque"))
+ this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+ else {
+ gzwarn << "No torque value set for the right hip.\n";
+ this->stiffness = 5.0;
+ }
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&PoseRightHip::OnUpdate, this));
+ }
+
+ void PoseRightHip::Init () {}
+
+ void PoseRightHip::OnUpdate () {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (this->count == 0) {
+ this->count++;
+ pthread_t thr_ice;
+ pthread_create(&thr_ice, NULL, &thread_RightHipICE, (void*) this);
+ }
+
+ // ----------ENCODERS----------
+ //GET pose3dencoders data from the hip (PAN&TILT)
+ this->righthip.encoders.pan = this->righthip.link_pan->GetRelativePose().rot.GetAsEuler().z;
+ this->righthip.encoders.tilt = this->righthip.link_tilt->GetRelativePose().rot.GetAsEuler().x;
+ this->righthip.encoders.roll = this->righthip.link_roll->GetRelativePose().rot.GetAsEuler().y;
+
+ if (this->righthip.motorsdata.pan >= 0) {
+ if (this->righthip.encoders.pan < this->righthip.motorsdata.pan) {
+ this->righthip.joint_pan->SetVelocity(0, -0.1);
+ this->righthip.joint_pan->SetMaxForce(0, this->stiffness);
+ } else {
+ this->righthip.joint_pan->SetVelocity(0, 0.1);
+ this->righthip.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->righthip.encoders.pan > this->righthip.motorsdata.pan) {
+ this->righthip.joint_pan->SetVelocity(0, 0.1);
+ this->righthip.joint_pan->SetMaxForce(0, this->stiffness);
+ } else {
+ this->righthip.joint_pan->SetVelocity(0, -0.1);
+ this->righthip.joint_pan->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ if (this->righthip.motorsdata.tilt >= 0) {
+ if (this->righthip.encoders.tilt < this->righthip.motorsdata.tilt) {
+ this->righthip.joint_tilt->SetVelocity(0, -0.1);
+ this->righthip.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->righthip.joint_tilt->SetVelocity(0, 0.1);
+ this->righthip.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->righthip.encoders.tilt > this->righthip.motorsdata.tilt) {
+ this->righthip.joint_tilt->SetVelocity(0, 0.1);
+ this->righthip.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->righthip.joint_tilt->SetVelocity(0, -0.1);
+ this->righthip.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ if (this->righthip.motorsdata.roll >= 0) {
+ if (this->righthip.encoders.roll < this->righthip.motorsdata.roll) {
+ this->righthip.joint_roll->SetVelocity(0, -0.1);
+ this->righthip.joint_roll->SetMaxForce(0, this->stiffness);
+ } else {
+ this->righthip.joint_roll->SetVelocity(0, 0.1);
+ this->righthip.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->righthip.encoders.roll > this->righthip.motorsdata.roll) {
+ this->righthip.joint_roll->SetVelocity(0, 0.1);
+ this->righthip.joint_roll->SetMaxForce(0, this->stiffness);
+ } else {
+ this->righthip.joint_roll->SetVelocity(0, -0.1);
+ this->righthip.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseRightHip* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_righthipencoders);
+
+ pose3DEncodersData->x = pose->righthip.encoders.x;
+ pose3DEncodersData->y = pose->righthip.encoders.y;
+ pose3DEncodersData->z = pose->righthip.encoders.z;
+ pose3DEncodersData->pan = pose->righthip.encoders.pan;
+ pose3DEncodersData->tilt = pose->righthip.encoders.tilt;
+ pose3DEncodersData->roll = pose->righthip.encoders.roll;
+ pose3DEncodersData->clock = pose->righthip.encoders.clock;
+ pose3DEncodersData->maxPan = pose->righthip.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->righthip.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->righthip.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->righthip.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_righthipencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseRightHip* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseRightHip* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_righthipmotors);
+
+ pose3DMotorsData->x = pose->righthip.motorsdata.x;
+ pose3DMotorsData->y = pose->righthip.motorsdata.y;
+ pose3DMotorsData->z = pose->righthip.motorsdata.z;
+ pose3DMotorsData->pan = pose->righthip.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->righthip.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->righthip.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->righthip.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->righthip.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_righthipmotors);
+
+ return pose3DMotorsData;
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_righthipmotors);
+
+ pose3DMotorsParams->maxPan = pose->righthip.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->righthip.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->righthip.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->righthip.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->righthip.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->righthip.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_righthipmotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_righthipmotors);
+
+ pose->righthip.motorsdata.x = data->x;
+ pose->righthip.motorsdata.y = data->y;
+ pose->righthip.motorsdata.z = data->z;
+ pose->righthip.motorsdata.pan = data->pan;
+ pose->righthip.motorsdata.tilt = data->tilt;
+ pose->righthip.motorsdata.roll = data->roll;
+ pose->righthip.motorsdata.panSpeed = data->panSpeed;
+ pose->righthip.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_righthipmotors);
+ }
+
+ gazebo::PoseRightHip* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_RightHipICE ( void* v ) {
+
+ gazebo::PoseRightHip* righthip = (gazebo::PoseRightHip*)v;
+ char* name = (char*) righthip->cfgfile_righthip.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 EndpointsEncoders = prop->getProperty("PoseRightHipEncoders.Endpoints");
+ std::cout << "PoseRightHipEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("PoseRightHipkMotors.Endpoints");
+ std::cout << "PoseRightHipMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterRightHipEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterRightHipkMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(righthip);
+ Ice::ObjectPtr motors = new Pose3DMotors(righthip);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("RightHipEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("RightHipMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->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;
+ }
+ }
+ }
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseRightHipEncoders.Endpoints=default -h localhost -p 8996
+PoseRightHipMotors.Endpoints=default -h localhost -p 8997
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef POSERIGHTHIP_H
+#define POSERIGHTHIP_H
+
+namespace gazebo {
+
+ void* thread_RightHipICE ( void* v );
+
+ class PoseRightHip : public ModelPlugin {
+ public:
+
+ PoseRightHip ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_righthip;
+ pthread_mutex_t mutex_righthipencoders;
+ pthread_mutex_t mutex_righthipmotors;
+
+ struct righthip_t {
+ physics::JointPtr joint_tilt, joint_pan;
+ physics::LinkPtr link_pan, link_tilt;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ righthip_t righthip;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // POSERIGHTHIP_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,266 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poserightknee.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+ GZ_REGISTER_MODEL_PLUGIN(PoseRightKnee)
+
+ PoseRightKnee::PoseRightKnee () {
+ pthread_mutex_init(&this->mutex_rightkneeencoders, NULL);
+ pthread_mutex_init(&this->mutex_rightkneemotors, NULL);
+ this->count = 0;
+ this->cycle = 50;
+ this->cfgfile_rightknee = std::string("--Ice.Config=poserightknee.cfg");
+
+ this->rightknee.motorsparams.maxPan = 1.57;
+ this->rightknee.motorsparams.minPan = -1.57;
+ this->rightknee.motorsparams.maxTilt = 0.5;
+ this->rightknee.motorsparams.minTilt = -0.5;
+
+ std::cout << "Constructor PoseRightKnee" << std::endl;
+ }
+
+ void PoseRightKnee::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ // LOAD CAMERA LEFT
+ if (!_sdf->HasElement("joint_pose3dencodersrightknee_pitch"))
+ gzerr << "pose3dencodersrightknee plugin missing <joint_pose3dencodersrightknee_pitch> element\n";
+
+ this->rightknee.joint_tilt = _model->GetJoint("right_knee");
+
+ if (!this->rightknee.joint_tilt)
+ gzerr << "Unable to find joint_pose3dencodersrightknee_pitch["
+ << _sdf->GetElement("joint_pose3dencodersrightknee_pitch")->GetValueString() << "]\n";
+
+ this->rightknee.link_tilt = _model->GetLink("right_shin");
+
+ //LOAD TORQUE
+ if (_sdf->HasElement("torque"))
+ this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+ else {
+ gzwarn << "No torque value set for the DiffDrive plugin.\n";
+ this->stiffness = 5.0;
+ }
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&PoseRightKnee::OnUpdate, this));
+ }
+
+ void PoseRightKnee::Init () {}
+
+ void PoseRightKnee::OnUpdate () {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (this->count == 0) {
+ this->count++;
+ pthread_t thr_ice;
+ pthread_create(&thr_ice, NULL, &thread_RightKneeICE, (void*) this);
+ }
+
+ // ----------ENCODERS----------
+ //GET pose3dencoders data from the right knee (PAN&TILT)
+ this->rightknee.encoders.tilt = this->rightknee.link_tilt->GetRelativePose().rot.GetAsEuler().x;
+
+ // ----------MOTORS----------
+ if (this->rightknee.motorsdata.tilt >= 0) {
+ if (this->rightknee.encoders.tilt < this->rightknee.motorsdata.tilt) {
+ this->rightknee.joint_tilt->SetVelocity(0, -0.1);
+ this->rightknee.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->rightknee.joint_tilt->SetVelocity(0, 0.1);
+ this->rightknee.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->rightknee.encoders.tilt > this->rightknee.motorsdata.tilt) {
+ this->rightknee.joint_tilt->SetVelocity(0, 0.1);
+ this->rightknee.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->rightknee.joint_tilt->SetVelocity(0, -0.1);
+ this->rightknee.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseRightKnee* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightkneeencoders);
+
+ pose3DEncodersData->x = pose->rightknee.encoders.x;
+ pose3DEncodersData->y = pose->rightknee.encoders.y;
+ pose3DEncodersData->z = pose->rightknee.encoders.z;
+ pose3DEncodersData->pan = pose->rightknee.encoders.pan;
+ pose3DEncodersData->tilt = pose->rightknee.encoders.tilt;
+ pose3DEncodersData->roll = pose->rightknee.encoders.roll;
+ pose3DEncodersData->clock = pose->rightknee.encoders.clock;
+ pose3DEncodersData->maxPan = pose->rightknee.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->rightknee.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->rightknee.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->rightknee.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_rightkneeencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseRightKnee* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseRightKnee* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightkneemotors);
+
+ pose3DMotorsData->x = pose->rightknee.motorsdata.x;
+ pose3DMotorsData->y = pose->rightknee.motorsdata.y;
+ pose3DMotorsData->z = pose->rightknee.motorsdata.z;
+ pose3DMotorsData->pan = pose->rightknee.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->rightknee.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->rightknee.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->rightknee.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->rightknee.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightkneemotors);
+
+ return pose3DMotorsData;
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightkneemotors);
+
+ pose3DMotorsParams->maxPan = pose->rightknee.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->rightknee.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->rightknee.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->rightknee.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->rightknee.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->rightknee.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightkneemotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightkneemotors);
+
+ pose->rightknee.motorsdata.x = data->x;
+ pose->rightknee.motorsdata.y = data->y;
+ pose->rightknee.motorsdata.z = data->z;
+ pose->rightknee.motorsdata.pan = data->pan;
+ pose->rightknee.motorsdata.tilt = data->tilt;
+ pose->rightknee.motorsdata.roll = data->roll;
+ pose->rightknee.motorsdata.panSpeed = data->panSpeed;
+ pose->rightknee.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightkneemotors);
+ }
+
+ gazebo::PoseRightKnee* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_RightKneeICE ( void* v ) {
+
+ gazebo::PoseRightKnee* rightknee = (gazebo::PoseRightKnee*)v;
+ char* name = (char*) rightknee->cfgfile_rightknee.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 EndpointsEncoders = prop->getProperty("PoseRightKneeEncoders.Endpoints");
+ std::cout << "PoseRightKneeEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("PoseRightKneeMotors.Endpoints");
+ std::cout << "PoseRightKneeMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterRightKneeEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterRightKneeMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(rightknee);
+ Ice::ObjectPtr motors = new Pose3DMotors(rightknee);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("RightKneeEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("RightKneeMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->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;
+ }
+ }
+
+ }
+
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseRightKneeEncoders.Endpoints=default -h localhost -p 8994
+PoseRightKneeMotors.Endpoints=default -h localhost -p 8995
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef POSERIGHTKNEE_H
+#define POSERIGHTKNEE_H
+
+namespace gazebo {
+
+ void* thread_RightKneeICE ( void* v );
+
+ class PoseRightKnee : public ModelPlugin {
+ public:
+
+ PoseRightKnee ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_rightknee;
+ pthread_mutex_t mutex_rightkneeencoders;
+ pthread_mutex_t mutex_rightkneemotors;
+
+ struct rightknee_t {
+ physics::JointPtr joint_tilt, joint_pan;
+ physics::LinkPtr link_pan, link_tilt;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ rightknee_t rightknee;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // POSERIGHTKNEE_H
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cc (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cc 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,298 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poserightshoulder.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+ GZ_REGISTER_MODEL_PLUGIN(PoseRightShoulder)
+
+ PoseRightShoulder::PoseRightShoulder () {
+ pthread_mutex_init(&this->mutex_rightshoulderencoders, NULL);
+ pthread_mutex_init(&this->mutex_rightshouldermotors, NULL);
+ this->count = 0;
+ this->cycle = 50;
+ this->cfgfile_rightshoulder = std::string("--Ice.Config=poserightshoulder.cfg");
+
+ this->rightshoulder.motorsparams.maxPan = 1.57;
+ this->rightshoulder.motorsparams.minPan = -1.57;
+ this->rightshoulder.motorsparams.maxTilt = 0.5;
+ this->rightshoulder.motorsparams.minTilt = -0.5;
+
+ std::cout << "Constructor PoseRightShoulder" << std::endl;
+ }
+
+ void PoseRightShoulder::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+ // LOAD CAMERA LEFT
+ if (!_sdf->HasElement("joint_pose3dencodersrightshoulder_pitch"))
+ gzerr << "pose3dencodersrightshoulder plugin missing <joint_pose3dencodersrightshoulder_pitch> element\n";
+ if (!_sdf->HasElement("joint_pose3dencodersrightshoulder_roll"))
+ gzerr << "pose3dencodersrightshoulder plugin missing <joint_pose3dencodersrightshoulder_roll> element\n";
+
+ this->rightshoulder.joint_roll = _model->GetJoint("rshoulder_roll");
+ this->rightshoulder.joint_tilt = _model->GetJoint("rshoulder_pitch");
+
+ if (!this->rightshoulder.joint_roll)
+ gzerr << "Unable to find joint_pose3dencodersrightshoulder_roll["
+ << _sdf->GetElement("joint_pose3dencodersrightshoulder_roll")->GetValueString() << "]\n";
+ if (!this->rightshoulder.joint_tilt)
+ gzerr << "Unable to find joint_pose3dencodersrightshoulder_pitch["
+ << _sdf->GetElement("joint_pose3dencodersrightshoulder_pitch")->GetValueString() << "]\n";
+
+ this->rightshoulder.link_roll = _model->GetLink("right_upper_arm");
+ this->rightshoulder.link_tilt = _model->GetLink("rightshoulder_pitch");
+
+ //LOAD TORQUE
+ if (_sdf->HasElement("torque"))
+ this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+ else {
+ gzwarn << "No torque value set for the DiffDrive plugin.\n";
+ this->stiffness = 5.0;
+ }
+
+ //LOAD POSE3DMOTORS
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&PoseRightShoulder::OnUpdate, this));
+
+ }
+
+ void PoseRightShoulder::Init () {}
+
+ void PoseRightShoulder::OnUpdate () {
+ long totalb, totala, diff;
+ struct timeval a, b;
+
+ gettimeofday(&a, NULL);
+ totala = a.tv_sec * 1000000 + a.tv_usec;
+
+ if (this->count == 0) {
+ this->count++;
+ pthread_t thr_ice;
+ pthread_create(&thr_ice, NULL, &thread_RightShoulderICE, (void*) this);
+ }
+
+ // ----------ENCODERS----------
+ //GET pose3dencoders data from the right shoulder (PAN&TILT)
+// this->rightshoulder.encoder.roll = this->rightshoulder.rightshoulder_link_roll->GetRelativePose().rot.GetAsEuler().y;
+// this->rightshoulder.encoder.tilt = this->rightshoulder.rightshoulder_link_tilt->GetRelativePose().rot.GetAsEuler().x;
+
+ this->rightshoulder.encoders.tilt = - this->rightshoulder.joint_tilt->GetAngle(0).Radian();
+ this->rightshoulder.encoders.roll = - this->rightshoulder.joint_roll->GetAngle(0).Radian();
+
+ // ----------MOTORS----------
+ if (this->rightshoulder.motorsdata.roll >= 0) {
+ if (this->rightshoulder.encoders.roll < this->rightshoulder.motorsdata.roll) {
+ this->rightshoulder.joint_roll->SetVelocity(0, -0.1);
+ this->rightshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->rightshoulder.joint_roll->SetVelocity(0, 0.1);
+ this->rightshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->rightshoulder.encoders.roll > this->rightshoulder.motorsdata.roll) {
+ this->rightshoulder.joint_roll->SetVelocity(0, 0.1);
+ this->rightshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+ //std::cout << "AQUI" << std::endl;
+ } else {
+ this->rightshoulder.joint_roll->SetVelocity(0, -0.1);
+ this->rightshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ if (this->rightshoulder.motorsdata.tilt >= 0) {
+ if (this->rightshoulder.encoders.tilt < this->rightshoulder.motorsdata.tilt) {
+ this->rightshoulder.joint_tilt->SetVelocity(0, -0.1);
+ this->rightshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->rightshoulder.joint_tilt->SetVelocity(0, 0.1);
+ this->rightshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ } else {
+ if (this->rightshoulder.encoders.tilt > this->rightshoulder.motorsdata.tilt) {
+ this->rightshoulder.joint_tilt->SetVelocity(0, 0.1);
+ this->rightshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+ } else {
+ this->rightshoulder.joint_tilt->SetVelocity(0, -0.1);
+ this->rightshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+ }
+ }
+
+ gettimeofday(&b, NULL);
+ totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+ diff = (totalb - totala) / 1000;
+ diff = cycle - diff;
+
+ if (diff < 10)
+ diff = 10;
+
+ //usleep(diff*1000);
+ sleep(diff / 1000);
+ }
+
+ class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+ public:
+
+ Pose3DEncoders ( gazebo::PoseRightShoulder* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DEncoders () {}
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightshoulderencoders);
+
+ pose3DEncodersData->x = pose->rightshoulder.encoders.x;
+ pose3DEncodersData->y = pose->rightshoulder.encoders.y;
+ pose3DEncodersData->z = pose->rightshoulder.encoders.z;
+ pose3DEncodersData->pan = pose->rightshoulder.encoders.pan;
+ pose3DEncodersData->tilt = pose->rightshoulder.encoders.tilt;
+ pose3DEncodersData->roll = pose->rightshoulder.encoders.roll;
+ pose3DEncodersData->clock = pose->rightshoulder.encoders.clock;
+ pose3DEncodersData->maxPan = pose->rightshoulder.encoders.maxPan;
+ pose3DEncodersData->minPan = pose->rightshoulder.encoders.minPan;
+ pose3DEncodersData->maxTilt = pose->rightshoulder.encoders.maxTilt;
+ pose3DEncodersData->minTilt = pose->rightshoulder.encoders.minTilt;
+
+ pthread_mutex_unlock(&pose->mutex_rightshoulderencoders);
+
+ return pose3DEncodersData;
+ }
+
+ gazebo::PoseRightShoulder* pose;
+
+ private:
+ jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+ };
+
+ class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+ public:
+
+ Pose3DMotors (gazebo::PoseRightShoulder* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+ this->pose = pose;
+ }
+
+ virtual ~Pose3DMotors() {}
+
+ virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightshouldermotors);
+
+ pose3DMotorsData->x = pose->rightshoulder.motorsdata.x;
+ pose3DMotorsData->y = pose->rightshoulder.motorsdata.y;
+ pose3DMotorsData->z = pose->rightshoulder.motorsdata.z;
+ pose3DMotorsData->pan = pose->rightshoulder.motorsdata.pan;
+ pose3DMotorsData->tilt = pose->rightshoulder.motorsdata.tilt;
+ pose3DMotorsData->roll = pose->rightshoulder.motorsdata.roll;
+ pose3DMotorsData->panSpeed = pose->rightshoulder.motorsdata.panSpeed;
+ pose3DMotorsData->tiltSpeed = pose->rightshoulder.motorsdata.tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightshouldermotors);
+
+ return pose3DMotorsData;
+
+ }
+
+ virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightshouldermotors);
+
+ pose3DMotorsParams->maxPan = pose->rightshoulder.motorsparams.maxPan;
+ pose3DMotorsParams->minPan = pose->rightshoulder.motorsparams.minPan;
+ pose3DMotorsParams->maxTilt = pose->rightshoulder.motorsparams.maxTilt;
+ pose3DMotorsParams->minTilt = pose->rightshoulder.motorsparams.minTilt;
+ pose3DMotorsParams->maxPanSpeed = pose->rightshoulder.motorsparams.maxPanSpeed;
+ pose3DMotorsParams->maxTiltSpeed = pose->rightshoulder.motorsparams.maxTiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightshouldermotors);
+
+ return pose3DMotorsParams;
+ }
+
+ virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+ pthread_mutex_lock(&pose->mutex_rightshouldermotors);
+
+ pose->rightshoulder.motorsdata.x = data->x;
+ pose->rightshoulder.motorsdata.y = data->y;
+ pose->rightshoulder.motorsdata.z = data->z;
+ pose->rightshoulder.motorsdata.pan = data->pan;
+ pose->rightshoulder.motorsdata.tilt = data->tilt;
+ pose->rightshoulder.motorsdata.roll = data->roll;
+ pose->rightshoulder.motorsdata.panSpeed = data->panSpeed;
+ pose->rightshoulder.motorsdata.tiltSpeed = data->tiltSpeed;
+
+ pthread_mutex_unlock(&pose->mutex_rightshouldermotors);
+
+ }
+
+ gazebo::PoseRightShoulder* pose;
+
+ private:
+ jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+ jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+ };
+
+ void* thread_RightShoulderICE ( void* v ) {
+
+ gazebo::PoseRightShoulder* rightshoulder = (gazebo::PoseRightShoulder*)v;
+ char* name = (char*) rightshoulder->cfgfile_rightshoulder.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 EndpointsEncoders = prop->getProperty("PoseRightShoulderEncoders.Endpoints");
+ std::cout << "PoseRightShoulderEncoders Endpoints > " << EndpointsEncoders << std::endl;
+ std::string EndpointsMotors = prop->getProperty("PoseRightShoulderMotors.Endpoints");
+ std::cout << "PoseRightShoulderMotors Endpoints > " << EndpointsMotors << std::endl;
+
+ Ice::ObjectAdapterPtr AdapterEncoders =
+ ic->createObjectAdapterWithEndpoints("AdapterRightShoulderEncoders", EndpointsEncoders);
+ Ice::ObjectAdapterPtr AdapterMotors =
+ ic->createObjectAdapterWithEndpoints("AdapterRightShoulderMotors", EndpointsMotors);
+
+ Ice::ObjectPtr encoders = new Pose3DEncoders(rightshoulder);
+ Ice::ObjectPtr motors = new Pose3DMotors(rightshoulder);
+
+ AdapterEncoders->add(encoders, ic->stringToIdentity("RightShoulderEncoders"));
+ AdapterMotors->add(motors, ic->stringToIdentity("RightShoulderMotors"));
+
+ AdapterEncoders->activate();
+ AdapterMotors->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;
+ }
+ }
+ }
+}
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cfg (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cfg 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseRightShoulderEncoders.Endpoints=default -h localhost -p 9996
+PoseRightShoulderMotors.Endpoints=default -h localhost -p 9997
Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.h (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.h 2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ * Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#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>
+
+#include "common.h"
+
+#ifndef POSERIGHTSHOULDER_H
+#define POSERIGHTSHOULDER_H
+
+namespace gazebo {
+
+ void* thread_RightShoulderICE ( void* v );
+
+ class PoseRightShoulder : public ModelPlugin {
+ public:
+
+ PoseRightShoulder ();
+ virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+ virtual void Init ();
+
+ std::string cfgfile_rightshoulder;
+ pthread_mutex_t mutex_rightshoulderencoders;
+ pthread_mutex_t mutex_rightshouldermotors;
+
+ struct rightshoulder_t {
+ physics::JointPtr joint_tilt, joint_roll;
+ physics::LinkPtr link_tilt, link_roll;
+ encoders_t encoders;
+ motorsdata_t motorsdata;
+ motorsparams_t motorsparams;
+ };
+
+ rightshoulder_t rightshoulder;
+
+ private:
+ void OnUpdate ();
+
+ event::ConnectionPtr updateConnection;
+ double stiffness;
+ int count;
+ int cycle;
+ };
+}
+
+#endif // POSERIGHTSHOULDER_H
More information about the Jderobot-admin
mailing list