[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