From 9eb821d91db6a66db9128f9c115a544f57925800 Mon Sep 17 00:00:00 2001 From: Francisco Perez Date: Fri, 21 Apr 2017 15:12:44 +0200 Subject: [PATCH] [Issue #783] Fixed cameras showing the same image, and updated copyright --- .../plugins/turtlebot/CMakeLists.txt | 2 - .../include/turtlebot/cameraproxy.hh | 72 --- .../turtlebot/include/turtlebot/debugtools.h | 5 +- .../include/turtlebot/interfaces/bumperi.h | 2 +- .../include/turtlebot/interfaces/camerai.h | 15 +- .../turtlebot/interfaces/cameraibase.h | 8 +- .../include/turtlebot/interfaces/laseri.h | 5 +- .../include/turtlebot/interfaces/motorsi.h | 19 + .../include/turtlebot/interfaces/pose3di.h | 7 +- .../turtlebot/interfaces/pushcamerai.h | 49 -- .../include/turtlebot/turtlebotcontrol.hh | 10 +- .../include/turtlebot/turtlebotice.hh | 14 +- .../include/turtlebot/turtlebotplugin.hh | 10 +- .../include/turtlebot/turtlebotsensors.hh | 8 +- .../plugins/turtlebot/src/cameraproxy.cc | 115 ---- .../turtlebot/src/interfaces/bumperi.cpp | 19 + .../turtlebot/src/interfaces/camera_dump.cpp | 299 ----------- .../turtlebot/src/interfaces/camerai.cpp | 15 +- .../turtlebot/src/interfaces/cameraibase.cpp | 8 +- .../turtlebot/src/interfaces/laseri.cpp | 20 + .../turtlebot/src/interfaces/motorsi.cpp | 19 + .../src/interfaces/pose3dencoders.cpp | 496 ------------------ .../turtlebot/src/interfaces/pose3di.cpp | 7 +- .../turtlebot/src/interfaces/pushcamerai.cpp | 59 --- .../plugins/turtlebot/src/turtlebotcontrol.cc | 10 +- .../plugins/turtlebot/src/turtlebotice.cc | 28 +- .../plugins/turtlebot/src/turtlebotplugin.cc | 15 +- .../plugins/turtlebot/src/turtlebotsensors.cc | 8 +- 28 files changed, 179 insertions(+), 1165 deletions(-) delete mode 100644 src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/cameraproxy.hh delete mode 100644 src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/pushcamerai.h delete mode 100644 src/drivers/gazeboserver/plugins/turtlebot/src/cameraproxy.cc delete mode 100644 src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/camera_dump.cpp delete mode 100644 src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pose3dencoders.cpp delete mode 100644 src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pushcamerai.cpp diff --git a/src/drivers/gazeboserver/plugins/turtlebot/CMakeLists.txt b/src/drivers/gazeboserver/plugins/turtlebot/CMakeLists.txt index ed443ac2a..a8f9da77f 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/CMakeLists.txt +++ b/src/drivers/gazeboserver/plugins/turtlebot/CMakeLists.txt @@ -25,7 +25,6 @@ set(SOURCES src/turtlebotplugin.cc src/turtlebotcontrol.cc src/turtlebotsensors.cc - src/cameraproxy.cc src/interfaces/motorsi.cpp src/interfaces/laseri.cpp @@ -33,7 +32,6 @@ set(SOURCES src/turtlebotplugin.cc #src/interfaces/kinectPlugini.cpp src/interfaces/camerai.cpp src/interfaces/bumperi.cpp - src/interfaces/pushcamerai.cpp src/interfaces/cameraibase.cpp ) diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/cameraproxy.hh b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/cameraproxy.hh deleted file mode 100644 index a7ac3f05e..000000000 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/cameraproxy.hh +++ /dev/null @@ -1,72 +0,0 @@ -/* - * Copyright (C) 1997-2015 JDE 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 3 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 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 : - * Victor Arribas Raigadas - */ - -#ifndef CAMERAPROXY -#define CAMERAPROXY - -#include -#include -#include - -namespace turtlebot{ - -class ICameraConsumer{ -public: - virtual void onCameraSensorBoostrap(const cv::Mat, const gazebo::sensors::CameraSensorPtr)=0; - virtual void onCameraSensorUpdate(const cv::Mat)=0; -}; - -typedef boost::shared_ptr ICameraConsumerPtr; - -class CameraProxy{ -public: - CameraProxy(); - virtual ~CameraProxy(); - - void registerCamera(gazebo::sensors::CameraSensorPtr cam); - void setActive(int id); - int getActive(); - void next(); - - void registerConsumer(ICameraConsumerPtr consumer); - -protected: - /// overrideable method that acts as pasarell between gazebo's raw image format and cv::Mat - virtual cv::Mat _createImageWrapper(const gazebo::sensors::CameraSensorPtr cam, const int id) const; - -private: - void _on_cam_bootstrap(); - void _on_cam_update(); - - -private: - boost::mutex lock; - int active_camera = -1; - gazebo::event::ConnectionPtr active_sub; - - cv::Mat img; - - std::vector cameras; - std::vector consumers; -}; - -}//NS - -#endif // CAMERAPROXY - diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/debugtools.h b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/debugtools.h index 3137cea27..f32c343a8 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/debugtools.h +++ b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/debugtools.h @@ -13,8 +13,9 @@ * * 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 : - * Victor Arribas Raigadas + * + * CLONE of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/include/quadrotor/interfaces/cameraibase.h + * Victor Arribas Raigadas */ diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/bumperi.h b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/bumperi.h index 9bd2682a1..6395d5df6 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/bumperi.h +++ b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/bumperi.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 1997-2015 JDE Developers Team + * Copyright (C) 1997-2017 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 diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/camerai.h b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/camerai.h index 9a1bea97b..79437175a 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/camerai.h +++ b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/camerai.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 1997-2015 JDE Developers Team + * Copyright (C) 1997-2017 JDE 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 @@ -13,8 +13,12 @@ * * 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 : - * Victor Arribas Raigadas + * + * REMIX of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/include/quadrotor/interfaces/camerai.h + * Victor Arribas Raigadas + * + * Authors: + * Francisco Perez Salgado */ #ifndef CAMERAI_H @@ -32,14 +36,15 @@ namespace interfaces{ class CameraI: public turtlebot::interfaces::CameraIBase { public: - CameraI (const turtlebot::TurtlebotSensors *sensor); + CameraI (const turtlebot::TurtlebotSensors *sensor, int cam_id); virtual ~CameraI (); - const int cam_id = TurtlebotSensors::CAM_LEFT; private: cv::Mat imgCached; const turtlebot::TurtlebotSensors *sensor; + const int cam_id; + /// Gazebo private: diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/cameraibase.h b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/cameraibase.h index 0f2ce2db4..12b7e3b85 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/cameraibase.h +++ b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/cameraibase.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 1997-2015 JDE Developers Team + * Copyright (C) 1997-2017 JDE 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 @@ -13,8 +13,10 @@ * * 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 : - * Victor Arribas Raigadas + * + * CLONE of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/include/quadrotor/interfaces/cameraibase.h + * Victor Arribas Raigadas + * */ #ifndef CAMERAIBASE_H diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/laseri.h b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/laseri.h index 727c15f31..bf1bdaf3b 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/laseri.h +++ b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/laseri.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 1997-2015 JDE Developers Team + * Copyright (C) 1997-2017 JDE 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 @@ -14,7 +14,8 @@ * 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 : - * Victor Arribas Raigadas + * Francisco Perez Salgado + * Aitor Martinez Fernandez */ #ifndef LASER_H diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/motorsi.h b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/motorsi.h index 5ee63dba3..8630570eb 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/motorsi.h +++ b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/motorsi.h @@ -1,3 +1,22 @@ +/* + * Copyright (C) 1997-2017 JDE 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 3 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 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 : + * Francisco Perez Salgado + */ + #ifndef MOTORSI_H #define MOTORSI_H diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/pose3di.h b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/pose3di.h index e3ed24c60..b37120baa 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/pose3di.h +++ b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/pose3di.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 1997-2015 JDE Developers Team + * Copyright (C) 1997-2017 JDE 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 @@ -13,8 +13,9 @@ * * 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 : - * Victor Arribas Raigadas + * + * CLONE of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/include/quadrotor/interfaces/pose3di.h + * Victor Arribas Raigadas */ diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/pushcamerai.h b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/pushcamerai.h deleted file mode 100644 index 07e30c42a..000000000 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/interfaces/pushcamerai.h +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (C) 1997-2015 JDE 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 3 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 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 : - * Victor Arribas Raigadas - */ - -#ifndef PUSHCAMERAI_H -#define PUSHCAMERAI_H - -#include -#include - - -namespace turtlebot{ -namespace interfaces{ - - -class PushCameraI: - public turtlebot::interfaces::CameraIBase, - public ICameraConsumer -{ -public: - PushCameraI (); - virtual ~PushCameraI (); - - - void onCameraSensorBoostrap(const cv::Mat, const gazebo::sensors::CameraSensorPtr); - void onCameraSensorUpdate(const cv::Mat); - -}; - - -}}//NS - -#endif // PUSHCAMERAI_H - diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotcontrol.hh b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotcontrol.hh index dcb320850..06ea89c0a 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotcontrol.hh +++ b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotcontrol.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 1997-2015 JDE Developers Team + * Copyright (C) 1997-2017 JDE 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 @@ -13,8 +13,12 @@ * * 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 : - * Victor Arribas Raigadas + * + * REMIX of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/include/quadrotor/quadrotorcontrol.hh + * Victor Arribas Raigadas + * + * Authors: + * Francisco Perez Salgado */ #ifndef TURTLEBOTCONTROL_H diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotice.hh b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotice.hh index 32bfaf9d7..0a332b861 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotice.hh +++ b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotice.hh @@ -13,8 +13,12 @@ * * 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 : - * Victor Arribas Raigadas + * + * REMIX of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/include/quadrotor/quadrotorice.hh + * Victor Arribas Raigadas + * + * Authors: + * Francisco Perez Salgado */ #ifndef TURTLEBOTICE_H @@ -32,13 +36,12 @@ #include #include #include -#include +#include #include #include #include #include -#include #include @@ -47,7 +50,7 @@ namespace turtlebot{ class TurtlebotIce { public: - TurtlebotIce(Ice::CommunicatorPtr ic, const TurtlebotSensors *sensors, TurtlebotControl *control, CameraProxy *camproxy); + TurtlebotIce(Ice::CommunicatorPtr ic, const TurtlebotSensors *sensors, TurtlebotControl *control); virtual ~TurtlebotIce(); void run(); @@ -67,7 +70,6 @@ private: private: const TurtlebotSensors *sensor; TurtlebotControl *control; - CameraProxy *camproxy; boost::mutex lock; }; diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotplugin.hh b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotplugin.hh index ff55fd466..80f9671f9 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotplugin.hh +++ b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotplugin.hh @@ -13,8 +13,12 @@ * * 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 : - * Victor Arribas Raigadas + * + * REMIX of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/include/quadrotor/quadrotorplugin.hh + * Victor Arribas Raigadas + * + * Authors: + * Francisco Perez Salgado */ #ifndef TURTLEBOTPLUGIN_H @@ -29,7 +33,6 @@ #include #include #include -#include #include @@ -45,7 +48,6 @@ public: private: TurtlebotSensors sensors; TurtlebotControl control; - CameraProxy cameraproxy; /// Gazebo diff --git a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotsensors.hh b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotsensors.hh index f360b1747..67dc2e51f 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotsensors.hh +++ b/src/drivers/gazeboserver/plugins/turtlebot/include/turtlebot/turtlebotsensors.hh @@ -13,8 +13,12 @@ * * 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 : - * Victor Arribas Raigadas + * + * REMIX of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/include/quadrotor/quadrotorsensors.hh + * Victor Arribas Raigadas + * + * Authors: + * Francisco Perez Salgado */ diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/cameraproxy.cc b/src/drivers/gazeboserver/plugins/turtlebot/src/cameraproxy.cc deleted file mode 100644 index b1ca2618d..000000000 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/cameraproxy.cc +++ /dev/null @@ -1,115 +0,0 @@ -/* - * Copyright (C) 1997-2015 JDE 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 3 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 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 : - * Victor Arribas Raigadas - */ - - - -#include - -using namespace turtlebot; - - -CameraProxy::CameraProxy(){ - -} - -CameraProxy::~CameraProxy(){ - -} - -void -CameraProxy::registerCamera(gazebo::sensors::CameraSensorPtr cam){ - cameras.push_back(cam); -} - -void -CameraProxy::registerConsumer(ICameraConsumerPtr consumer){ - consumers.push_back(consumer); -} - -void -CameraProxy::setActive(int id){ - lock.lock(); - - /* - /// deactivate previous - if (active_camera != -1){ - cameras[active_camera]->SetActive(false); - cameras[active_camera]->DisconnectUpdated(active_sub); - } - */ - - /// active new - if (id != -1){ - active_camera = id; - active_sub = cameras[active_camera]->ConnectUpdated( - boost::bind(&CameraProxy::_on_cam_bootstrap, this)); - cameras[0]->SetActive(true); - cameras[1]->SetActive(true); - } - - lock.unlock(); -} - -int -CameraProxy::getActive(){ - return active_camera; -} - -void -CameraProxy::next(){ - int next = (active_camera+1)%cameras.size(); - setActive(next); -} - -void -CameraProxy::_on_cam_bootstrap(){ - lock.lock(); - - cameras[active_camera]->DisconnectUpdated(active_sub); - - img = _createImageWrapper(cameras[active_camera], active_camera); - - for (ICameraConsumerPtr consumer : consumers){ - consumer->onCameraSensorBoostrap(img, cameras[active_camera]); - } - - active_sub = cameras[active_camera]->ConnectUpdated( - boost::bind(&CameraProxy::_on_cam_update, this)); - lock.unlock(); -} - -void -CameraProxy::_on_cam_update(){ - lock.lock(); - - for (ICameraConsumerPtr consumer : consumers){ - consumer->onCameraSensorUpdate(img); - } - - lock.unlock(); -} - - -cv::Mat -CameraProxy::_createImageWrapper(const gazebo::sensors::CameraSensorPtr cam, const int /*id*/) const{ - const unsigned char *data = cam->ImageData(); - uint32_t h = cam->ImageHeight(); - uint32_t w = cam->ImageWidth(); - return cv::Mat(h, w, CV_8UC3, (uint8_t*)data); -} diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/bumperi.cpp b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/bumperi.cpp index 32ac24877..3b6d79455 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/bumperi.cpp +++ b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/bumperi.cpp @@ -1,3 +1,22 @@ +/* + * Copyright (C) 1997-2017 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 3 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 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 : + * Francisco Perez Salgado + */ + #include "turtlebot/interfaces/bumperi.h" using namespace turtlebot::interfaces; diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/camera_dump.cpp b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/camera_dump.cpp deleted file mode 100644 index edd71c875..000000000 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/camera_dump.cpp +++ /dev/null @@ -1,299 +0,0 @@ -#include "gazebo.hh" -#include "plugins/CameraPlugin.hh" -#include "common/common.hh" -#include "transport/transport.hh" - -#include -#include -#include - -// ICE utils includes -#include -#include -#include - -#include - -#include - -#include - -#include -#include - -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: " <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: " <parentSensor->GetCamera()->GetName()<< std::endl; - if(count==0){ - std::vector tokens; - nameCamera = this->parentSensor->GetCamera()->GetName(); - nameGlobal = nameCamera; - //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=" + nameCamera + ".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()); - cameraDescription = (new jderobot::CameraDescription()); - cameraDescription->name = "camera Introrob"; - - 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 std::string& format, const Ice::Current& c){ - replyTask->pushJob(cb, format); - } - - virtual jderobot::ImageFormat getImageFormat(const Ice::Current& c) - { - jderobot::ImageFormat mFormats; - mFormats.push_back(colorspaces::ImageRGB8::FORMAT_RGB8.get()->name); - - return mFormats; - } - - virtual std::string startCameraStreaming(const Ice::Current&){ - - } - - virtual void stopCameraStreaming(const Ice::Current&) { - - } - - virtual void reset(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, std::string format){ - - mFormat = format; - 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 requests; - std::string mFormat; - }; - - typedef IceUtil::Handle 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 = EasyIce::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; - } - } - -} diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/camerai.cpp b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/camerai.cpp index 1474dc741..0d9fdc413 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/camerai.cpp +++ b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/camerai.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 1997-2015 JDE Developers Team + * Copyright (C) 1997-2017 JDE 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 @@ -13,8 +13,12 @@ * * 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 : - * Victor Arribas Raigadas + * + * REMIX of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/src/interfaces/camerai.cpp + * Victor Arribas Raigadas + * + * Authors: + * Francisco Perez Salgado */ #include @@ -23,8 +27,9 @@ using namespace turtlebot::interfaces; using namespace jderobot; -CameraI::CameraI (const TurtlebotSensors *sensor): - sensor(sensor) +CameraI::CameraI (const TurtlebotSensors *sensor, int cam_id): + sensor(sensor), + cam_id(cam_id) { cameraSensorConnection = sensor->cam[cam_id]->ConnectUpdated( boost::bind(&CameraI::onCameraSensorBoostrap, this)); diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/cameraibase.cpp b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/cameraibase.cpp index 43a4015f1..15514cbed 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/cameraibase.cpp +++ b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/cameraibase.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 1997-2015 JDE Developers Team + * Copyright (C) 1997-2017 JDE 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 @@ -13,8 +13,10 @@ * * 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 : - * Victor Arribas Raigadas + * + * CLONE of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/src/interfaces/cameraibase.cpp + * Victor Arribas Raigadas + * */ #include diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/laseri.cpp b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/laseri.cpp index 631ae2672..c87474a18 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/laseri.cpp +++ b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/laseri.cpp @@ -1,3 +1,23 @@ +/* + * Copyright (C) 1997-2017 JDE 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 3 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 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 : + * Francisco Perez Salgado + * Aitor Martinez Fernandez + */ + #include "turtlebot/interfaces/laseri.h" using namespace turtlebot::interfaces; diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/motorsi.cpp b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/motorsi.cpp index 896903fff..604d7581a 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/motorsi.cpp +++ b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/motorsi.cpp @@ -1,3 +1,22 @@ +/* + * Copyright (C) 1997-2017 JDE 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 3 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 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 : + * Francisco Perez Salgado + */ + #include "turtlebot/interfaces/motorsi.h" using namespace turtlebot::interfaces; diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pose3dencoders.cpp b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pose3dencoders.cpp deleted file mode 100644 index f36a566ba..000000000 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pose3dencoders.cpp +++ /dev/null @@ -1,496 +0,0 @@ -#include "pose3dencoders.h" -//#include "pose3dmotors.h" - - - -namespace gazebo { - - void *pose3dencodersICE(void* v); - - GZ_REGISTER_MODEL_PLUGIN(Pose3DEncoders) - - Pose3DEncoders::Pose3DEncoders() { - pthread_mutex_init(&mutex, NULL); - pthread_mutex_init(&mutexPose3DEncoders, NULL); - count = 0; - std::cout << "constructor pose3dencoders" << std::endl; - } - - void Pose3DEncoders::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { - - this->model = _model; - - // LOAD CAMERA LEFT - if (!_sdf->HasElement("left_joint_pose3dencoders_pan")) - gzerr << "pose3dencoders plugin missing element\n"; - if (!_sdf->HasElement("left_joint_pose3dencoders_tilt")) - gzerr << "pose3dencoders plugin missing element\n"; - - this->cameraLeft.joint_pose3dencoders_pan = this->model->GetJoint("pan_joint_left"); - this->cameraLeft.joint_pose3dencoders_tilt = this->model->GetJoint("tilt_joint_left"); - - if (!this->cameraLeft.joint_pose3dencoders_pan) - gzerr << "Unable to find left joint pose3dencoders_pan[" - << _sdf->GetElement("left_joint_pose3dencoders_pan")->Get() << "]\n"; - if (!this->cameraLeft.joint_pose3dencoders_tilt) - gzerr << "Unable to find left joint pose3dencoders_tilt[" - << _sdf->GetElement("left_joint_pose3dencoders_tilt")->Get() << "]\n"; - this->cameraLeft.camera_link_pan = this->model->GetLink("camera_column_body_left"); - this->cameraLeft.camera_link_tilt = this->model->GetLink("camera_top_body_left"); - - //LOAD CAMERA RIGHT - if (!_sdf->HasElement("right_joint_pose3dencoders_pan")) - gzerr << "Motors plugin missing element\n"; - if (!_sdf->HasElement("right_joint_pose3dencoders_tilt")) - gzerr << "Motors plugin missing element\n"; - - - this->cameraRight.joint_pose3dencoders_pan = this->model->GetJoint("pan_joint_right"); - this->cameraRight.joint_pose3dencoders_tilt = this->model->GetJoint("tilt_joint_right"); - - - if (!this->cameraRight.joint_pose3dencoders_pan) - gzerr << "Unable to find right joint pose3dencoders_pan[" - << _sdf->GetElement("right_joint_pose3dencoders_pan")->Get() << "]\n"; - if (!this->cameraRight.joint_pose3dencoders_tilt) - gzerr << "Unable to find right joint pose3dencoders_tilt[" - << _sdf->GetElement("right_joint_pose3dencoders_tilt")->Get() << "]\n"; - this->cameraRight.camera_link_pan = this->model->GetLink("camera_column_body_right"); - this->cameraRight.camera_link_tilt = this->model->GetLink("camera_top_body_right"); - - - //LOAD TORQUE - if (_sdf->HasElement("torque")) - this->torque = _sdf->GetElement("torque")->Get(); - else { - gzwarn << "No torque value set for the DiffDrive plugin.\n"; - this->torque = 5.0; - } - - //LOAD POSE3DMOTORS - - this->updateConnection = event::Events::ConnectWorldUpdateBegin( - boost::bind(&Pose3DEncoders::OnUpdate, this)); - - - } - - void Pose3DEncoders::Init() { - - - - } - - void Pose3DEncoders::OnUpdate() { - - if (count == 0) { - count++; - - std::string name = this->model->GetName(); - - namePose3DEncoders = std::string("--Ice.Config=" + name + "_pose3dencoders.cfg"); - pthread_t thr_gui; - pthread_create(&thr_gui, NULL, &pose3dencodersICE, (void*) this); - } - - // ----------ENCODERS---------- - //GET pose3dencoders data from left_camera (PAN&TILT) - this->cameraLeft.encoder.pan = this->cameraLeft.camera_link_pan->GetRelativePose().rot.GetAsEuler().z * 180.0 / M_PI; - if (this->cameraLeft.encoder.pan > 0) { - this->cameraLeft.encoder.pan = 180 - this->cameraLeft.encoder.pan; - } - if (this->cameraLeft.encoder.pan < 0) { - this->cameraLeft.encoder.pan = -(180 + this->cameraLeft.encoder.pan); - } - - //std::cout << this->cameraLeft.encoder.pan << std::endl; - this->cameraLeft.encoder.tilt = this->cameraLeft.camera_link_tilt->GetRelativePose().rot.GetAsEuler().y * 180.0 / M_PI; - //std::cout << this->cameraLeft.encoder.tilt << std::endl; - - - //GET pose3dencoders data from left_camera (PAN&TILT) - - this->cameraRight.encoder.pan = this->cameraRight.camera_link_pan->GetRelativePose().rot.GetAsEuler().z * 180.0 / M_PI; - if (this->cameraRight.encoder.pan > 0) { - this->cameraRight.encoder.pan = 180 - this->cameraRight.encoder.pan; - } - if (this->cameraRight.encoder.pan < 0) { - this->cameraRight.encoder.pan = -(180 + this->cameraRight.encoder.pan); - } - - //std::cout << this->cameraRight.pan << std::endl; - this->cameraRight.encoder.tilt = this->cameraRight.camera_link_tilt->GetRelativePose().rot.GetAsEuler().y * 180.0 / M_PI; - //std::cout << this->cameraRight.encoder.tilt << std::endl; - - double setPanRight = -50; - double setPanLeft = -50; - double setTiltRight = -10; - double setTiltLeft = -10; - // ----------MOTORS---------- - - - - if (this->cameraLeft.motor.pan >= 0) { - if (this->cameraLeft.encoder.pan < this->cameraLeft.motor.pan) { - this->cameraLeft.joint_pose3dencoders_pan->SetParam("vel", 0, -0.1); - this->cameraLeft.joint_pose3dencoders_pan->SetParam("fmax", 0, this->torque); - //std::cout << "AQUI" << std::endl; - } else { - this->cameraLeft.joint_pose3dencoders_pan->SetParam("vel", 0, 0); - this->cameraLeft.joint_pose3dencoders_pan->SetParam("fmax", 0, this->torque); - } - } else { - if (this->cameraLeft.encoder.pan > this->cameraLeft.motor.pan) { - this->cameraLeft.joint_pose3dencoders_pan->SetParam("vel", 0, 0.1); - this->cameraLeft.joint_pose3dencoders_pan->SetParam("fmax", 0, this->torque); - //std::cout << "AQUI" << std::endl; - } else { - this->cameraLeft.joint_pose3dencoders_pan->SetParam("vel", 0, 0); - this->cameraLeft.joint_pose3dencoders_pan->SetParam("fmax", 0, this->torque); - } - } - if (this->cameraRight.motor.pan >= 0) { - if (this->cameraRight.encoder.pan < this->cameraRight.motor.pan) { - this->cameraRight.joint_pose3dencoders_pan->SetParam("vel", 0, -0.1); - this->cameraRight.joint_pose3dencoders_pan->SetParam("fmax", 0, this->torque); - //std::cout << "AQUI" << std::endl; - } else { - this->cameraRight.joint_pose3dencoders_pan->SetParam("vel", 0, 0); - this->cameraRight.joint_pose3dencoders_pan->SetParam("fmax", 0, this->torque); - } - } else { - if (this->cameraRight.encoder.pan > this->cameraRight.motor.pan) { - this->cameraRight.joint_pose3dencoders_pan->SetParam("vel", 0, 0.1); - this->cameraRight.joint_pose3dencoders_pan->SetParam("fmax", 0, this->torque); - //std::cout << "AQUI" << std::endl; - } else { - this->cameraRight.joint_pose3dencoders_pan->SetParam("vel", 0, 0); - this->cameraRight.joint_pose3dencoders_pan->SetParam("fmax", 0, this->torque); - } - } - - if (this->cameraLeft.motor.tilt >= 0) { - if (this->cameraLeft.encoder.tilt < this->cameraLeft.motor.tilt) { - this->cameraLeft.joint_pose3dencoders_tilt->SetParam("vel", 0, -0.1); - this->cameraLeft.joint_pose3dencoders_tilt->SetParam("fmax", 0, this->torque); - //std::cout << "AQUI" << std::endl; - } else { - this->cameraLeft.joint_pose3dencoders_tilt->SetParam("vel", 0, 0); - this->cameraLeft.joint_pose3dencoders_tilt->SetParam("fmax", 0, this->torque); - } - } else { - if (this->cameraLeft.encoder.tilt > this->cameraLeft.motor.tilt) { - this->cameraLeft.joint_pose3dencoders_tilt->SetParam("vel", 0, 0.1); - this->cameraLeft.joint_pose3dencoders_tilt->SetParam("fmax", 0, this->torque); - //std::cout << "AQUI" << std::endl; - } else { - this->cameraLeft.joint_pose3dencoders_tilt->SetParam("vel", 0, 0); - this->cameraLeft.joint_pose3dencoders_tilt->SetParam("fmax", 0, this->torque); - } - } - if (this->cameraRight.motor.tilt >= 0) { - if (this->cameraRight.encoder.tilt < this->cameraRight.motor.tilt) { - this->cameraRight.joint_pose3dencoders_tilt->SetParam("vel", 0, -0.1); - this->cameraRight.joint_pose3dencoders_tilt->SetParam("fmax", 0, this->torque); - //std::cout << "AQUI" << std::endl; - } else { - this->cameraRight.joint_pose3dencoders_tilt->SetParam("vel", 0, 0); - this->cameraRight.joint_pose3dencoders_tilt->SetParam("fmax", 0, this->torque); - } - } else { - if (this->cameraRight.encoder.tilt > this->cameraRight.motor.tilt) { - this->cameraRight.joint_pose3dencoders_tilt->SetParam("vel", 0, 0.1); - this->cameraRight.joint_pose3dencoders_tilt->SetParam("fmax", 0, this->torque); - //std::cout << "AQUI" << std::endl; - } else { - this->cameraRight.joint_pose3dencoders_tilt->SetParam("vel", 0, 0); - this->cameraRight.joint_pose3dencoders_tilt->SetParam("fmax", 0, this->torque); - } - } - - - - - - } - - class Pose3DEncodersI : virtual public jderobot::Pose3DEncoders { - public: - - Pose3DEncodersI(gazebo::Pose3DEncoders* pose) : pose3DEncodersData(new jderobot::Pose3DEncodersData()) { - - this->pose = pose; - - } - - virtual ~Pose3DEncodersI() { - - } - - virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData(const Ice::Current&) { - - pthread_mutex_lock(&pose->mutex); - pose3DEncodersData->x = 0; - pose3DEncodersData->y = 0; - pose3DEncodersData->z = 0; - pose3DEncodersData->pan = pose->cameraLeft.encoder.pan; - pose3DEncodersData->tilt = pose->cameraLeft.encoder.tilt; - pose3DEncodersData->clock = 0; - pose3DEncodersData->roll = 0; - pthread_mutex_unlock(&pose->mutex); - - return pose3DEncodersData; - - } - - gazebo::Pose3DEncoders* pose; - - private: - jderobot::Pose3DEncodersDataPtr pose3DEncodersData; - - }; - - class Pose3DEncodersII : virtual public jderobot::Pose3DEncoders { - public: - - Pose3DEncodersII(gazebo::Pose3DEncoders* pose) : pose3DEncodersData(new jderobot::Pose3DEncodersData()) { - - this->pose = pose; - - } - - virtual ~Pose3DEncodersII() { - - } - - virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData(const Ice::Current&) { - - pthread_mutex_lock(&pose->mutex); - pose3DEncodersData->x = 0; - pose3DEncodersData->y = 0; - pose3DEncodersData->z = 0; - pose3DEncodersData->pan = pose->cameraRight.encoder.pan; - pose3DEncodersData->tilt = pose->cameraRight.encoder.tilt; - pose3DEncodersData->clock = 0; - pose3DEncodersData->roll = 0; - pthread_mutex_unlock(&pose->mutex); - - return pose3DEncodersData; - - } - - gazebo::Pose3DEncoders* pose; - - private: - jderobot::Pose3DEncodersDataPtr pose3DEncodersData; - - }; - - class Pose3DMotorsI : virtual public jderobot::Pose3DMotors { - - public: - - Pose3DMotorsI(gazebo::Pose3DEncoders* pose) : pose3DMotorsData(new jderobot::Pose3DMotorsData()) { - - this->pose = pose; - - } - - virtual ~Pose3DMotorsI(){ - - } - - virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData(const Ice::Current&){ - - pthread_mutex_lock(&pose->mutex); - pose3DMotorsData->x = 0; - pose3DMotorsData->y = 0; - pose3DMotorsData->z = 0; - pose3DMotorsData->pan = pose->cameraLeft.encoder.pan; - pose3DMotorsData->tilt = pose->cameraLeft.encoder.tilt; - pose3DMotorsData->roll = 0; - pose3DMotorsData->panSpeed = 0; - pose3DMotorsData->tiltSpeed = 0; - pthread_mutex_unlock(&pose->mutex); - - return pose3DMotorsData; - - - } - - virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams(const Ice::Current&) { - pthread_mutex_lock(&pose->mutex); - pose3DMotorsParams->maxPan = 0; - pose3DMotorsParams->minPan = 0; - pose3DMotorsParams->maxTilt = 0; - pose3DMotorsParams->minTilt = 0; - pose3DMotorsParams->maxPanSpeed = 0; - pose3DMotorsParams->maxTiltSpeed = 0; - pthread_mutex_unlock(&pose->mutex); - - return pose3DMotorsParams; - } - - virtual Ice::Int setPose3DMotorsData(const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current&) { - pthread_mutex_lock(&pose->mutex); - pose->cameraLeft.motor.x = data->x; - pose->cameraLeft.motor.y = data->y; - pose->cameraLeft.motor.z = data->z; - pose->cameraLeft.motor.pan = data->pan; - pose->cameraLeft.motor.tilt = data->tilt; - pose->cameraLeft.motor.roll = data->roll; - pose->cameraLeft.motor.panSpeed = data->panSpeed; - pose->cameraLeft.motor.tiltSpeed = data->tiltSpeed; - pthread_mutex_unlock(&pose->mutex); - - } - - gazebo::Pose3DEncoders* pose; - - private: - jderobot::Pose3DMotorsDataPtr pose3DMotorsData; - jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams; - - - }; - - class Pose3DMotorsII : virtual public jderobot::Pose3DMotors { - - public: - - Pose3DMotorsII(gazebo::Pose3DEncoders* pose) : pose3DMotorsData(new jderobot::Pose3DMotorsData()) { - - this->pose = pose; - - } - - virtual ~Pose3DMotorsII(){ - - } - - virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData(const Ice::Current&){ - - pthread_mutex_lock(&pose->mutex); - pose3DMotorsData->x = 0; - pose3DMotorsData->y = 0; - pose3DMotorsData->z = 0; - pose3DMotorsData->pan = pose->cameraRight.encoder.pan; - pose3DMotorsData->tilt = pose->cameraRight.encoder.tilt; - pose3DMotorsData->roll = 0; - pose3DMotorsData->panSpeed = 0; - pose3DMotorsData->tiltSpeed = 0; - pthread_mutex_unlock(&pose->mutex); - - return pose3DMotorsData; - - - } - - virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams(const Ice::Current&) { - pthread_mutex_lock(&pose->mutex); - pose3DMotorsParams->maxPan = 0; - pose3DMotorsParams->minPan = 0; - pose3DMotorsParams->maxTilt = 0; - pose3DMotorsParams->minTilt = 0; - pose3DMotorsParams->maxPanSpeed = 0; - pose3DMotorsParams->maxTiltSpeed = 0; - pthread_mutex_unlock(&pose->mutex); - - return pose3DMotorsParams; - } - - virtual Ice::Int setPose3DMotorsData(const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current&) { - pthread_mutex_lock(&pose->mutex); - pose->cameraRight.motor.x = data->x; - pose->cameraRight.motor.y = data->y; - pose->cameraRight.motor.z = data->z; - pose->cameraRight.motor.pan = data->pan; - pose->cameraRight.motor.tilt = data->tilt; - pose->cameraRight.motor.roll = data->roll; - pose->cameraRight.motor.panSpeed = data->panSpeed; - pose->cameraRight.motor.tiltSpeed = data->tiltSpeed; - pthread_mutex_unlock(&pose->mutex); - - } - - gazebo::Pose3DEncoders* pose; - - private: - jderobot::Pose3DMotorsDataPtr pose3DMotorsData; - jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams; - - - }; - - - void *pose3dencodersICE(void* v) { - - gazebo::Pose3DEncoders* base = (gazebo::Pose3DEncoders*)v; - char* name = (char*) base->namePose3DEncoders.c_str(); - Ice::CommunicatorPtr ic; - int argc = 1; - Ice::PropertiesPtr prop; - char* argv[] = {name}; - - try { - - ic = EasyIce::initialize(argc, argv); - - - prop = ic->getProperties(); - std::string Endpoints1 = prop->getProperty("Pose3DEncoders1.Endpoints"); - std::cout << "Pose3DEncoders1 Endpoints > " << Endpoints1 << std::endl; - std::string Endpoints2 = prop->getProperty("Pose3DEncoders2.Endpoints"); - std::cout << "Pose3DEncoders2 Endpoints > " << Endpoints2 << std::endl; - std::string Endpoints3 = prop->getProperty("Pose3DMotors1.Endpoints"); - std::cout << "Pose3DMotors1 Endpoints > " << Endpoints3 << std::endl; - std::string Endpoints4 = prop->getProperty("Pose3DMotors2.Endpoints"); - std::cout << "Pose3DMotors2 Endpoints > " << Endpoints4 << std::endl; - - - Ice::ObjectAdapterPtr adapter1 = - ic->createObjectAdapterWithEndpoints("Pose3DEncoders1", Endpoints1); - Ice::ObjectAdapterPtr adapter2 = - ic->createObjectAdapterWithEndpoints("Pose3DEncoders2", Endpoints2); - Ice::ObjectAdapterPtr adapter3 = - ic->createObjectAdapterWithEndpoints("Pose3DMotors1", Endpoints3); - Ice::ObjectAdapterPtr adapter4 = - ic->createObjectAdapterWithEndpoints("Pose3DMotors2", Endpoints4); - - - - Ice::ObjectPtr object1 = new Pose3DEncodersI(base); - Ice::ObjectPtr object2 = new Pose3DEncodersII(base); - Ice::ObjectPtr object3 = new Pose3DMotorsI(base); - Ice::ObjectPtr object4 = new Pose3DMotorsII(base); - - - - adapter1->add(object1, ic->stringToIdentity("Pose3DEncoders1")); - adapter2->add(object2, ic->stringToIdentity("Pose3DEncoders2")); - adapter3->add(object3, ic->stringToIdentity("Pose3DMotors1")); - adapter4->add(object4, ic->stringToIdentity("Pose3DMotors2")); - - adapter1->activate(); - adapter2->activate(); - adapter3->activate(); - adapter4->activate(); - - ic->waitForShutdown(); - } catch (const Ice::Exception& e) { - std::cerr << e << std::endl; - } catch (const char* msg) { - std::cerr << msg << std::endl; - } - if (ic) { - try { - ic->destroy(); - } catch (const Ice::Exception& e) { - std::cerr << e << std::endl; - } - } - - } - - -} diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pose3di.cpp b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pose3di.cpp index eb783a09a..71689f8c9 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pose3di.cpp +++ b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pose3di.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 1997-2015 JDE Developers Team + * Copyright (C) 1997-2017 JDE 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 @@ -13,8 +13,9 @@ * * 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 : - * Victor Arribas Raigadas + * + * CLONE of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/src/interfaces/pose3di.cpp + * Victor Arribas Raigadas */ diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pushcamerai.cpp b/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pushcamerai.cpp deleted file mode 100644 index 1b5884409..000000000 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/interfaces/pushcamerai.cpp +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright (C) 1997-2015 JDE 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 3 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 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 : - * Victor Arribas Raigadas - */ - -#include - -using namespace turtlebot::interfaces; -using namespace jderobot; - - -PushCameraI::PushCameraI () -{} - -PushCameraI::~PushCameraI () -{} - -void -PushCameraI::onCameraSensorBoostrap(const cv::Mat img, const gazebo::sensors::CameraSensorPtr /*camerasensor*/){ - ONDEBUG_INFO(std::cout << "PushCameraI::onCameraSensorBoostrap()" << std::endl;) - lock_guard_t RAII_lock(mutex); - imageDescription = new ImageDescription(); - imageDescription->format = "RGB8";// colorspaces::ImageRGB8::FORMAT_RGB8->name(); - imageDescription->height = img.rows; - imageDescription->width = img.cols; - imageDescription->size = img.rows*img.cols*3; - - //cameraDescription = new CameraDescription(); - //gazebo::rendering::CameraPtr cam = camerasensor->GetCamera(); - - imageData = new ImageData(); - imageData->description = imageDescription; - imageData->pixelData.resize(imageDescription->size); - memcpy(imageData->pixelData.data(), img.data, imageData->pixelData.size()); - - imageFormats = ImageFormats(1); - imageFormats.push_back(imageDescription->format); -} - -void -PushCameraI::onCameraSensorUpdate(const cv::Mat img){ - ONDEBUG_VERBOSE(std::cout << "PushCameraI::onCameraSensorUpdate()" << std::endl;) - lock_guard_t RAII_lock(mutex); - memcpy(imageData->pixelData.data(), img.data, imageData->pixelData.size()); -} diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotcontrol.cc b/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotcontrol.cc index 19a5d6fe1..ca60146ff 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotcontrol.cc +++ b/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotcontrol.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 1997-2015 JDE Developers Team + * Copyright (C) 1997-2017 JDE 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 @@ -13,8 +13,12 @@ * * 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 : - * Victor Arribas Raigadas + * + * REMIX of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/src/quadrotorcontrol.cc + * Victor Arribas Raigadas + * + * Authors: + * Francisco Perez Salgado */ diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotice.cc b/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotice.cc index 58dde7830..cc7ca7f37 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotice.cc +++ b/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotice.cc @@ -13,8 +13,12 @@ * * 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 : - * Francisco Perez Salgado + * + * REMIX of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/src/quadrotorice.cc + * Victor Arribas Raigadas + * + * Authors: + * Francisco Perez Salgado */ @@ -24,11 +28,10 @@ using namespace turtlebot; using namespace turtlebot::interfaces; using namespace Ice; -TurtlebotIce::TurtlebotIce(CommunicatorPtr ic, const TurtlebotSensors *sensors, TurtlebotControl *control, CameraProxy *camproxy): +TurtlebotIce::TurtlebotIce(CommunicatorPtr ic, const TurtlebotSensors *sensors, TurtlebotControl *control): ic(ic), sensor(sensors), - control(control), - camproxy(camproxy) + control(control) { assert(ic != 0); } @@ -108,22 +111,11 @@ void TurtlebotIce::bootstrap(){ name = prop->getProperty("Turtlebot.Bumper.Name"); adapter->add(bumperi, ic->stringToIdentity(name)); - //ObjectPtr camerai = new CameraI(sensor); - ObjectPtr cameraiL; - { - PushCameraI *_cameraiL = new PushCameraI(); - camproxy->registerConsumer(ICameraConsumerPtr(_cameraiL)); - cameraiL = ObjectPtr(_cameraiL); - } + ObjectPtr cameraiL = new CameraI(sensor,0); name = prop->getProperty("Turtlebot.CameraL.Name"); adapter->add(cameraiL, ic->stringToIdentity(name)); - ObjectPtr cameraiR; - { - PushCameraI *_cameraiR = new PushCameraI(); - camproxy->registerConsumer(ICameraConsumerPtr(_cameraiR)); - cameraiR = ObjectPtr(_cameraiR); - } + ObjectPtr cameraiR = new CameraI(sensor,1); name = prop->getProperty("Turtlebot.CameraR.Name"); adapter->add(cameraiR, ic->stringToIdentity(name)); diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotplugin.cc b/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotplugin.cc index c5c08758b..3c1757983 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotplugin.cc +++ b/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotplugin.cc @@ -13,8 +13,12 @@ * * 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 : - * Victor Arribas Raigadas + * + * REMIX of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/src/quadrotorplugin.cc + * Victor Arribas Raigadas + * + * Authors: + * Francisco Perez Salgado */ @@ -79,11 +83,6 @@ TurtlebotPlugin::Init(){ sensors.laser->SetActive(true); sensors.bumper->SetActive(true); - cameraproxy.registerCamera(sensors.cam[TurtlebotSensors::CAM_LEFT]); - cameraproxy.registerCamera(sensors.cam[TurtlebotSensors::CAM_RIGHT]); - cameraproxy.setActive(0); - cameraproxy.setActive(1); - control.Init(&sensors); icePlugin->start(); @@ -146,6 +145,6 @@ TurtlebotPlugin::InitializeIce(sdf::ElementPtr _sdf){ id.properties->setProperty("Turtlebot.Adapter.Endpoints", "tcp -h localhost -p "+port); //ToDo: use regex replace instead hardcored text. std::cout << _log_prefix << "\tcreate Ice plugin..." << std::endl; - icePlugin = TurtlebotIcePtr(new TurtlebotIce(ic, &sensors, &control, &cameraproxy)); + icePlugin = TurtlebotIcePtr(new TurtlebotIce(ic, &sensors, &control)); icePlugin->_log_prefix = _log_prefix; } diff --git a/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotsensors.cc b/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotsensors.cc index 933e05712..b4e23777e 100644 --- a/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotsensors.cc +++ b/src/drivers/gazeboserver/plugins/turtlebot/src/turtlebotsensors.cc @@ -13,8 +13,12 @@ * * 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 : - * Victor Arribas Raigadas + * + * REMIX of https://github.com/jderobot-varribas/gazeboplugin-quadrotor2/blob/2.1.0/src/quadrotorsensors.cc + * Victor Arribas Raigadas + * + * Authors: + * Francisco Perez Salgado */