[issue #831] Added new holonomic cars

src/drivers/gazeboserver/models/car_holo/model.config

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Holonomic car</name>
+  <version>1.0</version>
+  <sdf version="1.4">model.sdf</sdf>
+
+  <author>
+    <name>Satyaki Chakraborty</name>
+    <email>satyaki.cs15@gmail.com</email>
+  </author>
+
+  <description>
+    A car model. 
+  </description>
+
+</model>

src/drivers/gazeboserver/models/car_holo/model.sdf

@@ -0,0 +1,37 @@
+<?xml version='1.0'?>
+<sdf version="1.4">
+<model name="car">
+  <pose>0 0 0 0 0 0</pose>
+  <static>false</static>
+    <link name="link">
+      <gravity>false</gravity>
+      <inertial>
+        <mass>750.0</mass>
+        <inertia> 
+          <ixx>1</ixx>      
+          <ixy>0.0</ixy>        
+          <ixz>0.0</ixz>        
+          <iyy>1</iyy>      
+          <iyz>0.0</iyz>         
+          <izz>1</izz>      
+        </inertia>
+      </inertial>
+      <collision name="collision">
+        <geometry>
+	  <mesh>
+	    <uri>model://car_holo/meshes/coche_holo.dae</uri>
+	  </mesh>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model://car_holo/meshes/coche_holo.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+    <plugin name="holocarmotors" filename="libholoCarMotors.so"/>
+    <plugin name="holocarpose" filename="libholoCarPose3D.so"/>
+  </model>
+</sdf>

src/drivers/gazeboserver/models/taxi_holo/model.config

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Holonimic taxi</name>
+  <version>1.0</version>
+  <sdf version="1.4">model.sdf</sdf>
+
+  <author>
+    <name>Satyaki Chakraborty</name>
+    <email>satyaki.cs15@gmail.com</email>
+  </author>
+
+  <description>
+    A car model. 
+  </description>
+
+</model>

src/drivers/gazeboserver/models/taxi_holo/model.sdf

@@ -0,0 +1,37 @@
+<?xml version='1.0'?>
+<sdf version="1.4">
+<model name="car">
+  <pose>0 0 0 0 0 0</pose>
+  <static>false</static>
+    <link name="link">
+      <gravity>false</gravity>
+      <inertial>
+        <mass>750.0</mass>
+        <inertia> 
+          <ixx>1</ixx>      
+          <ixy>0.0</ixy>        
+          <ixz>0.0</ixz>        
+          <iyy>1</iyy>      
+          <iyz>0.0</iyz>         
+          <izz>1</izz>      
+        </inertia>
+      </inertial>
+      <collision name="collision">
+        <geometry>
+	  <mesh>
+	    <uri>model://taxi_holo/meshes/taxi_holo.dae</uri>
+	  </mesh>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>model://taxi_holo/meshes/taxi_holo.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+    <plugin name="holocarmotors" filename="libholoCarMotors.so"/>
+    <plugin name="holocarpose" filename="libholoCarPose3D.so"/>
+  </model>
+</sdf>

src/drivers/gazeboserver/plugins/holo_car/CMakeLists.txt

@@ -0,0 +1,41 @@
+
+include_directories(
+	${GAZEBO_INCLUDE_DIRS}
+    ${INTERFACES_CPP_DIR}
+    ${LIBS_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${easyiceconfig_INCLUDE_DIRS}
+)
+
+link_directories(
+    ${GAZEBO_LIBRARY_DIRS}
+    ${easyiceconfig_LIBRARY_DIRS}
+)
+#boost_system 
+add_library(holoCarMotors SHARED holoCarMotors.cc)
+target_link_libraries(holoCarMotors 
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES} 
+
+    ${easyiceconfig_LIBRARIES}
+    colorspacesmm
+    JderobotInterfaces
+)
+
+add_library(holoCarPose3D SHARED holoCarPose3d.cc)
+target_link_libraries(holoCarPose3D
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES} 
+
+    ${easyiceconfig_LIBRARIES}
+    colorspacesmm
+    JderobotInterfaces
+)
+
+
+INSTALL (TARGETS holoCarMotors DESTINATION share/jderobot/gazebo/plugins/holo_car/ COMPONENT core)
+INSTALL (TARGETS holoCarPose3D DESTINATION share/jderobot/gazebo/plugins/holo_car/ COMPONENT core)
+FILE(GLOB_RECURSE CFG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*.cfg)
+INSTALL (FILES ${CFG_FILES} DESTINATION share/jderobot/conf/ COMPONENT core)
+
+

src/drivers/gazeboserver/plugins/holo_car/holoCarMotors.cc

