[Jderobot-admin] jderobot-r1114 - trunk/src/testing/components/teleoperator

lr.morales en jderobot.org lr.morales en jderobot.org
Lun Nov 25 00:15:50 CET 2013


Author: lr.morales
Date: 2013-11-25 00:15:50 +0100 (Mon, 25 Nov 2013)
New Revision: 1114

Modified:
   trunk/src/testing/components/teleoperator/Gui.cpp
   trunk/src/testing/components/teleoperator/Gui.h
   trunk/src/testing/components/teleoperator/controlICE.cpp
   trunk/src/testing/components/teleoperator/controlICE.h
   trunk/src/testing/components/teleoperator/teleoperatorPC.cpp
Log:
#115 Fixes teleoperator to force interface disconnect on broken connection.
     Connects window close-signals to interface disconnection and program close.



Modified: trunk/src/testing/components/teleoperator/Gui.cpp
===================================================================
--- trunk/src/testing/components/teleoperator/Gui.cpp	2013-11-23 11:04:19 UTC (rev 1113)
+++ trunk/src/testing/components/teleoperator/Gui.cpp	2013-11-24 23:15:50 UTC (rev 1114)
@@ -29,10 +29,10 @@
 
     this->interfacesData = interfacesData;
 
-    this->laserShowed = FALSE;
-    this->camerasShowed = FALSE;
-    this->motorsShowed = FALSE;
-    this->encodersShowed = FALSE;
+    this->laserShowed = false;
+    this->camerasShowed = false;
+    this->motorsShowed = false;
+    this->encodersShowed = false;
 
     std::cout << "Loading glade\n";
     this->builder = Gtk::Builder::create_from_file("TeleoperatorPC.glade");
@@ -43,6 +43,7 @@
 
     this->builder->get_widget("main_window", this->main_window);
     this->builder->get_widget("help_window", this->help_window);
+    this->main_window->signal_hide().connect(sigc::mem_fun(this, &Gui::exit_button_clicked));
 
     //MOTORS    
     //GET WINDOWS    
@@ -109,6 +110,8 @@
     this->check_pose3d->signal_toggled().connect(sigc::mem_fun(this, &Gui::check_pose3d_clicked));
     this->cameras_canvas->signal_event().connect(sigc::mem_fun(this, &Gui::on_button_press_cameras_canvas));
     this->cameras_stop_button->signal_clicked().connect(sigc::mem_fun(this, &Gui::cameras_stop_button_clicked));
+    this->images_window->signal_hide().connect(sigc::mem_fun(this, &Gui::on_images_window_hide));
+    this->teleoperatorCameras_window->signal_hide().connect(sigc::mem_fun(this, &Gui::on_teleoperatorCameras_window_hide));
     //Init widgets
     this->cameras_canvas->add_events(Gdk::BUTTON_PRESS_MASK | Gdk::BUTTON_RELEASE_MASK | Gdk::VISIBILITY_NOTIFY_MASK | Gdk::BUTTON1_MOTION_MASK);
     this->cameras_canvas->get_size_request(this->cameras_previous_event_x, this->cameras_previous_event_y);
@@ -137,7 +140,6 @@
     std::ostringstream convert;
 
     convert << floorf(n * 100) / 100;
-    ;
 
     result = convert.str();
     return result;
