[Jderobot-admin] jderobot-r955 - in trunk/src/components: . recorder recorder/build recorder/build-independent replayer replayer/build replayer/build-independent
frivas en jderobot.org
frivas en jderobot.org
Jue Jul 25 10:33:58 CEST 2013
Author: frivas
Date: 2013-07-25 10:32:57 +0200 (Thu, 25 Jul 2013)
New Revision: 955
Added:
trunk/src/components/recorder/
trunk/src/components/recorder/CMakeLists.txt
trunk/src/components/recorder/build-independent/
trunk/src/components/recorder/build-independent/CMakeLists.txt
trunk/src/components/recorder/build/
trunk/src/components/recorder/build/CMakeLists.txt
trunk/src/components/recorder/build/cmake_uninstall.cmake.in
trunk/src/components/recorder/poolWriteEncoders.cpp
trunk/src/components/recorder/poolWriteEncoders.h
trunk/src/components/recorder/poolWriteImages.cpp
trunk/src/components/recorder/poolWriteImages.h
trunk/src/components/recorder/poolWriteLasers.cpp
trunk/src/components/recorder/poolWriteLasers.h
trunk/src/components/recorder/poolWritePointCloud.cpp
trunk/src/components/recorder/poolWritePointCloud.h
trunk/src/components/recorder/poolWritePose3dEncoders.cpp
trunk/src/components/recorder/poolWritePose3dEncoders.h
trunk/src/components/recorder/recorder.cfg
trunk/src/components/recorder/recorder.cpp
trunk/src/components/recorder/recordergui.cpp
trunk/src/components/recorder/recordergui.glade
trunk/src/components/recorder/recordergui.h
trunk/src/components/replayer/
trunk/src/components/replayer/CMakeLists.txt
trunk/src/components/replayer/build-independent/
trunk/src/components/replayer/build-independent/CMakeLists.txt
trunk/src/components/replayer/build/
trunk/src/components/replayer/build/CMakeLists.txt
trunk/src/components/replayer/build/cmake_uninstall.cmake.in
trunk/src/components/replayer/control.cpp
trunk/src/components/replayer/control.h
trunk/src/components/replayer/replayer.cfg
trunk/src/components/replayer/replayer.cpp
trunk/src/components/replayer/replayergui.cpp
trunk/src/components/replayer/replayergui.glade
trunk/src/components/replayer/replayergui.h
Log:
[24] a?\195?\177adidos nuevos componentes replayer y recorder
Added: trunk/src/components/recorder/CMakeLists.txt
===================================================================
--- trunk/src/components/recorder/CMakeLists.txt (rev 0)
+++ trunk/src/components/recorder/CMakeLists.txt 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,29 @@
+SET(SOURCE_FILES recorder.cpp recordergui.cpp poolWriteImages.cpp poolWritePose3dEncoders.cpp poolWriteLasers.cpp poolWritePointCloud.cpp poolWriteEncoders.cpp)
+
+add_definitions(-DGLADE_DIR="${gladedir}")
+
+set( CMAKE_CXX_FLAGS "-Wno-deprecated" ) # Opciones para el compilador-lgsl -lgslcblas -lGL -lGLU -lglut -lgazebo
+
+include_directories(
+ ${INTERFACES_CPP_DIR}
+ ${LIBS_DIR}/
+ ${CMAKE_CURRENT_SOURCE_DIR}
+ ${CMAKE_CURRENT_SOURCE_DIR}/cameras
+)
+
+add_executable (recorder ${SOURCE_FILES})
+
+
+TARGET_LINK_LIBRARIES(recorder
+ ${CMAKE_THREAD_LIBS_INIT}
+ ${GLUT_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+ ${gtkmm_LIBRARIES}
+ ${libglademm_LIBRARIES}
+ ${gtkglextmm_LIBRARIES}
+ ${LIBS_DIR}/colorspaces/libcolorspacesmm.so
+ ${INTERFACES_CPP_DIR}/jderobot/libJderobotInterfaces.so
+ ${LIBS_DIR}/jderobotutil/libjderobotutil.so
+ ${gsl_LIBRARIES}
+ ${ZeroCIce_LIBRARIES}
+)
Added: trunk/src/components/recorder/build/CMakeLists.txt
===================================================================
--- trunk/src/components/recorder/build/CMakeLists.txt (rev 0)
+++ trunk/src/components/recorder/build/CMakeLists.txt 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,108 @@
+project (JDEROBOT_KINECTVIEWER)
+
+cmake_minimum_required(VERSION 2.8)
+
+# ENV VARS
+SET(gladedir ./)
+
+SET( INTERFACES_CPP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../interfaces/cpp) # Directorio con las interfaces ICE en C++
+SET( LIBS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../libs) # Directorio donde se encuentran las librerias propias de jderobot
+SET( SLICE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../interfaces/slice) # Directorio donde se encuentran las interfaces ICE
+SET( LIBS_NEEDED colorspaces jderobotutil ) # Librerias de las que depende el componente
+SET( DEPS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../Deps) # Directorio donde se encuentran las dependencias
+
+###################
+# #
+# CHECK SYSTEM #
+# #
+###################
+
+ # FIND & CHECK PRINCIPAL LIBRARIES
+
+include(FindPkgConfig)
+
+PKG_CHECK_MODULES(opencv REQUIRED opencv)
+include_directories(${opencv_INCLUDE_DIRS})
+link_directories(${opencv_LIBRARY_DIRS})
+
+PKG_CHECK_MODULES(gtkmm REQUIRED gtkmm-2.4)
+include_directories(${gtkmm_INCLUDE_DIRS})
+link_directories(${gtkmm_LIBRARY_DIRS})
+
+PKG_CHECK_MODULES(libglademm REQUIRED libglademm-2.4)
+include_directories(${libglademm_INCLUDE_DIRS})
+link_directories(${libglademm_LIBRARY_DIRS})
+
+PKG_CHECK_MODULES(gtkglextmm REQUIRED gtkglextmm-1.2)
+include_directories(${gtkglextmm_INCLUDE_DIRS})
+link_directories(${gtkglextmm_LIBRARY_DIRS})
+
+PKG_CHECK_MODULES(gsl REQUIRED gsl)
+include_directories(${gsl_INCLUDE_DIRS})
+link_directories(${gsl_LIBRARIES_DIRS})
+
+include(${DEPS_DIR}/gearbox/CMakeLists.txt)
+include(${DEPS_DIR}/ice/CMakeLists.txt)
+include(${DEPS_DIR}/opencv/CMakeLists.txt)
+include(${DEPS_DIR}/opengl/CMakeLists.txt)
+
+# START RECURSIVE
+add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/../../.. ${CMAKE_CURRENT_SOURCE_DIR}/../../..)
+add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/.. ${CMAKE_CURRENT_SOURCE_DIR}/..)
+
+###################
+# #
+# INSTALL #
+# #
+###################
+
+# Install libraries
+INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../../../src/interfaces/cpp/jderobot/libJderobotInterfaces.so DESTINATION /usr/local/lib/jderobot)
+
+FOREACH(currentLibFile ${LIBS_NEEDED})
+ SET (new_lib "lib${currentLibFile}.so")
+ MESSAGE("${new_lib}")
+ INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../../../src/libs/${currentLibFile}/${new_lib} DESTINATION /usr/local/lib/jderobot)
+ENDFOREACH(currentLibFile)
+
+# Install libraries headers
+FILE(GLOB_RECURSE HEADERS_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../../../src/libs/*.h)
+FOREACH(currentSourceFile ${HEADERS_FILES})
+ string(REGEX REPLACE ".*/(.*/).*.h" "\\1" new_source1 ${currentSourceFile})
+ INSTALL (FILES ${currentSourceFile} DESTINATION /usr/local/include/jderobot/${new_source1})
+ENDFOREACH(currentSourceFile)
+
+# Install Executables
+FILE(GLOB_RECURSE BIN_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../*cfg)
+ string(REGEX REPLACE ".*/(.*).cfg" "\\1" new_source1 ${BIN_FILES})
+INSTALL (FILES ../${new_source1} DESTINATION /usr/local/bin PERMISSIONS OWNER_EXECUTE GROUP_EXECUTE WORLD_EXECUTE)
+
+# Install interfaces headers
+FILE(GLOB HEADER_INTERFACE_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../../../src/interfaces/cpp/jderobot/*.h)
+INSTALL (FILES ${HEADER_INTERFACE_FILES} DESTINATION /usr/local/include/jderobot/jderobot)
+
+# Install slice
+FILE(GLOB SLICE_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../../../src/interfaces/slice/jderobot/*.ice)
+INSTALL (FILES ${SLICE_FILES} DESTINATION /usr/local/include/jderobot/slice)
+
+# Install CONF
+FILE(GLOB_RECURSE CONF_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../*cfg)
+INSTALL (FILES ${CONF_FILES} DESTINATION /usr/local/share/jderobot/conf)
+
+# Install Glade
+FILE(GLOB_RECURSE GLADE_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../*.glade)
+INSTALL (FILES ${GLADE_FILES} DESTINATION /usr/local/share/jderobot/glade)
+
+###################
+# #
+# UNINSTALL #
+# #
+###################
+
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/cmake_uninstall.cmake.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
+ IMMEDIATE @ONLY)
+
+add_custom_target(uninstall
+ COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake)
Added: trunk/src/components/recorder/build/cmake_uninstall.cmake.in
===================================================================
--- trunk/src/components/recorder/build/cmake_uninstall.cmake.in (rev 0)
+++ trunk/src/components/recorder/build/cmake_uninstall.cmake.in 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,22 @@
+if (NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+ message(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"")
+endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+
+file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
+string(REGEX REPLACE "\n" ";" files "${files}")
+list(REVERSE files)
+foreach (file ${files})
+ message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"")
+ if (EXISTS "$ENV{DESTDIR}${file}")
+ execute_process(
+ COMMAND @CMAKE_COMMAND@ -E remove "$ENV{DESTDIR}${file}"
+ OUTPUT_VARIABLE rm_out
+ RESULT_VARIABLE rm_retval
+ )
+ if(NOT ${rm_retval} EQUAL 0)
+ message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"")
+ endif (NOT ${rm_retval} EQUAL 0)
+ else (EXISTS "$ENV{DESTDIR}${file}")
+ message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.")
+ endif (EXISTS "$ENV{DESTDIR}${file}")
+endforeach(file)
\ No newline at end of file
Added: trunk/src/components/recorder/build-independent/CMakeLists.txt
===================================================================
--- trunk/src/components/recorder/build-independent/CMakeLists.txt (rev 0)
+++ trunk/src/components/recorder/build-independent/CMakeLists.txt 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,72 @@
+ cmake_minimum_required(VERSION 2.8)
+
+
+SET( SOURCE_FILES ../recorder.cpp ../recordergui.cpp ../poolWriteImages.cpp ../poolWritePose3dEncoders.cpp ../poolWriteLasers.cpp ../poolWritePointCloud.cpp ../poolWriteEncoders.cpp)
+SET( LIBS_DIR /usr/local/lib/jderobot/)
+
+
+
+include_directories(
+ /usr/local/include/jderobot
+)
+
+PROJECT(recorderProject)
+add_executable (recorder ${SOURCE_FILES})
+
+#automated opencv
+include(FindPkgConfig)
+PKG_CHECK_MODULES(opencv REQUIRED opencv)
+include_directories(${opencv_INCLUDE_DIRS})
+link_directories(${opencv_LIBRARY_DIRS})
+
+
+
+#manual ICE
+FIND_PATH( Ice_INCLUDE_DIR NAMES Ice/Ice.h PATHS ENV C++LIB ENV)
+
+IF( Ice_INCLUDE_DIR )
+ FIND_LIBRARY( Ice_LIBRARY1 NAMES Ice PATHS ENV C++LIB ENV PATH PATH_SUFFIXES lib lib64 )
+ FIND_LIBRARY( Ice_LIBRARY2 NAMES IceUtil PATHS ENV C++LIB ENV PATH PATH_SUFFIXES lib lib64)
+ SET (Ice_LIBRARIES ${Ice_LIBRARY1} ${Ice_LIBRARY2})
+ IF( Ice_LIBRARIES )
+ MESSAGE ("-- Ice found at ${Ice_LIBRARIES}")
+ include_directories(${Ice_INCLUDE_DIR})
+ link_directories(${Ice_LIBRARIES})
+ ENDIF( Ice_LIBRARIES )
+ENDIF(Ice_INCLUDE_DIR)
+
+IF(NOT Ice_LIBRARIES)
+ MESSAGE ("*** Ice not found")
+ENDIF()
+
+
+#automated gtk
+PKG_CHECK_MODULES(gtkmm REQUIRED gtkmm-2.4)
+include_directories(${gtkmm_INCLUDE_DIRS})
+link_directories(${gtkmm_LIBRARY_DIRS})
+#automated gtkmm
+PKG_CHECK_MODULES(libglademm REQUIRED libglademm-2.4)
+include_directories(${libglademm_INCLUDE_DIRS})
+link_directories(${libglademm_LIBRARY_DIRS})
+#automated gtkglextmm
+PKG_CHECK_MODULES(gtkglextmm REQUIRED gtkglextmm-x11-1.2)
+include_directories(${gtkglextmm_INCLUDE_DIRS})
+link_directories(${gtkglextmm_LIBRARY_DIRS})
+
+
+
+set( CMAKE_CXX_FLAGS "-Wno-deprecated" ) # Opciones para el compilador
+
+
+
+TARGET_LINK_LIBRARIES(recorder
+ ${opencv_LIBRARIES}
+ ${Ice_LIBRARIES}
+ ${gtkglextmm_LIBRARIES}
+ ${libglademm_LIBRARIES}
+ ${gtkmm_LIBRARIES}
+ ${LIBS_DIR}/libcolorspacesmm.so
+ ${LIBS_DIR}/libJderobotInterfaces.so
+ ${LIBS_DIR}/libjderobotice.so
+ ${LIBS_DIR}/libjderobotutil.so
+)
Added: trunk/src/components/recorder/poolWriteEncoders.cpp
===================================================================
--- trunk/src/components/recorder/poolWriteEncoders.cpp (rev 0)
+++ trunk/src/components/recorder/poolWriteEncoders.cpp 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,116 @@
+/*
+ * poolWriteEncoders.cpp
+ *
+ * Created on: 23/07/2013
+ * Author: frivas
+ */
+
+#include "poolWriteEncoders.h"
+
+namespace recorder {
+
+poolWriteEncoders::poolWriteEncoders(jderobot::EncodersPrx prx, int freq, int poolSize, int encoderID) {
+ // TODO Auto-generated constructor stub
+ pthread_mutex_init(&(this->mutex), NULL);
+ this->poolSize=poolSize;
+ this->encoderID=encoderID;
+ this->active=true;
+ this->prx=prx;
+ this->freq=freq;
+ std::stringstream filePath;
+ filePath << "data/encoders/encoder" << this->encoderID << "/encoderData.jde";
+ this->cycle = 1000.0/freq;
+ this->outfile.open(filePath.str().c_str());
+ gettimeofday(&lastTime,NULL);
+}
+
+poolWriteEncoders::~poolWriteEncoders() {
+ this->outfile.close();
+ // TODO Auto-generated destructor stub
+}
+
+bool poolWriteEncoders::getActive(){
+ return this->active;
+}
+
+void poolWriteEncoders::consumer_thread(){
+// while(this->active){
+
+
+ pthread_mutex_lock(&(this->mutex));
+ if (this->encoders.size()>0){
+ //std::cout << " camara: " << cameraID << this->images.size() << std::endl;
+
+ recorder::encoders data2Save;
+ data2Save = this->encoders[0];
+ this->encoders.erase(this->encoders.begin());
+ long long int relative;
+ relative=*(this->its.begin());
+ this->its.erase(this->its.begin());
+ pthread_mutex_unlock(&(this->mutex));
+
+
+ std::stringstream idString;//create a stringstream
+ idString << this->encoderID;//add number to the stream
+ std::stringstream relativeString;//create a stringstream
+ relativeString << relative;//add number to the stream
+
+
+ std::string Path="data/encoders/encoder" + idString.str() + "/" + relativeString.str();
+ std::ofstream outfileBinary(Path.c_str(), std::ios::out | std::ios::binary);
+ outfileBinary.write((const char *)&data2Save, sizeof(recorder::encoders));
+ outfileBinary.close();
+
+ this->outfile << relative << std::endl;
+
+ }
+ else
+ pthread_mutex_unlock(&(this->mutex));
+ usleep(1000);
+
+
+// }
+}
+
+void poolWriteEncoders::producer_thread(struct timeval inicio){
+ //std::cout << "productor entro" << std::endl;
+// while(this->active){
+ jderobot::EncodersDataPtr dataPtr=this->prx->getEncodersData();
+ recorder::encoders data;
+
+ data.robotx=dataPtr->robotx;
+ data.roboty=dataPtr->roboty;
+ data.robottheta=dataPtr->robottheta;
+ data.robotcos=dataPtr->robotcos;
+ data.robotsin=dataPtr->robotsin;
+
+ struct timeval now;
+ gettimeofday(&now,NULL);
+ long long int relative;
+ relative=((now.tv_sec*1000000+now.tv_usec) - (inicio.tv_sec*1000000+inicio.tv_usec))/1000;
+ pthread_mutex_lock(&(this->mutex));
+ while (this->encoders.size() > this->poolSize){
+ pthread_mutex_unlock(&(this->mutex));
+ usleep(100);
+ pthread_mutex_lock(&(this->mutex));
+ }
+ this->encoders.push_back(data);
+ this->its.push_back(relative);
+ pthread_mutex_unlock(&(this->mutex));
+ gettimeofday(&now,NULL);
+
+ long long int totalNow=now.tv_sec*1000000+now.tv_usec;
+ long long int totalLast=lastTime.tv_sec*1000000+lastTime.tv_usec;
+
+ float sleepTime =this->cycle - (totalNow-totalLast)/1000.;
+
+ //std::cout << "productor: " << this->cameraID << ", sleep: " << sleepTime << std::endl;
+ if(sleepTime < 0 )
+ sleepTime = 0;
+ usleep(sleepTime*1000);
+ gettimeofday(&lastTime,NULL);
+ //std::cout << "productor salgo" << std::endl;
+// }
+}
+
+} /* namespace gbRGBD2PC */
Added: trunk/src/components/recorder/poolWriteEncoders.h
===================================================================
--- trunk/src/components/recorder/poolWriteEncoders.h (rev 0)
+++ trunk/src/components/recorder/poolWriteEncoders.h 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,68 @@
+/*
+* Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Authors : Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>,
+ * Jose María Cañas Plaza <jmplaza en gsyc.es>
+ *
+ */
+
+#ifndef POOLWRITEENCODERS_H_
+#define POOLWRITEENCODERS_H_
+
+#include <pthread.h>
+#include <boost/thread/thread.hpp>
+#include <stdio.h>
+#include <jderobot/encoders.h>
+#include <fstream>
+
+
+namespace recorder {
+
+struct encoders{
+ float robotx;
+ float roboty;
+ float robottheta;
+ float robotcos;
+ float robotsin;
+ };
+
+class poolWriteEncoders {
+public:
+ poolWriteEncoders(jderobot::EncodersPrx prx, int freq, int poolSize, int encoderID);
+ virtual ~poolWriteEncoders();
+ bool getActive();
+ //void produceImage(cv::Mat image, long long int it);
+ void consumer_thread();
+ void producer_thread(struct timeval inicio);
+
+
+private:
+ pthread_mutex_t mutex;
+ std::vector<recorder::encoders> encoders;
+ std::vector<long long int> its;
+ int poolSize;
+ int encoderID;
+ bool active;
+ struct timeval lastTime;
+ int freq;
+ float cycle;
+ jderobot::EncodersPrx prx;
+ std::ofstream outfile;
+
+};
+} /* namespace recorder */
+#endif /* POOLWRITEENCODERS_H_ */
Added: trunk/src/components/recorder/poolWriteImages.cpp
===================================================================
--- trunk/src/components/recorder/poolWriteImages.cpp (rev 0)
+++ trunk/src/components/recorder/poolWriteImages.cpp 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,108 @@
+/*
+ * poolWriteImages.cpp
+ *
+ * Created on: 03/05/2013
+ * Author: frivas
+ */
+
+#include "poolWriteImages.h"
+
+namespace recorder{
+poolWriteImages::poolWriteImages(jderobot::CameraPrx prx, int freq, int poolSize, int cameraID, std::string imageFormat, std::vector<int> compression_params) {
+ // TODO Auto-generated constructor stub
+ pthread_mutex_init(&(this->mutex), NULL);
+ this->poolSize=poolSize;
+ this->compression_params=compression_params;
+ this->cameraID=cameraID;
+ this->active=true;
+ this->imageFormat=imageFormat;
+ this->prx=prx;
+ this->freq=freq;
+ std::stringstream filePath;
+ filePath << "data/images/camera" << this->cameraID << "/cameraData.jde";
+ this->cycle = 1000.0/freq;
+ this->outfile.open(filePath.str().c_str());
+ gettimeofday(&lastTime,NULL);
+}
+
+poolWriteImages::~poolWriteImages() {
+ this->outfile.close();
+ // TODO Auto-generated destructor stub
+}
+
+bool poolWriteImages::getActive(){
+ return this->active;
+}
+
+void poolWriteImages::consumer_thread(){
+// while(this->active){
+ //std::cout << "consumidor entro" << std::endl;
+
+ pthread_mutex_lock(&(this->mutex));
+ if (this->images.size()>0){
+ //std::cout << " camara: " << cameraID << this->images.size() << std::endl;
+ cv::Mat img2Save;
+
+ this->images.begin()->copyTo(img2Save);
+ this->images.erase(this->images.begin());
+
+ long long int relative;
+ relative=*(this->its.begin());
+ this->its.erase(this->its.begin());
+ pthread_mutex_unlock(&(this->mutex));
+
+ std::stringstream buff;//create a stringstream
+
+ buff << "data/images/camera" << cameraID << "/" << relative << "." << imageFormat;
+ cv::imwrite(buff.str(), img2Save,this->compression_params);
+ this->outfile << relative<< std::endl;
+ }
+ else
+ pthread_mutex_unlock(&(this->mutex));
+ usleep(1000);
+ //std::cout << "consumidor salgo" << std::endl;
+
+// }
+}
+
+void poolWriteImages::producer_thread( struct timeval inicio){
+//(cv::Mat image, long long int it){
+ //std::cout << "productor entro" << std::endl;
+// while(this->active){
+ jderobot::ImageDataPtr imageData=this->prx->getImageData();
+ colorspaces::Image::FormatPtr fmt = colorspaces::Image::Format::searchFormat(imageData->description->format);
+ cv::Mat image;
+ image.create(cv::Size(imageData->description->width, imageData->description->height), CV_8UC3);
+ memcpy((unsigned char *) image.data ,&(imageData->pixelData[0]), image.cols*image.rows * 3);
+ struct timeval now;
+ gettimeofday(&now,NULL);
+ long long int relative;
+ relative=((now.tv_sec*1000000+now.tv_usec) - (inicio.tv_sec*1000000+inicio.tv_usec))/1000;
+ pthread_mutex_lock(&(this->mutex));
+ while (this->images.size() > this->poolSize){
+ pthread_mutex_unlock(&(this->mutex));
+ usleep(100);
+ pthread_mutex_lock(&(this->mutex));
+ }
+ this->images.push_back(image);
+ this->its.push_back(relative);
+ pthread_mutex_unlock(&(this->mutex));
+ gettimeofday(&now,NULL);
+
+ long long int totalNow=now.tv_sec*1000000+now.tv_usec;
+ long long int totalLast=lastTime.tv_sec*1000000+lastTime.tv_usec;
+
+ float sleepTime =this->cycle - (totalNow-totalLast)/1000.;
+
+ //std::cout << "productor: " << this->cameraID << ", sleep: " << sleepTime << std::endl;
+ if(sleepTime < 0 )
+ sleepTime = 0;
+ usleep(sleepTime*1000);
+ gettimeofday(&lastTime,NULL);
+ //std::cout << "productor salgo" << std::endl;
+// }
+}
+
+
+} //namespace
+
Added: trunk/src/components/recorder/poolWriteImages.h
===================================================================
--- trunk/src/components/recorder/poolWriteImages.h (rev 0)
+++ trunk/src/components/recorder/poolWriteImages.h 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,72 @@
+/*
+* Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Authors : Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>,
+ * Jose María Cañas Plaza <jmplaza en gsyc.es>
+ *
+ */
+#ifndef POOLWRITEIMAGES_H_
+#define POOLWRITEIMAGES_H_
+
+#include <pthread.h>
+#include <boost/thread/thread.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <stdio.h>
+#include <time.h>
+#include <jderobot/camera.h>
+#include <colorspaces/colorspacesmm.h>
+#include <fstream>
+
+
+
+namespace recorder{
+
+
+class poolWriteImages {
+public:
+ poolWriteImages(jderobot::CameraPrx prx, int freq, int poolSize, int cameraID, std::string imageFormat, std::vector<int> compression_params);
+ virtual ~poolWriteImages();
+ bool getActive();
+ //void produceImage(cv::Mat image, long long int it);
+ void consumer_thread();
+ void producer_thread( struct timeval inicio);
+
+
+private:
+ pthread_mutex_t mutex;
+ std::vector<cv::Mat> images;
+ std::vector<long long int> its;
+ int poolSize;
+ std::vector<int> compression_params;
+ int cameraID;
+ bool active;
+ std::string imageFormat;
+ struct timeval lastTime;
+ int freq;
+ float cycle;
+ jderobot::CameraPrx prx;
+ std::ofstream outfile;
+
+
+ //threads
+
+};
+} //NAMESPACE
+
+#endif /* POOLWRITEIMAGES_H_ */
Added: trunk/src/components/recorder/poolWriteLasers.cpp
===================================================================
--- trunk/src/components/recorder/poolWriteLasers.cpp (rev 0)
+++ trunk/src/components/recorder/poolWriteLasers.cpp 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,111 @@
+/*
+ * poolWriteLasers.cpp
+ *
+ * Created on: 10/05/2013
+ * Author: frivas
+ */
+
+#include "poolWriteLasers.h"
+
+namespace recorder{
+poolWriteLasers::poolWriteLasers(jderobot::LaserPrx prx, int freq, int poolSize, int laserID) {
+ // TODO Auto-generated constructor stub
+ pthread_mutex_init(&(this->mutex), NULL);
+ this->poolSize=poolSize;
+ this->laserID=laserID;
+ this->active=true;
+ this->prx=prx;
+ this->freq=freq;
+ std::stringstream filePath;
+ filePath << "data/lasers/laser" << this->laserID << "/laserData.jde";
+ this->cycle = 1000.0/freq;
+ this->outfile.open(filePath.str().c_str());
+ gettimeofday(&lastTime,NULL);
+}
+
+poolWriteLasers::~poolWriteLasers() {
+ this->outfile.close();
+ // TODO Auto-generated destructor stub
+}
+
+bool poolWriteLasers::getActive(){
+ return this->active;
+}
+
+void poolWriteLasers::consumer_thread(){
+// while(this->active){
+ //std::cout << "consumidor entro" << std::endl;
+
+ pthread_mutex_lock(&(this->mutex));
+ if (this->lasers.size()>0){
+ //std::cout << " camara: " << cameraID << this->images.size() << std::endl;
+
+ std::vector<int> data2Save(lasers[0]);
+ this->lasers.erase(this->lasers.begin());
+ long long int relative;
+ relative=*(this->its.begin());
+ this->its.erase(this->its.begin());
+ pthread_mutex_unlock(&(this->mutex));
+
+
+ std::stringstream idString;//create a stringstream
+ idString << this->laserID;//add number to the stream
+ std::stringstream relativeString;//create a stringstream
+ relativeString << relative;//add number to the stream
+
+
+ std::string Path="data/lasers/laser" + idString.str() + "/" + relativeString.str();
+ std::ofstream outfileBinary(Path.c_str(), std::ios::out | std::ios::binary);
+ outfileBinary.write((const char *)&data2Save.front(), data2Save.size()*sizeof(int));
+ outfileBinary.close();
+
+ this->outfile << relative << " " << data2Save.size() << std::endl;
+ }
+ else
+ pthread_mutex_unlock(&(this->mutex));
+ usleep(1000);
+ //std::cout << "consumidor salgo" << std::endl;
+
+// }
+}
+
+void poolWriteLasers::producer_thread(struct timeval inicio){
+ //std::cout << "productor entro" << std::endl;
+// while(this->active){
+ jderobot::LaserDataPtr dataPtr=this->prx->getLaserData();
+ /*std::cout << "size: "<< dataPtr->distanceData.size() << std::endl;
+ std::cout << "numLaser:" << dataPtr->numLaser << std::endl;*/
+
+ struct timeval now;
+ gettimeofday(&now,NULL);
+ long long int relative;
+ relative=((now.tv_sec*1000000+now.tv_usec) - (inicio.tv_sec*1000000+inicio.tv_usec))/1000;
+ pthread_mutex_lock(&(this->mutex));
+ while (this->lasers.size() > this->poolSize){
+ pthread_mutex_unlock(&(this->mutex));
+ usleep(100);
+ pthread_mutex_lock(&(this->mutex));
+ }
+ this->lasers.push_back(dataPtr->distanceData);
+ this->its.push_back(relative);
+ pthread_mutex_unlock(&(this->mutex));
+ gettimeofday(&now,NULL);
+
+ long long int totalNow=now.tv_sec*1000000+now.tv_usec;
+ long long int totalLast=lastTime.tv_sec*1000000+lastTime.tv_usec;
+
+ float sleepTime =this->cycle - (totalNow-totalLast)/1000.;
+
+ //std::cout << "productor: " << this->cameraID << ", sleep: " << sleepTime << std::endl;
+ if(sleepTime < 0 )
+ sleepTime = 0;
+ usleep(sleepTime*1000);
+ gettimeofday(&lastTime,NULL);
+ //std::cout << "productor salgo" << std::endl;
+// }
+}
+
+
+} //namespace
+
+
Added: trunk/src/components/recorder/poolWriteLasers.h
===================================================================
--- trunk/src/components/recorder/poolWriteLasers.h (rev 0)
+++ trunk/src/components/recorder/poolWriteLasers.h 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,68 @@
+/*
+* Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Authors : Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>,
+ * Jose María Cañas Plaza <jmplaza en gsyc.es>
+ *
+ */
+
+#ifndef POOLWRITELASERS_H_
+#define POOLWRITELASERS_H_
+
+#include <pthread.h>
+#include <boost/thread/thread.hpp>
+#include <stdio.h>
+#include <time.h>
+#include <jderobot/laser.h>
+#include <colorspaces/colorspacesmm.h>
+#include <fstream>
+
+
+
+namespace recorder{
+
+
+class poolWriteLasers {
+public:
+ poolWriteLasers(jderobot::LaserPrx prx, int freq, int poolSize, int laserID);
+ virtual ~poolWriteLasers();
+ bool getActive();
+ //void produceImage(cv::Mat image, long long int it);
+ void consumer_thread();
+ void producer_thread(struct timeval inicio);
+
+
+private:
+ pthread_mutex_t mutex;
+ std::vector<std::vector <int> > lasers;
+ std::vector<long long int> its;
+ int poolSize;
+ int laserID;
+ bool active;
+ struct timeval lastTime;
+ int freq;
+ float cycle;
+ jderobot::LaserPrx prx;
+ std::ofstream outfile;
+
+
+ //threads
+
+};
+} //NAMESPACE
+
+#endif /* POOLWRITELASERS_H_ */
Added: trunk/src/components/recorder/poolWritePointCloud.cpp
===================================================================
--- trunk/src/components/recorder/poolWritePointCloud.cpp (rev 0)
+++ trunk/src/components/recorder/poolWritePointCloud.cpp 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,108 @@
+/*
+ * poolWritePointCloud.cpp
+ *
+ * Created on: 11/05/2013
+ * Author: frivas
+ */
+
+#include "poolWritePointCloud.h"
+
+namespace recorder {
+
+poolWritePointCloud::poolWritePointCloud(jderobot::pointCloudPrx prx, int freq, int poolSize, int pclID) {
+ // TODO Auto-generated constructor stub
+ pthread_mutex_init(&(this->mutex), NULL);
+ this->poolSize=poolSize;
+ this->pclID=pclID;
+ this->active=true;
+ this->prx=prx;
+ this->freq=freq;
+ std::stringstream filePath;
+ filePath << "data/pointClouds/pointCloud" << this->pclID << "/pointCloudData.jde";
+ this->cycle = 1000.0/freq;
+ this->outfile.open(filePath.str().c_str());
+ gettimeofday(&lastTime,NULL);
+}
+
+poolWritePointCloud::~poolWritePointCloud() {
+ this->outfile.close();
+ // TODO Auto-generated destructor stub
+}
+
+bool poolWritePointCloud::getActive(){
+ return this->active;
+}
+
+void poolWritePointCloud::consumer_thread(){
+// while(this->active){
+ //std::cout << "consumidor entro" << std::endl;
+
+ pthread_mutex_lock(&(this->mutex));
+ if (this->pointCloud.size()>0){
+ //std::cout << " camara: " << cameraID << this->images.size() << std::endl;
+
+ std::vector<jderobot::RGBPoint> data2Save(pointCloud[0]);
+ this->pointCloud.erase(this->pointCloud.begin());
+ long long int relative;
+ relative=*(this->its.begin());
+ this->its.erase(this->its.begin());
+ pthread_mutex_unlock(&(this->mutex));
+
+
+ std::stringstream idString;//create a stringstream
+ idString << this->pclID;//add number to the stream
+ std::stringstream relativeString;//create a stringstream
+ relativeString << relative;//add number to the stream
+
+
+ std::string Path="data/pointClouds/pointCloud" + idString.str() + "/" + relativeString.str();
+ std::ofstream outfileBinary(Path.c_str(), std::ios::out | std::ios::binary);
+ outfileBinary.write((const char *)&data2Save.front(), data2Save.size()*sizeof(jderobot::RGBPoint));
+ outfileBinary.close();
+
+ this->outfile << relative << " " << data2Save.size() << std::endl;
+ }
+ else
+ pthread_mutex_unlock(&(this->mutex));
+ usleep(1000);
+ //std::cout << "consumidor salgo" << std::endl;
+
+// }
+}
+
+void poolWritePointCloud::producer_thread(struct timeval inicio){
+ //std::cout << "productor entro" << std::endl;
+// while(this->active){
+ jderobot::pointCloudDataPtr dataPtr=this->prx->getCloudData();
+
+
+ struct timeval now;
+ gettimeofday(&now,NULL);
+ long long int relative;
+ relative=((now.tv_sec*1000000+now.tv_usec) - (inicio.tv_sec*1000000+inicio.tv_usec))/1000;
+ pthread_mutex_lock(&(this->mutex));
+ while (this->pointCloud.size() > this->poolSize){
+ pthread_mutex_unlock(&(this->mutex));
+ usleep(100);
+ pthread_mutex_lock(&(this->mutex));
+ }
+ this->pointCloud.push_back(dataPtr->p);
+ this->its.push_back(relative);
+ pthread_mutex_unlock(&(this->mutex));
+ gettimeofday(&now,NULL);
+
+ long long int totalNow=now.tv_sec*1000000+now.tv_usec;
+ long long int totalLast=lastTime.tv_sec*1000000+lastTime.tv_usec;
+
+ float sleepTime =this->cycle - (totalNow-totalLast)/1000.;
+
+ //std::cout << "productor: " << this->cameraID << ", sleep: " << sleepTime << std::endl;
+ if(sleepTime < 0 )
+ sleepTime = 0;
+ usleep(sleepTime*1000);
+ gettimeofday(&lastTime,NULL);
+ //std::cout << "productor salgo" << std::endl;
+// }
+}
+
+} /* namespace recorder */
Added: trunk/src/components/recorder/poolWritePointCloud.h
===================================================================
--- trunk/src/components/recorder/poolWritePointCloud.h (rev 0)
+++ trunk/src/components/recorder/poolWritePointCloud.h 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,65 @@
+/*
+* Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Authors : Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>,
+ * Jose María Cañas Plaza <jmplaza en gsyc.es>
+ *
+ */
+
+#ifndef POOLWRITEPOINTCLOUD_H_
+#define POOLWRITEPOINTCLOUD_H_
+
+#include <pthread.h>
+#include <boost/thread/thread.hpp>
+#include <stdio.h>
+#include <time.h>
+#include <jderobot/pointcloud.h>
+#include <colorspaces/colorspacesmm.h>
+#include <fstream>
+
+namespace recorder {
+
+class poolWritePointCloud {
+public:
+ poolWritePointCloud(jderobot::pointCloudPrx prx, int freq, int poolSize, int pclID);
+ virtual ~poolWritePointCloud();
+ bool getActive();
+ //void produceImage(cv::Mat image, long long int it);
+ void consumer_thread();
+ void producer_thread(struct timeval inicio);
+
+
+private:
+ pthread_mutex_t mutex;
+ std::vector<jderobot::RGBPointsPCL > pointCloud;
+ std::vector<long long int> its;
+ int poolSize;
+ int pclID;
+ bool active;
+ struct timeval lastTime;
+ int freq;
+ float cycle;
+ jderobot::pointCloudPrx prx;
+ std::ofstream outfile;
+
+
+ //threads
+
+};
+
+} /* namespace recorder */
+#endif /* POOLWRITEPOINTCLOUD_H_ */
Added: trunk/src/components/recorder/poolWritePose3dEncoders.cpp
===================================================================
--- trunk/src/components/recorder/poolWritePose3dEncoders.cpp (rev 0)
+++ trunk/src/components/recorder/poolWritePose3dEncoders.cpp 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,123 @@
+/*
+ * poolWritePose3dEncoders.cpp
+ *
+ * Created on: 10/05/2013
+ * Author: frivas
+ */
+
+#include "poolWritePose3dEncoders.h"
+
+namespace recorder{
+
+
+poolWritePose3dEncoders::poolWritePose3dEncoders(jderobot::Pose3DEncodersPrx prx, int freq, int poolSize, int encoderID) {
+ // TODO Auto-generated constructor stub
+ pthread_mutex_init(&(this->mutex), NULL);
+ this->poolSize=poolSize;
+ this->encoderID=encoderID;
+ this->active=true;
+ this->prx=prx;
+ this->freq=freq;
+ std::stringstream filePath;
+ filePath << "data/pose3dencoders/pose3dencoder" << this->encoderID << "/pose3dencoderData.jde";
+ this->cycle = 1000.0/freq;
+ this->outfile.open(filePath.str().c_str());
+ gettimeofday(&lastTime,NULL);
+}
+
+poolWritePose3dEncoders::~poolWritePose3dEncoders() {
+ this->outfile.close();
+ // TODO Auto-generated destructor stub
+}
+
+bool poolWritePose3dEncoders::getActive(){
+ return this->active;
+}
+
+void poolWritePose3dEncoders::consumer_thread(){
+// while(this->active){
+
+
+ pthread_mutex_lock(&(this->mutex));
+ if (this->encoders.size()>0){
+ //std::cout << " camara: " << cameraID << this->images.size() << std::endl;
+
+ recorder::pose3dencoders data2Save;
+ data2Save = this->encoders[0];
+ this->encoders.erase(this->encoders.begin());
+ long long int relative;
+ relative=*(this->its.begin());
+ this->its.erase(this->its.begin());
+ pthread_mutex_unlock(&(this->mutex));
+
+
+ std::stringstream idString;//create a stringstream
+ idString << this->encoderID;//add number to the stream
+ std::stringstream relativeString;//create a stringstream
+ relativeString << relative;//add number to the stream
+
+
+ std::string Path="data/pose3dencoders/pose3dencoder" + idString.str() + "/" + relativeString.str();
+ std::ofstream outfileBinary(Path.c_str(), std::ios::out | std::ios::binary);
+ outfileBinary.write((const char *)&data2Save, sizeof(recorder::pose3dencoders));
+ outfileBinary.close();
+
+ this->outfile << relative << std::endl;
+
+ }
+ else
+ pthread_mutex_unlock(&(this->mutex));
+ usleep(1000);
+
+
+// }
+}
+
+void poolWritePose3dEncoders::producer_thread(struct timeval inicio){
+ //std::cout << "productor entro" << std::endl;
+// while(this->active){
+ jderobot::Pose3DEncodersDataPtr dataPtr=this->prx->getPose3DEncodersData();
+ recorder::pose3dencoders data;
+ data.clock=dataPtr->clock;
+ data.maxPan=dataPtr->maxPan;
+ data.maxTilt=dataPtr->maxTilt;
+ data.minPan=dataPtr->minPan;
+ data.minTilt=dataPtr->minTilt;
+ data.pan=dataPtr->pan;
+ data.roll=dataPtr->roll;
+ data.tilt=dataPtr->tilt;
+ data.x=dataPtr->x;
+ data.y=dataPtr->y;
+ data.z=dataPtr->z;
+
+ struct timeval now;
+ gettimeofday(&now,NULL);
+ long long int relative;
+ relative=((now.tv_sec*1000000+now.tv_usec) - (inicio.tv_sec*1000000+inicio.tv_usec))/1000;
+ pthread_mutex_lock(&(this->mutex));
+ while (this->encoders.size() > this->poolSize){
+ pthread_mutex_unlock(&(this->mutex));
+ usleep(100);
+ pthread_mutex_lock(&(this->mutex));
+ }
+ this->encoders.push_back(data);
+ this->its.push_back(relative);
+ pthread_mutex_unlock(&(this->mutex));
+ gettimeofday(&now,NULL);
+
+ long long int totalNow=now.tv_sec*1000000+now.tv_usec;
+ long long int totalLast=lastTime.tv_sec*1000000+lastTime.tv_usec;
+
+ float sleepTime =this->cycle - (totalNow-totalLast)/1000.;
+
+ //std::cout << "productor: " << this->cameraID << ", sleep: " << sleepTime << std::endl;
+ if(sleepTime < 0 )
+ sleepTime = 0;
+ usleep(sleepTime*1000);
+ gettimeofday(&lastTime,NULL);
+ //std::cout << "productor salgo" << std::endl;
+// }
+}
+
+
+} //namespace
Added: trunk/src/components/recorder/poolWritePose3dEncoders.h
===================================================================
--- trunk/src/components/recorder/poolWritePose3dEncoders.h (rev 0)
+++ trunk/src/components/recorder/poolWritePose3dEncoders.h 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,80 @@
+/*
+* Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Authors : Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>,
+ * Jose María Cañas Plaza <jmplaza en gsyc.es>
+ *
+ */
+
+#ifndef poolWritePose3dEncoders_H_
+#define poolWritePose3dEncoders_H_
+
+#include <pthread.h>
+#include <boost/thread/thread.hpp>
+#include <stdio.h>
+#include <time.h>
+#include <jderobot/pose3dencoders.h>
+#include <fstream>
+
+
+
+namespace recorder{
+
+struct pose3dencoders{
+ float x;
+ float y;
+ float z;
+ float pan;
+ float tilt;
+ float roll;
+ int clock;
+ float maxPan;
+ float maxTilt;
+ float minPan;
+ float minTilt;
+};
+
+
+class poolWritePose3dEncoders {
+public:
+ poolWritePose3dEncoders(jderobot::Pose3DEncodersPrx prx, int freq, int poolSize, int encoderID);
+ virtual ~poolWritePose3dEncoders();
+ bool getActive();
+ //void produceImage(cv::Mat image, long long int it);
+ void consumer_thread();
+ void producer_thread(struct timeval inicio);
+
+
+private:
+ pthread_mutex_t mutex;
+ std::vector<pose3dencoders> encoders;
+ std::vector<long long int> its;
+ int poolSize;
+ int encoderID;
+ bool active;
+ struct timeval lastTime;
+ int freq;
+ float cycle;
+ jderobot::Pose3DEncodersPrx prx;
+ std::ofstream outfile;
+
+
+ //threads
+
+};
+} //NAMESPACE
+#endif /* poolWritePose3dEncoders_H_ */
Added: trunk/src/components/recorder/recorder.cfg
===================================================================
--- trunk/src/components/recorder/recorder.cfg (rev 0)
+++ trunk/src/components/recorder/recorder.cfg 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,29 @@
+Recorder.FileName=datos.jde
+Recorder.nCameras=4
+Recorder.nDethSensors=2
+Recorder.nLasers=0
+Recorder.DepthSensor1.Proxy=pointcloud1:tcp -h 127.0.0.1 -p 9998
+Recorder.DepthSensor2.Proxy=pointcloud1:tcp -h 127.0.0.1 -p 9998
+Recorder.Laser1.Proxy=Laser:tcp -h localhost -p 9996
+Recorder.Camera1.Proxy=cameraA:tcp -h localhost -p 9998
+Recorder.Camera2.Proxy=cameraB:tcp -h localhost -p 9998
+Recorder.Camera3.Proxy=cameraA:tcp -h localhost -p 9998
+Recorder.Camera4.Proxy=cameraB:tcp -h localhost -p 9998
+Recorder.nPose3dEncoders=2
+Recorder.Pose3DEncoders1.Proxy=Pose3DEncoders1:tcp -h localhost -p 9993
+Recorder.Pose3DEncoders2.Proxy=pose3dencoders2:tcp -h localhost -p 9999
+Recorder.nEncoders=0
+Recorder.Encoders1.Proxy=Encoders:tcp -h localhost -p 9997
+Recorder.GUI=1
+Recorder.nConsumers=2
+Recorder.poolSize=10
+
+Recorder.Hostname=localhost
+Recorder.Port=9998
+Recorder.Format=png
+Recorder.PngCompression=0
+#from 0 to 9. A higher value means a smaller size
+Recorder.JpgQuality=80
+# from 0 to 100 (the higher is the better)
+Recorder.Laser.Samples=180
+Recorder.Hz=20
Added: trunk/src/components/recorder/recorder.cpp
===================================================================
--- trunk/src/components/recorder/recorder.cpp (rev 0)
+++ trunk/src/components/recorder/recorder.cpp 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,656 @@
+#include <iostream>
+#include <fstream>
+#include <pthread.h>
+#include <signal.h>
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+#include <jderobot/camera.h>
+#include <jderobot/motors.h>
+#include <jderobot/ptmotors.h>
+#include <jderobot/laser.h>
+#include <jderobot/encoders.h>
+#include <jderobot/ptencoders.h>
+#include <jderobot/pose3dmotors.h>
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pointcloud.h>
+#include <colorspaces/colorspacesmm.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <errno.h>
+#include <sys/stat.h>
+#include <string.h>
+#include "recordergui.h"
+#include "poolWriteImages.h"
+#include "poolWritePose3dEncoders.h"
+#include "poolWriteLasers.h"
+#include "poolWritePointCloud.h"
+#include "poolWriteEncoders.h"
+
+bool recording=false;
+struct timeval inicio;
+bool globalActive=true;
+Ice::CommunicatorPtr ic;
+pthread_t consumerThreads[100];
+pthread_t producerThreads[100];
+int nCameras=0;
+int nDepthSensors=0;
+int nLasers=0;
+int nPose3dEncoders=0;
+int nEncoders=0;
+int totalConsumers=0;
+int totalProducers=0;
+bool killed=false;
+
+//how to include this threads under the class
+//cameras
+//thread for the camera consumer
+void* camera_pool_consumer_thread(void* foo_ptr){
+ recorder::poolWriteImages* pool =static_cast<recorder::poolWriteImages*>(foo_ptr);
+ while (globalActive){
+ if (recording)
+ pool->consumer_thread();
+ else
+ usleep(1000);
+ }
+
+ pthread_exit(NULL);
+ return NULL;
+}
+//thread for the camera producer
+void* camera_pool_producer_thread(void* foo_ptr){
+ recorder::poolWriteImages* pool =static_cast<recorder::poolWriteImages*>(foo_ptr);
+ while (globalActive){
+ if (recording)
+ pool->producer_thread(inicio);
+ else
+ usleep(1000);
+ }
+ pthread_exit(NULL);
+ return NULL;
+}
+
+//lasers
+// thread for the laser consumer
+void* laser_pool_consumer_thread(void* foo_ptr){
+ recorder::poolWriteLasers* pool =static_cast<recorder::poolWriteLasers*>(foo_ptr);
+ while (globalActive){
+ if (recording)
+ pool->consumer_thread();
+ else
+ usleep(1000);
+ }
+ pthread_exit(NULL);
+ return NULL;
+}
+// thread for the laser producer
+void* laser_pool_producer_thread(void* foo_ptr){
+ recorder::poolWriteLasers* pool =static_cast<recorder::poolWriteLasers*>(foo_ptr);
+ while (globalActive){
+ if (recording)
+ pool->producer_thread(inicio);
+ else
+ usleep(1000);
+ }
+ pthread_exit(NULL);
+ return NULL;
+}
+
+//lasers
+// thread for the pose3dencoders consumer
+void* pose3dencoders_pool_consumer_thread(void* foo_ptr){
+ recorder::poolWritePose3dEncoders* pool =static_cast<recorder::poolWritePose3dEncoders*>(foo_ptr);
+ while (globalActive){
+ if (recording)
+ pool->consumer_thread();
+ else
+ usleep(1000);
+ }
+ pthread_exit(NULL);
+ return NULL;
+}
+// thread for the pose3dencoders producer
+void* pose3dencoders_pool_producer_thread(void* foo_ptr){
+ recorder::poolWritePose3dEncoders* pool =static_cast<recorder::poolWritePose3dEncoders*>(foo_ptr);
+ while (globalActive){
+ if (recording)
+ pool->producer_thread(inicio);
+ else
+ usleep(1000);
+ }
+ pthread_exit(NULL);
+ return NULL;
+}
+
+//pointcloud
+// thread for the pose3dencoders consumer
+void* pointcloud_pool_consumer_thread(void* foo_ptr){
+ recorder::poolWritePointCloud* pool =static_cast<recorder::poolWritePointCloud*>(foo_ptr);
+ while (globalActive){
+ if (recording)
+ pool->consumer_thread();
+ else
+ usleep(1000);
+ }
+ pthread_exit(NULL);
+ return NULL;
+}
+// thread for the pose3dencoders producer
+void* pointcloud_pool_producer_thread(void* foo_ptr){
+ recorder::poolWritePointCloud* pool =static_cast<recorder::poolWritePointCloud*>(foo_ptr);
+ while (globalActive){
+ if (recording)
+ pool->producer_thread(inicio);
+ else
+ usleep(1000);
+ }
+ pthread_exit(NULL);
+ return NULL;
+}
+
+
+// thread for the encoders consumer
+void* encoders_pool_consumer_thread(void* foo_ptr){
+ recorder::poolWriteEncoders* pool =static_cast<recorder::poolWriteEncoders*>(foo_ptr);
+ while (globalActive){
+ if (recording)
+ pool->consumer_thread();
+ else
+ usleep(1000);
+ }
+ pthread_exit(NULL);
+ return NULL;
+}
+// thread for the encoders producer
+void* encoders_pool_producer_thread(void* foo_ptr){
+ recorder::poolWriteEncoders* pool =static_cast<recorder::poolWriteEncoders*>(foo_ptr);
+ while (globalActive){
+ if (recording)
+ pool->producer_thread(inicio);
+ else
+ usleep(1000);
+ }
+ pthread_exit(NULL);
+ return NULL;
+}
+
+
+void exitApplication(int s){
+ globalActive=false;
+ killed=true;
+ std::cout << totalConsumers << std::endl;
+ for (int i = 0; i < totalConsumers; i++) {
+ pthread_join(consumerThreads[i], NULL);
+ }
+ for (int i = 0; i < totalProducers; i++) {
+ pthread_join(producerThreads[i], NULL);
+ }
+ //odenamdos los ficheros.
+ for (int i=0; i< nCameras; i++){
+ std::stringstream instruction1;
+ instruction1 << "sort -n data/images/camera" << i+1 << "/cameraData.jde >tempSORT.temp";
+ system(instruction1.str().c_str());
+ std::stringstream instruction2;
+ instruction2 << "mv tempSORT.temp data/images/camera" << i+1 << "/cameraData.jde";
+ system(instruction2.str().c_str());
+ }
+ for (int i=0; i< nLasers; i++){
+ std::stringstream instruction1;
+ instruction1 << "sort -n data/lasers/laser" << i+1 << "/laserData.jde >tempSORT.temp";
+ system(instruction1.str().c_str());
+ std::stringstream instruction2;
+ instruction2 << "mv tempSORT.temp data/lasers/laser" << i+1 << "/laserData.jde";
+ system(instruction2.str().c_str());
+ }
+ for (int i=0; i< nPose3dEncoders; i++){
+ std::stringstream instruction1;
+ instruction1 << "sort -n data/pose3dencoders/pose3dencoder" << i+1 << "/pose3dencoderData.jde >tempSORT.temp";
+ system(instruction1.str().c_str());
+ std::stringstream instruction2;
+ instruction2 << "mv tempSORT.temp data/pose3dencoders/pose3dencoder" << i+1 << "/pose3dencoderData.jde";
+ system(instruction2.str().c_str());
+ }
+ for (int i=0; i< nDepthSensors; i++){
+ std::stringstream instruction1;
+ instruction1 << "sort -n data/pointClouds/pointCloud" << i+1 << "/pointCloudData.jde >tempSORT.temp";
+ system(instruction1.str().c_str());
+ std::stringstream instruction2;
+ instruction2 << "mv tempSORT.temp data/pointClouds/pointCloud" << i+1 << "/pointCloudData.jde";
+ system(instruction2.str().c_str());
+ }
+ for (int i=0; i< nEncoders; i++){
+ std::stringstream instruction1;
+ instruction1 << "sort -n data/encoders/encoder" << i+1 << "/encoderData.jde >tempSORT.temp";
+ system(instruction1.str().c_str());
+ std::stringstream instruction2;
+ instruction2 << "mv tempSORT.temp data/encoders/encoder" << i+1 << "/encoderData.jde";
+ system(instruction2.str().c_str());
+ }
+
+ if (ic)
+ ic->destroy();
+}
+
+
+int main(int argc, char** argv){
+
+
+
+ struct sigaction sigIntHandler;
+
+ sigIntHandler.sa_handler = exitApplication;
+ sigemptyset(&sigIntHandler.sa_mask);
+ sigIntHandler.sa_flags = 0;
+
+ sigaction(SIGINT, &sigIntHandler, NULL);
+
+ int accion=0;
+ int sentido=10;
+
+ int status;
+
+ struct timeval a, b, t2;
+ int cycle = 100;
+ long totalb,totala;
+ long diff;
+
+ // INTERFACE
+ std::vector<jderobot::LaserPrx> lprx;
+ std::vector<jderobot::CameraPrx> cprx;
+ std::vector<jderobot::Pose3DEncodersPrx> pose3dencoders;
+ std::vector<jderobot::EncodersPrx> encoders;
+ std::vector <jderobot::pointCloudPrx> prx;
+
+
+ //INTERFACE DATA
+ jderobot::EncodersDataPtr ed;
+ jderobot::LaserDataPtr ld;
+
+ jderobot::ImageDataPtr imageData;
+
+ //pools
+
+ pthread_attr_t attr;
+ //images
+ std::vector<recorder::poolWriteImages*> poolImages;
+ int nConsumidores;
+ int poolSize;
+ //lasers
+ std::vector<recorder::poolWriteLasers*> poolLasers;
+ //pose3dencoders
+ std::vector<recorder::poolWritePose3dEncoders*> poolPose3dEncoders;
+ //encoders
+ std::vector<recorder::poolWriteEncoders*> poolEncoders;
+ //pointClouds
+ std::vector<recorder::poolWritePointCloud*> poolPointClouds;
+
+ //numero de lasers
+
+ int Hz = 10;
+ int muestrasLaser = 180;
+ int pngCompressRatio;
+ int jpgQuality;
+ std::string imageFormat;
+ std::vector<int> compression_params;
+
+
+ //---------------- INPUT ARGUMENTS ---------------//
+
+ if (argc<2){
+ std::cout << std::endl << "USE: ./mycomponent --Ice.Config=mycomponent.cfg" << std::endl;
+ }
+
+ //---------------- INPUT ARGUMENTS -----------------//
+
+ try{
+ //creamos el directorio principal
+ struct stat buf;
+ char dire[]="./data/";
+ if( stat( dire, &buf ) == -1 )
+ {
+ system("mkdir data");
+ }
+
+ Ice::PropertiesPtr prop;
+
+ ic = Ice::initialize(argc,argv);
+
+ prop = ic->getProperties();
+
+ Hz = prop->getPropertyAsInt("Recorder.Hz");
+ cycle = 1000.0/Hz;
+
+ pthread_attr_init(&attr);
+ pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
+
+
+ nCameras = prop->getPropertyAsIntWithDefault("Recorder.nCameras",0);
+ if (nCameras > 0 ){
+ struct stat buf;
+ char dire[]="./data/images/";
+ if( stat( dire, &buf ) == -1 )
+ {
+ system("mkdir ./data/images/");
+ }
+ imageFormat=prop->getProperty("Recorder.Format");
+ if (imageFormat.compare(std::string("png"))==0){
+ pngCompressRatio=prop->getPropertyAsIntWithDefault("Recorder.PngCompression",3);
+ compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
+ compression_params.push_back(pngCompressRatio);
+ }
+ else if (imageFormat.compare(std::string("jpg"))==0){
+ jpgQuality=prop->getPropertyAsIntWithDefault("Recorder.JpgQuality",95);
+ compression_params.push_back(CV_IMWRITE_JPEG_QUALITY);
+ compression_params.push_back(jpgQuality);
+ }
+ else{
+ throw "Image format is not valid";
+ }
+ nConsumidores=prop->getPropertyAsIntWithDefault("Recorder.nConsumers",2);
+ poolSize=prop->getPropertyAsIntWithDefault("Recorder.poolSize",10);
+
+
+ }
+
+ for (int i=0; i< nCameras; i++){
+ struct stat buf;
+ std::stringstream cameraPath;
+ cameraPath << "./data/images/camera" << i+1;
+ if( stat( cameraPath.str().c_str(), &buf ) == -1 )
+ {
+ std::stringstream instruction;
+ instruction << "mkdir " << cameraPath.str();
+ system(instruction.str().c_str());
+ }
+
+
+ std::stringstream sProxy;
+ // Get driver camera
+ sProxy << "Recorder.Camera" << i+1 << ".Proxy";
+ Ice::ObjectPrx camara = ic->propertyToProxy(sProxy.str());
+ if (0==camara)
+ throw "Could not create proxy to camera1 server";
+ // cast to CameraPrx
+ jderobot::CameraPrx cprxAux = jderobot::CameraPrx::checkedCast(camara);
+ if (0== cprxAux)
+ throw "Invalid proxy";
+ else
+ cprx.push_back(cprxAux);
+
+ //pool
+ recorder::poolWriteImages *temp = new recorder::poolWriteImages(cprxAux, Hz,poolSize,i+1,imageFormat,compression_params);
+ poolImages.push_back(temp);
+
+
+ for (int j=i*nConsumidores; j< i*nConsumidores+nConsumidores; j++){
+ pthread_create(&consumerThreads[j], &attr, camera_pool_consumer_thread,temp);
+ totalConsumers++;
+ }
+ pthread_create(&producerThreads[i], &attr, camera_pool_producer_thread,temp);
+ totalProducers++;
+ }
+
+
+
+ nLasers= prop->getPropertyAsInt("Recorder.nLasers");
+ if (nLasers > 0){
+ struct stat buf;
+ char dire[]="./data/lasers/";
+ if( stat( dire, &buf ) == -1 )
+ {
+ system("mkdir data/lasers/");
+ }
+ }
+ for (int i=0; i< nLasers; i++){
+ struct stat buf;
+ std::stringstream claserPath;
+ claserPath << "./data/lasers/laser" << i+1;
+ if( stat( claserPath.str().c_str(), &buf ) == -1 )
+ {
+ std::stringstream instruction;
+ instruction << "mkdir " << claserPath.str();
+ system(instruction.str().c_str());
+ }
+
+ // Contact to LASER interface
+ std::stringstream sProxy;
+ sProxy << "Recorder.Laser" << i+1 << ".Proxy";
+
+ Ice::ObjectPrx baseLaser = ic->propertyToProxy(sProxy.str());
+ if (0==baseLaser)
+ throw "Could not create proxy with laser";
+
+ // Cast to laser
+ jderobot::LaserPrx laserPrx = jderobot::LaserPrx::checkedCast(baseLaser);
+ if (0== laserPrx)
+ throw "Invalid proxy Mycomponent.Laser.Proxy";
+ lprx.push_back(laserPrx);
+ recorder::poolWriteLasers *temp = new recorder::poolWriteLasers(laserPrx, Hz,poolSize,i+1);
+
+ poolLasers.push_back(temp);
+ pthread_create(&consumerThreads[totalConsumers], &attr, laser_pool_consumer_thread,temp);
+ totalConsumers++;
+ pthread_create(&producerThreads[totalProducers], &attr, laser_pool_producer_thread,temp);
+ totalProducers++;
+ }
+
+
+ nPose3dEncoders= prop->getPropertyAsInt("Recorder.nPose3dEncoders");
+ if (nPose3dEncoders > 0){
+ struct stat buf;
+ char dire[]="./data/pose3dencoders/";
+ if( stat( dire, &buf ) == -1 )
+ {
+ system("mkdir data/pose3dencoders/");
+ }
+ }
+ for (int i=0; i< nPose3dEncoders; i++){
+ struct stat buf;
+ std::stringstream claserPath;
+ claserPath << "./data/pose3dencoders/pose3dencoder" << i+1;
+ if( stat( claserPath.str().c_str(), &buf ) == -1 )
+ {
+ std::stringstream instruction;
+ instruction << "mkdir " << claserPath.str();
+ system(instruction.str().c_str());
+ }
+
+ // Contact to POSE3DENCODERS interface
+ std::stringstream sProxy;
+ sProxy << "Recorder.Pose3DEncoders" << i+1 << ".Proxy";
+
+ Ice::ObjectPrx base = ic->propertyToProxy(sProxy.str());
+ if (0==base)
+ throw "Could not create proxy with pose3dencoders";
+
+ // Cast to Pose3DEncodersPrx
+ jderobot::Pose3DEncodersPrx prx = jderobot::Pose3DEncodersPrx::checkedCast(base);
+ if (0== prx)
+ throw "Invalid proxy Mycomponent.pose3dencoders.Proxy";
+ pose3dencoders.push_back(prx);
+ recorder::poolWritePose3dEncoders*temp = new recorder::poolWritePose3dEncoders(prx, Hz,poolSize,i+1);
+
+ poolPose3dEncoders.push_back(temp);
+ pthread_create(&consumerThreads[totalConsumers], &attr, pose3dencoders_pool_consumer_thread,temp);
+ totalConsumers++;
+ pthread_create(&producerThreads[totalProducers], &attr, pose3dencoders_pool_producer_thread,temp);
+ totalProducers++;
+ }
+
+ nEncoders= prop->getPropertyAsInt("Recorder.nEncoders");
+ if (nEncoders > 0){
+ struct stat buf;
+ char dire[]="./data/encoders/";
+ if( stat( dire, &buf ) == -1 )
+ {
+ system("mkdir data/encoders/");
+ }
+ }
+ for (int i=0; i< nEncoders; i++){
+ struct stat buf;
+ std::stringstream claserPath;
+ claserPath << "./data/encoders/encoder" << i+1;
+ if( stat( claserPath.str().c_str(), &buf ) == -1 )
+ {
+ std::stringstream instruction;
+ instruction << "mkdir " << claserPath.str();
+ system(instruction.str().c_str());
+ }
+
+ // Contact to ENCODERS interface
+ std::stringstream sProxy;
+ sProxy << "Recorder.Encoders" << i+1 << ".Proxy";
+
+ Ice::ObjectPrx base = ic->propertyToProxy(sProxy.str());
+ if (0==base)
+ throw "Could not create proxy with encoders";
+
+ // Cast to EncodersPrx
+ jderobot::EncodersPrx prx = jderobot::EncodersPrx::checkedCast(base);
+ if (0== prx)
+ throw "Invalid proxy Mycomponent.encoders.Proxy";
+ encoders.push_back(prx);
+ recorder::poolWriteEncoders*temp = new recorder::poolWriteEncoders(prx, Hz,poolSize,i+1);
+
+ poolEncoders.push_back(temp);
+ pthread_create(&consumerThreads[totalConsumers], &attr, encoders_pool_consumer_thread,temp);
+ totalConsumers++;
+ pthread_create(&producerThreads[totalProducers], &attr, encoders_pool_producer_thread,temp);
+ totalProducers++;
+ }
+
+ nDepthSensors = prop->getPropertyAsIntWithDefault("Recorder.nDethSensors",0);
+ if (nDepthSensors){
+ struct stat buf;
+ char dire[]="./data/pointClouds/";
+ if( stat( dire, &buf ) == -1 )
+ {
+ system("mkdir data/pointClouds/");
+ }
+ }
+ for (int i=0; i< nDepthSensors; i++){
+ struct stat buf;
+ std::stringstream claserPath;
+ claserPath << "./data/pointClouds/pointCloud" << i+1;
+ if( stat( claserPath.str().c_str(), &buf ) == -1 )
+ {
+ std::stringstream instruction;
+ instruction << "mkdir " << claserPath.str();
+ system(instruction.str().c_str());
+ }
+
+
+ std::stringstream sProxy;
+ // Get driver camera
+ sProxy << "Recorder.DepthSensor" << i+1 << ".Proxy";
+
+ Ice::ObjectPrx kinect = ic->propertyToProxy(sProxy.str());
+ if (0==kinect){
+ throw "Could not create proxy with Kinect1";
+ }
+ // Cast to KINECT
+ jderobot::pointCloudPrx prxAux = jderobot::pointCloudPrx::checkedCast(kinect);
+ if (0== prxAux){
+ throw std::string("Invalid proxy Recorder.Kinect1.Proxy");
+ }
+ prx.push_back(prxAux);
+ recorder::poolWritePointCloud* temp = new recorder::poolWritePointCloud(prxAux, Hz,poolSize,i+1);
+ poolPointClouds.push_back(temp);
+ pthread_create(&consumerThreads[totalConsumers], &attr, pointcloud_pool_consumer_thread,temp);
+ totalConsumers++;
+ pthread_create(&producerThreads[totalProducers], &attr, pointcloud_pool_producer_thread,temp);
+ totalProducers++;
+ }
+
+ //****************************** Processing the Control ******************************///
+
+
+ //---------------- ITERATIONS CONTROL -----------//
+ //muestreo para el laser
+ muestrasLaser = prop->getPropertyAsInt("Recorder.Laser.Samples");
+ std::string robotName = prop->getPropertyWithDefault("Recorder.Hostname","localhost");
+ std::string robotPort = prop->getPropertyWithDefault("Recorder.Port","9999");
+
+ long timeRelative = 0;
+
+
+ int guiActive=prop->getPropertyAsIntWithDefault("Recorder.GUI",0);
+ recorder::recordergui *gui;
+
+ if (guiActive){
+ gui = new recorder::recordergui();
+ }
+ long long int iteration=0;
+
+
+ while(globalActive){
+ //gui activado
+ if (guiActive){
+ gui->set_iteration(iteration);
+ gui->update();
+ globalActive=gui->get_active();
+ recording=gui->get_recording();
+ }
+ else{
+ recording=true;
+ }
+
+ if (recording){
+ if (iteration==0){
+ gettimeofday(&inicio,NULL);
+ }
+ iteration++;
+ gettimeofday(&b,NULL);
+ totalb=b.tv_sec*1000000+b.tv_usec;
+ //calculamos la velocidad de grabación actual
+ float myfps=1000000./((float)totalb-(float)totala);
+ if (guiActive){
+ gui->set_fps((int)myfps);
+ }
+ gettimeofday(&a,NULL);
+ totala=a.tv_sec*1000000+a.tv_usec;
+
+ gettimeofday(&b,NULL);
+ totalb=b.tv_sec*1000000+b.tv_usec;
+ std::cout << "Recorder takes " << (totalb-totala)/1000 << " ms" << std::endl;
+
+ diff = (totalb-totala)/1000;
+ if(diff < 0 || diff > cycle)
+ diff = cycle;
+ else
+ diff = cycle-diff;
+
+ //Sleep Algorithm
+ usleep(diff*1000);
+ if(diff < 10)
+ usleep(10*1000);
+
+ // std::cout << cycle <<" ->" << diff << " ->" << timeRelative<< std::endl;
+ timeRelative+= diff + (totalb-totala)/1000;
+ // std::cout << "->" << diff << " ->" << timeRelative<< std::endl;
+ }
+ else{
+ usleep(10*1000);
+ }
+
+ }
+
+ //--------------ITERATIONS CONTROL-------------//
+ }
+ catch (const Ice::Exception& ex) {
+ std::cerr << ex << std::endl;
+ status = 1;
+ }
+ catch (const char* msg) {
+ std::cerr << msg << std::endl;
+ status = 1;
+ }
+
+ if (!killed)
+ exitApplication(1);
+
+ return 0;
+}
Added: trunk/src/components/recorder/recordergui.cpp
===================================================================
--- trunk/src/components/recorder/recordergui.cpp (rev 0)
+++ trunk/src/components/recorder/recordergui.cpp 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,100 @@
+/*
+* Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Authors : Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>,
+ * Jose María Cañas Plaza <jmplaza en gsyc.es>
+ *
+ */
+
+#include "recordergui.h"
+
+namespace recorder {
+
+ recordergui::recordergui(): gtkmain(0,0) {
+ recording=false;
+ exit=false;
+
+
+ std::cout << "Loading glade\n";
+ refXml = Gnome::Glade::Xml::create("./recordergui.glade");
+
+ /*Get widgets*/
+ refXml->get_widget("window1",mainwindow);
+ refXml->get_widget("entry2",this->w_entry_fps);
+ refXml->get_widget("entry1",this->w_entry_iteration);
+ refXml->get_widget("button1",this->w_record);
+ refXml->get_widget("button2",this->w_stop);
+ refXml->get_widget("button3",this->w_exit);
+
+ this->w_record->signal_clicked().connect(sigc::mem_fun(this,&recordergui::button_record_clicked));
+ this->w_stop->signal_clicked().connect(sigc::mem_fun(this,&recordergui::button_stop_clicked));
+ this->w_exit->signal_clicked().connect(sigc::mem_fun(this,&recordergui::button_exit_clicked));
+
+ this->w_stop->hide();
+
+ mainwindow->show();
+
+
+ }
+
+ recordergui::~recordergui() {
+
+ }
+
+ void recordergui::update(){
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+ }
+
+ void recordergui::set_iteration(int value){
+ std::stringstream v;//create a stringstream
+ v << value;//add number to the stream
+ this->w_entry_iteration->set_text(v.str().c_str());
+ }
+
+ void recordergui::set_fps(int value){
+ std::stringstream v;//create a stringstream
+ v << value;//add number to the stream
+ this->w_entry_fps->set_text(v.str().c_str());
+ }
+
+ void recordergui::button_record_clicked(){
+ this->recording=true;
+ this->w_record->hide();
+ this->w_stop->show();
+ }
+
+ void recordergui::button_stop_clicked(){
+ this->w_stop->hide();
+ this->recording=false;
+ }
+
+ void recordergui::button_exit_clicked(){
+ this->recording=false;
+ this->exit=true;
+ }
+
+ bool recordergui::get_active(){
+ return !this->exit;
+ }
+
+ bool recordergui::get_recording(){
+ return this->recording;
+ }
+
+
+}//namespace
Added: trunk/src/components/recorder/recordergui.glade
===================================================================
--- trunk/src/components/recorder/recordergui.glade (rev 0)
+++ trunk/src/components/recorder/recordergui.glade 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,252 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!DOCTYPE glade-interface SYSTEM "glade-2.0.dtd">
+<!--Generated with glade3 3.4.2 on Tue Apr 16 17:31:03 2013 -->
+<glade-interface>
+ <widget class="GtkWindow" id="window1">
+ <child>
+ <widget class="GtkVBox" id="vbox1">
+ <property name="visible">True</property>
+ <property name="orientation">GTK_ORIENTATION_VERTICAL</property>
+ <property name="orientation">GTK_ORIENTATION_VERTICAL</property>
+ <child>
+ <widget class="GtkMenuBar" id="menubar1">
+ <property name="visible">True</property>
+ <child>
+ <widget class="GtkMenuItem" id="menuitem1">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">_Archivo</property>
+ <property name="use_underline">True</property>
+ <child>
+ <widget class="GtkMenu" id="menu1">
+ <property name="visible">True</property>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem1">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">gtk-new</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem2">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">gtk-open</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem3">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">gtk-save</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem4">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">gtk-save-as</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkSeparatorMenuItem" id="separatormenuitem1">
+ <property name="visible">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem5">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">gtk-quit</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkMenuItem" id="menuitem2">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">_Editar</property>
+ <property name="use_underline">True</property>
+ <child>
+ <widget class="GtkMenu" id="menu2">
+ <property name="visible">True</property>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem6">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">gtk-cut</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem7">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">gtk-copy</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem8">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">gtk-paste</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem9">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">gtk-delete</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkMenuItem" id="menuitem3">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">_Ver</property>
+ <property name="use_underline">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkMenuItem" id="menuitem4">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">Ay_uda</property>
+ <property name="use_underline">True</property>
+ <child>
+ <widget class="GtkMenu" id="menu3">
+ <property name="visible">True</property>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem10">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">gtk-about</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkHSeparator" id="hseparator1">
+ <property name="visible">True</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkHBox" id="hbox1">
+ <property name="visible">True</property>
+ <child>
+ <widget class="GtkButton" id="button1">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ <property name="label" translatable="yes">Record</property>
+ <property name="response_id">0</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button2">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ <property name="label" translatable="yes">Stop</property>
+ <property name="response_id">0</property>
+ </widget>
+ <packing>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkTable" id="table1">
+ <property name="visible">True</property>
+ <property name="n_rows">2</property>
+ <property name="n_columns">2</property>
+ <child>
+ <widget class="GtkLabel" id="label1">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">Iteration</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkLabel" id="label2">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">fps</property>
+ </widget>
+ <packing>
+ <property name="top_attach">1</property>
+ <property name="bottom_attach">2</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkEntry" id="entry1">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="progress_pulse_step">0.20999999344348907</property>
+ </widget>
+ <packing>
+ <property name="left_attach">1</property>
+ <property name="right_attach">2</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkEntry" id="entry2">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="editing_canceled">True</property>
+ <property name="invisible_char">●</property>
+ </widget>
+ <packing>
+ <property name="left_attach">1</property>
+ <property name="right_attach">2</property>
+ <property name="top_attach">1</property>
+ <property name="bottom_attach">2</property>
+ </packing>
+ </child>
+ </widget>
+ <packing>
+ <property name="position">2</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button3">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ <property name="label" translatable="yes">Exit</property>
+ <property name="response_id">0</property>
+ </widget>
+ <packing>
+ <property name="position">3</property>
+ </packing>
+ </child>
+ </widget>
+ <packing>
+ <property name="position">2</property>
+ </packing>
+ </child>
+ </widget>
+ </child>
+ </widget>
+</glade-interface>
Added: trunk/src/components/recorder/recordergui.h
===================================================================
--- trunk/src/components/recorder/recordergui.h (rev 0)
+++ trunk/src/components/recorder/recordergui.h 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,86 @@
+/*
+* Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Authors : Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>,
+ * Jose María Cañas Plaza <jmplaza en gsyc.es>
+ *
+ */
+
+#ifndef RECORDER_GUI_H
+#define RECORDER_GUI_H
+
+#include <string>
+#include <iostream>
+#include <gtkmm.h>
+#include <libglademm.h>
+#include <IceUtil/Thread.h>
+#include <IceUtil/Time.h>
+#include <colorspaces/colorspacesmm.h>
+
+namespace recorder {
+ class recordergui {
+ public:
+
+ recordergui();
+ virtual ~recordergui();
+ void update();
+ void set_iteration(int value);
+ void set_fps(int value);
+ bool get_recording();
+ bool get_active();
+
+
+
+ private:
+ void button_record_clicked();
+ void button_stop_clicked();
+ void button_exit_clicked();
+
+ bool recording;
+ bool exit;
+
+
+
+
+ Glib::RefPtr<Gnome::Glade::Xml> refXml;
+ Gtk::Main gtkmain;
+ Gtk::Window* mainwindow;
+
+ Gtk::Entry *w_entry_iteration;
+ Gtk::Entry *w_entry_fps;
+ Gtk::Button *w_record;
+ Gtk::Button *w_stop;
+ Gtk::Button *w_exit;
+ /*Gtk::VScale *vscale_pos_x;
+ Gtk::VScale *vscale_pos_y;
+ Gtk::VScale *vscale_pos_z;
+ Gtk::VScale *vscale_foa_x;
+ Gtk::VScale *vscale_foa_y;
+ Gtk::VScale *vscale_foa_z;
+ Gtk::VScale *vscale_fx;
+ Gtk::VScale *vscale_fy;
+ Gtk::VScale *vscale_u0;
+ Gtk::VScale *vscale_v0;
+ Gtk::VScale *vscale_roll;
+ Gtk::Image *gtk_image;
+ Gtk::Button* button_center;
+
+ Controller * controller;*/
+ };
+}//namespace
+
+#endif /*RECORDER_GUI_H*/
Added: trunk/src/components/replayer/CMakeLists.txt
===================================================================
--- trunk/src/components/replayer/CMakeLists.txt (rev 0)
+++ trunk/src/components/replayer/CMakeLists.txt 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,29 @@
+SET(SOURCE_FILES replayer.cpp replayergui.cpp control.cpp)
+
+add_definitions(-DGLADE_DIR="${gladedir}")
+
+set( CMAKE_CXX_FLAGS "-Wno-deprecated" ) # Opciones para el compilador-lgsl -lgslcblas -lGL -lGLU -lglut -lgazebo
+
+include_directories(
+ ${INTERFACES_CPP_DIR}
+ ${LIBS_DIR}/
+ ${CMAKE_CURRENT_SOURCE_DIR}
+ ${CMAKE_CURRENT_SOURCE_DIR}/cameras
+)
+
+add_executable (replayer ${SOURCE_FILES})
+
+
+TARGET_LINK_LIBRARIES(replayer
+ ${CMAKE_THREAD_LIBS_INIT}
+ ${GLUT_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+ ${gtkmm_LIBRARIES}
+ ${libglademm_LIBRARIES}
+ ${gtkglextmm_LIBRARIES}
+ ${LIBS_DIR}/colorspaces/libcolorspacesmm.so
+ ${INTERFACES_CPP_DIR}/jderobot/libJderobotInterfaces.so
+ ${LIBS_DIR}/jderobotutil/libjderobotutil.so
+ ${gsl_LIBRARIES}
+ ${ZeroCIce_LIBRARIES}
+)
Added: trunk/src/components/replayer/build/CMakeLists.txt
===================================================================
--- trunk/src/components/replayer/build/CMakeLists.txt (rev 0)
+++ trunk/src/components/replayer/build/CMakeLists.txt 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,108 @@
+project (JDEROBOT_KINECTVIEWER)
+
+cmake_minimum_required(VERSION 2.8)
+
+# ENV VARS
+SET(gladedir ./)
+
+SET( INTERFACES_CPP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../interfaces/cpp) # Directorio con las interfaces ICE en C++
+SET( LIBS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../libs) # Directorio donde se encuentran las librerias propias de jderobot
+SET( SLICE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../interfaces/slice) # Directorio donde se encuentran las interfaces ICE
+SET( LIBS_NEEDED colorspaces jderobotutil ) # Librerias de las que depende el componente
+SET( DEPS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../Deps) # Directorio donde se encuentran las dependencias
+
+###################
+# #
+# CHECK SYSTEM #
+# #
+###################
+
+ # FIND & CHECK PRINCIPAL LIBRARIES
+
+include(FindPkgConfig)
+
+PKG_CHECK_MODULES(opencv REQUIRED opencv)
+include_directories(${opencv_INCLUDE_DIRS})
+link_directories(${opencv_LIBRARY_DIRS})
+
+PKG_CHECK_MODULES(gtkmm REQUIRED gtkmm-2.4)
+include_directories(${gtkmm_INCLUDE_DIRS})
+link_directories(${gtkmm_LIBRARY_DIRS})
+
+PKG_CHECK_MODULES(libglademm REQUIRED libglademm-2.4)
+include_directories(${libglademm_INCLUDE_DIRS})
+link_directories(${libglademm_LIBRARY_DIRS})
+
+PKG_CHECK_MODULES(gtkglextmm REQUIRED gtkglextmm-1.2)
+include_directories(${gtkglextmm_INCLUDE_DIRS})
+link_directories(${gtkglextmm_LIBRARY_DIRS})
+
+PKG_CHECK_MODULES(gsl REQUIRED gsl)
+include_directories(${gsl_INCLUDE_DIRS})
+link_directories(${gsl_LIBRARIES_DIRS})
+
+include(${DEPS_DIR}/gearbox/CMakeLists.txt)
+include(${DEPS_DIR}/ice/CMakeLists.txt)
+include(${DEPS_DIR}/opencv/CMakeLists.txt)
+include(${DEPS_DIR}/opengl/CMakeLists.txt)
+
+# START RECURSIVE
+add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/../../.. ${CMAKE_CURRENT_SOURCE_DIR}/../../..)
+add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/.. ${CMAKE_CURRENT_SOURCE_DIR}/..)
+
+###################
+# #
+# INSTALL #
+# #
+###################
+
+# Install libraries
+INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../../../src/interfaces/cpp/jderobot/libJderobotInterfaces.so DESTINATION /usr/local/lib/jderobot)
+
+FOREACH(currentLibFile ${LIBS_NEEDED})
+ SET (new_lib "lib${currentLibFile}.so")
+ MESSAGE("${new_lib}")
+ INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../../../src/libs/${currentLibFile}/${new_lib} DESTINATION /usr/local/lib/jderobot)
+ENDFOREACH(currentLibFile)
+
+# Install libraries headers
+FILE(GLOB_RECURSE HEADERS_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../../../src/libs/*.h)
+FOREACH(currentSourceFile ${HEADERS_FILES})
+ string(REGEX REPLACE ".*/(.*/).*.h" "\\1" new_source1 ${currentSourceFile})
+ INSTALL (FILES ${currentSourceFile} DESTINATION /usr/local/include/jderobot/${new_source1})
+ENDFOREACH(currentSourceFile)
+
+# Install Executables
+FILE(GLOB_RECURSE BIN_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../*cfg)
+ string(REGEX REPLACE ".*/(.*).cfg" "\\1" new_source1 ${BIN_FILES})
+INSTALL (FILES ../${new_source1} DESTINATION /usr/local/bin PERMISSIONS OWNER_EXECUTE GROUP_EXECUTE WORLD_EXECUTE)
+
+# Install interfaces headers
+FILE(GLOB HEADER_INTERFACE_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../../../src/interfaces/cpp/jderobot/*.h)
+INSTALL (FILES ${HEADER_INTERFACE_FILES} DESTINATION /usr/local/include/jderobot/jderobot)
+
+# Install slice
+FILE(GLOB SLICE_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../../../src/interfaces/slice/jderobot/*.ice)
+INSTALL (FILES ${SLICE_FILES} DESTINATION /usr/local/include/jderobot/slice)
+
+# Install CONF
+FILE(GLOB_RECURSE CONF_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../*cfg)
+INSTALL (FILES ${CONF_FILES} DESTINATION /usr/local/share/jderobot/conf)
+
+# Install Glade
+FILE(GLOB_RECURSE GLADE_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../*.glade)
+INSTALL (FILES ${GLADE_FILES} DESTINATION /usr/local/share/jderobot/glade)
+
+###################
+# #
+# UNINSTALL #
+# #
+###################
+
+configure_file(
+ "${CMAKE_CURRENT_SOURCE_DIR}/cmake_uninstall.cmake.in"
+ "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
+ IMMEDIATE @ONLY)
+
+add_custom_target(uninstall
+ COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake)
Added: trunk/src/components/replayer/build/cmake_uninstall.cmake.in
===================================================================
--- trunk/src/components/replayer/build/cmake_uninstall.cmake.in (rev 0)
+++ trunk/src/components/replayer/build/cmake_uninstall.cmake.in 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,22 @@
+if (NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+ message(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"")
+endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+
+file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
+string(REGEX REPLACE "\n" ";" files "${files}")
+list(REVERSE files)
+foreach (file ${files})
+ message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"")
+ if (EXISTS "$ENV{DESTDIR}${file}")
+ execute_process(
+ COMMAND @CMAKE_COMMAND@ -E remove "$ENV{DESTDIR}${file}"
+ OUTPUT_VARIABLE rm_out
+ RESULT_VARIABLE rm_retval
+ )
+ if(NOT ${rm_retval} EQUAL 0)
+ message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"")
+ endif (NOT ${rm_retval} EQUAL 0)
+ else (EXISTS "$ENV{DESTDIR}${file}")
+ message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.")
+ endif (EXISTS "$ENV{DESTDIR}${file}")
+endforeach(file)
\ No newline at end of file
Added: trunk/src/components/replayer/build-independent/CMakeLists.txt
===================================================================
--- trunk/src/components/replayer/build-independent/CMakeLists.txt (rev 0)
+++ trunk/src/components/replayer/build-independent/CMakeLists.txt 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,70 @@
+ cmake_minimum_required(VERSION 2.8)
+
+
+SET( SOURCE_FILES ../replayer.cpp ../replayergui.cpp ../control.cpp)
+SET( LIBS_DIR /usr/local/lib/jderobot/)
+
+
+
+include_directories(
+ /usr/local/include/jderobot
+)
+
+PROJECT(replayerProject)
+add_executable (replayer ${SOURCE_FILES})
+
+#automated opencv
+include(FindPkgConfig)
+PKG_CHECK_MODULES(opencv REQUIRED opencv)
+include_directories(${opencv_INCLUDE_DIRS})
+link_directories(${opencv_LIBRARY_DIRS})
+
+
+
+#manual ICE
+FIND_PATH( Ice_INCLUDE_DIR NAMES Ice/Ice.h PATHS ENV C++LIB ENV)
+
+IF( Ice_INCLUDE_DIR )
+ FIND_LIBRARY( Ice_LIBRARY1 NAMES Ice PATHS ENV C++LIB ENV PATH PATH_SUFFIXES lib lib64 )
+ FIND_LIBRARY( Ice_LIBRARY2 NAMES IceUtil PATHS ENV C++LIB ENV PATH PATH_SUFFIXES lib lib64)
+ SET (Ice_LIBRARIES ${Ice_LIBRARY1} ${Ice_LIBRARY2})
+ IF( Ice_LIBRARIES )
+ MESSAGE ("-- Ice found at ${Ice_LIBRARIES}")
+ include_directories(${Ice_INCLUDE_DIR})
+ link_directories(${Ice_LIBRARIES})
+ ENDIF( Ice_LIBRARIES )
+ENDIF(Ice_INCLUDE_DIR)
+
+IF(NOT Ice_LIBRARIES)
+ MESSAGE ("*** Ice not found")
+ENDIF()
+
+
+#automated gtk
+PKG_CHECK_MODULES(gtkmm REQUIRED gtkmm-2.4)
+include_directories(${gtkmm_INCLUDE_DIRS})
+link_directories(${gtkmm_LIBRARY_DIRS})
+#automated gtkmm
+PKG_CHECK_MODULES(libglademm REQUIRED libglademm-2.4)
+include_directories(${libglademm_INCLUDE_DIRS})
+link_directories(${libglademm_LIBRARY_DIRS})
+#automated gtkglextmm
+PKG_CHECK_MODULES(gtkglextmm REQUIRED gtkglextmm-x11-1.2)
+include_directories(${gtkglextmm_INCLUDE_DIRS})
+link_directories(${gtkglextmm_LIBRARY_DIRS})
+
+
+set( CMAKE_CXX_FLAGS "-Wno-deprecated" ) # Opciones para el compilador
+
+
+
+TARGET_LINK_LIBRARIES(replayer
+ ${opencv_LIBRARIES}
+ ${Ice_LIBRARIES}
+ ${gtkglextmm_LIBRARIES}
+ ${libglademm_LIBRARIES}
+ ${gtkmm_LIBRARIES}
+ ${LIBS_DIR}/libcolorspacesmm.so
+ ${LIBS_DIR}/libJderobotInterfaces.so
+ ${LIBS_DIR}/libjderobotutil.so
+)
Added: trunk/src/components/replayer/control.cpp
===================================================================
--- trunk/src/components/replayer/control.cpp (rev 0)
+++ trunk/src/components/replayer/control.cpp 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,119 @@
+/*
+ * control.cpp
+ *
+ * Created on: 16/05/2013
+ * Author: frivas
+ */
+
+#include "control.h"
+
+namespace replayer {
+
+control::control(long long int initState) {
+ this->nProcFinished=0;
+ this->play=true;
+ this->repeat=true;
+ this->newTime=initState;
+ this->nRepetitions=0;
+
+}
+
+control::~control() {
+ // TODO Auto-generated destructor stub
+}
+
+void control::lock(){
+ this->controlMutex.lock();
+}
+
+void control::unlock(){
+ this->controlMutex.unlock();
+}
+
+bool control::getPlay(){
+ bool localPlay;
+ this->controlMutex.lock();
+ localPlay=this->play;
+ this->controlMutex.unlock();
+ return localPlay;
+}
+
+long long int control::getTime(){
+ long long int localTime;
+ this->controlMutex.lock();
+ localTime=this->newTime;
+ this->controlMutex.unlock();
+ return localTime;
+}
+
+long long int control::wait(){
+ long long int localTime;
+ {
+ IceUtil::Mutex::Lock sync(this->controlMutex);
+ this->nProcFinished=this->nProcFinished+1;
+ this->sem.wait(sync);
+ localTime=this->newTime;
+ this->nRepetitions=this->nRepetitions+1;
+ }
+ return localTime;
+}
+
+void control::checkStatus(){
+ this->controlMutex.lock();
+ if (this->nProcFinished != this->nProcess){
+ }
+ else{
+ if (this->repeat){
+ std::cout << "--------------REINICIO" << std::endl;
+ IceUtil::Time now = IceUtil::Time::now();
+ long long int nowInt=(now.toMicroSeconds())/1000;
+ this->newTime=nowInt;
+ this->nProcFinished=0;
+ this->sem.broadcast();
+ }
+ }
+ this->controlMutex.unlock();
+}
+
+void control::stop(){
+ this->controlMutex.lock();
+ this->play=false;
+ IceUtil::Time now = IceUtil::Time::now();
+ long long int nowInt=(now.toMicroSeconds())/1000;
+ std::cout << "now_ " << nowInt << std::endl;
+
+ this->timeToResume=nowInt-this->newTime;
+ this->controlMutex.unlock();
+}
+
+void control::resume(){
+ this->controlMutex.lock();
+ this->play=true;
+ IceUtil::Time now = IceUtil::Time::now();
+ long long int nowInt=(now.toMicroSeconds())/1000;
+ std::cout << "now_ " << nowInt << std::endl;
+ std::cout << "TIEMPO RELATIVO: " << this->timeToResume << std::endl;
+ this->newTime=nowInt-this->timeToResume;
+ this->controlMutex.unlock();
+}
+
+void control::setRepeat(bool active){
+ this->repeat=active;
+}
+
+void control::setProcesses(int procs){
+ this->nProcess=procs;
+}
+
+void control::setStep(int step){
+ this->controlMutex.lock();
+ IceUtil::Time n = IceUtil::Time::now();
+ long long int nowInt=(n.toMicroSeconds())/1000;
+ std::cout << "avanzo" << step << std::endl;
+ this->timeToResume=this->timeToResume + step;
+ this->newTime=nowInt-this->timeToResume;
+ this->controlMutex.unlock();
+}
+
+} /* namespace replayer */
+
Added: trunk/src/components/replayer/control.h
===================================================================
--- trunk/src/components/replayer/control.h (rev 0)
+++ trunk/src/components/replayer/control.h 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,70 @@
+/*
+ *
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 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 Rivas <franciscomiguel.rivas en urjc.es>
+ *
+ */
+
+#ifndef CONTROL_H_
+#define CONTROL_H_
+#include <iostream>
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+namespace replayer {
+
+class control {
+public:
+ control(long long int initState);
+ virtual ~control();
+ void lock();
+ void unlock();
+ bool getPlay();
+ long long int getTime();
+ long long int wait();
+ void checkStatus();
+ void stop();
+ void resume();
+ void setRepeat(bool active);
+ void setProcesses(int procs);
+ void setStep(int step);
+
+private:
+ //controladores de video
+ bool repeat;
+ //mutex de control de acceso a control
+ IceUtil::Mutex controlMutex;
+ //condicional utilizado como un semaforo
+ IceUtil::Cond sem;
+ //numero de procesos terminados
+ int nProcFinished;
+ //numero de procesos en total
+ int nProcess;
+ //nuevo tiempo para reiniciar la reproduccion
+ long long int newTime;
+ //nuevo tiempo relativo para reanudar la reproduccion
+ long long int timeToResume;
+ //reproduccion
+ bool play;
+ //numero de repeticiones
+ bool nRepetitions;
+
+};
+
+} /* namespace replayer */
+#endif /* CONTROL_H_ */
Added: trunk/src/components/replayer/replayer.cfg
===================================================================
--- trunk/src/components/replayer/replayer.cfg (rev 0)
+++ trunk/src/components/replayer/replayer.cfg 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,73 @@
+#without registry
+Replayer.Endpoints=default -h localhost -p 9999
+
+Replayer.nCameras=2
+Replayer.nPointClouds=0
+Replayer.nPose3dEncoders=2
+Replayer.nEncoders=1
+Replayer.nLasers=1
+Replayer.replayControlActive=1
+
+#camera 1
+Replayer.Camera.0.Name=cam_sensor_left
+Replayer.Camera.0.ImageWidth=320
+Replayer.Camera.0.ImageHeight=240
+Replayer.Camera.0.Format=RGB8
+Replayer.Camera.0.Dir=data/images/camera1/
+Replayer.Camera.0.FileFormat=png
+
+#camera 2
+Replayer.Camera.1.Name=cam_sensor_right
+Replayer.Camera.1.ImageWidth=320
+Replayer.Camera.1.ImageHeight=240
+Replayer.Camera.1.Format=RGB8
+Replayer.Camera.1.Dir=data/images/camera2/
+Replayer.Camera.1.FileFormat=png
+
+#camera 3
+Replayer.Camera.2.Name=cameraAB
+Replayer.Camera.2.ImageWidth=640
+Replayer.Camera.2.ImageHeight=480
+Replayer.Camera.2.Format=RGB8
+Replayer.Camera.2.Dir=data/images/camera3/
+Replayer.Camera.2.FileFormat=png
+
+#camera 3
+Replayer.Camera.3.Name=cameraBB
+Replayer.Camera.3.ImageWidth=640
+Replayer.Camera.3.ImageHeight=480
+Replayer.Camera.3.Format=RGB8
+Replayer.Camera.3.Dir=data/images/camera4/
+Replayer.Camera.3.FileFormat=png
+
+#pointCloud1
+Replayer.PointCloud.0.Name=pointcloud1
+Replayer.PointCloud.0.Dir=data/pointClouds/pointCloud1/
+
+#pointCloud2
+Replayer.PointCloud.1.Name=pointcloud2
+Replayer.PointCloud.1.Dir=data/pointClouds/pointCloud2/
+
+#pose3dencoders1
+Replayer.pose3dencoder.0.Name=Pose3DEncoders1
+Replayer.pose3dencoder.0.Dir=data/pose3dencoders/pose3dencoder1/
+
+#pose3dencoders2
+Replayer.pose3dencoder.1.Name=Pose3DEncoders2
+Replayer.pose3dencoder.1.Dir=data/pose3dencoders/pose3dencoder2/
+
+#encoders1
+Replayer.encoder.0.Name=Encoders
+Replayer.encoder.0.Dir=data/encoders/encoder1/
+
+#encoders1
+Replayer.laser.0.Name=Laser
+Replayer.laser.0.Dir=data/lasers/laser1/
+
+
+Replayer.Hostname=localhost
+Replayer.Port=9998
+
+Replayer.Speed=1
+Replayer.FileName=.
+Replayer.Sensors=datos.jde
Added: trunk/src/components/replayer/replayer.cpp
===================================================================
--- trunk/src/components/replayer/replayer.cpp (rev 0)
+++ trunk/src/components/replayer/replayer.cpp 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,1154 @@
+/*
+ *
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 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 Rivas <franciscomiguel.rivas en urjc.es>
+ *
+ */
+
+
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+#include <tr1/memory>
+#include <list>
+#include <sstream>
+#include <iostream>
+#include <fstream>
+
+// Library includes
+#include <math.h>
+#include <cv.h>
+#include <highgui.h>
+
+#include <colorspaces/colorspacesmm.h>
+#include <jderobot/camera.h>
+#include <jderobot/pointcloud.h>
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/encoders.h>
+#include <jderobot/replayControl.h>
+#include <jderobot/laser.h>
+#include "replayergui.h"
+#include "control.h"
+
+
+namespace replayer {
+
+
+
+ control* controller;
+
+ class CameraI: virtual public jderobot::Camera {
+ public:
+ CameraI(std::string& propertyPrefix, Ice::CommunicatorPtr ic, long long int initStateIN): prefix(propertyPrefix) {
+ std::cout << "Creating: " << propertyPrefix << std::endl;
+ imageDescription = (new jderobot::ImageDescription());
+ prop = ic->getProperties();
+ //cameraDescription = (new jderobot::CameraDescription());
+ startThread = false;
+ this->width=prop->getPropertyAsIntWithDefault(propertyPrefix + "ImageWidth",320);
+ this->height=prop->getPropertyAsIntWithDefault(propertyPrefix + "ImageHeight",240);
+ this->dataPath=prop->getProperty(propertyPrefix+"Dir");
+ this->fileFormat=prop->getProperty(propertyPrefix+"FileFormat");
+
+ std::cout << "PATH " << this->dataPath << std::endl;
+ std::cout << "FORMAT: " << this->fileFormat << std::endl;
+
+ this->initState=initStateIN;
+ //sync task
+ syncTask = new SyncTask(this,this->dataPath, this->fileFormat);
+ syncTask->start();
+ //reply task
+ replyTask = new ReplyTask(this);
+ replyTask->start(); // my own thread
+
+ }
+
+ std::string getName () {
+ return (cameraDescription->name);
+ }
+
+ std::string getRobotName () {
+ return prop->getProperty("Replayer.RobotName");
+
+ }
+
+ virtual ~CameraI() {
+ std::cout << "Stopping and joining thread for camera: " << cameraDescription->name << std::endl;
+ }
+
+ virtual jderobot::ImageDescriptionPtr getImageDescription(const Ice::Current& c){
+ return imageDescription;
+ }
+
+ virtual jderobot::CameraDescriptionPtr getCameraDescription(const Ice::Current& c){
+ return cameraDescription;
+ }
+
+ virtual Ice::Int setCameraDescription(const jderobot::CameraDescriptionPtr &description, const Ice::Current& c) {
+ return 0;
+ }
+
+ virtual void getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr& cb,const Ice::Current& c){
+ replyTask->pushJob(cb);
+ }
+
+ virtual std::string startCameraStreaming(const Ice::Current&){
+ std::cout << "Should be made anything to start camera streaming: " << cameraDescription->name << std::endl;
+ return std::string("");
+ }
+
+ virtual void stopCameraStreaming(const Ice::Current&) {
+ std::cout << "Should be made anything to stop camera streaming: " << cameraDescription->name << std::endl;
+ }
+ /*virtual void update(cv::Mat imageIn){
+ imageIn.copyTo(this->image);
+ //std::cout << "update" << endl;
+
+ if(!startThread){
+ startThread = true;
+ replyTask = new ReplyTask(this);
+ replyTask->start(); // my own thread
+ }
+ }*/
+
+ private:
+ class SyncTask: public IceUtil::Thread{
+ public:
+ SyncTask(CameraI* camera, std::string pathIn, std::string fileFormatIN){
+ this->mycamera=camera;
+ this->path=pathIn;
+ this->initiated=false;
+ this->fileFormat=fileFormatIN;
+ this->onPause=false;
+ }
+ ~SyncTask(){
+ }
+ virtual void run(){
+ std::string line;
+ std::string fileName(this->path + "cameraData.jde");
+ std::ifstream myfile(fileName.c_str());
+ if (!myfile.is_open())
+ std::cout << "Error while trying to open: " << fileName << std::endl;
+ while(this->isAlive()){
+ while ( myfile.good() ){
+ bool playing=controller->getPlay();
+
+ this->onPause=!playing;
+ while (!playing){
+ playing=controller->getPlay();
+ long long int pauseStatus= controller->getTime();
+ if (pauseStatus != this->mycamera->initState){
+ this->mycamera->initState=pauseStatus;
+ break;
+ }
+ //check if w
+ std::cout << "not playing" << std::endl;
+ usleep(10000);
+ continue;
+ }
+
+ if (this->onPause){
+ this->mycamera->initState=controller->getTime();
+ myfile.close();
+ myfile.open(fileName.c_str());
+ }
+
+ getline (myfile,line);
+ std::istringstream sTemp(line);
+ long long int relative;
+ sTemp >> relative;
+ lastRelative=relative;
+
+
+ //tiempo para comprobar si vamos muy desacompasados y para rewind - forward
+ IceUtil::Time pretime = IceUtil::Time::now();
+ long long int checkState=(pretime.toMicroSeconds())/1000;
+
+
+
+ while((((relative) - (checkState - this->mycamera->initState ))<0)&&(myfile.good())){
+ //no hacemos nada, estamos fuera de tiempo tenemos que avanzar al siguiente frame
+ getline (myfile,line);
+ std::istringstream sTemp(line);
+ sTemp >> relative;
+ }
+ if (!myfile.good()){
+ if (this->onPause)
+ continue;
+ else
+ break;
+ }
+
+
+ cv::Mat tempImage=cv::imread(this->path + line + "." + this->fileFormat);
+
+ IceUtil::Time a = IceUtil::Time::now();
+ long long int actualState=(a.toMicroSeconds())/1000;
+ /*std::cout << "Relativo fichero: " << relative << std::endl;
+ std::cout << "Relativo global: " << (actualState - this->mycamera->initState ) << std::endl;
+ std::cout << "Duermo: " << (relative) - (actualState - this->mycamera->initState ) << std::endl;*/
+ if ((actualState - this->mycamera->initState ) < relative ){
+ usleep(((relative) - (actualState - this->mycamera->initState ))*1000);
+ }
+ else{
+ std::cout << "TIMEOUT" << std::endl;
+ }
+
+ this->mycamera->dataMutex.lock();
+
+ tempImage.copyTo(this->mycamera->image);
+
+
+
+ /*cv::imshow("lector", this->mycamera->image);
+ cv::waitKey(0);*/
+ this->mycamera->dataMutex.unlock();
+
+ }
+ myfile.close();
+ //control.controlMutex.lock();
+ this->mycamera->initState=controller->wait();
+ myfile.open(fileName.c_str());
+
+ }
+
+ }
+ private:
+ std::string path;
+ CameraI* mycamera;
+ bool initiated;
+ std::string fileFormat;
+ bool onPause;
+ long long int lastRelative;
+ };
+
+
+ class ReplyTask: public IceUtil::Thread{
+ public:
+ ReplyTask(CameraI* camera){
+ std::cout << "safeThread" <<std::endl;
+ this->mycamera=camera;
+ }
+
+ void pushJob(const jderobot::AMD_ImageProvider_getImageDataPtr& cb){
+ IceUtil::Mutex::Lock sync(requestsMutex);
+ requests.push_back(cb);
+ }
+
+ virtual void run(){
+ mycamera->imageDescription->width = this->mycamera->width;
+ mycamera->imageDescription->height = this->mycamera->height;
+ mycamera->imageDescription->size = this->mycamera->width*this->mycamera->height*3;
+ mycamera->imageDescription->format = "RGB8";
+
+ jderobot::ImageDataPtr reply(new jderobot::ImageData);
+ reply->description = mycamera->imageDescription;
+ IceUtil::Time a, b;
+ int cycle = 48;
+ long totalb,totala;
+ long diff;
+
+ while(this->isAlive()){
+ a = IceUtil::Time::now();
+ totala=a.toMicroSeconds();
+
+
+ IceUtil::Time t = IceUtil::Time::now();
+ reply->timeStamp.seconds = (long)t.toSeconds();
+ reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
+
+ reply->pixelData.resize(mycamera->imageDescription->size);
+
+ //image = cv::imread(mycamera->fileName);
+ this->mycamera->dataMutex.lock();
+ if (this->mycamera->image.rows != 0){
+ memcpy( &(reply->pixelData[0]), (unsigned char *) this->mycamera->image.data, this->mycamera->image.rows*this->mycamera->image.cols*3);
+ }
+
+ this->mycamera->dataMutex.unlock();
+ { //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
+
+ b = IceUtil::Time::now();
+ totalb=b.toMicroSeconds();
+
+ diff = (totalb-totala)/1000;
+ diff = cycle-diff;
+
+ if(diff < 33)
+ diff = 33;
+
+
+ /*Sleep Algorithm*/
+ usleep(diff*1000);
+ }
+ }
+
+ CameraI* mycamera;
+ IceUtil::Mutex requestsMutex;
+ std::list<jderobot::AMD_ImageProvider_getImageDataPtr> requests;
+ };
+
+ typedef IceUtil::Handle<ReplyTask> ReplyTaskPtr;
+ std::string prefix;
+ colorspaces::Image::FormatPtr imageFmt;
+ jderobot::ImageDescriptionPtr imageDescription;
+ jderobot::CameraDescriptionPtr cameraDescription;
+ ReplyTaskPtr replyTask;
+ bool startThread;
+ Ice::PropertiesPtr prop;
+ cv::Mat image;
+ IceUtil::Mutex dataMutex;
+ IceUtil::Time ref;
+ int width;
+ int height;
+ typedef IceUtil::Handle<SyncTask> SyncTaskPtr;
+ SyncTaskPtr syncTask;
+ long long int initState;
+ std::string fileFormat;
+ std::string dataPath;
+
+
+ }; // end class CameraI
+
+ //BEGIN pointCloudI
+ class PointCloudI: virtual public jderobot::pointCloud{
+ public:
+ PointCloudI (std::string& propertyPrefix, Ice::CommunicatorPtr ic, long long int initStateIN):prefix(propertyPrefix),KData(new jderobot::pointCloudData()) {
+ this->prop = ic->getProperties();
+ this->dataPath=prop->getProperty(propertyPrefix+"Dir");
+ this->initState=initStateIN;
+ v = new SyncTask(this,this->dataPath);
+ v->start();
+ }
+
+ virtual jderobot::pointCloudDataPtr getCloudData(const Ice::Current&){
+ //check si los dos son iguales
+ this->m.lock();
+ jderobot::pointCloudDataPtr localData(KData);
+ this->m.unlock();
+ return localData;
+ };
+
+ private:
+
+ class SyncTask :public IceUtil::Thread{
+ public:
+ SyncTask(PointCloudI* cloud, std::string pathIn){
+ this->myPointCloud=cloud;
+ this->path=pathIn;
+ this->initiated=false;
+ this->onPause=false;
+ }
+
+ virtual void run(){
+ std::string line;
+ std::string fileName(this->path + "pointCloudData.jde");
+ std::ifstream myfile(fileName.c_str());
+ if (!myfile.is_open())
+ std::cout << "-----Error while trying to open: " << fileName << std::endl;
+ while(this->isAlive()){
+ while ( myfile.good() ){
+ bool playing=controller->getPlay();
+ this->onPause=!playing;
+ while (!playing){
+ playing=controller->getPlay();
+ long long int pauseStatus= controller->getTime();
+ if (pauseStatus != this->myPointCloud->initState){
+ this->myPointCloud->initState=pauseStatus;
+ break;
+ }
+ //check if w
+ usleep(10000);
+ continue;
+ }
+
+ if (this->onPause){
+ this->myPointCloud->initState=controller->getTime();
+ myfile.close();
+ myfile.open(fileName.c_str());
+ }
+
+
+ getline (myfile,line);
+ std::istringstream sTemp(line);
+ long long int relative;
+ sTemp >> relative;
+ int sizeVector;
+ sTemp >> sizeVector;
+
+ //tiempo para comprobar si vamos muy desacompasados y para rewind - forward
+ IceUtil::Time pretime = IceUtil::Time::now();
+ long long int checkState=(pretime.toMicroSeconds())/1000;
+
+ while((((relative) - (checkState - this->myPointCloud->initState ))<0)&&(myfile.good())){
+ //no hacemos nada, estamos fuera de tiempo tenemos que avanzar al siguiente frame
+ getline (myfile,line);
+ std::istringstream sTemp(line);
+ sTemp >> relative;
+ sTemp >> sizeVector;
+
+ }
+ if (!myfile.good()){
+ if (this->onPause)
+ continue;
+ else
+ break;
+ }
+ std::ostringstream relativeFile;
+ relativeFile << relative;
+ std::string localFile(this->path + relativeFile.str());
+
+ std::ifstream infile(localFile.c_str(), std::ios::in | std::ios::binary);
+
+
+ IceUtil::Time a = IceUtil::Time::now();
+ long long int actualState=(a.toMicroSeconds())/1000;
+ /*std::cout << "Relativo fichero: " << relative << std::endl;
+ std::cout << "Relativo global: " << (actualState - this->myPointCloud->initState ) << std::endl;
+ std::cout << "Duermo: " << (relative) - (actualState - this->myPointCloud->initState ) << std::endl;*/
+ if ((actualState - this->myPointCloud->initState ) < relative ){
+ usleep(((relative) - (actualState - this->myPointCloud->initState ))*1000);
+ }
+ else{
+ std::cout << "TIMEOUT" << std::endl;
+ }
+
+ this->myPointCloud->m.lock();
+ this->myPointCloud->KData->p.resize(sizeVector);
+ infile.read((char *)&this->myPointCloud->KData->p.front(), this->myPointCloud->KData->p.size()*sizeof(jderobot::RGBPoint));
+ this->myPointCloud->m.unlock();
+ }
+ myfile.close();
+ //control.controlMutex.lock();
+ this->myPointCloud->initState=controller->wait();
+ myfile.open(fileName.c_str());
+
+ }
+
+ }
+ PointCloudI* myPointCloud;
+ std::string path;
+ bool initiated;
+ bool onPause;
+
+ };
+ typedef IceUtil::Handle<SyncTask> SyncTaskPtr;
+ SyncTaskPtr v;
+ std::string prefix;
+ jderobot::pointCloudDataPtr KData;
+ Ice::PropertiesPtr prop;
+ std::string dataPath;
+ IceUtil::Mutex m;
+ long long int initState;
+
+
+ };
+
+
+
+
+ //BEGIN LaserI
+ class LaserI: virtual public jderobot::Laser{
+ public:
+ LaserI (std::string& propertyPrefix, Ice::CommunicatorPtr ic, long long int initStateIN):prefix(propertyPrefix),KData(new jderobot::LaserData()) {
+ this->prop = ic->getProperties();
+ this->dataPath=prop->getProperty(propertyPrefix+"Dir");
+ this->initState=initStateIN;
+ v = new SyncTask(this,this->dataPath);
+ v->start();
+ }
+
+ virtual jderobot::LaserDataPtr getLaserData(const Ice::Current&){
+ //check si los dos son iguales
+ this->m.lock();
+ jderobot::LaserDataPtr localData(KData);
+ //solucion??
+ localData->numLaser=180;
+ this->m.unlock();
+ return localData;
+ };
+
+ private:
+
+ class SyncTask :public IceUtil::Thread{
+ public:
+ SyncTask(LaserI* laser, std::string pathIn){
+ this->myLaser=laser;
+ this->path=pathIn;
+ this->initiated=false;
+ this->onPause=false;
+ }
+
+ virtual void run(){
+ std::string line;
+ std::string fileName(this->path + "laserData.jde");
+ std::ifstream myfile(fileName.c_str());
+ if (!myfile.is_open())
+ std::cout << "-----Error while trying to open: " << fileName << std::endl;
+ while(this->isAlive()){
+ while ( myfile.good() ){
+ bool playing=controller->getPlay();
+ this->onPause=!playing;
+ while (!playing){
+ playing=controller->getPlay();
+ long long int pauseStatus= controller->getTime();
+ if (pauseStatus != this->myLaser->initState){
+ this->myLaser->initState=pauseStatus;
+ break;
+ }
+ //check if w
+ usleep(10000);
+ continue;
+ }
+
+ if (this->onPause){
+ this->myLaser->initState=controller->getTime();
+ myfile.close();
+ myfile.open(fileName.c_str());
+ }
+
+
+ getline (myfile,line);
+ std::istringstream sTemp(line);
+ long long int relative;
+ sTemp >> relative;
+ int sizeVector;
+ sTemp >> sizeVector;
+
+ //tiempo para comprobar si vamos muy desacompasados y para rewind - forward
+ IceUtil::Time pretime = IceUtil::Time::now();
+ long long int checkState=(pretime.toMicroSeconds())/1000;
+
+ while((((relative) - (checkState - this->myLaser->initState ))<0)&&(myfile.good())){
+ //no hacemos nada, estamos fuera de tiempo tenemos que avanzar al siguiente frame
+ getline (myfile,line);
+ std::istringstream sTemp(line);
+ sTemp >> relative;
+ sTemp >> sizeVector;
+
+ }
+ if (!myfile.good()){
+ if (this->onPause)
+ continue;
+ else
+ break;
+ }
+ std::ostringstream relativeFile;
+ relativeFile << relative;
+ std::string localFile(this->path + relativeFile.str());
+
+ std::ifstream infile(localFile.c_str(), std::ios::in | std::ios::binary);
+
+
+ IceUtil::Time a = IceUtil::Time::now();
+ long long int actualState=(a.toMicroSeconds())/1000;
+ /*std::cout << "Relativo fichero: " << relative << std::endl;
+ std::cout << "Relativo global: " << (actualState - this->myPointCloud->initState ) << std::endl;
+ std::cout << "Duermo: " << (relative) - (actualState - this->myPointCloud->initState ) << std::endl;*/
+ if ((actualState - this->myLaser->initState ) < relative ){
+ usleep(((relative) - (actualState - this->myLaser->initState ))*1000);
+ }
+ else{
+ std::cout << "TIMEOUT" << std::endl;
+ }
+
+ this->myLaser->m.lock();
+ this->myLaser->KData->distanceData.resize(sizeVector);
+ infile.read((char *)&this->myLaser->KData->distanceData.front(), this->myLaser->KData->distanceData.size()*sizeof(int));
+ this->myLaser->m.unlock();
+ }
+ myfile.close();
+ //control.controlMutex.lock();
+ this->myLaser->initState=controller->wait();
+ myfile.open(fileName.c_str());
+
+ }
+
+ }
+ LaserI* myLaser;
+ std::string path;
+ bool initiated;
+ bool onPause;
+
+ };
+ typedef IceUtil::Handle<SyncTask> SyncTaskPtr;
+ SyncTaskPtr v;
+ std::string prefix;
+ jderobot::LaserDataPtr KData;
+ Ice::PropertiesPtr prop;
+ std::string dataPath;
+ IceUtil::Mutex m;
+ long long int initState;
+
+
+ };
+
+
+
+
+
+
+ //BEGIN Pose3DEncodersI
+ class Pose3DEncodersI: virtual public jderobot::Pose3DEncoders{
+ public:
+ Pose3DEncodersI (std::string& propertyPrefix, Ice::CommunicatorPtr ic, long long int initStateIN):prefix(propertyPrefix),encData(new jderobot::Pose3DEncodersData()) {
+ this->prop = ic->getProperties();
+ this->dataPath=prop->getProperty(propertyPrefix+"Dir");
+ this->initState=initStateIN;
+ v = new SyncTask(this,this->dataPath);
+ v->start();
+ }
+
+ virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData(const Ice::Current&){
+ //check si los dos son iguales
+ this->m.lock();
+ jderobot::Pose3DEncodersDataPtr localData(encData);
+ this->m.unlock();
+ return localData;
+ };
+
+ private:
+
+ class SyncTask :public IceUtil::Thread{
+ public:
+ SyncTask(Pose3DEncodersI* enc, std::string pathIn){
+ this->myPose3d=enc;
+ this->path=pathIn;
+ this->initiated=false;
+ this->onPause=false;
+ }
+
+ virtual void run(){
+ std::string line;
+ std::string fileName(this->path + "pose3dencoderData.jde");
+ std::ifstream myfile(fileName.c_str());
+ if (!myfile.is_open())
+ std::cout << "-----Error while trying to open: " << fileName << std::endl;
+ while(this->isAlive()){
+ while ( myfile.good() ){
+ bool playing=controller->getPlay();
+ this->onPause=!playing;
+ while (!playing){
+ playing=controller->getPlay();
+ long long int pauseStatus= controller->getTime();
+ if (pauseStatus != this->myPose3d->initState){
+ this->myPose3d->initState=pauseStatus;
+ break;
+ }
+ //check if w
+ usleep(10000);
+ continue;
+ }
+
+ if (this->onPause){
+ this->myPose3d->initState=controller->getTime();
+ myfile.close();
+ myfile.open(fileName.c_str());
+ }
+
+
+ getline (myfile,line);
+ std::istringstream sTemp(line);
+ long long int relative;
+ sTemp >> relative;
+ int sizeVector;
+ sTemp >> sizeVector;
+
+ //tiempo para comprobar si vamos muy desacompasados y para rewind - forward
+ IceUtil::Time pretime = IceUtil::Time::now();
+ long long int checkState=(1000000+pretime.toMicroSeconds())/1000;
+
+ while((((relative) - (checkState - this->myPose3d->initState ))<0)&&(myfile.good())){
+ //no hacemos nada, estamos fuera de tiempo tenemos que avanzar al siguiente frame
+ getline (myfile,line);
+ std::istringstream sTemp(line);
+ sTemp >> relative;
+ sTemp >> sizeVector;
+
+ }
+ if (!myfile.good()){
+ if (this->onPause)
+ continue;
+ else
+ break;
+ }
+ std::ostringstream relativeFile;
+ relativeFile << relative;
+ std::string localFile(this->path + relativeFile.str());
+
+ std::ifstream infile(localFile.c_str(), std::ios::in | std::ios::binary);
+
+
+ IceUtil::Time a = IceUtil::Time::now();
+ long long int actualState=(a.toMicroSeconds())/1000;
+ /*std::cout << "Relativo fichero: " << relative << std::endl;
+ std::cout << "Relativo global: " << (actualState - this->myPointCloud->initState ) << std::endl;
+ std::cout << "Duermo: " << (relative) - (actualState - this->myPointCloud->initState ) << std::endl;*/
+ if ((actualState - this->myPose3d->initState ) < relative ){
+ usleep(((relative) - (actualState - this->myPose3d->initState ))*1000);
+ }
+ else{
+ std::cout << "TIMEOUT" << std::endl;
+ }
+
+ this->myPose3d->m.lock();
+ infile.read((char *)&this->myPose3d->tempData, sizeof(pose3dencoders));
+ //hago la copia al interfaz....
+ this->myPose3d->encData->x=this->myPose3d->tempData.x;
+ this->myPose3d->encData->y=this->myPose3d->tempData.y;
+ this->myPose3d->encData->z=this->myPose3d->tempData.z;
+ this->myPose3d->encData->pan=this->myPose3d->tempData.pan;
+ this->myPose3d->encData->tilt=this->myPose3d->tempData.tilt;
+ this->myPose3d->encData->roll=this->myPose3d->tempData.roll;
+ this->myPose3d->encData->maxPan=this->myPose3d->tempData.maxPan;
+ this->myPose3d->encData->maxTilt=this->myPose3d->tempData.maxTilt;
+ this->myPose3d->encData->minPan=this->myPose3d->tempData.minPan;
+ this->myPose3d->encData->minTilt=this->myPose3d->tempData.minTilt;
+ this->myPose3d->m.unlock();
+ }
+ myfile.close();
+ //control.controlMutex.lock();
+ this->myPose3d->initState=controller->wait();
+ myfile.open(fileName.c_str());
+
+ }
+
+ }
+ Pose3DEncodersI* myPose3d;
+ std::string path;
+ bool initiated;
+ bool onPause;
+
+ };
+
+ struct pose3dencoders{
+ float x;
+ float y;
+ float z;
+ float pan;
+ float tilt;
+ float roll;
+ int clock;
+ float maxPan;
+ float maxTilt;
+ float minPan;
+ float minTilt;
+ };
+
+ typedef IceUtil::Handle<SyncTask> SyncTaskPtr;
+ SyncTaskPtr v;
+ std::string prefix;
+ jderobot::Pose3DEncodersDataPtr encData;
+ Ice::PropertiesPtr prop;
+ std::string dataPath;
+ IceUtil::Mutex m;
+ long long int initState;
+ pose3dencoders tempData;
+
+
+
+
+ };
+
+
+
+ //BEGIN Pose3DEncodersI
+ class EncodersI: virtual public jderobot::Encoders{
+ public:
+ EncodersI (std::string& propertyPrefix, Ice::CommunicatorPtr ic, long long int initStateIN):prefix(propertyPrefix),encData(new jderobot::EncodersData()) {
+ this->prop = ic->getProperties();
+ this->dataPath=prop->getProperty(propertyPrefix+"Dir");
+ this->initState=initStateIN;
+ v = new SyncTask(this,this->dataPath);
+ v->start();
+ }
+
+ virtual jderobot::EncodersDataPtr getEncodersData(const Ice::Current&){
+ //check si los dos son iguales
+ this->m.lock();
+ jderobot::EncodersDataPtr localData(encData);
+ this->m.unlock();
+ return localData;
+ };
+
+ private:
+
+ class SyncTask :public IceUtil::Thread{
+ public:
+ SyncTask(EncodersI* enc, std::string pathIn){
+ this->myEncoder=enc;
+ this->path=pathIn;
+ this->initiated=false;
+ this->onPause=false;
+ }
+
+ virtual void run(){
+ std::string line;
+ std::string fileName(this->path + "encoderData.jde");
+ std::ifstream myfile(fileName.c_str());
+ if (!myfile.is_open())
+ std::cout << "-----Error while trying to open: " << fileName << std::endl;
+ while(this->isAlive()){
+ while ( myfile.good() ){
+ bool playing=controller->getPlay();
+ this->onPause=!playing;
+ while (!playing){
+ playing=controller->getPlay();
+ long long int pauseStatus= controller->getTime();
+ if (pauseStatus != this->myEncoder->initState){
+ this->myEncoder->initState=pauseStatus;
+ break;
+ }
+ //check if w
+ usleep(10000);
+ continue;
+ }
+
+ if (this->onPause){
+ this->myEncoder->initState=controller->getTime();
+ myfile.close();
+ myfile.open(fileName.c_str());
+ }
+
+
+ getline (myfile,line);
+ std::istringstream sTemp(line);
+ long long int relative;
+ sTemp >> relative;
+ int sizeVector;
+ sTemp >> sizeVector;
+
+ //tiempo para comprobar si vamos muy desacompasados y para rewind - forward
+ IceUtil::Time pretime = IceUtil::Time::now();
+ long long int checkState=(pretime.toMicroSeconds())/1000;
+
+ while((((relative) - (checkState - this->myEncoder->initState ))<0)&&(myfile.good())){
+ //no hacemos nada, estamos fuera de tiempo tenemos que avanzar al siguiente frame
+ getline (myfile,line);
+ std::istringstream sTemp(line);
+ sTemp >> relative;
+ sTemp >> sizeVector;
+
+ }
+ if (!myfile.good()){
+ if (this->onPause)
+ continue;
+ else
+ break;
+ }
+ std::ostringstream relativeFile;
+ relativeFile << relative;
+ std::string localFile(this->path + relativeFile.str());
+
+ std::ifstream infile(localFile.c_str(), std::ios::in | std::ios::binary);
+
+
+ IceUtil::Time a = IceUtil::Time::now();
+ long long int actualState=(a.toMicroSeconds())/1000;
+ /*std::cout << "Relativo fichero: " << relative << std::endl;
+ std::cout << "Relativo global: " << (actualState - this->myPointCloud->initState ) << std::endl;
+ std::cout << "Duermo: " << (relative) - (actualState - this->myPointCloud->initState ) << std::endl;*/
+ if ((actualState - this->myEncoder->initState ) < relative ){
+ usleep(((relative) - (actualState - this->myEncoder->initState ))*1000);
+ }
+ else{
+ std::cout << "TIMEOUT" << std::endl;
+ }
+
+ this->myEncoder->m.lock();
+ infile.read((char *)&this->myEncoder->tempData, sizeof(encoders));
+ //hago la copia al interfaz....
+ this->myEncoder->encData->robotx=this->myEncoder->tempData.robotx;
+ this->myEncoder->encData->roboty=this->myEncoder->tempData.roboty;
+ this->myEncoder->encData->robottheta=this->myEncoder->tempData.robottheta;
+ this->myEncoder->encData->robotcos=this->myEncoder->tempData.robotcos;
+ this->myEncoder->encData->robotsin=this->myEncoder->tempData.robotsin;
+ this->myEncoder->m.unlock();
+ }
+ myfile.close();
+ //control.controlMutex.lock();
+ this->myEncoder->initState=controller->wait();
+ myfile.open(fileName.c_str());
+
+ }
+
+ }
+ EncodersI* myEncoder;
+ std::string path;
+ bool initiated;
+ bool onPause;
+
+ };
+
+ struct encoders{
+ float robotx;
+ float roboty;
+ float robottheta;
+ float robotcos;
+ float robotsin;
+ };
+
+ typedef IceUtil::Handle<SyncTask> SyncTaskPtr;
+ SyncTaskPtr v;
+ std::string prefix;
+ jderobot::EncodersDataPtr encData;
+ Ice::PropertiesPtr prop;
+ std::string dataPath;
+ IceUtil::Mutex m;
+ long long int initState;
+ encoders tempData;
+
+
+
+
+ };
+
+
+
+
+
+ //BEGIN replayControllerI
+ class replayControllerI: virtual public jderobot::replayControl{
+ public:
+ replayControllerI(){
+
+ }
+ ~replayControllerI(){
+
+ }
+
+ virtual bool pause(const Ice::Current&){
+ controller->stop();
+ return true;
+ }
+ virtual bool resume(const Ice::Current&){
+ controller->resume();
+ return true;
+ }
+ virtual void setReplay( bool replay, const Ice::Current&){
+ controller->setRepeat(replay);
+ }
+ virtual bool setStep( Ice::Int step, const Ice::Current&){
+ controller->setStep(step);
+ return true;
+ }
+ virtual Ice::Long getTime(const Ice::Current&){
+ //return the rurrent time
+ return 0;
+ }
+ virtual bool goTo(Ice::Long seek, const Ice::Current&){
+ //set the current position to seek
+ return 0;
+ }
+ private:
+
+ };
+
+
+
+} //namespace
+
+
+int main(int argc, char** argv) {
+
+ std::vector<Ice::ObjectPtr> cameras;
+ IceUtil::Time a = IceUtil::Time::now();
+ long long int initState=(a.toMicroSeconds())/1000;
+ int nProcs=0;
+
+
+ Ice::CommunicatorPtr ic;
+ try{
+ ic = Ice::initialize(argc, argv);
+ Ice::PropertiesPtr prop = ic->getProperties();
+
+ replayer::controller= new replayer::control(initState);
+
+ int nCameras = prop->getPropertyAsIntWithDefault("Replayer.nCameras",0);
+ std::cout << "Cameras to load: " << nCameras << std::endl;
+ std::string Endpoints = prop->getProperty("Replayer.Endpoints");
+ cameras.resize(nCameras);
+ Ice::ObjectAdapterPtr adapter =ic->createObjectAdapterWithEndpoints("Replayer", Endpoints);
+ for (int i=0; i<nCameras; i++) {//build camera objects
+ std::stringstream objIdS;
+ objIdS << i;
+ std::string objId = objIdS.str();
+ std::string objPrefix("Replayer.Camera." + objId + ".");
+ std::string cameraName = prop->getProperty(objPrefix + "Name");
+ std::cout << "Camera name: " << cameraName << std::endl;
+
+ if (cameraName.size() == 0) { //no name specified, we create one using the index
+ cameraName = "camera" + objId;
+ prop->setProperty(objPrefix + "Name",cameraName);//set the value
+ }
+
+ std::cout << "Creating camera " << cameraName << std::endl;
+
+
+
+ cameras[i] = new replayer::CameraI(objPrefix,ic,initState);
+
+
+
+ adapter->add(cameras[i], ic->stringToIdentity(cameraName));
+ adapter->activate();
+ nProcs++;
+ }
+ int nPointClouds = prop->getPropertyAsIntWithDefault("Replayer.nPointClouds",0);
+ for (int i=0; i<nPointClouds; i++) {//build camera objects
+ std::stringstream objIdS;
+ objIdS << i;
+ std::string objId = objIdS.str();
+ std::string objPrefix("Replayer.PointCloud." + objId + ".");
+ std::string Name = prop->getProperty(objPrefix + "Name");
+ std::cout << "pointCloud name: " << Name << std::endl;
+
+ if (Name.size() == 0) { //no name specified, we create one using the index
+ Name = "pointcloud" + objId;
+ prop->setProperty(objPrefix + "Name",Name);//set the value
+ }
+
+ std::cout << "Creating pointCloud " << Name << std::endl;
+
+
+
+ Ice::ObjectPtr object= new replayer::PointCloudI(objPrefix,ic,initState);
+
+
+
+ adapter->add(object, ic->stringToIdentity(Name));
+ nProcs++;
+ }
+
+ int nLasers = prop->getPropertyAsIntWithDefault("Replayer.nLasers",0);
+ for (int i=0; i<nLasers; i++) {//build camera objects
+ std::stringstream objIdS;
+ objIdS << i;
+ std::string objId = objIdS.str();
+ std::string objPrefix("Replayer.laser." + objId + ".");
+ std::string Name = prop->getProperty(objPrefix + "Name");
+ std::cout << "laser name: " << Name << std::endl;
+
+ if (Name.size() == 0) { //no name specified, we create one using the index
+ Name = "laser" + objId;
+ prop->setProperty(objPrefix + "Name",Name);//set the value
+ }
+
+ std::cout << "Creating laser " << Name << std::endl;
+
+
+
+ Ice::ObjectPtr object= new replayer::LaserI(objPrefix,ic,initState);
+
+
+
+ adapter->add(object, ic->stringToIdentity(Name));
+ nProcs++;
+ }
+
+
+
+ int nPose3dEncoders = prop->getPropertyAsIntWithDefault("Replayer.nPose3dEncoders",0);
+ for (int i=0; i<nPose3dEncoders; i++) {//build camera objects
+ std::stringstream objIdS;
+ objIdS << i;
+ std::string objId = objIdS.str();
+ std::string objPrefix("Replayer.pose3dencoder." + objId + ".");
+ std::string Name = prop->getProperty(objPrefix + "Name");
+ std::cout << "pose3dencoders name: " << Name << std::endl;
+
+ if (Name.size() == 0) { //no name specified, we create one using the index
+ Name = "pose3dencoders" + objId;
+ prop->setProperty(objPrefix + "Name",Name);//set the value
+ }
+
+ std::cout << "Creating pose3dencoders " << Name << std::endl;
+
+
+
+ Ice::ObjectPtr object= new replayer::Pose3DEncodersI(objPrefix,ic,initState);
+
+
+
+ adapter->add(object, ic->stringToIdentity(Name));
+ nProcs++;
+ }
+
+ int nEncoders = prop->getPropertyAsIntWithDefault("Replayer.nEncoders",0);
+ for (int i=0; i<nEncoders; i++) {//build camera objects
+ std::stringstream objIdS;
+ objIdS << i;
+ std::string objId = objIdS.str();
+ std::string objPrefix("Replayer.encoder." + objId + ".");
+ std::string Name = prop->getProperty(objPrefix + "Name");
+ std::cout << "encoders name: " << Name << std::endl;
+
+ if (Name.size() == 0) { //no name specified, we create one using the index
+ Name = "encoders" + objId;
+ prop->setProperty(objPrefix + "Name",Name);//set the value
+ }
+
+ std::cout << "Creating encoders " << Name << std::endl;
+
+
+
+ Ice::ObjectPtr object= new replayer::EncodersI(objPrefix,ic,initState);
+
+
+
+ adapter->add(object, ic->stringToIdentity(Name));
+ nProcs++;
+ }
+
+
+ int controllerActive = prop->getPropertyAsIntWithDefault("Replayer.replayControlActive",0);
+ if (controllerActive){
+ Ice::ObjectPtr rc= new replayer::replayControllerI();
+ adapter->add(rc, ic->stringToIdentity("replayControllerA"));
+ }
+
+ adapter->activate();
+ replayer::controller->setProcesses(nProcs);
+
+ //replayer::replayergui* gui = new replayer::replayergui(replayer::controller);
+
+ //do it beeter
+ while (1){
+ //gui->update();
+ replayer::controller->checkStatus();
+ usleep(50000);
+ }
+
+
+
+ ic->waitForShutdown();
+ }catch (const Ice::Exception& ex) {
+ std::cerr << ex << std::endl;
+ exit(-1);
+ } catch (const char* msg) {
+ std::cerr << msg << std::endl;
+ exit(-1);
+ }
+ return 0;
+}
Added: trunk/src/components/replayer/replayergui.cpp
===================================================================
--- trunk/src/components/replayer/replayergui.cpp (rev 0)
+++ trunk/src/components/replayer/replayergui.cpp 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,68 @@
+/*
+ * recordergui.cpp
+ *
+ * Created on: 14/05/2013
+ * Author: frivas
+ */
+
+#include "replayergui.h"
+
+
+namespace replayer {
+
+replayergui::replayergui(control* c): gtkmain(0,0) {
+ // TODO Auto-generated constructor stub
+
+ this->controller=c;
+ std::cout << "Loading glade\n";
+ refXml = Gnome::Glade::Xml::create("./replayergui.glade");
+
+ /*Get widgets*/
+ refXml->get_widget("window1",mainwindow);
+ refXml->get_widget("button1", w_stop);
+ refXml->get_widget("button2", w_resume);
+ refXml->get_widget("button3", w_step);
+ refXml->get_widget("togglebutton1", w_repeat);
+
+ w_repeat->set_active(true);
+
+ w_stop->signal_clicked().connect(sigc::mem_fun(this,&replayergui::on_clicked_stop));
+ w_resume->signal_clicked().connect(sigc::mem_fun(this,&replayergui::on_clicked_resume));
+ w_repeat->signal_clicked().connect(sigc::mem_fun(this,&replayergui::on_change_repeat));
+ w_step->signal_clicked().connect(sigc::mem_fun(this,&replayergui::on_clicked_step));
+
+
+ mainwindow->show();
+
+}
+
+replayergui::~replayergui() {
+ // TODO Auto-generated destructor stub
+}
+
+void replayergui::update(){
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+}
+
+void replayergui::on_clicked_stop(){
+ std::cout << "paro" << std::endl;
+ this->controller->stop();
+}
+
+void replayergui::on_clicked_resume(){
+ std::cout << "reinicio" << std::endl;
+ this->controller->resume();
+}
+
+void replayergui::on_change_repeat(){
+ std::cout << "repito" << std::endl;
+ this->controller->setRepeat(this->w_repeat->get_active());
+}
+
+void replayergui::on_clicked_step(){
+ std::cout << "step" << std::endl;
+ this->controller->setStep(100);
+}
+
+} /* namespace recorder */
Added: trunk/src/components/replayer/replayergui.glade
===================================================================
--- trunk/src/components/replayer/replayergui.glade (rev 0)
+++ trunk/src/components/replayer/replayergui.glade 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,308 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<glade-interface>
+ <!-- interface-requires gtk+ 2.24 -->
+ <!-- interface-naming-policy project-wide -->
+ <widget class="GtkWindow" id="window1">
+ <property name="width_request">640</property>
+ <property name="height_request">200</property>
+ <property name="can_focus">False</property>
+ <property name="title" translatable="yes">Replayer</property>
+ <child>
+ <widget class="GtkVBox" id="vbox1">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <child>
+ <widget class="GtkMenuBar" id="menubar1">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <child>
+ <widget class="GtkMenuItem" id="menuitem1">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="label" translatable="yes">_Archivo</property>
+ <property name="use_underline">True</property>
+ <child>
+ <widget class="GtkMenu" id="menu1">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem1">
+ <property name="label">gtk-new</property>
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem2">
+ <property name="label">gtk-open</property>
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem3">
+ <property name="label">gtk-save</property>
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem4">
+ <property name="label">gtk-save-as</property>
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkSeparatorMenuItem" id="separatormenuitem1">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem5">
+ <property name="label">gtk-quit</property>
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkMenuItem" id="menuitem2">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="label" translatable="yes">_Editar</property>
+ <property name="use_underline">True</property>
+ <child>
+ <widget class="GtkMenu" id="menu2">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem6">
+ <property name="label">gtk-cut</property>
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem7">
+ <property name="label">gtk-copy</property>
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem8">
+ <property name="label">gtk-paste</property>
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem9">
+ <property name="label">gtk-delete</property>
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkMenuItem" id="menuitem3">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="label" translatable="yes">_Ver</property>
+ <property name="use_underline">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkMenuItem" id="menuitem4">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="label" translatable="yes">Ay_uda</property>
+ <property name="use_underline">True</property>
+ <child>
+ <widget class="GtkMenu" id="menu3">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <child>
+ <widget class="GtkImageMenuItem" id="imagemenuitem10">
+ <property name="label">gtk-about</property>
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <property name="use_action_appearance">False</property>
+ <property name="use_underline">True</property>
+ <property name="use_stock">True</property>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ </widget>
+ </child>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="fill">True</property>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkHSeparator" id="hseparator1">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="fill">True</property>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkHScale" id="hscale1">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="round_digits">1</property>
+ </widget>
+ <packing>
+ <property name="expand">True</property>
+ <property name="fill">True</property>
+ <property name="position">2</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkHSeparator" id="hseparator2">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="fill">True</property>
+ <property name="position">3</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkHBox" id="hbox1">
+ <property name="visible">True</property>
+ <property name="can_focus">False</property>
+ <child>
+ <widget class="GtkButton" id="button1">
+ <property name="label" translatable="yes">STOP</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ <property name="use_action_appearance">False</property>
+ </widget>
+ <packing>
+ <property name="expand">True</property>
+ <property name="fill">True</property>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button2">
+ <property name="label" translatable="yes">RESUME</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ <property name="use_action_appearance">False</property>
+ </widget>
+ <packing>
+ <property name="expand">True</property>
+ <property name="fill">True</property>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button3">
+ <property name="label" translatable="yes">Step</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ <property name="use_action_appearance">False</property>
+ </widget>
+ <packing>
+ <property name="expand">True</property>
+ <property name="fill">True</property>
+ <property name="position">2</property>
+ </packing>
+ </child>
+ <child>
+ <placeholder/>
+ </child>
+ <child>
+ <placeholder/>
+ </child>
+ <child>
+ <placeholder/>
+ </child>
+ <child>
+ <placeholder/>
+ </child>
+ <child>
+ <placeholder/>
+ </child>
+ <child>
+ <placeholder/>
+ </child>
+ <child>
+ <widget class="GtkToggleButton" id="togglebutton1">
+ <property name="label" translatable="yes">Repeat</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ <property name="use_action_appearance">False</property>
+ </widget>
+ <packing>
+ <property name="expand">True</property>
+ <property name="fill">True</property>
+ <property name="position">9</property>
+ </packing>
+ </child>
+ </widget>
+ <packing>
+ <property name="expand">True</property>
+ <property name="fill">True</property>
+ <property name="position">4</property>
+ </packing>
+ </child>
+ </widget>
+ </child>
+ </widget>
+</glade-interface>
Added: trunk/src/components/replayer/replayergui.h
===================================================================
--- trunk/src/components/replayer/replayergui.h (rev 0)
+++ trunk/src/components/replayer/replayergui.h 2013-07-25 08:32:57 UTC (rev 955)
@@ -0,0 +1,62 @@
+/*
+ *
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 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 Rivas <franciscomiguel.rivas en urjc.es>
+ *
+ */
+#ifndef RECORDERGUI_H_
+#define RECORDERGUI_H_
+
+#include <string>
+#include <iostream>
+#include <gtkmm.h>
+#include <libglademm.h>
+#include <IceUtil/Thread.h>
+#include <IceUtil/Time.h>
+#include "control.h"
+
+namespace replayer {
+
+class replayergui {
+public:
+ replayergui(control *c);
+ virtual ~replayergui();
+ void update();
+
+private:
+ Glib::RefPtr<Gnome::Glade::Xml> refXml;
+ Gtk::Main gtkmain;
+ Gtk::Window* mainwindow;
+ Gtk::Button* w_stop;
+ Gtk::Button* w_resume;
+ Gtk::Button* w_step;
+ Gtk::ToggleButton* w_repeat;
+
+ control *controller;
+
+ //gtk functions
+ void on_clicked_stop();
+ void on_clicked_resume();
+ void on_change_repeat();
+ void on_clicked_step();
+
+
+};
+
+} /* namespace recorder */
+#endif /* RECORDERGUI_H_ */
More information about the Jderobot-admin
mailing list