Frivas 793 replayer refactor

src/drivers/cameraserver/CMakeLists.txt

@@ -17,19 +17,19 @@ link_directories(
 add_executable (cameraserver ${SOURCE_FILES})
 
 TARGET_LINK_LIBRARIES(cameraserver
-    jderobotutil
+    	jderobotutil
 	${OpenCV_LIBRARIES}       
-    ${OpenCVGUI_LIBRARIES} 
+    	${OpenCVGUI_LIBRARIES} 
 	colorspacesmm
 	JderobotInterfaces
 	${easyiceconfig_LIBRARIES} 
 	${ZeroCIce_LIBRARIES}
 	${CMAKE_THREAD_LIBS_INIT} 
 	${ZLIB_LIBRARIES}
 	logger
-    crypto
-ns
-		${GLOG_LIBRARIES}
+    	crypto
+	ns
+	${GLOG_LIBRARIES}
 	)
 
 install(TARGETS cameraserver

src/tools/cameraCalibrator/cameraCalibrator.cpp

@@ -269,7 +269,6 @@ struct cameraData{
 
 int main(int argc, char** argv){
 
-	int status,i;
 	Ice::CommunicatorPtr ic;
 	Ice::PropertiesPtr prop;
 	cv::Size boardSize;

src/tools/cameraview/cameraview.cpp

@@ -30,7 +30,6 @@
 
 int main(int argc, char** argv){
 
-	int status;
 	cameraview::Viewer viewer;
 	Ice::CommunicatorPtr ic;
 
@@ -50,5 +49,5 @@ int main(int argc, char** argv){
 		viewer.displayFrameRate(0);
 	}
 
-	return status;
+	return 0;
 }

src/tools/replayer2/CMakeLists.txt

@@ -1,4 +1,9 @@
-	SET(SOURCE_FILES replayer.cpp replayergui.cpp control.cpp)
+	SET(SOURCE_FILES
+			replayer.cpp
+			replayergui.cpp
+			utils/SyncController
+			interfaces/ReplayerCamera
+            interfaces/SyncTask.cpp interfaces/SyncTask.h interfaces/ReplayerPointCloud.cpp interfaces/ReplayerPointCloud.h interfaces/ReplayerLaser.cpp interfaces/ReplayerLaser.h interfaces/ReplayerPose3DEncoders.cpp interfaces/ReplayerPose3DEncoders.h interfaces/ReplayerPose3D.cpp interfaces/ReplayerPose3D.h interfaces/ReplayerEncoders.cpp interfaces/ReplayerEncoders.h utils/ReplayControllerInterface.cpp utils/ReplayControllerInterface.h)
 
 add_definitions(-DGLADE_DIR="${gladedir}")
 
@@ -25,21 +30,23 @@ add_executable (replayer2 ${SOURCE_FILES})
 TARGET_LINK_LIBRARIES(replayer2
     ns
     logger
-    ${CMAKE_THREAD_LIBS_INIT} 
     ${GLUT_LIBRARIES}
     ${OpenCV_LIBRARIES}
     ${OpenCVGUI_LIBRARIES}
     ${gtkmm_LIBRARIES}
     ${libglademm_LIBRARIES}
     ${gtkglextmm_LIBRARIES}
+	jderobotutil
     colorspacesmm
     JderobotInterfaces
-    jderobotutil
     ${gsl_LIBRARIES}
     ${easyiceconfig_LIBRARIES} 
     ${ZeroCIce_LIBRARIES}
     ${resourcelocator_LIBRARIES}
-		${GLOG_LIBRARIES}
+	${GLOG_LIBRARIES}
+	${ZLIB_LIBRARIES}
+	crypto
+	${CMAKE_THREAD_LIBS_INIT}
 
 		)
 

src/tools/replayer2/interfaces/ReplayerCamera.cpp

@@ -0,0 +1,44 @@
+//
+// Created by frivas on 27/04/17.
+//
+
+#include "ReplayerCamera.h"
+
+namespace replayer {
+    ReplayerCamera::ReplayerCamera(std::string propertyPrefix, Ice::CommunicatorPtr ic,
+                                             replayer::SyncControllerPtr syncController, long long int initStateIN) :
+            jderobot::CameraHandler(propertyPrefix,ic),
+            syncController(syncController)
+
+    {
+        imageDescription = (new jderobot::ImageDescription());
+        prop = ic->getProperties();
+        cameraDescription = (new jderobot::CameraDescription());
+        imageDescription->width=prop->getPropertyAsIntWithDefault(propertyPrefix + "ImageWidth",320);
+        imageDescription->height=prop->getPropertyAsIntWithDefault(propertyPrefix + "ImageHeight",240);
+        this->dataPath=prop->getProperty(propertyPrefix+"Dir");
+        this->fileFormat=prop->getProperty(propertyPrefix+"FileFormat");
+        imageDescription->format = prop->getProperty(propertyPrefix+"Format");
+        imageDescription->size = width*height*3;
+
+        LOG(INFO)<< "PATH " + this->dataPath ;
+        LOG(INFO)<< "FORMAT: " + this->fileFormat ;
+
+
+        this->initState=initStateIN;
+        //sync task
+        syncTask = new CameraSyncTask(this,this->dataPath, this->fileFormat);
+        syncTask->start();
+        //reply task
+        replyTask = new ReplyTask(this,30); //30 fps ~ real time
+        replyTask->start(); // my own thread
+
+    }
+
+    void
+    ReplayerCamera::getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr &cb, const std::string &format,
+                                       const Ice::Current &c) {
+        replyTask->pushJob(cb, format);
+    }
+
+}

src/tools/replayer2/interfaces/ReplayerCamera.h

@@ -0,0 +1,82 @@
+//
+// Created by frivas on 27/04/17.
+//
+
+#ifndef JDEROBOT_REPLAYERCAMERA_H
+#define JDEROBOT_REPLAYERCAMERA_H
+
+
+#include <jderobotutil/CameraHandler.h>
+#include <jderobotutil/CameraTask.h>
+#include <jderobot/logger/Logger.h>
+#include <utils/SyncController.h>
+#include <opencv2/imgcodecs.hpp>
+#include "SyncTask.h"
+
+namespace replayer {
+    class ReplayerCamera : public jderobot::CameraHandler {
+    public:
+        ReplayerCamera(std::string propertyPrefix, Ice::CommunicatorPtr ic,replayer::SyncControllerPtr syncController,long long int initStateIN);
+        void getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr &cb, const std::string &format,const Ice::Current &c);
+
+
+    private:
+
+
+        /*************  CameraSyncTask ***********************/
+        class CameraSyncTask: public SyncTask<ReplayerCamera*>{
+        public:
+            CameraSyncTask(ReplayerCamera* camera, std::string pathIn, std::string fileFormatIN):SyncTask(pathIn,"cameraData.jde",camera){
+                this->path=pathIn;
+                this->fileFormat=fileFormatIN;
+            }
+
+            virtual void generateData(){
+                cv::Mat tempImage = cv::imread(this->path + lineData + "." + this->fileFormat);
+                tempImage.copyTo(this->interface->image);
+            }
+        private:
+            std::string fileFormat;
+        };
+
+
+        /*************  ReplyTask ***********************/
+        class ReplyTask : public jderobot::CameraTask {
+        public:
+            ReplyTask(ReplayerCamera* camera, int fps):jderobot::CameraTask(camera,fps),mycamera(camera) {
+
+            }
+
+            virtual void createCustomImage(cv::Mat &image) {
+
+                this->mycamera->dataMutex.lock();
+                image = this->mycamera->image.clone();
+                this->mycamera->dataMutex.unlock();
+            }
+            ReplayerCamera* mycamera;
+        };
+
+
+        typedef IceUtil::Handle <ReplyTask> ReplyTaskPtr;
+        ReplyTaskPtr replyTask;
+
+
+    bool startThread;
+    Ice::PropertiesPtr prop;
+    cv::Mat image;
+    int width;
+    int height;
+    typedef IceUtil::Handle<CameraSyncTask> SyncTaskPtr;
+    SyncTaskPtr syncTask;
+    std::string fileFormat;
+    std::string dataPath;
+    std::string format;
+
+    public:
+        SyncControllerPtr syncController;
+        long long int initState;
+        IceUtil::Mutex dataMutex;
+
+    };
+}
+#endif //JDEROBOT_REPLAYERCAMERA_H

src/tools/replayer2/interfaces/ReplayerEncoders.cpp

@@ -0,0 +1,30 @@
+//
+// Created by frivas on 30/04/17.
+//
+
+#include "ReplayerEncoders.h"
+
+
+namespace replayer{
+
+    ReplayerEncoders::ReplayerEncoders(std::string &propertyPrefix, Ice::CommunicatorPtr ic,
+                                       SyncControllerPtr syncController, long long int initStateIN) :
+            prefix(propertyPrefix),
+            encData(new jderobot::EncodersData()),
+            syncController(syncController)
+    {
+        this->prop = ic->getProperties();
+        this->dataPath = prop->getProperty(propertyPrefix + "Dir");
+        this->initState = initStateIN;
+        v = new EncodersSyncTask(this, this->dataPath);
+        v->start();
+    }
+
+    jderobot::EncodersDataPtr ReplayerEncoders::getEncodersData(const Ice::Current &) {
+        //check si los dos son iguales
+        this->m.lock();
+        jderobot::EncodersDataPtr localData(encData);
+        this->m.unlock();
+        return localData;
+    };
+}

src/tools/replayer2/interfaces/ReplayerEncoders.h

@@ -0,0 +1,84 @@
+//
+// Created by frivas on 30/04/17.
+//
+
+#ifndef JDEROBOT_REPLAYERENCODERS_H
+#define JDEROBOT_REPLAYERENCODERS_H
+
+
+#include <jderobot/logger/Logger.h>
+#include "SyncTask.h"
+#include <Ice/Ice.h>
+#include <utils/SyncController.h>
+#include <laser.h>
+#include <encoders.h>
+
+namespace  replayer {
+    class ReplayerEncoders : virtual public jderobot::Encoders {
+    public:
+        ReplayerEncoders(std::string &propertyPrefix, Ice::CommunicatorPtr ic,SyncControllerPtr syncController, long long int initStateIN);
+
+        virtual jderobot::EncodersDataPtr getEncodersData(const Ice::Current &);
+
+    private:
+
+        class EncodersSyncTask : public SyncTask<ReplayerEncoders*>{
+        public:
+            EncodersSyncTask(ReplayerEncoders* encoders, std::string pathIn):
+                    SyncTask(pathIn,"encoderData.jde",encoders),
+                    myEncoder(encoders)
+            {
+                this->path=pathIn;
+            }
+
+            virtual void generateData(){
+                long long int relative;
+                int sizeVector;
+
+                std::istringstream sTemp(lineData);
+                sTemp >> relative;
+                sTemp >> sizeVector;
+
+                std::ostringstream relativeFile;
+                relativeFile << relative;
+                std::string localFile(this->path + relativeFile.str());
+
+                std::ifstream infile(localFile.c_str(), std::ios::in | std::ios::binary);
+                infile.read((char *)&this->myEncoder->tempData, sizeof(encoders));
+                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;
+
+            }
+        private:
+            ReplayerEncoders* myEncoder;
+        };
+
+        struct encoders {
+            float robotx;
+            float roboty;
+            float robottheta;
+            float robotcos;
+            float robotsin;
+        };
+
+        typedef IceUtil::Handle<EncodersSyncTask> SyncTaskPtr;
+        SyncTaskPtr v;
+        std::string prefix;
+        jderobot::EncodersDataPtr encData;
+        Ice::PropertiesPtr prop;
+        std::string dataPath;
+        IceUtil::Mutex m;
+        encoders tempData;
+
+    public:
+        SyncControllerPtr syncController;
+        long long int initState;
+        IceUtil::Mutex dataMutex;
+
+
+    };
+}
+#endif //JDEROBOT_REPLAYERENCODERS_H

src/tools/replayer2/interfaces/ReplayerLaser.cpp

@@ -0,0 +1,5 @@
+//
+// Created by frivas on 30/04/17.
+//
+
+#include "ReplayerLaser.h"