@@ -150,66 +152,18 @@
 }
 
 void Gui::displayCameras() {
-    //pthread_mutex_lock(&interfacesData->imagesData_mutex);
     if (interfacesData->imagesReady) {
         try {
-            /*
-            IplImage* image = cvCreateImage(cvSize(interfacesData->imageDataLeftReceived->description->width,interfacesData->imageDataLeftReceived->description->height), 8 ,3);
-    
-    memcpy((unsigned char *) image->imageData,&(interfacesData->imageDataLeftReceived->pixelData[0]),image->width*image->height * 3);
-    
-    colorspaces::Image::FormatPtr fmt = colorspaces::Image::Format::searchFormat(interfacesData->imageDataLeftReceived->description->format);
-    
-    if (!fmt)
-       throw "Format not supported";
-                
-        Glib::RefPtr<Gdk::Pixbuf> imgBuff =
-            Gdk::Pixbuf:: create_from_data((const guint8*)image->imageData,
-                                        Gdk::COLORSPACE_RGB,
-                                        false,
-                                        image->depth,
-                                        image->width,
-                                        image->height,
-                                        image->widthStep);
-
-    IplImage* image2 = cvCreateImage(cvSize(interfacesData->imageDataRightReceived->description->width,interfacesData->imageDataRightReceived->description->height), 8 ,3);
-    
-    memcpy((unsigned char *) image2->imageData,&(interfacesData->imageDataRightReceived->pixelData[0]),image2->width*image2->height * 3);
-    
-    colorspaces::Image::FormatPtr fmt2 = colorspaces::Image::Format::searchFormat(interfacesData->imageDataRightReceived->description->format);
-    
-    if (!fmt2)
-       throw "Format not supported";
-                
-        Glib::RefPtr<Gdk::Pixbuf> imgBuff2 =
-            Gdk::Pixbuf:: create_from_data((const guint8*)image2->imageData,
-                                        Gdk::COLORSPACE_RGB,
-                                        false,
-                                        image2->depth,
-                                        image2->width,
-                                        image2->height,
-                                        image2->widthStep);
-
-             */
             this->image_left_camera->clear();
-            //if(interfacesData->imgBuff->get_pixels()!=NULL)
-            //std::cout << interfacesData->imgBuff->get_pixels() << std::endl;
             this->image_left_camera->set(interfacesData->imgBuff);
-            //if(interfacesData->imgBuff2->get_pixels()!=NULL)
             this->image_right_camera->clear();
-
             this->image_right_camera->set(interfacesData->imgBuff2);
 
-            //cvReleaseImage(&image);
-            //cvReleaseImage(&image2);
-
         } catch (const char* msg) {
             std::cerr << "CAZADO " << msg << std::endl;
 
         }
-
     }
-    //pthread_mutex_unlock(&interfacesData->imagesData_mutex);
 }
 
 void Gui::drawLaser() {
@@ -307,7 +261,6 @@
     int v, w;
 
     widget2motors(this->previous_event_x, this->previous_event_y, &v, &w);
-    v = v * 10;
 
     this->interfacesData->motorsDataToSend.v = v;
     this->interfacesData->motorsDataToSend.w = w;
@@ -317,7 +270,7 @@
 bool Gui::on_button_press_cameras_canvas(GdkEvent* event) {
     float event_x = event->button.x;
     float event_y = event->button.y;
-    static gboolean dragging = FALSE;
+    static gboolean dragging = false;
 
     switch (event->type) {
         case GDK_BUTTON_PRESS:
@@ -343,7 +296,7 @@
             break;
 
         case GDK_BUTTON_RELEASE:
-            dragging = FALSE;
+            dragging = false;
             break;
         default:
             break;
@@ -355,7 +308,7 @@
 bool Gui::on_button_press_motors_canvas(GdkEvent* event) {
     float event_x = event->button.x;
     float event_y = event->button.y;
-    static gboolean dragging = FALSE;
+    static gboolean dragging = false;
 
     if(event_x < 0) event_x = 0;
     if(event_y < 0) event_y = 0;
@@ -387,7 +340,7 @@
             break;
 
         case GDK_BUTTON_RELEASE:
-            dragging = FALSE;
+            dragging = false;
             break;
         default:
             break;
@@ -396,21 +349,31 @@
 }
 
 void Gui::on_motors_window_hide(){
-    if(check_motors->get_active())
+    if(this->check_motors->get_active())
         this->check_motors->set_active(false);
 }
 
 void Gui::on_encoders_window_hide(){
-    if(check_encoders->get_active())
+    if(this->check_encoders->get_active())
         this->check_encoders->set_active(false);
 }
 
 void Gui::on_laser_window_hide(){
-    if(check_laser->get_active())
+    if(this->check_laser->get_active())
         this->check_laser->set_active(false);
 }
 
+void Gui::on_images_window_hide(){
+	if(this->check_cameras->get_active())
+		this->check_cameras->set_active(false);
+}
 
+void Gui::on_teleoperatorCameras_window_hide(){
+	if(this->check_pose3d->get_active())
+		this->check_pose3d->set_active(false);
+}
+
+
 void Gui::stop_button_clicked() {
     this->previous_event_x = 100;
     this->previous_event_y = 100;
@@ -424,7 +387,6 @@
 }
 
 void Gui::exit_button_clicked() {
-
     this->interfacesData->exit = true;
     exit(1);
 
@@ -433,54 +395,43 @@
 void Gui::check_pose3d_clicked() {
 
     if (!check_pose3d->get_active()) {
-        this->interfacesData->pose3DEncodersInterface.checkEnd = TRUE;
-        this->interfacesData->pose3DMotorsInterface.checkEnd = TRUE;
-        this->teleoperatorCameras_window->hide();
+        this->interfacesData->pose3DEncodersInterface.checkEnd = true;
+        this->interfacesData->pose3DMotorsInterface.checkEnd = true;
     } else {
-        this->teleoperatorCameras_window->show();
-        this->interfacesData->pose3DEncodersInterface.checkInit = TRUE;
-        this->interfacesData->pose3DMotorsInterface.checkInit = TRUE;
+        this->interfacesData->pose3DEncodersInterface.checkInit = true;
+        this->interfacesData->pose3DMotorsInterface.checkInit = true;
     }
 }
 
 void Gui::check_laser_clicked() {
     if (!check_laser->get_active()) {
-        this->interfacesData->laserInterface.checkEnd = TRUE;
-
-        this->laser_window->hide();
+        this->interfacesData->laserInterface.checkEnd = true;
     } else {
-        this->laser_window->show();
-        this->interfacesData->laserInterface.checkInit = TRUE;
+        this->interfacesData->laserInterface.checkInit = true;
     }
 }
 
 void Gui::check_motors_clicked() {
     if (!check_motors->get_active()) {
-        this->interfacesData->motorsInterface.checkEnd = TRUE;
-        this->motors_window->hide();
+        this->interfacesData->motorsInterface.checkEnd = true;
     } else {
-        this->motors_window->show();
-        this->interfacesData->motorsInterface.checkInit = TRUE;
+        this->interfacesData->motorsInterface.checkInit = true;
     }
 }
 
 void Gui::check_encoders_clicked() {
     if (!check_encoders->get_active()) {
-        this->interfacesData->encodersInterface.checkEnd = TRUE;
-        this->encoders_window->hide();
+        this->interfacesData->encodersInterface.checkEnd = true;
     } else {
-        this->encoders_window->show();
-        this->interfacesData->encodersInterface.checkInit = TRUE;
+        this->interfacesData->encodersInterface.checkInit = true;
     }
 }
 
 void Gui::check_cameras_clicked() {
     if (!check_cameras->get_active()) {
-        this->interfacesData->camerasInterface.checkEnd = TRUE;
-        this->images_window->hide();
+        this->interfacesData->camerasInterface.checkEnd = true;
     } else {
-        this->images_window->show();
-        this->interfacesData->camerasInterface.checkInit = TRUE;
+        this->interfacesData->camerasInterface.checkInit = true;
     }
 }
 
@@ -519,26 +470,59 @@
 void Gui::show_windows() {
 
     if ((this->interfacesData->pose3DEncodersInterface.activated) && (this->interfacesData->pose3DMotorsInterface.activated)) {
-        if (this->teleoperatorCameras_window->is_visible())
+        if (this->teleoperatorCameras_window->get_visible()){
             displayPose3DEncodersData();
+        }else{
+        	this->teleoperatorCameras_window->show();
+        }
+    }else if(this->teleoperatorCameras_window->get_visible()){
+    	this->teleoperatorCameras_window->hide();
     }
-    if ((this->interfacesData->laserInterface.activated) && (this->interfacesData->laserReady)) {
-        if (this->laser_window->is_visible())
-            drawLaser();
+
+    if (this->interfacesData->laserInterface.activated) {
+        if (this->laser_window->get_visible()){
+        	if(this->interfacesData->laserReady)
+        		drawLaser();
+        }else{
+        	this->laser_window->show();
+        }
+    }else if(this->laser_window->get_visible()){
+    	this->laser_window->hide();
     }
+
     if (this->interfacesData->motorsInterface.activated) {
-        if (this->motors_window->is_visible())
+        if (this->motors_window->get_visible()){
             displayMotorsData();
+        }else{
+        	this->motors_window->show();
+        }
+    }else if(this->motors_window->get_visible()){
+    	this->motors_window->hide();
     }
+
     if (this->interfacesData->encodersInterface.activated) {
-        if (this->encoders_window->is_visible() && (this->interfacesData->encodersReady))
-            displayEncodersData();
+        if (this->encoders_window->get_visible()){
+        	if(this->interfacesData->encodersReady)
+        		displayEncodersData();
+        }else{
+        	this->encoders_window->show();
+        }
+
+    }else if(this->encoders_window->get_visible()){
+    	this->encoders_window->hide();
     }
+
+
     pthread_mutex_lock(&interfacesData->imagesData_mutex);
 
     if (this->interfacesData->camerasInterface.activated) {
-        if (this->images_window->is_visible())
+        if (this->images_window->get_visible()){
             displayCameras();
+        }else{
+        	this->images_window->show();
+        }
+    }else if(this->images_window->get_visible()){
+    	this->images_window->hide();
     }
 
     pthread_mutex_unlock(&interfacesData->imagesData_mutex);

Modified: trunk/src/testing/components/teleoperator/Gui.h
===================================================================
--- trunk/src/testing/components/teleoperator/Gui.h	2013-11-23 11:04:19 UTC (rev 1113)
+++ trunk/src/testing/components/teleoperator/Gui.h	2013-11-24 23:15:50 UTC (rev 1114)
@@ -23,7 +23,6 @@
 #ifndef GUI_H
 #define	GUI_H
 
-#include <visionlib/colorspaces/colorspacesmm.h>
 #include <gtk-2.0/gtk/gtk.h>
 //#include<gtk-2.0/gdk/gdk.h>
 #include <gtkmm-2.4/gtkmm.h>
@@ -111,6 +110,8 @@
     void check_pose3d_clicked();
     void displayCameras();
     bool on_button_press_cameras_canvas(GdkEvent* event);
+    void on_images_window_hide();
+    void on_teleoperatorCameras_window_hide();
     void cameras_handler();
     void displayPose3DEncodersData();
     void Pose3Dencoders2widget(float* v, float* w);

Modified: trunk/src/testing/components/teleoperator/controlICE.cpp
===================================================================
--- trunk/src/testing/components/teleoperator/controlICE.cpp	2013-11-23 11:04:19 UTC (rev 1113)
+++ trunk/src/testing/components/teleoperator/controlICE.cpp	2013-11-24 23:15:50 UTC (rev 1114)
@@ -21,32 +21,31 @@
  */
 
 #include "controlICE.h"
-#include "Gui.h"
 
 controlICE::controlICE(SharedMemory *interfacesData) {
     this->interfacesData = interfacesData;
 
-    interfacesData->imagesReady = FALSE;
-    interfacesData->encodersReady = FALSE;
-    interfacesData->laserReady = FALSE;
-    interfacesData->motorsInterface.activated = FALSE;
-    interfacesData->motorsInterface.checkInit = FALSE;
-    interfacesData->motorsInterface.checkEnd = FALSE;
-    interfacesData->encodersInterface.activated = FALSE;
-    interfacesData->encodersInterface.checkInit = FALSE;
-    interfacesData->encodersInterface.checkEnd = FALSE;
-    interfacesData->laserInterface.activated = FALSE;
-    interfacesData->laserInterface.checkInit = FALSE;
-    interfacesData->laserInterface.checkEnd = FALSE;
-    interfacesData->camerasInterface.activated = FALSE;
-    interfacesData->camerasInterface.checkInit = FALSE;
-    interfacesData->camerasInterface.checkEnd = FALSE;
-    interfacesData->pose3DEncodersInterface.activated = FALSE;
-    interfacesData->pose3DEncodersInterface.checkInit = FALSE;
-    interfacesData->pose3DEncodersInterface.checkEnd = FALSE;
-    interfacesData->pose3DMotorsInterface.activated = FALSE;
-    interfacesData->pose3DMotorsInterface.checkInit = FALSE;
-    interfacesData->pose3DMotorsInterface.checkEnd = FALSE;
+    interfacesData->imagesReady = false;
+    interfacesData->encodersReady = false;
+    interfacesData->laserReady = false;
+    interfacesData->motorsInterface.activated = false;
+    interfacesData->motorsInterface.checkInit = false;
+    interfacesData->motorsInterface.checkEnd = false;
+    interfacesData->encodersInterface.activated = false;
+    interfacesData->encodersInterface.checkInit = false;
+    interfacesData->encodersInterface.checkEnd = false;
+    interfacesData->laserInterface.activated = false;
+    interfacesData->laserInterface.checkInit = false;
+    interfacesData->laserInterface.checkEnd = false;
+    interfacesData->camerasInterface.activated = false;
+    interfacesData->camerasInterface.checkInit = false;
+    interfacesData->camerasInterface.checkEnd = false;
+    interfacesData->pose3DEncodersInterface.activated = false;
+    interfacesData->pose3DEncodersInterface.checkInit = false;
+    interfacesData->pose3DEncodersInterface.checkEnd = false;
+    interfacesData->pose3DMotorsInterface.activated = false;
+    interfacesData->pose3DMotorsInterface.checkInit = false;
+    interfacesData->pose3DMotorsInterface.checkEnd = false;
 
     interfacesData->Pose3DMotorsDataToSendLeft.tilt = 0;
     interfacesData->Pose3DMotorsDataToSendLeft.pan = 0;
@@ -61,38 +60,76 @@
 controlICE::~controlICE() {
 }
 
-void controlICE::getDataGazebo() {
+void controlICE::getData() {
     if (interfacesData->motorsInterface.activated) {
-        interfacesData->motorsDataReceived.v = mprx->getV();
-        interfacesData->motorsDataReceived.w = mprx->getW();
-        interfacesData->motorsDataReceived.l = mprx->getL();
+        try{
+            interfacesData->motorsDataReceived.v = mprx->getV();
+            interfacesData->motorsDataReceived.w = mprx->getW();
+            interfacesData->motorsDataReceived.l = mprx->getL();
+        }catch(const Ice::Exception& ex) {
+            std::cerr << ex << std::endl;
+
+            interfacesData->motorsInterface.checkEnd = true;
+        }
     }
     if (interfacesData->laserInterface.activated) {
-        interfacesData->laserDataReceived = lprx->getLaserData();
-        interfacesData->laserReady = TRUE;
+        interfacesData->laserReady = false;
+        try{
+            interfacesData->laserDataReceived = lprx->getLaserData();
+            interfacesData->laserReady = true;
+        }catch(const Ice::Exception& ex) {
+            std::cerr << ex << std::endl;
+
+            interfacesData->laserInterface.checkEnd = true;
+        }
     }
     if (interfacesData->pose3DEncodersInterface.activated) {
-        interfacesData->Pose3DEncodersDataReceivedLeft = p3deprxLeft->getPose3DEncodersData();
-        interfacesData->Pose3DEncodersDataReceivedRight = p3deprxRight->getPose3DEncodersData();
+        try{
+            interfacesData->Pose3DEncodersDataReceivedLeft = p3deprxLeft->getPose3DEncodersData();
+            interfacesData->Pose3DEncodersDataReceivedRight = p3deprxRight->getPose3DEncodersData();
+        }catch(const Ice::Exception& ex) {
+            std::cerr << ex << std::endl;
+
+            interfacesData->pose3DEncodersInterface.checkEnd = true;
+        }
     }
     pthread_mutex_lock(&interfacesData->imagesData_mutex);
     if (interfacesData->camerasInterface.activated) {
-        interfacesData->imagesReady = FALSE;
+        interfacesData->imagesReady = false;
+        try{
+            interfacesData->imageDataLeftReceived = cprxLeft->getImageData();
+            interfacesData->imageDataRightReceived = cprxRight->getImageData();
 
-        interfacesData->imageDataLeftReceived = cprxLeft->getImageData();
-        interfacesData->imageDataRightReceived = cprxRight->getImageData();
+            createImage1();
+            createImage2();
 
-        createImage1();
-        createImage2();
-        interfacesData->imagesReady = TRUE;
+            interfacesData->imagesReady = true;
+
+        }catch(const Ice::Exception& ex) {
+            std::cerr << ex << std::endl;
+
+            interfacesData->camerasInterface.checkEnd = true;
+        }catch(const char* ex){
+            std::cerr << ex << std::endl;
+
+            interfacesData->camerasInterface.checkEnd = true;
+        }
+
     }
     pthread_mutex_unlock(&interfacesData->imagesData_mutex);
 
     if (interfacesData->encodersInterface.activated) {
-        interfacesData->encodersDataReceived = eprx->getEncodersData();
-        interfacesData->encodersReady = TRUE;
+        interfacesData->encodersReady = false;
+        try{
+            interfacesData->encodersDataReceived = eprx->getEncodersData();
+            interfacesData->encodersReady = true;
+        }catch(const Ice::Exception& ex) {
+            std::cerr << ex << std::endl;
 
+            interfacesData->encodersInterface.checkEnd = true;
+        }
 
+
     }
 }
 
@@ -139,27 +176,38 @@
             this->image2->widthStep);
 }
 
-void controlICE::sendDataGazebo() {
+void controlICE::sendData() {
 
 
     if (interfacesData->motorsInterface.activated) {
-        mprx->setV(interfacesData->motorsDataToSend.v);
-        mprx->setW(interfacesData->motorsDataToSend.w);
+        try{
+            mprx->setV(interfacesData->motorsDataToSend.v);
+            mprx->setW(interfacesData->motorsDataToSend.w);
+        }catch(const Ice::Exception& ex) {
+            std::cerr << ex << std::endl;
+
+            interfacesData->motorsInterface.checkEnd = true;
+        }
     }
 
     if (interfacesData->pose3DMotorsInterface.activated) {
+        try{
+            interfacesData->Pose3DMotorsDataLeft = new jderobot::Pose3DMotorsData();
 
-        interfacesData->Pose3DMotorsDataLeft = new jderobot::Pose3DMotorsData();
+            interfacesData->Pose3DMotorsDataLeft->tilt = interfacesData->Pose3DMotorsDataToSendLeft.tilt;
+            interfacesData->Pose3DMotorsDataLeft->pan = interfacesData->Pose3DMotorsDataToSendLeft.pan;
+            p3dmprxLeft->setPose3DMotorsData(interfacesData->Pose3DMotorsDataLeft);
 
-        interfacesData->Pose3DMotorsDataLeft->tilt = interfacesData->Pose3DMotorsDataToSendLeft.tilt;
-        interfacesData->Pose3DMotorsDataLeft->pan = interfacesData->Pose3DMotorsDataToSendLeft.pan;
-        p3dmprxLeft->setPose3DMotorsData(interfacesData->Pose3DMotorsDataLeft);
+            interfacesData->Pose3DMotorsDataRight = new jderobot::Pose3DMotorsData();
 
-        interfacesData->Pose3DMotorsDataRight = new jderobot::Pose3DMotorsData();
+            interfacesData->Pose3DMotorsDataRight->tilt = interfacesData->Pose3DMotorsDataToSendRight.tilt;
+            interfacesData->Pose3DMotorsDataRight->pan = interfacesData->Pose3DMotorsDataToSendRight.pan;
+            p3dmprxRight->setPose3DMotorsData(interfacesData->Pose3DMotorsDataRight);
+        }catch(const Ice::Exception& ex) {
+            std::cerr << ex << std::endl;
 
-        interfacesData->Pose3DMotorsDataRight->tilt = interfacesData->Pose3DMotorsDataToSendRight.tilt;
-        interfacesData->Pose3DMotorsDataRight->pan = interfacesData->Pose3DMotorsDataToSendRight.pan;
-        p3dmprxRight->setPose3DMotorsData(interfacesData->Pose3DMotorsDataRight);
+            interfacesData->pose3DMotorsInterface.checkEnd = true;
+        }
     }
 
 
@@ -187,15 +235,20 @@
         if (0 == this->lprx)
             throw "Invalid proxy introrob.Laser.Proxy";
 
+        interfacesData->laserInterface.activated = true;
+
         std::cout << "Laser added" << std::endl;
 
-
     } catch (const Ice::Exception& ex) {
         std::cerr << ex << std::endl;
     } catch (const char* msg) {
         std::cerr << msg << std::endl;
     }
 
+    // Couldn't activate correctly... ensure deactivation
+    if(!interfacesData->laserInterface.activated)
+        interfacesData->laserInterface.checkEnd=true;
+
 }
 
 void controlICE::endLaser() {
@@ -203,13 +256,17 @@
 
         this->icLaser->shutdown();
         this->icLaser->destroy();
+
         std::cout << "Laser removed" << std::endl;
+
     } catch (const Ice::Exception& ex) {
         std::cerr << ex << std::endl;
     } catch (const char* msg) {
         std::cerr << msg << std::endl;
     }
 
+    interfacesData->laserInterface.activated = false;
+
 }
 
 void controlICE::initPose3DEncoders() {
@@ -241,6 +298,7 @@
             throw "Invalid proxy introrob.Pose3DEncodersRight.Proxy";
 
 
+        interfacesData->pose3DEncodersInterface.activated = true;
         std::cout << "Pose3Dencoders added" << std::endl;
 
 
@@ -250,6 +308,10 @@
         std::cerr << msg << std::endl;
     }
 
+    // Couldn't activate correctly... ensure deactivation
+    if(!interfacesData->pose3DEncodersInterface.activated)
+        interfacesData->pose3DEncodersInterface.checkEnd=true;
+
 }
 
 void controlICE::endPose3DEncoders() {
@@ -257,6 +319,7 @@
 
         this->icPose3DEncoders->shutdown();
         this->icPose3DEncoders->destroy();
+
         std::cout << "Pose3Dencoders removed" << std::endl;
 
     } catch (const Ice::Exception& ex) {
@@ -265,6 +328,7 @@
         std::cerr << msg << std::endl;
     }
 
+    interfacesData->pose3DEncodersInterface.activated = false;
 }
 
 void controlICE::initPose3DMotors() {
@@ -296,6 +360,7 @@
             throw "Invalid proxy introrob.Pose3DMotorsRight.Proxy";
 
 
+        interfacesData->pose3DMotorsInterface.activated = true;
         std::cout << "Pose3DMotors added" << std::endl;
 
 
@@ -305,6 +370,10 @@
         std::cerr << msg << std::endl;
     }
 
+    // Couldn't activate correctly... ensure deactivation
+    if(!interfacesData->pose3DMotorsInterface.activated)
+        interfacesData->pose3DMotorsInterface.checkEnd=true;
+
 }
 
 void controlICE::endPose3DMotors() {
@@ -312,6 +381,7 @@
 
         this->icPose3DMotors->shutdown();
         this->icPose3DMotors->destroy();
+
         std::cout << "Pose3DMotors removed" << std::endl;
 
     } catch (const Ice::Exception& ex) {
@@ -320,6 +390,8 @@
         std::cerr << msg << std::endl;
     }
 
+    interfacesData->pose3DMotorsInterface.activated = false;
+
 }
 
 void controlICE::initMotors() {
@@ -341,6 +413,7 @@
         if (0 == this->mprx)
             throw "Invalid proxy introrob.Motors.Proxy";
 
+        interfacesData->motorsInterface.activated = true;
         std::cout << "Motors added" << std::endl;
 
 
@@ -350,6 +423,10 @@
         std::cerr << msg << std::endl;
     }
 
+    // Couldn't activate correctly... ensure deactivation
+    if(!interfacesData->motorsInterface.activated)
+        interfacesData->motorsInterface.checkEnd=true;
+
 }
 
 void controlICE::endMotors() {
@@ -357,6 +434,7 @@
 
         this->icMotors->shutdown();
         this->icMotors->destroy();
+
         std::cout << "Motors removed" << std::endl;
 
     } catch (const Ice::Exception& ex) {
@@ -365,6 +443,8 @@
         std::cerr << msg << std::endl;
     }
 