@@ -0,0 +1,179 @@
+#include <holoCarMotors.h>
+
+/*
+enum {
+    FRONTRIGHT,
+    FRONTLEFT
+};
+*/
+
+namespace gazebo
+{
+    void *motorsICE(void* v);
+
+
+    Motors::Motors(){
+        pthread_mutex_init(&mutex, NULL);
+        pthread_mutex_init(&mutexMotor, NULL);
+        count = 0;
+        std::cout << "constructor motors" << std::endl;
+        //this->motorspeed[FRONTLEFT] = this->motorspeed[FRONTRIGHT] =  0;
+        this->motorsteer = 0;
+        this->motorspeed = 0;
+    }
+    void Motors::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
+    {
+        this->model = _model;
+        this->node = transport::NodePtr(new transport::Node());
+        this->node->Init(this->model->GetWorld()->GetName());
+
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                    boost::bind(&Motors::OnUpdate, this));
+    }
+
+    void Motors::Init() {
+
+    }
+
+
+    // Called by the world update start event
+    void Motors::PID()
+    {
+
+    }
+
+    void Motors::OnUpdate()
+    {
+
+        if (count == 0) {
+
+            robotMotors.v = 0;
+            robotMotors.w = 0;
+            robotMotors.wheelMin= -0.52359;
+            robotMotors.wheelMax= 0.52359;
+            robotMotors.targetRightSteerPos=robotMotors.targetLeftSteerPos=0;
+
+            count++;
+            //std::string name = this->model->GetName();
+            //std::cout << "Model name " << name << std::endl;
+
+            //nameMotors = std::string("--Ice.Config=" + name +"motors.cfg");
+            nameMotors = std::string("--Ice.Config=holoCarMotors.cfg");
+            pthread_t thr_gui;
+            pthread_create(&thr_gui, NULL, &motorsICE, (void*) this);
+
+        }
+
+        float z = model->GetRelativeLinearVel().z;
+        math::Vector3 vel(robotMotors.v,0,z);
+
+        math::Quaternion rot = model->GetWorldPose().rot;
+        vel = rot.GetAsMatrix3()*vel;
+
+        this->model->SetLinearVel(vel);
+        this->model->SetAngularVel(math::Vector3(0,0,robotMotors.w));
+    }
+
+    class MotorsI : virtual public jderobot::Motors {
+        public:
+
+            MotorsI(gazebo::Motors* base) {
+                this->base = base;
+            }
+
+            virtual ~MotorsI() {
+            };
+
+            virtual float getV(const Ice::Current&) {
+
+                float v_return;
+                pthread_mutex_lock(&base->mutexMotor);
+                //v_return = base->vel;
+                v_return = base->robotMotors.v;
+                pthread_mutex_unlock(&base->mutexMotor);
+                return v_return;
+            };
+
+            virtual float getW(const Ice::Current&) {
+                float w_return;
+                pthread_mutex_lock(&base->mutexMotor);
+                //w_return = base->w;
+                w_return = base->robotMotors.w;
+                pthread_mutex_unlock(&base->mutexMotor);
+                return w_return;
+            };
+
+            virtual float getL(const Ice::Current&) {
+                return 0.;
+            };
+
+            virtual Ice::Int setV(Ice::Float v, const Ice::Current&) {
+                pthread_mutex_lock(&base->mutexMotor);
+                //base->vel = v;
+                base->robotMotors.v = v;
+                pthread_mutex_unlock(&base->mutexMotor);
+                return 0;
+            };
+
+            virtual Ice::Int setW(Ice::Float _w, const Ice::Current&) {
+                pthread_mutex_lock(&base->mutexMotor);
+                //base->w = _w;
+                base->robotMotors.w = _w;
+                pthread_mutex_unlock(&base->mutexMotor);
+                return 0;
+            };
+
+            virtual Ice::Int setL(Ice::Float l, const Ice::Current&) {
+                return 0;
+            };
+
+        public:
+            gazebo::Motors* base;
+        }; // end class MotorsI
+
+
+    void *motorsICE(void* v) {
+
+        gazebo::Motors* base = (gazebo::Motors*)v;
+
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        char* name = (char*) base->nameMotors.c_str();
+        std::cout << name << "\n";
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+
+            ic = EasyIce::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string Endpoints = prop->getProperty("Motors.Endpoints");
+            std::cout << "Motors Endpoints > " << Endpoints << std::endl;
+
+            Ice::ObjectAdapterPtr adapter =
+                    ic->createObjectAdapterWithEndpoints("Motors", Endpoints);
+            Ice::ObjectPtr object = new MotorsI(base);
+
+            adapter->add(object, ic->stringToIdentity("Motors"));
+
+            adapter->activate();
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+
+    }
+    // Register this plugin with the simulator
+    GZ_REGISTER_MODEL_PLUGIN(Motors)
+
+}

src/drivers/gazeboserver/plugins/holo_car/holoCarMotors.cfg

@@ -0,0 +1,3 @@
+#without registry
+Motors.Endpoints=default -h 0.0.0.0 -p 9999:ws -h 0.0.0.0 -p 11002
+