diff --git a/src/drivers/cameraserver/CMakeLists.txt b/src/drivers/cameraserver/CMakeLists.txt index 8be69daec..9ac41e6f8 100644 --- a/src/drivers/cameraserver/CMakeLists.txt +++ b/src/drivers/cameraserver/CMakeLists.txt @@ -17,9 +17,9 @@ link_directories( add_executable (cameraserver ${SOURCE_FILES}) TARGET_LINK_LIBRARIES(cameraserver - jderobotutil + jderobotutil ${OpenCV_LIBRARIES} - ${OpenCVGUI_LIBRARIES} + ${OpenCVGUI_LIBRARIES} colorspacesmm JderobotInterfaces ${easyiceconfig_LIBRARIES} @@ -27,9 +27,9 @@ TARGET_LINK_LIBRARIES(cameraserver ${CMAKE_THREAD_LIBS_INIT} ${ZLIB_LIBRARIES} logger - crypto -ns - ${GLOG_LIBRARIES} + crypto + ns + ${GLOG_LIBRARIES} ) install(TARGETS cameraserver diff --git a/src/tools/cameraCalibrator/cameraCalibrator.cpp b/src/tools/cameraCalibrator/cameraCalibrator.cpp index f7ee172bd..aec72a27f 100644 --- a/src/tools/cameraCalibrator/cameraCalibrator.cpp +++ b/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; diff --git a/src/tools/cameraview/cameraview.cpp b/src/tools/cameraview/cameraview.cpp index cc681c6e1..61484afd7 100644 --- a/src/tools/cameraview/cameraview.cpp +++ b/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; } diff --git a/src/tools/replayer2/CMakeLists.txt b/src/tools/replayer2/CMakeLists.txt index 0b9363183..05b7b8333 100644 --- a/src/tools/replayer2/CMakeLists.txt +++ b/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} ) diff --git a/src/tools/replayer2/interfaces/ReplayerCamera.cpp b/src/tools/replayer2/interfaces/ReplayerCamera.cpp new file mode 100644 index 000000000..2c2845edb --- /dev/null +++ b/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); + } + +} \ No newline at end of file diff --git a/src/tools/replayer2/interfaces/ReplayerCamera.h b/src/tools/replayer2/interfaces/ReplayerCamera.h new file mode 100644 index 000000000..8596b5d99 --- /dev/null +++ b/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 +#include +#include +#include +#include +#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{ + 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 ReplyTaskPtr; + ReplyTaskPtr replyTask; + + + bool startThread; + Ice::PropertiesPtr prop; + cv::Mat image; + int width; + int height; + typedef IceUtil::Handle 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 diff --git a/src/tools/replayer2/interfaces/ReplayerEncoders.cpp b/src/tools/replayer2/interfaces/ReplayerEncoders.cpp new file mode 100644 index 000000000..deed90ef6 --- /dev/null +++ b/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; + }; +} \ No newline at end of file diff --git a/src/tools/replayer2/interfaces/ReplayerEncoders.h b/src/tools/replayer2/interfaces/ReplayerEncoders.h new file mode 100644 index 000000000..6f17a76fb --- /dev/null +++ b/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 +#include "SyncTask.h" +#include +#include +#include +#include + +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{ + 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 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 diff --git a/src/tools/replayer2/interfaces/ReplayerLaser.cpp b/src/tools/replayer2/interfaces/ReplayerLaser.cpp new file mode 100644 index 000000000..e0e5dc23e --- /dev/null +++ b/src/tools/replayer2/interfaces/ReplayerLaser.cpp @@ -0,0 +1,5 @@ +// +// Created by frivas on 30/04/17. +// + +#include "ReplayerLaser.h" diff --git a/src/tools/replayer2/interfaces/ReplayerLaser.h b/src/tools/replayer2/interfaces/ReplayerLaser.h new file mode 100644 index 000000000..953646949 --- /dev/null +++ b/src/tools/replayer2/interfaces/ReplayerLaser.h @@ -0,0 +1,102 @@ +// +// Created by frivas on 30/04/17. +// + +#ifndef JDEROBOT_REPLAYERLASER_H +#define JDEROBOT_REPLAYERLASER_H + + +#include +#include "SyncTask.h" +#include +#include +#include +#include + + +namespace replayer { + + class ReplayerLaser : virtual public jderobot::Laser { + public: + ReplayerLaser (std::string& propertyPrefix, Ice::CommunicatorPtr ic,replayer::SyncControllerPtr syncController, long long int initStateIN): + prefix(propertyPrefix), + KData(new jderobot::LaserData()), + syncController(syncController) + { + this->prop = ic->getProperties(); + this->dataPath=prop->getProperty(propertyPrefix+"Dir"); + this->initState=initStateIN; + syncTask = new LaserSyncTask(this,this->dataPath); + syncTask->start(); + } + virtual jderobot::LaserDataPtr getLaserData(const Ice::Current&){ + this->dataMutex.lock(); + jderobot::LaserDataPtr localData(KData); + localData->numLaser=180; + this->dataMutex.unlock(); + return localData; + }; + private: + + class LaserSyncTask : public SyncTask{ + public: + LaserSyncTask(ReplayerLaser* laser, std::string pathIn): + SyncTask(pathIn,"laserData.jde",laser), + myLaser(laser) + { + this->path=pathIn; + } + + virtual void generateData(){ + long long int relative; + int sizeVector; + int maxRange; + int minRange; + float maxAngle; + float minAngle; + + std::istringstream sTemp(lineData); + sTemp >> relative; + sTemp >> sizeVector; + + sTemp >> maxRange; + sTemp >> minRange; + sTemp >> maxAngle; + sTemp >> minAngle; + + std::ostringstream relativeFile; + relativeFile << relative; + std::string localFile(this->path + relativeFile.str()); + + std::ifstream infile(localFile.c_str(), std::ios::in | std::ios::binary); + this->myLaser->KData->distanceData.resize(sizeVector); + this->myLaser->KData->minAngle = minAngle; + this->myLaser->KData->maxAngle = maxAngle; + this->myLaser->KData->minRange = minRange; + this->myLaser->KData->maxRange = maxRange; + infile.read((char *)&this->myLaser->KData->distanceData.front(), this->myLaser->KData->distanceData.size()*sizeof(int)); + + } + private: + ReplayerLaser* myLaser; + }; + + typedef IceUtil::Handle SyncTaskPtr; + SyncTaskPtr syncTask; + std::string prefix; + jderobot::LaserDataPtr KData; + Ice::PropertiesPtr prop; + std::string dataPath; + + public: + SyncControllerPtr syncController; + long long int initState; + IceUtil::Mutex dataMutex; + + + }; + +} + + +#endif //JDEROBOT_REPLAYERLASER_H diff --git a/src/tools/replayer2/interfaces/ReplayerPointCloud.cpp b/src/tools/replayer2/interfaces/ReplayerPointCloud.cpp new file mode 100644 index 000000000..31a277c26 --- /dev/null +++ b/src/tools/replayer2/interfaces/ReplayerPointCloud.cpp @@ -0,0 +1,31 @@ +// +// Created by frivas on 29/04/17. +// + +#include "ReplayerPointCloud.h" + + + +namespace replayer { + replayer::ReplayerPointCloud::ReplayerPointCloud(std::string &propertyPrefix, Ice::CommunicatorPtr ic, + replayer::SyncControllerPtr syncController, + long long int initStateIN) : + prefix(propertyPrefix), + KData(new jderobot::pointCloudData()), + syncController(syncController) + { + this->prop = ic->getProperties(); + this->dataPath = prop->getProperty(propertyPrefix + "Dir"); + this->initState = initStateIN; + syncTask = new PointCloudSyncTask(this, this->dataPath); + syncTask->start(); + } + + jderobot::pointCloudDataPtr ReplayerPointCloud::getCloudData(const Ice::Current &) { + this->dataMutex.lock(); + jderobot::pointCloudDataPtr localData(KData); + this->dataMutex.unlock(); + return localData; + }; + +} \ No newline at end of file diff --git a/src/tools/replayer2/interfaces/ReplayerPointCloud.h b/src/tools/replayer2/interfaces/ReplayerPointCloud.h new file mode 100644 index 000000000..8b10fa912 --- /dev/null +++ b/src/tools/replayer2/interfaces/ReplayerPointCloud.h @@ -0,0 +1,73 @@ +// +// Created by frivas on 29/04/17. +// + +#ifndef JDEROBOT_REPLAYERPOINTCLOUD_H +#define JDEROBOT_REPLAYERPOINTCLOUD_H + + +#include +#include +#include "SyncTask.h" +#include +#include + + +namespace replayer { + + class ReplayerPointCloud : virtual public jderobot::pointCloud { + public: + ReplayerPointCloud(std::string &propertyPrefix, Ice::CommunicatorPtr ic,replayer::SyncControllerPtr syncController, long long int initStateIN); + + virtual jderobot::pointCloudDataPtr getCloudData(const Ice::Current &); + + private: + + class PointCloudSyncTask : public SyncTask{ + public: + PointCloudSyncTask(ReplayerPointCloud* pointCloud, std::string pathIn): + SyncTask(pathIn,"pointCloudData.jde",pointCloud), + myPointCloud(pointCloud) + { + 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); + this->myPointCloud->KData->p.resize(sizeVector); + infile.read((char *)&this->myPointCloud->KData->p.front(), this->myPointCloud->KData->p.size()*sizeof(jderobot::RGBPoint)); + + } + private: + ReplayerPointCloud* myPointCloud; + }; + + typedef IceUtil::Handle SyncTaskPtr; + SyncTaskPtr syncTask; + std::string prefix; + jderobot::pointCloudDataPtr KData; + Ice::PropertiesPtr prop; + std::string dataPath; + + public: + SyncControllerPtr syncController; + long long int initState; + IceUtil::Mutex dataMutex; + + + }; + +} +#endif //JDEROBOT_REPLAYERPOINTCLOUD_H + diff --git a/src/tools/replayer2/interfaces/ReplayerPose3D.cpp b/src/tools/replayer2/interfaces/ReplayerPose3D.cpp new file mode 100644 index 000000000..bfdf51ced --- /dev/null +++ b/src/tools/replayer2/interfaces/ReplayerPose3D.cpp @@ -0,0 +1,28 @@ +// +// Created by frivas on 30/04/17. +// + +#include "ReplayerPose3D.h" + + +namespace replayer { + ReplayerPose3D::ReplayerPose3D(std::string &propertyPrefix, Ice::CommunicatorPtr ic, + replayer::SyncControllerPtr syncController, long long int initStateIN) : + prefix(propertyPrefix), + pose3dData(new jderobot::Pose3DData()), + syncController(syncController) { + this->prop = ic->getProperties(); + this->dataPath = prop->getProperty(propertyPrefix + "Dir"); + this->initState = initStateIN; + v = new Pose3DEncodersSyncTask(this, this->dataPath); + v->start(); + } + + jderobot::Pose3DDataPtr ReplayerPose3D::getPose3DData(const Ice::Current &) { + this->dataMutex.lock(); + jderobot::Pose3DDataPtr localData(pose3dData); + this->dataMutex.unlock(); + return localData; + }; + +} \ No newline at end of file diff --git a/src/tools/replayer2/interfaces/ReplayerPose3D.h b/src/tools/replayer2/interfaces/ReplayerPose3D.h new file mode 100644 index 000000000..8d92216e3 --- /dev/null +++ b/src/tools/replayer2/interfaces/ReplayerPose3D.h @@ -0,0 +1,97 @@ +// +// Created by frivas on 30/04/17. +// + +#ifndef JDEROBOT_REPLAYERPOSE3D_H +#define JDEROBOT_REPLAYERPOSE3D_H + + +#include +#include "SyncTask.h" +#include +#include +#include + +namespace replayer { + class ReplayerPose3D : virtual public jderobot::Pose3D { + public: + ReplayerPose3D(std::string &propertyPrefix, Ice::CommunicatorPtr ic, SyncControllerPtr syncController,long long int initStateIN); + + virtual jderobot::Pose3DDataPtr getPose3DData(const Ice::Current &); + + virtual Ice::Int setPose3DData(const jderobot::Pose3DDataPtr &, const Ice::Current &) { + return 0; + }; + + private: + + + class Pose3DEncodersSyncTask : public SyncTask { + public: + Pose3DEncodersSyncTask(ReplayerPose3D *pose3D, std::string pathIn) : + SyncTask(pathIn, "pose3dData.jde", pose3D), + myPose3d(pose3D) { + 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->myPose3d->tempData, sizeof(pose3d)); + //hago la copia al interfaz.... + this->myPose3d->pose3dData->x = this->myPose3d->tempData.x; + this->myPose3d->pose3dData->y = this->myPose3d->tempData.y; + //std::cout<myPose3d->tempData.y<myPose3d->pose3dData->z = this->myPose3d->tempData.z; + this->myPose3d->pose3dData->h = this->myPose3d->tempData.h; + this->myPose3d->pose3dData->q0 = this->myPose3d->tempData.q0; + this->myPose3d->pose3dData->q1 = this->myPose3d->tempData.q1; + this->myPose3d->pose3dData->q2 = this->myPose3d->tempData.q2; + this->myPose3d->pose3dData->q3 = this->myPose3d->tempData.q3; + + } + + private: + ReplayerPose3D *myPose3d; + }; + + struct pose3d { + float x; + float y; + float z; + float h; + float q0; + float q1; + float q2; + float q3; + }; + + typedef IceUtil::Handle SyncTaskPtr; + SyncTaskPtr v; + std::string prefix; + jderobot::Pose3DDataPtr pose3dData; + Ice::PropertiesPtr prop; + std::string dataPath; + pose3d tempData; + + public: + SyncControllerPtr syncController; + long long int initState; + IceUtil::Mutex dataMutex; + + + }; + +} + +#endif //JDEROBOT_REPLAYERPOSE3D_H diff --git a/src/tools/replayer2/interfaces/ReplayerPose3DEncoders.cpp b/src/tools/replayer2/interfaces/ReplayerPose3DEncoders.cpp new file mode 100644 index 000000000..07c1c6084 --- /dev/null +++ b/src/tools/replayer2/interfaces/ReplayerPose3DEncoders.cpp @@ -0,0 +1,28 @@ +// +// Created by frivas on 30/04/17. +// + +#include "ReplayerPose3DEncoders.h" + +namespace replayer { + ReplayerPose3DEncoders::ReplayerPose3DEncoders(std::string &propertyPrefix, Ice::CommunicatorPtr ic, + replayer::SyncControllerPtr syncController, + long long int initStateIN) : + prefix(propertyPrefix), + encData(new jderobot::Pose3DEncodersData()), + syncController(syncController) { + this->prop = ic->getProperties(); + this->dataPath = prop->getProperty(propertyPrefix + "Dir"); + this->initState = initStateIN; + v = new Pose3DEncodersSyncTask(this, this->dataPath); + v->start(); + } + + jderobot::Pose3DEncodersDataPtr ReplayerPose3DEncoders::getPose3DEncodersData(const Ice::Current &) { + this->dataMutex.lock(); + jderobot::Pose3DEncodersDataPtr localData(encData); + this->dataMutex.unlock(); + return localData; + }; + +} \ No newline at end of file diff --git a/src/tools/replayer2/interfaces/ReplayerPose3DEncoders.h b/src/tools/replayer2/interfaces/ReplayerPose3DEncoders.h new file mode 100644 index 000000000..115c08b5f --- /dev/null +++ b/src/tools/replayer2/interfaces/ReplayerPose3DEncoders.h @@ -0,0 +1,98 @@ +// +// Created by frivas on 30/04/17. +// + +#ifndef JDEROBOT_REPLAYERPOSE3DENCODERS_H +#define JDEROBOT_REPLAYERPOSE3DENCODERS_H + +#include +#include "SyncTask.h" +#include +#include +#include +#include + +namespace replayer { + class ReplayerPose3DEncoders : virtual public jderobot::Pose3DEncoders { + public: + ReplayerPose3DEncoders(std::string &propertyPrefix, Ice::CommunicatorPtr ic, + replayer::SyncControllerPtr syncController, long long int initStateIN); + + virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData(const Ice::Current &); + + private: + + + + class Pose3DEncodersSyncTask : public SyncTask{ + public: + Pose3DEncodersSyncTask(ReplayerPose3DEncoders* pose3DEncoders, std::string pathIn): + SyncTask(pathIn,"pose3dencoderData.jde",pose3DEncoders), + myPose3DEncoders(pose3DEncoders) + { + 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->myPose3DEncoders->tempData, sizeof(pose3dencoders)); + this->myPose3DEncoders->encData->x = this->myPose3DEncoders->tempData.x; + this->myPose3DEncoders->encData->y = this->myPose3DEncoders->tempData.y; + this->myPose3DEncoders->encData->z = this->myPose3DEncoders->tempData.z; + this->myPose3DEncoders->encData->pan = this->myPose3DEncoders->tempData.pan; + this->myPose3DEncoders->encData->tilt = this->myPose3DEncoders->tempData.tilt; + this->myPose3DEncoders->encData->roll = this->myPose3DEncoders->tempData.roll; + this->myPose3DEncoders->encData->maxPan = this->myPose3DEncoders->tempData.maxPan; + this->myPose3DEncoders->encData->maxTilt = this->myPose3DEncoders->tempData.maxTilt; + this->myPose3DEncoders->encData->minPan = this->myPose3DEncoders->tempData.minPan; + this->myPose3DEncoders->encData->minTilt = this->myPose3DEncoders->tempData.minTilt; + + } + private: + ReplayerPose3DEncoders* myPose3DEncoders; + }; + + 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 SyncTaskPtr; + SyncTaskPtr v; + std::string prefix; + jderobot::Pose3DEncodersDataPtr encData; + Ice::PropertiesPtr prop; + pose3dencoders tempData; + std::string dataPath; + + + public: + SyncControllerPtr syncController; + long long int initState; + IceUtil::Mutex dataMutex; + + + }; +} + +#endif //JDEROBOT_REPLAYERPOSE3DENCODERS_H diff --git a/src/tools/replayer2/interfaces/SyncTask.cpp b/src/tools/replayer2/interfaces/SyncTask.cpp new file mode 100644 index 000000000..da68b9eb4 --- /dev/null +++ b/src/tools/replayer2/interfaces/SyncTask.cpp @@ -0,0 +1,5 @@ +// +// Created by frivas on 28/04/17. +// + +#include "SyncTask.h" diff --git a/src/tools/replayer2/interfaces/SyncTask.h b/src/tools/replayer2/interfaces/SyncTask.h new file mode 100644 index 000000000..22c4b312c --- /dev/null +++ b/src/tools/replayer2/interfaces/SyncTask.h @@ -0,0 +1,113 @@ +// +// Created by frivas on 28/04/17. +// + +#ifndef JDEROBOT_SYNCTASK_H +#define JDEROBOT_SYNCTASK_H + + +#include +#include +#include + +namespace replayer { + + template + class SyncTask : public IceUtil::Thread { + public: + SyncTask(const std::string& pathIn, const std::string& jdeFilePath,InterfaceType interface) { + this->path = pathIn; + this->jdeFilePath = jdeFilePath; + this->onPause = false; + this->interface=interface; + } + + ~SyncTask() { + } + + + virtual void generateData()=0; + + virtual void run() { + std::string fileName(this->path + this->jdeFilePath); + std::ifstream myfile(fileName.c_str()); + if (!myfile.is_open()) + LOG(ERROR) << "Error while trying to open: " + fileName; + while (this->isAlive()) { + while (myfile.good()) { + bool playing = this->interface->syncController->getPlay(); + + this->onPause = !playing; + while (!playing) { + playing = this->interface->syncController->getPlay(); + long long int pauseStatus = this->interface->syncController->getSyncTime(); + if (pauseStatus != this->interface->initState) { + this->interface->initState = pauseStatus; + break; + } + usleep(10000); + continue; + } + + if (this->onPause) { + this->interface->initState = this->interface->syncController->getSyncTime(); + myfile.close(); + myfile.open(fileName.c_str()); + } + + getline(myfile, lineData); + std::istringstream sTemp(lineData); + long long int relative; + sTemp >> relative; + IceUtil::Time pretime = IceUtil::Time::now(); + long long int checkState = (pretime.toMicroSeconds()) / 1000; + + + while ((((relative) - (checkState - this->interface->initState)) < 0) && (myfile.good())) { + getline(myfile, lineData); + std::stringstream ssTemp(lineData); + std::string sTemp; + std::getline(ssTemp, sTemp, ' '); + std::istringstream usTemp(sTemp); + usTemp >> relative; + } + if (!myfile.good()) { + if (this->onPause) + continue; + else + break; + } + + + + IceUtil::Time a = IceUtil::Time::now(); + long long int actualState = (a.toMicroSeconds()) / 1000; + if ((actualState - this->interface->initState) < relative) { + usleep(((relative) - (actualState - this->interface->initState)) * 1000); + } + this->interface->dataMutex.lock(); + generateData(); + this->interface->dataMutex.unlock(); + + } + myfile.close(); + this->interface->initState = this->interface->syncController->wait(); + myfile.open(fileName.c_str()); + + } + + } + + private: + std::string jdeFilePath; + bool onPause; + protected: + std::string lineData; + std::string path; + InterfaceType interface; + + + }; + +} +#endif //JDEROBOT_SYNCTASK_H diff --git a/src/tools/replayer2/replayer.cfg b/src/tools/replayer2/replayer.cfg index 2fa6d30bc..5aae9e91c 100644 --- a/src/tools/replayer2/replayer.cfg +++ b/src/tools/replayer2/replayer.cfg @@ -10,18 +10,18 @@ NamingService.Enabled=0 NamingService.Proxy=NamingServiceJdeRobot:default -h 0.0.0.0 -p 10000 #without registry -Replayer.Endpoints=default -h localhost -p 9999 +Replayer.Endpoints=default -h localhost -p 9998 -Replayer.nCameras=0 +Replayer.nCameras=2 Replayer.nPointClouds=0 Replayer.nPose3dEncoders=0 -Replayer.nPose3d=1 +Replayer.nPose3d=0 Replayer.nEncoders=0 Replayer.nLasers=0 Replayer.replayControl.Active=1 Replayer.replayControl.Name=replayControllerA Replayer.startPlaying=1 -Replayer.repeat=0 +Replayer.repeat=1 diff --git a/src/tools/replayer2/replayer.cpp b/src/tools/replayer2/replayer.cpp index 1c6d8e434..399f48e6c 100644 --- a/src/tools/replayer2/replayer.cpp +++ b/src/tools/replayer2/replayer.cpp @@ -28,12 +28,8 @@ #include #include #include - // Library includes -#include #include -#include - #include #include #include @@ -43,1133 +39,19 @@ #include #include #include "replayergui.h" -#include "control.h" #include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include "easyiceconfig/EasyIce.h" bool componentAlive; - -namespace replayer { - - - - control* controller; - - class CameraI: virtual public jderobot::Camera { - public: - CameraI(std::string& propertyPrefix, Ice::CommunicatorPtr ic, long long int initStateIN): prefix(propertyPrefix) { - LOG(INFO)<< "Creating " + propertyPrefix; - - 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"); - this->format = prop->getProperty(propertyPrefix+"Format"); - LOG(INFO)<< "PATH " + this->dataPath ; - LOG(INFO)<< "FORMAT: " + this->fileFormat ; - - - 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 (cameraDescription->name); - - } - - virtual ~CameraI() { - LOG(INFO)<< "Stopping and joining thread for camera: " + cameraDescription->name; - } - - virtual jderobot::ImageDescriptionPtr getImageDescription(const Ice::Current& c){ - return imageDescription; - } - - virtual jderobot::CameraDescriptionPtr getCameraDescription(const Ice::Current& c){ - return cameraDescription; - } - - virtual Ice::Int setCameraDescription(const jderobot::CameraDescriptionPtr &description, const Ice::Current& c) { - return 0; - } - - virtual void getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr& cb, const std::string& format, const Ice::Current& c){ - replyTask->pushJob(cb); - } - - virtual jderobot::ImageFormat getImageFormat(const Ice::Current& c) - { - jderobot::ImageFormat formats; - - formats.push_back(colorspaces::ImageRGB8::FORMAT_RGB8.get()->name); - - return formats; - } - - virtual std::string startCameraStreaming(const Ice::Current&){ - LOG(INFO)<< "Should be made anything to start camera streaming: " + cameraDescription->name; - return std::string(""); - } - - virtual void stopCameraStreaming(const Ice::Current&) { - LOG(INFO)<< "Should be made anything to stop camera streaming: " + cameraDescription->name; - } - - virtual void reset(const Ice::Current&) - { - } - - /*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()) - LOG(ERROR)<< "Error while trying to open: " + fileName; - while(this->isAlive()){ - while ( myfile.good() ){ - bool playing=controller->getPlay(); - - this->onPause=!playing; - while (!playing){ - playing=controller->getPlay(); - long long int pauseStatus= controller->getSyncTime(); - 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->getSyncTime(); - 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); - } - - - 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){ - 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 = this->mycamera->format; - - 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 requests; - }; - - typedef IceUtil::Handle 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 SyncTaskPtr; - SyncTaskPtr syncTask; - long long int initState; - std::string fileFormat; - std::string dataPath; - std::string format; - - }; // 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()) - LOG(ERROR) <<"-----Error while trying to open: " + fileName; - while(this->isAlive()){ - while ( myfile.good() ){ - bool playing=controller->getPlay(); - this->onPause=!playing; - while (!playing){ - playing=controller->getPlay(); - long long int pauseStatus= controller->getSyncTime(); - if (pauseStatus != this->myPointCloud->initState){ - this->myPointCloud->initState=pauseStatus; - break; - } - //check if w - usleep(10000); - continue; - } - - if (this->onPause){ - this->myPointCloud->initState=controller->getSyncTime(); - 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); - } - - 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 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()) - LOG(ERROR) <<"-----Error while trying to open: " + fileName; - while(this->isAlive()){ - while ( myfile.good() ){ - bool playing=controller->getPlay(); - this->onPause=!playing; - while (!playing){ - playing=controller->getPlay(); - long long int pauseStatus= controller->getSyncTime(); - if (pauseStatus != this->myLaser->initState){ - this->myLaser->initState=pauseStatus; - break; - } - //check if w - usleep(10000); - continue; - } - - if (this->onPause){ - this->myLaser->initState=controller->getSyncTime(); - myfile.close(); - myfile.open(fileName.c_str()); - } - - - getline (myfile,line); - std::istringstream sTemp(line); - long long int relative; - sTemp >> relative; - int sizeVector; - sTemp >> sizeVector; - int maxRange; - sTemp >> maxRange; - int minRange; - sTemp >> minRange; - float maxAngle; - sTemp >> maxAngle; - float minAngle; - sTemp >> minAngle; - - - //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; - - sTemp >> maxRange; - sTemp >> minRange; - sTemp >> maxAngle; - sTemp >> minAngle; - - } - 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); - } - this->myLaser->m.lock(); - this->myLaser->KData->distanceData.resize(sizeVector); - this->myLaser->KData->minAngle = minAngle; - this->myLaser->KData->maxAngle = maxAngle; - this->myLaser->KData->minRange = minRange; - this->myLaser->KData->maxRange = maxRange; - 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 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()) - LOG(ERROR) <<"-----Error while trying to open: " + fileName; - while(this->isAlive()){ - while ( myfile.good() ){ - bool playing=controller->getPlay(); - this->onPause=!playing; - while (!playing){ - playing=controller->getPlay(); - long long int pauseStatus= controller->getSyncTime(); - if (pauseStatus != this->myPose3d->initState){ - this->myPose3d->initState=pauseStatus; - break; - } - //check if w - usleep(10000); - continue; - } - - if (this->onPause){ - this->myPose3d->initState=controller->getSyncTime(); - 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->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); - } - - 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 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 Pose3DI - class Pose3DI: virtual public jderobot::Pose3D{ - public: - Pose3DI (std::string& propertyPrefix, Ice::CommunicatorPtr ic, long long int initStateIN):prefix(propertyPrefix),pose3dData(new jderobot::Pose3DData()) { - this->prop = ic->getProperties(); - this->dataPath=prop->getProperty(propertyPrefix+"Dir"); - this->initState=initStateIN; - v = new SyncTask(this,this->dataPath); - v->start(); - } - - virtual jderobot::Pose3DDataPtr getPose3DData(const Ice::Current&){ - //check si los dos son iguales - this->m.lock(); - jderobot::Pose3DDataPtr localData(pose3dData); - this->m.unlock(); - return localData; - }; - - virtual Ice::Int setPose3DData(const jderobot::Pose3DDataPtr&, const Ice::Current&){ - //check si los dos son iguales - this->m.lock(); - //jderobot::Pose3DDataPtr localData(pose3dData); - //jderobot::Pose3DDataPtr pose3dData; - this->m.unlock(); - // - }; - - private: - - class SyncTask :public IceUtil::Thread{ - public: - SyncTask(Pose3DI* pose3d, std::string pathIn){ - this->myPose3d=pose3d; - this->path=pathIn; - this->initiated=false; - this->onPause=false; - } - - virtual void run(){ - std::string line; - std::string fileName(this->path + "pose3dData.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->getSyncTime(); - if (pauseStatus != this->myPose3d->initState){ - this->myPose3d->initState=pauseStatus; - break; - } - //check if w - usleep(10000); - continue; - } - - if (this->onPause){ - this->myPose3d->initState=controller->getSyncTime(); - 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->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(pose3d)); - //hago la copia al interfaz.... - this->myPose3d->pose3dData->x=this->myPose3d->tempData.x; - this->myPose3d->pose3dData->y=this->myPose3d->tempData.y; - //std::cout<myPose3d->tempData.y<myPose3d->pose3dData->z=this->myPose3d->tempData.z; - this->myPose3d->pose3dData->h=this->myPose3d->tempData.h; - this->myPose3d->pose3dData->q0=this->myPose3d->tempData.q0; - this->myPose3d->pose3dData->q1=this->myPose3d->tempData.q1; - this->myPose3d->pose3dData->q2=this->myPose3d->tempData.q2; - this->myPose3d->pose3dData->q3=this->myPose3d->tempData.q3; - this->myPose3d->m.unlock(); - } - myfile.close(); - //control.controlMutex.lock(); - this->myPose3d->initState=controller->wait(); - myfile.open(fileName.c_str()); - - } - - } - Pose3DI* myPose3d; - std::string path; - bool initiated; - bool onPause; - - }; - - struct pose3d{ - float x; - float y; - float z; - float h; - float q0; - float q1; - float q2; - float q3; - }; - - typedef IceUtil::Handle SyncTaskPtr; - SyncTaskPtr v; - std::string prefix; - jderobot::Pose3DDataPtr pose3dData; - Ice::PropertiesPtr prop; - std::string dataPath; - IceUtil::Mutex m; - long long int initState; - pose3d 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()) - LOG(ERROR) <<"-----Error while trying to open: " + fileName; - while(this->isAlive()){ - while ( myfile.good() ){ - bool playing=controller->getPlay(); - this->onPause=!playing; - while (!playing){ - playing=controller->getPlay(); - long long int pauseStatus= controller->getSyncTime(); - if (pauseStatus != this->myEncoder->initState){ - this->myEncoder->initState=pauseStatus; - break; - } - //check if w - usleep(10000); - continue; - } - - if (this->onPause){ - this->myEncoder->initState=controller->getSyncTime(); - 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); - } - - 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 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 controller->getRelativeTime(); - } - virtual bool goTo(Ice::Long seek, const Ice::Current&){ - //set the current position to seek - return 0; - } - virtual jderobot::ReplayerStatus getStatus(const Ice::Current&){ - return controller->getstatus(); - } - - - private: - - }; - - - -} //namespace - bool killed; Ice::CommunicatorPtr ic; jderobot::nsPtr ns; @@ -1210,6 +92,7 @@ int main(int argc, char** argv) { sigaction(SIGINT, &sigIntHandler, NULL); + replayer::SyncControllerPtr syncControllerPtr; try{ ic = EasyIce::initialize(argc, argv); @@ -1218,8 +101,8 @@ int main(int argc, char** argv) { bool startPlaying = (bool)prop->getPropertyAsIntWithDefault(componentPrefix+".startPlaying",1); bool repeat = (bool)prop->getPropertyAsIntWithDefault(componentPrefix+".repeat",1); - replayer::controller= new replayer::control(initState,startPlaying,repeat); - +// replayer::controller= new replayer::SyncController(initState,startPlaying,repeat); + syncControllerPtr=replayer::SyncControllerPtr(new replayer::SyncController(initState,startPlaying,repeat)); // Naming Service int nsActive = prop->getPropertyAsIntWithDefault("NamingService.Enabled", 0); if (nsActive) @@ -1248,7 +131,7 @@ int main(int argc, char** argv) { cameraName = "camera" + objId; prop->setProperty(objPrefix + "Name",cameraName);//set the value } - cameras[i] = new replayer::CameraI(objPrefix,ic,initState); + cameras[i] = new replayer::ReplayerCamera(objPrefix,ic,syncControllerPtr,initState); adapter->add(cameras[i], ic->stringToIdentity(cameraName)); if (ns) ns->bind(cameraName, Endpoints, "::jderobot::Camera"); @@ -1272,7 +155,7 @@ int main(int argc, char** argv) { - Ice::ObjectPtr object= new replayer::PointCloudI(objPrefix,ic,initState); + Ice::ObjectPtr object= new replayer::ReplayerPointCloud(objPrefix,ic,syncControllerPtr,initState); @@ -1295,13 +178,7 @@ int main(int argc, char** argv) { Name = "laser" + objId; prop->setProperty(objPrefix + "Name",Name);//set the value } - - - - Ice::ObjectPtr object= new replayer::LaserI(objPrefix,ic,initState); - - - + Ice::ObjectPtr object= new replayer::ReplayerLaser(objPrefix,ic,syncControllerPtr,initState); adapter->add(object, ic->stringToIdentity(Name)); if (ns) ns->bind(Name, Endpoints, object->ice_staticId()); @@ -1324,7 +201,7 @@ int main(int argc, char** argv) { prop->setProperty(objPrefix + "Name",Name);//set the value } - Ice::ObjectPtr object= new replayer::Pose3DEncodersI(objPrefix,ic,initState); + Ice::ObjectPtr object= new replayer::ReplayerPose3DEncoders(objPrefix,ic,syncControllerPtr,initState); @@ -1343,13 +220,13 @@ int main(int argc, char** argv) { std::string objPrefix(componentPrefix+".pose3d." + objId + "."); std::string Name = prop->getProperty(objPrefix + "Name"); LOG(INFO) <<"Creating pose3d " + Name ; - + if (Name.size() == 0) { //no name specified, we create one using the index Name = "pose3d" + objId; prop->setProperty(objPrefix + "Name",Name);//set the value } - Ice::ObjectPtr object= new replayer::Pose3DI(objPrefix,ic,initState); + Ice::ObjectPtr object= new replayer::ReplayerPose3D(objPrefix,ic,syncControllerPtr,initState); @@ -1374,7 +251,7 @@ int main(int argc, char** argv) { prop->setProperty(objPrefix + "Name",Name);//set the value } - Ice::ObjectPtr object= new replayer::EncodersI(objPrefix,ic,initState); + Ice::ObjectPtr object= new replayer::ReplayerEncoders(objPrefix,ic,syncControllerPtr,initState); @@ -1387,7 +264,7 @@ int main(int argc, char** argv) { int controllerActive = prop->getPropertyAsIntWithDefault(componentPrefix+".replayControl.Active",0); if (controllerActive){ - Ice::ObjectPtr rc= new replayer::replayControllerI(); + Ice::ObjectPtr rc= new replayer::ReplayControllerInterface(syncControllerPtr); std::string objPrefix(componentPrefix+".replayControl."); std::string Name = prop->getProperty(objPrefix + "Name"); @@ -1397,14 +274,14 @@ int main(int argc, char** argv) { } adapter->activate(); - replayer::controller->setProcesses(nProcs); + syncControllerPtr->setProcesses(nProcs); //replayer::replayergui* gui = new replayer::replayergui(replayer::controller); - //do it beeter - while (1){ + + while (true){ //gui->update(); - replayer::controller->checkStatus(); + syncControllerPtr->checkStatus(); usleep(1000000); } diff --git a/src/tools/replayer2/replayergui.cpp b/src/tools/replayer2/replayergui.cpp index 69616ce8a..a105f0019 100644 --- a/src/tools/replayer2/replayergui.cpp +++ b/src/tools/replayer2/replayergui.cpp @@ -12,7 +12,7 @@ namespace replayer { const std::string gladepath = resourcelocator::findGladeFile("replayergui.glade"); -replayergui::replayergui(control* c): gtkmain(0,0) { +replayergui::replayergui(SyncController* c): gtkmain(0,0) { // TODO Auto-generated constructor stub this->controller=c; diff --git a/src/tools/replayer2/replayergui.h b/src/tools/replayer2/replayergui.h index d1cadd936..c984381fc 100644 --- a/src/tools/replayer2/replayergui.h +++ b/src/tools/replayer2/replayergui.h @@ -28,14 +28,14 @@ #include #include #include -#include "control.h" +#include "utils/SyncController.h" #include namespace replayer { class replayergui { public: - replayergui(control *c); + replayergui(SyncController *c); virtual ~replayergui(); void update(); @@ -48,7 +48,7 @@ class replayergui { Gtk::Button* w_step; Gtk::ToggleButton* w_repeat; - control *controller; + SyncController *controller; //gtk functions void on_clicked_stop(); diff --git a/src/tools/replayer2/utils/ReplayControllerInterface.cpp b/src/tools/replayer2/utils/ReplayControllerInterface.cpp new file mode 100644 index 000000000..444d5652d --- /dev/null +++ b/src/tools/replayer2/utils/ReplayControllerInterface.cpp @@ -0,0 +1,5 @@ +// +// Created by frivas on 30/04/17. +// + +#include "ReplayControllerInterface.h" diff --git a/src/tools/replayer2/utils/ReplayControllerInterface.h b/src/tools/replayer2/utils/ReplayControllerInterface.h new file mode 100644 index 000000000..f43bcb40c --- /dev/null +++ b/src/tools/replayer2/utils/ReplayControllerInterface.h @@ -0,0 +1,57 @@ +// +// Created by frivas on 30/04/17. +// + +#ifndef JDEROBOT_ReplayControllerInterfaceNTERFACE_H +#define JDEROBOT_ReplayControllerInterfaceNTERFACE_H + +#include +#include "SyncController.h" + +namespace replayer{ + + class ReplayControllerInterface: virtual public jderobot::replayControl{ + public: + ReplayControllerInterface(replayer::SyncControllerPtr syncControllerPtr): + controller(syncControllerPtr) + { + + } + ~ReplayControllerInterface(){ + + } + + 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 controller->getRelativeTime(); + } + virtual bool goTo(Ice::Long seek, const Ice::Current&){ + //set the current position to seek + return 0; + } + virtual jderobot::ReplayerStatus getStatus(const Ice::Current&){ + return controller->getstatus(); + } + + + private: + replayer::SyncControllerPtr controller; + }; + +} + +#endif //JDEROBOT_ReplayControllerInterfaceNTERFACE_H diff --git a/src/tools/replayer2/control.cpp b/src/tools/replayer2/utils/SyncController.cpp similarity index 81% rename from src/tools/replayer2/control.cpp rename to src/tools/replayer2/utils/SyncController.cpp index edeea20bb..e5ebe1ad7 100644 --- a/src/tools/replayer2/control.cpp +++ b/src/tools/replayer2/utils/SyncController.cpp @@ -5,11 +5,11 @@ * Author: frivas */ -#include "control.h" +#include "SyncController.h" namespace replayer { -control::control(long long int initState, bool play_in, bool repeat_in) { +SyncController::SyncController(long long int initState, bool play_in, bool repeat_in) { this->nProcFinished=0; this->play=play_in; this->paused=false; @@ -25,19 +25,19 @@ control::control(long long int initState, bool play_in, bool repeat_in) { } -control::~control() { +SyncController::~SyncController() { // TODO Auto-generated destructor stub } -void control::lock(){ +void SyncController::lock(){ this->controlMutex.lock(); } -void control::unlock(){ +void SyncController::unlock(){ this->controlMutex.unlock(); } -bool control::getPlay(){ +bool SyncController::getPlay(){ bool localPlay; this->controlMutex.lock(); localPlay=this->play; @@ -45,7 +45,7 @@ bool control::getPlay(){ return localPlay; } -long long int control::getSyncTime(){ +long long int SyncController::getSyncTime(){ long long int localTime; this->controlMutex.lock(); localTime=this->newTime; @@ -53,7 +53,7 @@ long long int control::getSyncTime(){ return localTime; } -long long int control::getRelativeTime(){ +long long int SyncController::getRelativeTime(){ long long int localTime; this->controlMutex.lock(); if (this->play) @@ -64,7 +64,7 @@ long long int control::getRelativeTime(){ return localTime; } -long long int control::wait(){ +long long int SyncController::wait(){ long long int localTime; { IceUtil::Mutex::Lock sync(this->controlMutex); @@ -76,7 +76,7 @@ long long int control::wait(){ return localTime; } -void control::checkStatus(){ +void SyncController::checkStatus(){ this->controlMutex.lock(); if (this->nProcFinished != this->nProcess){ } @@ -96,7 +96,7 @@ void control::checkStatus(){ this->controlMutex.unlock(); } -void control::stop(){ +void SyncController::stop(){ this->controlMutex.lock(); this->play=false; this->paused=true; @@ -108,7 +108,7 @@ void control::stop(){ this->controlMutex.unlock(); } -void control::resume(){ +void SyncController::resume(){ this->controlMutex.lock(); this->play=true; this->paused=false; @@ -121,15 +121,15 @@ void control::resume(){ this->controlMutex.unlock(); } -void control::setRepeat(bool active){ +void SyncController::setRepeat(bool active){ this->repeat=active; } -void control::setProcesses(int procs){ +void SyncController::setProcesses(int procs){ this->nProcess=procs; } -void control::setStep(int step){ +void SyncController::setStep(int step){ this->controlMutex.lock(); IceUtil::Time n = IceUtil::Time::now(); long long int nowInt=(n.toMicroSeconds())/1000; @@ -139,7 +139,7 @@ void control::setStep(int step){ } -jderobot::ReplayerStatus control::getstatus(){ +jderobot::ReplayerStatus SyncController::getstatus(){ if (status==jderobot::FINISHED){ } else if (this->paused){ diff --git a/src/tools/replayer2/control.h b/src/tools/replayer2/utils/SyncController.h similarity index 87% rename from src/tools/replayer2/control.h rename to src/tools/replayer2/utils/SyncController.h index 7cadd2ae0..350deed3d 100644 --- a/src/tools/replayer2/control.h +++ b/src/tools/replayer2/utils/SyncController.h @@ -27,14 +27,15 @@ #include #include #include +#include namespace replayer { -class control { +class SyncController { public: - control(long long int initState, bool play_in, bool repeat_in); - virtual ~control(); + SyncController(long long int initState, bool play_in, bool repeat_in); + virtual ~SyncController(); void lock(); void unlock(); bool getPlay(); @@ -52,7 +53,7 @@ class control { private: //controladores de video bool repeat; - //mutex de control de acceso a control + //mutex de SyncController de acceso a SyncController IceUtil::Mutex controlMutex; //condicional utilizado como un semaforo IceUtil::Cond sem; @@ -67,7 +68,7 @@ class control { //reproduccion bool play; //numero de repeticiones - bool nRepetitions; + int nRepetitions; //replayer status jderobot::ReplayerStatus status; //paused @@ -75,5 +76,7 @@ class control { }; + typedef boost::shared_ptr SyncControllerPtr; + } /* namespace replayer */ #endif /* CONTROL_H_ */ diff --git a/src/tools/rgbdManualCalibrator/controller.cpp b/src/tools/rgbdManualCalibrator/controller.cpp index f46499fd9..beb71c50c 100644 --- a/src/tools/rgbdManualCalibrator/controller.cpp +++ b/src/tools/rgbdManualCalibrator/controller.cpp @@ -395,8 +395,6 @@ namespace rgbdManualCalibrator { if (d != 0 ){ float xp,yp,zp,camx,camy,camz; float ux,uy,uz; - float x,y; - float k; float c1x, c1y, c1z; float fx,fy,fz; float fmod; diff --git a/src/tools/rgbdManualCalibrator/drawarea.cpp b/src/tools/rgbdManualCalibrator/drawarea.cpp index 34aa6378e..55b64cd2b 100644 --- a/src/tools/rgbdManualCalibrator/drawarea.cpp +++ b/src/tools/rgbdManualCalibrator/drawarea.cpp @@ -190,6 +190,7 @@ namespace rgbdManualCalibrator { } bool DrawArea::on_expose_event(GdkEventExpose* event) { + return true; } void DrawArea::my_expose_event(){ @@ -249,13 +250,10 @@ namespace rgbdManualCalibrator { } void DrawArea::drawScene() { + int i; - - int i,c,row,j,k; float r,lati,longi,dx,dy,dz; float matColors[4]; - float Xp_sensor, Yp_sensor; - float dpan=0.5,dtilt=0.5; // Absolute Frame of Reference // floor @@ -443,6 +441,7 @@ namespace rgbdManualCalibrator { old_x=x; old_y=y; + return true; } bool DrawArea::on_drawarea_scroll(GdkEventScroll * event) { @@ -471,6 +470,7 @@ namespace rgbdManualCalibrator { this->glcam_pos.Y = this->glcam_pos.Y - vy; this->glcam_pos.Z = this->glcam_pos.Z - vz; } + return true; } int @@ -655,6 +655,7 @@ DrawArea::read_world_file(char* worldfile){ } while(i!=EOF); fclose(myfile); + return 0; } void diff --git a/src/tools/rgbdViewer/drawarea.cpp b/src/tools/rgbdViewer/drawarea.cpp index 06b1017b1..496f6d51d 100644 --- a/src/tools/rgbdViewer/drawarea.cpp +++ b/src/tools/rgbdViewer/drawarea.cpp @@ -253,11 +253,9 @@ namespace rgbdViewer { } void DrawArea::drawScene() { - int i,c,row,j,k; + int i; float r,lati,longi,dx,dy,dz; float matColors[4]; - float Xp_sensor, Yp_sensor; - float dpan=0.5,dtilt=0.5; // Absolute Frame of Reference // floor @@ -659,6 +657,7 @@ DrawArea::read_world_file(char* worldfile){ } while(i!=EOF); fclose(myfile); + return 0; } void diff --git a/src/tools/rgbdViewer/rgbdViewer.cfg b/src/tools/rgbdViewer/rgbdViewer.cfg index a2d0e9693..e743018cf 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cfg +++ b/src/tools/rgbdViewer/rgbdViewer.cfg @@ -1,11 +1,11 @@ rgbdViewer.CameraRGBActive=1 #rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h 193.147.14.20 -p 9998 rgbdViewer.CameraRGB.Fps=10 -rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h localhost -p 9999 +rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h localhost -p 9998 rgbdViewer.CameraDEPTHActive=1 rgbdViewer.CameraDEPTH.Fps=10 #rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h 193.147.14.20 -p 9998 -rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h localhost -p 9999 +rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h localhost -p 9998 rgbdViewer.pointCloudActive=0 #rgbdViewer.pointCloud.Proxy=pointcloud1:tcp -h 193.147.14.20 -p 9998 rgbdViewer.pointCloud.Fps=10 diff --git a/src/tools/rgbdViewer/rgbdViewer.cpp b/src/tools/rgbdViewer/rgbdViewer.cpp index 71840a93a..4d1c24d92 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cpp +++ b/src/tools/rgbdViewer/rgbdViewer.cpp @@ -103,7 +103,7 @@ void *gui_thread(void* arg){ */ int main(int argc, char** argv){ - int status,i; + int i; Ice::CommunicatorPtr ic; int n_components=0; pthread_t threads[MAX_COMPONENTS]; @@ -116,8 +116,7 @@ int main(int argc, char** argv){ bool rgbCamSelected=false; bool depthCamSelected=false; bool pointCloudSelected=false; - int globalWidth; - int globalHeight; + @@ -223,5 +222,5 @@ int main(int argc, char** argv){ std::cout << "final" << std::endl; if (ic) ic->destroy(); - return status; + return 0; } diff --git a/src/tools/rgbdViewer/rgbdViewergui.cpp b/src/tools/rgbdViewer/rgbdViewergui.cpp index 3f670e0aa..324ac18c4 100644 --- a/src/tools/rgbdViewer/rgbdViewergui.cpp +++ b/src/tools/rgbdViewer/rgbdViewergui.cpp @@ -337,8 +337,6 @@ bool rgbdViewergui::on_clicked_event_rgb(GdkEventButton* event) { //d=d*10; float xp,yp,zp,camx,camy,camz; float ux,uy,uz; - float x,y; - float k; float c1x, c1y, c1z; float fx,fy,fz; float fmod;