+    interfacesData->motorsInterface.activated = false;
+
 }
 
 void controlICE::initEncoders() {
@@ -386,6 +466,7 @@
         if (0 == this->eprx)
             throw "Invalid proxy introrob.Encoders.Proxy";
 
+        interfacesData->encodersInterface.activated = true;
         std::cout << "Encoders added" << std::endl;
 
 
@@ -395,6 +476,10 @@
         std::cerr << msg << std::endl;
     }
 
+    // Couldn't activate correctly... ensure deactivation
+    if(!interfacesData->encodersInterface.activated)
+        interfacesData->encodersInterface.checkEnd=true;
+
 }
 
 void controlICE::endEncoders() {
@@ -402,6 +487,7 @@
 
         this->icEncoders->shutdown();
         this->icEncoders->destroy();
+
         std::cout << "Encoders removed" << std::endl;
 
     } catch (const Ice::Exception& ex) {
@@ -410,6 +496,7 @@
         std::cerr << msg << std::endl;
     }
 
+    interfacesData->encodersInterface.activated = false;
 }
 
 void controlICE::initCameras() {
@@ -441,6 +528,7 @@
             throw "Invalid proxy introrob.CameraRight.Proxy";
 
 
+        interfacesData->camerasInterface.activated = true;
         std::cout << "Cameras added" << std::endl;
 
 
@@ -450,6 +538,11 @@
         std::cerr << msg << std::endl;
     }
 
+    // Couldn't activate correctly... ensure deactivation
+    if(!interfacesData->camerasInterface.activated)
+        interfacesData->camerasInterface.checkEnd=true;
+
+
 }
 
 void controlICE::endCameras() {
@@ -457,6 +550,7 @@
 
         this->icCameras->shutdown();
         this->icCameras->destroy();
+
         std::cout << "Cameras removed" << std::endl;
 
     } catch (const Ice::Exception& ex) {
@@ -465,74 +559,64 @@
         std::cerr << msg << std::endl;
     }
 
+    interfacesData->camerasInterface.activated = false;
+
 }
 
 void controlICE::checkInterfaces() {
 
     if (interfacesData->laserInterface.checkInit) {
         this->initLaser();
-        interfacesData->laserInterface.activated = TRUE;
-        interfacesData->laserInterface.checkInit = FALSE;
+        interfacesData->laserInterface.checkInit = false;
     }
     if (interfacesData->laserInterface.checkEnd) {
         this->endLaser();
-        interfacesData->laserInterface.activated = FALSE;
-        interfacesData->laserInterface.checkEnd = FALSE;
+        interfacesData->laserInterface.checkEnd = false;
     }
 
     if (interfacesData->camerasInterface.checkInit) {
         this->initCameras();
-        interfacesData->camerasInterface.activated = TRUE;
-        interfacesData->camerasInterface.checkInit = FALSE;
+        interfacesData->camerasInterface.checkInit = false;
     }
     if (interfacesData->camerasInterface.checkEnd) {
         this->endCameras();
-        interfacesData->camerasInterface.activated = FALSE;
-        interfacesData->camerasInterface.checkEnd = FALSE;
+        interfacesData->camerasInterface.checkEnd = false;
     }
 
     if (interfacesData->pose3DEncodersInterface.checkInit) {
         this->initPose3DEncoders();
-        interfacesData->pose3DEncodersInterface.activated = TRUE;
-        interfacesData->pose3DEncodersInterface.checkInit = FALSE;
+        interfacesData->pose3DEncodersInterface.checkInit = false;
     }
     if (interfacesData->pose3DEncodersInterface.checkEnd) {
         this->endPose3DEncoders();
-        interfacesData->pose3DEncodersInterface.activated = FALSE;
-        interfacesData->pose3DEncodersInterface.checkEnd = FALSE;
+        interfacesData->pose3DEncodersInterface.checkEnd = false;
     }
 
     if (interfacesData->pose3DMotorsInterface.checkInit) {
         this->initPose3DMotors();
-        interfacesData->pose3DMotorsInterface.activated = TRUE;
-        interfacesData->pose3DMotorsInterface.checkInit = FALSE;
+        interfacesData->pose3DMotorsInterface.checkInit = false;
     }
     if (interfacesData->pose3DMotorsInterface.checkEnd) {
         this->endPose3DMotors();
-        interfacesData->pose3DMotorsInterface.activated = FALSE;
-        interfacesData->pose3DMotorsInterface.checkEnd = FALSE;
+        interfacesData->pose3DMotorsInterface.checkEnd = false;
     }
 
     if (interfacesData->motorsInterface.checkInit) {
         this->initMotors();
-        interfacesData->motorsInterface.activated = TRUE;
-        interfacesData->motorsInterface.checkInit = FALSE;
+        interfacesData->motorsInterface.checkInit = false;
     }
     if (interfacesData->motorsInterface.checkEnd) {
         this->endMotors();
-        interfacesData->motorsInterface.activated = FALSE;
-        interfacesData->motorsInterface.checkEnd = FALSE;
+        interfacesData->motorsInterface.checkEnd = false;
     }
 
     if (interfacesData->encodersInterface.checkInit) {
         this->initEncoders();
-        interfacesData->encodersInterface.activated = TRUE;
-        interfacesData->encodersInterface.checkInit = FALSE;
+        interfacesData->encodersInterface.checkInit = false;
     }
     if (interfacesData->encodersInterface.checkEnd) {
         this->endEncoders();
-        interfacesData->encodersInterface.activated = FALSE;
-        interfacesData->encodersInterface.checkEnd = FALSE;
+        interfacesData->encodersInterface.checkEnd = false;
     }
 
 }

Modified: trunk/src/testing/components/teleoperator/controlICE.h
===================================================================
--- trunk/src/testing/components/teleoperator/controlICE.h	2013-11-23 11:04:19 UTC (rev 1113)
+++ trunk/src/testing/components/teleoperator/controlICE.h	2013-11-24 23:15:50 UTC (rev 1114)
@@ -43,8 +43,8 @@
     controlICE(const controlICE& orig);
     virtual ~controlICE();
 
-    void getDataGazebo();
-    void sendDataGazebo();
+    void getData();
+    void sendData();
     void initLaser();
     void endLaser();
     void initPose3DMotors();

Modified: trunk/src/testing/components/teleoperator/teleoperatorPC.cpp
===================================================================
--- trunk/src/testing/components/teleoperator/teleoperatorPC.cpp	2013-11-23 11:04:19 UTC (rev 1113)
+++ trunk/src/testing/components/teleoperator/teleoperatorPC.cpp	2013-11-24 23:15:50 UTC (rev 1114)
@@ -57,8 +57,8 @@
             totala = a.tv_sec * 1000000 + a.tv_usec;
 
             control->checkInterfaces(); //Check if interfaces are activated and init/end them
-            control->getDataGazebo(); //Get sensor data from gazebo
-            control->sendDataGazebo(); //Send data to Gazebo
+            control->getData(); //Get sensor data
+            control->sendData(); //Send data to interfaces
 
             gettimeofday(&b, NULL);
             totalb = b.tv_sec * 1000000 + b.tv_usec;



More information about the Jderobot-admin mailing list