[Jderobot-admin] jderobot-r981 - trunk/src/stable/components/openniServer
rocapal en jderobot.org
rocapal en jderobot.org
Jue Sep 12 10:17:16 CEST 2013
Author: rocapal
Date: 2013-09-12 10:16:15 +0200 (Thu, 12 Sep 2013)
New Revision: 981
Modified:
trunk/src/stable/components/openniServer/CMakeLists.txt
trunk/src/stable/components/openniServer/myprogeo.cpp
trunk/src/stable/components/openniServer/myprogeo.h
trunk/src/stable/components/openniServer/openniServer.cfg
trunk/src/stable/components/openniServer/openniServer.cpp
Log:
#34 fixed bug
Modified: trunk/src/stable/components/openniServer/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/openniServer/CMakeLists.txt 2013-09-11 15:04:07 UTC (rev 980)
+++ trunk/src/stable/components/openniServer/CMakeLists.txt 2013-09-12 08:16:15 UTC (rev 981)
@@ -49,6 +49,7 @@
${INTERFACES_CPP_DIR}/jderobot/libJderobotInterfaces.so
${OPENNI2_LIB}/libOpenNI2.so
${NITE2_LIB}/libNiTE2.so
+ ${Boost_LIBRARIES}
)
else()
TARGET_LINK_LIBRARIES(openniServer
@@ -61,6 +62,7 @@
${LIBS_DIR}/colorspaces/libcolorspacesmm.so
${INTERFACES_CPP_DIR}/jderobot/libJderobotInterfaces.so
${OPENNI2_LIB}/libOpenNI2.so
+ ${Boost_LIBRARIES}
)
endif()
ENDIF()
Modified: trunk/src/stable/components/openniServer/myprogeo.cpp
===================================================================
--- trunk/src/stable/components/openniServer/myprogeo.cpp 2013-09-11 15:04:07 UTC (rev 980)
+++ trunk/src/stable/components/openniServer/myprogeo.cpp 2013-09-12 08:16:15 UTC (rev 981)
@@ -24,220 +24,227 @@
namespace openniServer {
-myprogeo::myprogeo(){
- std::cout << "CREADO" << std::endl;
+myprogeo::myprogeo() {
+ std::cout << "CREADO" << std::endl;
}
-myprogeo::~myprogeo(){
+myprogeo::~myprogeo() {
}
/* gets the calibration of the camera from a file */
int myprogeo::load_cam_line(FILE *myfile,int cam)
-{
- char word1[MAX_BUFFER],word2[MAX_BUFFER];
- int i=0;
- char buffer_file[MAX_BUFFER];
- double roll;
+{
+ char word1[MAX_BUFFER],word2[MAX_BUFFER];
+ int i=0;
+ char buffer_file[MAX_BUFFER];
+ double roll;
- buffer_file[0]=fgetc(myfile);
- if (feof(myfile)) return EOF;
- if (buffer_file[0]==(char)255) return EOF;
- if (buffer_file[0]=='#') {while(fgetc(myfile)!='\n'); return 0;}
- if (buffer_file[0]==' ') {while(buffer_file[0]==' ') buffer_file[0]=fgetc(myfile);}
- if (buffer_file[0]=='\t') {while(buffer_file[0]=='\t') buffer_file[0]=fgetc(myfile);}
-
- //Captures a line and then we will process it with sscanf checking that the last character is \n. We can't doit with fscanf because this function does not difference \n from blank space.
- while((buffer_file[i]!='\n') &&
- (buffer_file[i] != (char)255) &&
- (i<MAX_BUFFER-1) ) {
- buffer_file[++i]=fgetc(myfile);
- }
-
- if (i >= MAX_BUFFER-1) {
- printf("%s...\n", buffer_file);
- printf ("Line too long in config file!\n");
- return -1;
- }
- buffer_file[++i]='\0';
-
-
- if (sscanf(buffer_file,"%s",word1)!=1) return 0;
- // return EOF; empty line
- else {
- if (strcmp(word1,"positionX")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].position.X=(float)atof(word2);
+ buffer_file[0]=fgetc(myfile);
+ if (feof(myfile)) return EOF;
+ if (buffer_file[0]==(char)255) return EOF;
+ if (buffer_file[0]=='#') {
+ while(fgetc(myfile)!='\n');
+ return 0;
}
- else if (strcmp(word1,"positionY")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].position.Y=(float)atof(word2);
+ if (buffer_file[0]==' ') {
+ while(buffer_file[0]==' ') buffer_file[0]=fgetc(myfile);
}
- else if (strcmp(word1,"positionZ")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].position.Z=(float)atof(word2);
+ if (buffer_file[0]=='\t') {
+ while(buffer_file[0]=='\t') buffer_file[0]=fgetc(myfile);
}
- else if (strcmp(word1,"positionH")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].position.H=(float)atof(word2);
+
+ //Captures a line and then we will process it with sscanf checking that the last character is \n. We can't doit with fscanf because this function does not difference \n from blank space.
+ while((buffer_file[i]!='\n') &&
+ (buffer_file[i] != (char)255) &&
+ (i<MAX_BUFFER-1) ) {
+ buffer_file[++i]=fgetc(myfile);
}
- else if (strcmp(word1,"FOApositionX")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].foa.X=(float)atof(word2);
+
+ if (i >= MAX_BUFFER-1) {
+ printf("%s...\n", buffer_file);
+ printf ("Line too long in config file!\n");
+ return -1;
}
- else if (strcmp(word1,"FOApositionY")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].foa.Y=(float)atof(word2);
+ buffer_file[++i]='\0';
+
+
+ if (sscanf(buffer_file,"%s",word1)!=1) return 0;
+ // return EOF; empty line
+ else {
+ if (strcmp(word1,"positionX")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].position.X=(float)atof(word2);
+ }
+ else if (strcmp(word1,"positionY")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].position.Y=(float)atof(word2);
+ }
+ else if (strcmp(word1,"positionZ")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].position.Z=(float)atof(word2);
+ }
+ else if (strcmp(word1,"positionH")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].position.H=(float)atof(word2);
+ }
+ else if (strcmp(word1,"FOApositionX")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].foa.X=(float)atof(word2);
+ }
+ else if (strcmp(word1,"FOApositionY")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].foa.Y=(float)atof(word2);
+ }
+ else if (strcmp(word1,"FOApositionZ")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].foa.Z=(float)atof(word2);
+ }
+ else if (strcmp(word1,"FOApositionH")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].foa.H=(float)atof(word2);
+ }
+ else if (strcmp(word1,"roll")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].roll=(float)atof(word2);
+ }
+ else if (strcmp(word1,"f")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].fdistx=(float)atof(word2);
+ cameras[cam].fdisty=(float)atof(word2);
+ }
+ else if (strcmp(word1,"fx")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].fdistx=(float)atof(word2);
+ }
+ else if (strcmp(word1,"fy")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].fdisty=(float)atof(word2);
+ }
+ else if (strcmp(word1,"skew")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].skew=(float)atof(word2);
+ }
+ else if (strcmp(word1,"u0")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].u0=(float)atof(word2);
+ }
+ else if (strcmp(word1,"v0")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].v0=(float)atof(word2);
+ }
+ else if (strcmp(word1,"columns")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].columns=(int)atoi(word2);
+ }
+ else if (strcmp(word1,"rows")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].rows=(int)atoi(word2);
+ }
}
- else if (strcmp(word1,"FOApositionZ")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].foa.Z=(float)atof(word2);
- }
- else if (strcmp(word1,"FOApositionH")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].foa.H=(float)atof(word2);
- }
- else if (strcmp(word1,"roll")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].roll=(float)atof(word2);
- }
- else if (strcmp(word1,"f")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].fdistx=(float)atof(word2);
- cameras[cam].fdisty=(float)atof(word2);
- }
- else if (strcmp(word1,"fx")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].fdistx=(float)atof(word2);
- }
- else if (strcmp(word1,"fy")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].fdisty=(float)atof(word2);
- }
- else if (strcmp(word1,"skew")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].skew=(float)atof(word2);
- }
- else if (strcmp(word1,"u0")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].u0=(float)atof(word2);
- }
- else if (strcmp(word1,"v0")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].v0=(float)atof(word2);
- }
- else if (strcmp(word1,"columns")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].columns=(int)atoi(word2);
- }
- else if (strcmp(word1,"rows")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].rows=(int)atoi(word2);
- }
- }
- return 1;
+ return 1;
}
/* gets the calibration of the camera from a file */
void myprogeo::load_cam(char *fich_in,int cam, int w, int h)
{
- FILE *entrada;
- int i;
- if (strlen(fich_in) ==0 ){
- std::cout << w << ", " << h << std::endl;
- this->cameras[cam].fdistx=515;
- this->cameras[cam].fdisty=515;
- this->cameras[cam].u0=h/2;
- this->cameras[cam].v0=w/2;
- this->cameras[cam].position.X=0;
- this->cameras[cam].position.Y=0;
- this->cameras[cam].position.Z=0;
- this->cameras[cam].foa.X=0;
- this->cameras[cam].foa.Y=1;
- this->cameras[cam].foa.Z=0;
- update_camera_matrix(&cameras[cam]);
+ FILE *entrada;
+ int i;
+ if (strlen(fich_in) ==0 ) {
+ std::cout << w << ", " << h << std::endl;
+ this->cameras[cam].fdistx=515;
+ this->cameras[cam].fdisty=515;
+ this->cameras[cam].u0=h/2;
+ this->cameras[cam].v0=w/2;
+ this->cameras[cam].position.X=0;
+ this->cameras[cam].position.Y=0;
+ this->cameras[cam].position.Z=0;
+ this->cameras[cam].foa.X=0;
+ this->cameras[cam].foa.Y=1;
+ this->cameras[cam].foa.Z=0;
+ update_camera_matrix(&cameras[cam]);
-
- }
- else{
- xmlReader(&(this->cameras[cam]), fich_in);
- update_camera_matrix(&cameras[cam]);
- }
- /*this->cameras[cam].position.H=1;
- this->cameras[cam].foa.H=1;*/
- /*std::cout << fich_in << std::endl;
- entrada=fopen(fich_in,"r");
- if(entrada==NULL){
+ }
+ else {
+ xmlReader(&(this->cameras[cam]), fich_in);
+ update_camera_matrix(&cameras[cam]);
+ }
+ /*this->cameras[cam].position.H=1;
+ this->cameras[cam].foa.H=1;*/
+
+ /*std::cout << fich_in << std::endl;
+ entrada=fopen(fich_in,"r");
+ if(entrada==NULL){
printf("tracker3D: camera input calibration file %s does not exits\n",fich_in);
- }else{
+ }else{
do{i=load_cam_line(entrada,cam);}while(i!=EOF);
fclose(entrada);
- } */
-
- display_camerainfo(cameras[cam]);
+ } */
+
+ display_camerainfo(cameras[cam]);
}
void
-myprogeo::mybackproject(float x, float y, float* xp, float* yp, float* zp, float* camx, float* camy, float* camz, int cam){
- HPoint2D p;
- HPoint3D pro;
-
+myprogeo::mybackproject(float x, float y, float* xp, float* yp, float* zp, float* camx, float* camy, float* camz, int cam) {
+ HPoint2D p;
+ HPoint3D pro;
- p.x=GRAPHIC_TO_OPTICAL_X(x,y);
- p.y=GRAPHIC_TO_OPTICAL_Y(x,y);
- p.h=1;
- backproject(&pro,p,cameras[cam]);
- *xp=pro.X;
- *yp=pro.Y;
- *zp=pro.Z;
- *camx=cameras[cam].position.X;
- *camy=cameras[cam].position.Y;
- *camz=cameras[cam].position.Z;
+ p.x=GRAPHIC_TO_OPTICAL_X(x,y);
+ p.y=GRAPHIC_TO_OPTICAL_Y(x,y);
+ p.h=1;
+ backproject(&pro,p,cameras[cam]);
+ *xp=pro.X;
+ *yp=pro.Y;
+ *zp=pro.Z;
+
+ *camx=cameras[cam].position.X;
+ *camy=cameras[cam].position.Y;
+ *camz=cameras[cam].position.Z;
}
-void
-myprogeo::myproject(float x, float y, float z, float* xp, float* yp, int cam){
- HPoint2D p;
- HPoint3D p3;
+void
+myprogeo::myproject(float x, float y, float z, float* xp, float* yp, int cam) {
+ HPoint2D p;
+ HPoint3D p3;
- p3.X=x;
- p3.Y=y;
- p3.Z=z;
- p3.H=1;
-
- project(p3, &p, cameras[cam]);
- *xp=p.x;
- *yp=p.y;
+ p3.X=x;
+ p3.Y=y;
+ p3.Z=z;
+ p3.H=1;
+
+ project(p3, &p, cameras[cam]);
+ *xp=p.x;
+ *yp=p.y;
}
void
-myprogeo::mygetcameraposition(float *x, float *y, float *z, int cam){
- *x=cameras[cam].position.X;
- *y=cameras[cam].position.Y;
- *z=cameras[cam].position.Z;
+myprogeo::mygetcameraposition(float *x, float *y, float *z, int cam) {
+ *x=cameras[cam].position.X;
+ *y=cameras[cam].position.Y;
+ *z=cameras[cam].position.Z;
}
-void
-myprogeo::mygetcamerafoa(float *x, float *y, float *z, int cam){
- *x=cameras[cam].foa.X;
- *y=cameras[cam].foa.Y;
- *z=cameras[cam].foa.Z;
+void
+myprogeo::mygetcamerafoa(float *x, float *y, float *z, int cam) {
+ *x=cameras[cam].foa.X;
+ *y=cameras[cam].foa.Y;
+ *z=cameras[cam].foa.Z;
}
-void
-myprogeo::mygetcamerasize(float *w, float *h, int cam){
- *w = cameras[cam].columns;
- *h = cameras[cam].rows;
+void
+myprogeo::mygetcamerasize(float *w, float *h, int cam) {
+ *w = cameras[cam].columns;
+ *h = cameras[cam].rows;
}
-TPinHoleCamera
-myprogeo::getCamera(int camera){
- return cameras[camera];
+TPinHoleCamera
+myprogeo::getCamera(int camera) {
+ return cameras[camera];
}
} //namespace
Modified: trunk/src/stable/components/openniServer/myprogeo.h
===================================================================
--- trunk/src/stable/components/openniServer/myprogeo.h 2013-09-11 15:04:07 UTC (rev 980)
+++ trunk/src/stable/components/openniServer/myprogeo.h 2013-09-12 08:16:15 UTC (rev 981)
@@ -40,23 +40,23 @@
#define MAX_BUFFER 1024
namespace openniServer {
- class myprogeo {
- public:
- myprogeo();
- ~myprogeo();
- int load_cam_line(FILE *myfile,int cam);
- void load_cam(char *fich_in,int cam, int w, int h);
- void mybackproject(float x, float y, float* xp, float* yp, float* zp, float* camx, float* camy, float* camz, int cam);
- void myproject(float x, float y, float z, float* xp, float* yp, int cam);
- void mygetcameraposition(float *x, float *y, float *z, int cam);
- void mygetcamerafoa(float *x, float *y, float *z, int cam);
- void mygetcamerasize(float *w, float *h, int cam);
- TPinHoleCamera getCamera(int camera);
+class myprogeo {
+public:
+ myprogeo();
+ ~myprogeo();
+ int load_cam_line(FILE *myfile,int cam);
+ void load_cam(char *fich_in,int cam, int w, int h);
+ void mybackproject(float x, float y, float* xp, float* yp, float* zp, float* camx, float* camy, float* camz, int cam);
+ void myproject(float x, float y, float z, float* xp, float* yp, int cam);
+ void mygetcameraposition(float *x, float *y, float *z, int cam);
+ void mygetcamerafoa(float *x, float *y, float *z, int cam);
+ void mygetcamerasize(float *w, float *h, int cam);
+ TPinHoleCamera getCamera(int camera);
- private:
- /* cameras */
- TPinHoleCamera cameras[MAX_CAMERAS];
- };
+private:
+ /* cameras */
+ TPinHoleCamera cameras[MAX_CAMERAS];
+};
} // namespace
#endif /*KINECTVIEWER_MYPROGEO_H*/
Modified: trunk/src/stable/components/openniServer/openniServer.cfg
===================================================================
--- trunk/src/stable/components/openniServer/openniServer.cfg 2013-09-11 15:04:07 UTC (rev 980)
+++ trunk/src/stable/components/openniServer/openniServer.cfg 2013-09-12 08:16:15 UTC (rev 981)
@@ -12,13 +12,13 @@
openniServer.deviceId=0
openniServer.CameraRGB.Name=cameraA
openniServer.CameraRGB.Format=RGB8
-openniServer.CameraRGB.fps=10
+openniServer.CameraRGB.fps=20
openniServer.CameraRGB.PlayerDetection=0
#openniServer.calibration=./config/camRGB
openniServer.CameraDEPTH.Name=cameraB
openniServer.CameraDEPTH.Format=RGB8
-openniServer.CameraDEPTH.fps=10
+openniServer.CameraDEPTH.fps=20
openniServer.CameraDEPTH.PlayerDetection=0
openniServer.PointCloud.Name=pointcloud1
Modified: trunk/src/stable/components/openniServer/openniServer.cpp
===================================================================
--- trunk/src/stable/components/openniServer/openniServer.cpp 2013-09-11 15:04:07 UTC (rev 980)
+++ trunk/src/stable/components/openniServer/openniServer.cpp 2013-09-12 08:16:15 UTC (rev 981)
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 1997-2012 JDE Developers Teameldercare.camRGB
+ * Copyright (C) 1997-2012 JDE Developers Team
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
@@ -15,8 +15,8 @@
* along with this program. If not, see http://www.gnu.org/licenses/.
*
* Authors : Jose María Cañas <jmplaza en gsyc.es>
- Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>
-
+ * Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>
+ *
*/
/** \file openniServer.cpp
@@ -46,7 +46,7 @@
#ifdef WITH_NITE2
- #include "NiTE.h"
+#include "NiTE.h"
#endif
@@ -55,18 +55,17 @@
#define NUM_THREADS 5
#define MAX_LENGHT 10000
-#define CHECK_RC(rc, what) \
-if (rc != openni::STATUS_OK) \
-{ \
- std::cout << what << " failed: " << openni::OpenNI::getExtendedError() << std::endl; \
- \
+#define CHECK_RC(rc, what) \
+if (rc != openni::STATUS_OK) \
+{ \
+ std::cout << what << " failed: " << openni::OpenNI::getExtendedError() << std::endl; \
}
#ifdef WITH_NITE2
- nite::UserTracker* m_pUserTracker;
- nite::UserTrackerFrameRef userTrackerFrame;
- nite::Status rcN;
+nite::UserTracker* m_pUserTracker;
+nite::UserTrackerFrameRef userTrackerFrame;
+nite::Status rcN;
#endif
@@ -88,7 +87,7 @@
pthread_t updateThread;
-namespace openniServer{
+namespace openniServer {
std::vector<int> distances;
@@ -113,265 +112,222 @@
void* updateThread(void*)
{
- openni::Status rc = openni::STATUS_OK;
- rc = openni::OpenNI::initialize();
- if (rc != openni::STATUS_OK)
- {
- std::cout << SELCAM << ": Initialize failed: "<< openni::OpenNI::getExtendedError() <<std::endl;
- }
+ openni::Status rc = openni::STATUS_OK;
+ rc = openni::OpenNI::initialize();
+ if (rc != openni::STATUS_OK)
+ {
+ std::cout << SELCAM << ": Initialize failed: "<< openni::OpenNI::getExtendedError() <<std::endl;
+ }
- openni::Array<openni::DeviceInfo> deviceList;
- openni::OpenNI::enumerateDevices(&deviceList);
+ openni::Array<openni::DeviceInfo> deviceList;
+ openni::OpenNI::enumerateDevices(&deviceList);
- const char* deviceUri;
- //checking the number off connected devices
- if (deviceList.getSize() < 1)
- {
- std::cout << "Missing devices" << std::endl;
- openni::OpenNI::shutdown();
- }
+ const char* deviceUri;
+ //checking the number off connected devices
+ if (deviceList.getSize() < 1)
+ {
+ std::cout << "Missing devices" << std::endl;
+ openni::OpenNI::shutdown();
+ }
- //getting the Uri of the selected device
- deviceUri = deviceList[SELCAM].getUri();
+ //getting the Uri of the selected device
+ deviceUri = deviceList[SELCAM].getUri();
- //getting the device from the uri
- openni::VideoStream depth;
- rc = m_device.open(deviceUri);
- if (rc != openni::STATUS_OK)
- {
- std::cout << SELCAM << " : Couldn't open device " << deviceUri << ": " << openni::OpenNI::getExtendedError() << std::endl;
- openni::OpenNI::shutdown();
- }
+ //getting the device from the uri
+ openni::VideoStream depth;
+ rc = m_device.open(deviceUri);
+ if (rc != openni::STATUS_OK)
+ {
+ std::cout << SELCAM << " : Couldn't open device " << deviceUri << ": " << openni::OpenNI::getExtendedError() << std::endl;
+ openni::OpenNI::shutdown();
+ }
- //NITE
- #ifdef WITH_NITE2
- m_pUserTracker = new nite::UserTracker;
- nite::NiTE::initialize();
+ //NITE
+#ifdef WITH_NITE2
+ m_pUserTracker = new nite::UserTracker;
+ nite::NiTE::initialize();
- if (m_pUserTracker->create(&m_device) != nite::STATUS_OK)
- {
- std::cout << "OpenniServer: Couldn't create userTracker " << std::endl;
- }
- #endif
+ if (m_pUserTracker->create(&m_device) != nite::STATUS_OK)
+ {
+ std::cout << "OpenniServer: Couldn't create userTracker " << std::endl;
+ }
+#endif
-
- if (cameraR && cameraD)
- m_device.setImageRegistrationMode( openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR );
+ //depth
+ if (cameraD) {
+ rc = depth.create(m_device, openni::SENSOR_DEPTH);
+ if (rc == openni::STATUS_OK)
+ {
+ rc = depth.start();
+ if (rc != openni::STATUS_OK)
+ {
+ std::cout << "OpenniServer: Couldn't start depth stream: "<< openni::OpenNI::getExtendedError() << std::endl;
+ depth.destroy();
+ }
+ }
+ else
+ {
+ std::cout << "OpenniServer: Couldn't find depth stream: " << openni::OpenNI::getExtendedError() << std::endl;
+ }
+ }
+ //color
+ if (cameraR) {
+ rc = color.create(m_device, openni::SENSOR_COLOR);
+ if (rc == openni::STATUS_OK)
+ {
+ rc = color.start();
+ if (rc != openni::STATUS_OK)
+ {
+ std::cout << "OpenniServer: Couldn't start color stream: " << openni::OpenNI::getExtendedError() << std::endl;
+ color.destroy();
+ }
+ }
+ else
+ {
+ std::cout << "OpenniServer: Couldn't find color stream: " << openni::OpenNI::getExtendedError() << std::endl;
+ }
+ }
- //depth
- if (cameraD){
- rc = depth.create(m_device, openni::SENSOR_DEPTH);
- if (rc == openni::STATUS_OK)
- {
- rc = depth.start();
- if (rc != openni::STATUS_OK)
- {
- std::cout << "OpenniServer: Couldn't start depth stream: "<< openni::OpenNI::getExtendedError() << std::endl;
- depth.destroy();
- }
- }
- else
- {
- std::cout << "OpenniServer: Couldn't find depth stream: " << openni::OpenNI::getExtendedError() << std::endl;
- }
- }
- //color
- if (cameraR){
- rc = color.create(m_device, openni::SENSOR_COLOR);
- if (rc == openni::STATUS_OK)
- {
- rc = color.start();
- if (rc != openni::STATUS_OK)
- {
- std::cout << "OpenniServer: Couldn't start color stream: " << openni::OpenNI::getExtendedError() << std::endl;
- color.destroy();
- }
- }
- else
- {
- std::cout << "OpenniServer: Couldn't find color stream: " << openni::OpenNI::getExtendedError() << std::endl;
- }
- }
+ if (cameraR && cameraD)
+ {
+ m_device.setImageRegistrationMode( openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR );
+ }
- openni::VideoMode depthVideoMode;
- openni::VideoMode colorVideoMode;
+ openni::VideoMode depthVideoMode;
+ openni::VideoMode colorVideoMode;
- /*std::cout << "DEPTH ------------------------" << std::endl;
- const openni::SensorInfo *depthSensorInfo = m_device.getSensorInfo(openni::SENSOR_DEPTH);
- for(int i=0;i < depthSensorInfo->getSupportedVideoModes().getSize();i++)
- {
- openni::VideoMode videoMode = depthSensorInfo->getSupportedVideoModes()[i];
+ if (cameraR) {
+ colorVideoMode.setResolution(configWidth,configHeight);
+ colorVideoMode.setFps( configFps );
+ colorVideoMode.setPixelFormat( openni::PIXEL_FORMAT_RGB888 );
+ color.setVideoMode(colorVideoMode);
+ color.setMirroringEnabled(false);
+ }
+ if (cameraD) {
+ depthVideoMode.setResolution(configWidth,configHeight);
+ depthVideoMode.setFps( configFps );
+ depthVideoMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
- std::cout << "fps: " << videoMode.getFps() << "x: " << videoMode.getResolutionX() << "y " << videoMode.getResolutionY() << std::endl;
- }
+ rc= depth.setVideoMode(depthVideoMode);
- std::cout << "COLOR ------------------------" << std::endl;
- const openni::SensorInfo *COLORSensorInfo = m_device.getSensorInfo(openni::SENSOR_COLOR);
+ if (rc != openni::STATUS_OK)
+ {
+ std::cout << "OpenniServer: error at set depth videoMode: " << openni::OpenNI::getExtendedError() << std::endl;
+ }
+ depth.setMirroringEnabled(false);
+ }
- for(int i=0;i < COLORSensorInfo->getSupportedVideoModes().getSize();i++)
- {
- openni::VideoMode videoMode = COLORSensorInfo->getSupportedVideoModes()[i];
- std::cout << "fps: " << videoMode.getFps() << "x: " << videoMode.getResolutionX() << "y " << videoMode.getResolutionY() << std::endl;
- }*/
+ std::cout << "fps:" << configFps << "w" << configWidth << "h" << configHeight << std::endl;
- //std::cout << "voy a fijar resolucion" << std::endl;
- if (cameraR){
- colorVideoMode.setResolution(configWidth,configHeight);
- colorVideoMode.setFps( configFps );
- colorVideoMode.setPixelFormat( openni::PIXEL_FORMAT_RGB888 );
- //std::cout << "color" << std::endl;
- color.setVideoMode(colorVideoMode);
- color.setMirroringEnabled(true);
- }
- if (cameraD){
- depthVideoMode.setResolution(configWidth,configHeight);
- depthVideoMode.setFps( configFps );
- depthVideoMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
- //std::cout << "depth" << std::endl;
- rc= depth.setVideoMode(depthVideoMode);
- //std::cout << "done" << std::endl;
- if (rc != openni::STATUS_OK)
- {
- std::cout << "OpenniServer: error at set depth videoMode: " << openni::OpenNI::getExtendedError() << std::endl;
- //color.destroy();
- }
- depth.setMirroringEnabled(false);
- }
+ if (cameraR && cameraD) {
+ if (depth.isValid() && color.isValid())
+ {
+ depthVideoMode = depth.getVideoMode();
+ colorVideoMode = color.getVideoMode();
- std::cout << "fps:" << configFps << "w" << configWidth << "h" << configHeight << std::endl;
+ int depthWidth = depthVideoMode.getResolutionX();
+ int depthHeight = depthVideoMode.getResolutionY();
+ int colorWidth = colorVideoMode.getResolutionX();
+ int colorHeight = colorVideoMode.getResolutionY();
- if (cameraR && cameraD){
+ if (depthWidth == colorWidth &&
+ depthHeight == colorHeight)
+ {
+ m_width = depthWidth;
+ m_height = depthHeight;
+ }
+ else
+ {
+ std::cout << "Error - expect color and depth to be in same resolution: D: " << depthWidth << "x" << depthHeight << "C: " << colorWidth << "x" << colorHeight << std::endl;
- if (depth.isValid() && color.isValid())
- {
- depthVideoMode = depth.getVideoMode();
- colorVideoMode = color.getVideoMode();
- int depthWidth = depthVideoMode.getResolutionX();
- int depthHeight = depthVideoMode.getResolutionY();
- int colorWidth = colorVideoMode.getResolutionX();
- int colorHeight = colorVideoMode.getResolutionY();
+ }
+ }
+ else if (depth.isValid())
+ {
+ depthVideoMode = depth.getVideoMode();
+ m_width = depthVideoMode.getResolutionX();
+ m_height = depthVideoMode.getResolutionY();
+ }
+ else if (color.isValid())
+ {
+ colorVideoMode = color.getVideoMode();
+ m_width = colorVideoMode.getResolutionX();
+ m_height = colorVideoMode.getResolutionY();
+ }
+ else
+ {
+ std::cout << "Error - expects at least one of the streams to be valid..." << std::endl;
+ }
+ }
- if (depthWidth == colorWidth &&
- depthHeight == colorHeight)
- {
- m_width = depthWidth;
- m_height = depthHeight;
- }
- else
- {
- std::cout << "Error - expect color and depth to be in same resolution: D: " << depthWidth << "x" << depthHeight << "C: " << colorWidth << "x" << colorHeight << std::endl;
+ distances.resize(m_width*m_height);
+ pixelsID.resize(m_width*m_height);
+ m_streams = new openni::VideoStream*[2];
+ m_streams[0] = &depth;
+ m_streams[1] = &color;
- }
- }
- else if (depth.isValid())
- {
- depthVideoMode = depth.getVideoMode();
- m_width = depthVideoMode.getResolutionX();
- m_height = depthVideoMode.getResolutionY();
- }
- else if (color.isValid())
- {
- colorVideoMode = color.getVideoMode();
- m_width = colorVideoMode.getResolutionX();
- m_height = colorVideoMode.getResolutionY();
- }
- else
- {
- std::cout << "Error - expects at least one of the streams to be valid..." << std::endl;
- }
- }
-
- distances.resize(m_width*m_height);
- pixelsID.resize(m_width*m_height);
- /*std::cout << "aqui" << std::endl;
- m_device.Device::setDepthColorSyncEnabled(true);
- std::cout << "2" << std::endl;*/
- m_streams = new openni::VideoStream*[2];
- m_streams[0] = &depth;
- m_streams[1] = &color;
-
+ //diferente en arm que en x86???
- //diferente en arm que en x86???
+ struct timeval Na, Nb;
+ gettimeofday(&Na,NULL);
- struct timeval Na, Nb;
+ while(componentAlive) {
+ long long int timeInicio = Na.tv_sec*1000000+Na.tv_usec;
+ gettimeofday(&Nb,NULL);
+ long long int timeNew = Nb.tv_sec*1000000+Nb.tv_usec;
+ gettimeofday(&Na,NULL);
- gettimeofday(&Na,NULL);
+ pthread_mutex_lock(&mutex);
- while(componentAlive){
- long long int timeInicio = Na.tv_sec*1000000+Na.tv_usec;
- gettimeofday(&Nb,NULL);
- long long int timeNew = Nb.tv_sec*1000000+Nb.tv_usec;
- //std::cout << "Tiempo completo: " << (timeNew - timeInicio)/1000 << std::endl;
- gettimeofday(&Na,NULL);
- pthread_mutex_lock(&mutex);
+ int changedIndex;
+ openni::Status rc = openni::OpenNI::waitForAnyStream(m_streams, 2, &changedIndex);
- int changedIndex;
+#ifndef WITH_NITE2
+ if (rc != openni::STATUS_OK)
+ {
+ std::cout << "Wait failed" << std::endl;
+ }
- openni::Status rc = openni::OpenNI::waitForAnyStream(m_streams, 2, &changedIndex);
- #ifndef WITH_NITE2
- if (rc != openni::STATUS_OK)
- {
- std::cout << "Wait failed" << std::endl;
- }
+ if (cameraD)
+ depth.readFrame(&m_depthFrame);
+ if (cameraR)
+ color.readFrame(&m_colorFrame);
- /*switch (changedIndex)
- {
- case 0:
- depth.readFrame(&m_depthFrame);
- break;
- case 1:
- color.readFrame(&m_colorFrame);
- break;
- default:
- std::cout << "Error in wait" << std::endl;
- break;
- }*/
+ //nite
+#else
+ if (segmentationType==1) {
+ color.readFrame(&m_colorFrame);
+ rcN = m_pUserTracker->readFrame(&userTrackerFrame);
+ m_depthFrame = userTrackerFrame.getDepthFrame();
+ if (rcN != nite::STATUS_OK)
+ {
+ std::cout << "GetNextData failed" << std::endl;
+ }
+ }
+#endif
- if (cameraD)
- depth.readFrame(&m_depthFrame);
- if (cameraR)
- color.readFrame(&m_colorFrame);
+ pthread_mutex_unlock(&mutex);
+ //OJO it control
+ usleep(1000);
- //nite
-
- #else
- if (segmentationType==1){
- color.readFrame(&m_colorFrame);
- rcN = m_pUserTracker->readFrame(&userTrackerFrame);
- m_depthFrame = userTrackerFrame.getDepthFrame();
- if (rcN != nite::STATUS_OK)
- {
- std::cout << "GetNextData failed" << std::endl;
- //return;
- }
- }
- #endif
-
-
-
-
- pthread_mutex_unlock(&mutex);
- //OJO it control
- usleep(1000);
-
- }
- return NULL;
+ }
+ return NULL;
}
@@ -379,239 +335,228 @@
/**
* \brief Class which contains all the functions and variables to make run the Robot Cameras
*/
- class CameraRGB: virtual public jderobot::Camera {
+class CameraRGB: virtual public jderobot::Camera {
public:
- CameraRGB(std::string& propertyPrefix, const Ice::PropertiesPtr propIn)
- : prefix(propertyPrefix),
- imageFmt(),
- imageDescription(new jderobot::ImageDescription()),
- cameraDescription(new jderobot::CameraDescription()),
- replyTask()
- {
- Ice::PropertiesPtr prop = propIn;
+ CameraRGB(std::string& propertyPrefix, const Ice::PropertiesPtr propIn)
+ : prefix(propertyPrefix),
+ imageFmt(),
+ imageDescription(new jderobot::ImageDescription()),
+ cameraDescription(new jderobot::CameraDescription()),
+ replyTask()
+ {
+ Ice::PropertiesPtr prop = propIn;
- //fill cameraDescription
- cameraDescription->name = prop->getProperty(prefix+"Name");
- if (cameraDescription->name.size() == 0)
- std::cout << "Camera name not configured" << std::endl;
+ //fill cameraDescription
+ cameraDescription->name = prop->getProperty(prefix+"Name");
+ if (cameraDescription->name.size() == 0)
+ std::cout << "Camera name not configured" << std::endl;
- cameraDescription->shortDescription = prop->getProperty(prefix + "ShortDescription");
+ cameraDescription->shortDescription = prop->getProperty(prefix + "ShortDescription");
- //fill imageDescription
- imageDescription->width = configWidth;
- imageDescription->height = configHeight;
- int playerdetection = prop->getPropertyAsIntWithDefault(prefix+"PlayerDetection",0);
- #ifndef WITH_NITE2
- playerdetection=0;
- #endif
- int fps = prop->getPropertyAsIntWithDefault(prefix+"fps",5);
- //we use formats according to colorspaces
- std::string fmtStr = prop->getPropertyWithDefault(prefix+"Format","YUY2");//default format YUY2
- imageFmt = colorspaces::Image::Format::searchFormat(fmtStr);
- if (!imageFmt)
- std::cout << "Format " << fmtStr << " unknown" << std::endl;
- imageDescription->size = imageDescription->width * imageDescription->height * CV_ELEM_SIZE(imageFmt->cvType);
- imageDescription->format = imageFmt->name;
+ //fill imageDescription
+ imageDescription->width = configWidth;
+ imageDescription->height = configHeight;
+ int playerdetection = prop->getPropertyAsIntWithDefault(prefix+"PlayerDetection",0);
+#ifndef WITH_NITE2
+ playerdetection=0;
+#endif
+ int fps = prop->getPropertyAsIntWithDefault(prefix+"fps",5);
+ //we use formats according to colorspaces
+ std::string fmtStr = prop->getPropertyWithDefault(prefix+"Format","YUY2");//default format YUY2
+ imageFmt = colorspaces::Image::Format::searchFormat(fmtStr);
+ if (!imageFmt)
+ std::cout << "Format " << fmtStr << " unknown" << std::endl;
+ imageDescription->size = imageDescription->width * imageDescription->height * CV_ELEM_SIZE(imageFmt->cvType);
+ imageDescription->format = imageFmt->name;
- std::cout << "Starting thread for camera: " << cameraDescription->name << std::endl;
- replyTask = new ReplyTask(this,fps, playerdetection);
+ std::cout << "Starting thread for camera: " << cameraDescription->name << std::endl;
+ replyTask = new ReplyTask(this,fps, playerdetection);
- this->control=replyTask->start();//my own thread
- }
+ this->control=replyTask->start();//my own thread
+ }
- virtual ~CameraRGB(){
- std::cout << "-------------------------------------------Stopping and joining thread for camera: " << cameraDescription->name << std::endl;
- replyTask->destroy();
- this->control.join();
- color.stop();
- color.destroy();
- }
-
- virtual jderobot::ImageDescriptionPtr getImageDescription(const Ice::Current& c){
- return imageDescription;
- }
+ virtual ~CameraRGB() {
+ std::cout << "-------------------------------------------Stopping and joining thread for camera: " << cameraDescription->name << std::endl;
+ replyTask->destroy();
+ this->control.join();
+ color.stop();
+ color.destroy();
+ }
- virtual jderobot::CameraDescriptionPtr getCameraDescription(const Ice::Current& c){
- return cameraDescription;
- }
+ virtual jderobot::ImageDescriptionPtr getImageDescription(const Ice::Current& c) {
+ return imageDescription;
+ }
- virtual void getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr& cb,const Ice::Current& c){
- replyTask->pushJob(cb);
- }
+ virtual jderobot::CameraDescriptionPtr getCameraDescription(const Ice::Current& c) {
+ return cameraDescription;
+ }
- 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 getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr& cb,const Ice::Current& c) {
+ replyTask->pushJob(cb);
+ }
- virtual void stopCameraStreaming(const Ice::Current&) {
- std::cout << "Should be made anything to stop camera streaming: " << cameraDescription->name << std::endl;
- }
+ 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 Ice::Int setCameraDescription(const jderobot::CameraDescriptionPtr&, const Ice::Current&){
- return 0;
- }
+ virtual void stopCameraStreaming(const Ice::Current&) {
+ std::cout << "Should be made anything to stop camera streaming: " << cameraDescription->name << std::endl;
+ }
+ virtual Ice::Int setCameraDescription(const jderobot::CameraDescriptionPtr&, const Ice::Current&) {
+ return 0;
+ }
+
private:
- class ReplyTask: public IceUtil::Thread{
- public:
- ReplyTask(CameraRGB* camera, int fps, int playerdetection):mycameravga(camera),_done(false) {
- segmentation=playerdetection;
- this->fps=fps;
- }
-
+ class ReplyTask: public IceUtil::Thread {
+ public:
+ ReplyTask(CameraRGB* camera, int fps, int playerdetection):mycameravga(camera),_done(false) {
+ segmentation=playerdetection;
+ this->fps=fps;
+ }
- void pushJob(const jderobot::AMD_ImageProvider_getImageDataPtr& cb){
- IceUtil::Mutex::Lock sync(requestsMutex);
- requests.push_back(cb);
- }
- virtual void run(){
+ void pushJob(const jderobot::AMD_ImageProvider_getImageDataPtr& cb) {
+ IceUtil::Mutex::Lock sync(requestsMutex);
+ requests.push_back(cb);
+ }
- jderobot::ImageDataPtr reply(new jderobot::ImageData);
- reply->description = mycameravga->imageDescription;
- reply->pixelData.resize(mycameravga->imageDescription->width*mycameravga->imageDescription->height*3);
- srcRGB = new cv::Mat(cv::Size(mycameravga->imageDescription->width,mycameravga->imageDescription->height),CV_8UC3);
- cv::Mat dst_resize;
+ virtual void run() {
+ jderobot::ImageDataPtr reply(new jderobot::ImageData);
+ reply->description = mycameravga->imageDescription;
+ reply->pixelData.resize(mycameravga->imageDescription->width*mycameravga->imageDescription->height*3);
+ srcRGB = new cv::Mat(cv::Size(mycameravga->imageDescription->width,mycameravga->imageDescription->height),CV_8UC3);
+ cv::Mat dst_resize;
- struct timeval a, b;
- int cycle; // duración del ciclo
- long totala;
- long totalpre=0;
- long diff;
- cycle=(float)(1/(float)fps)*1000000;
-
-
- while(!(_done)){
- gettimeofday(&a,NULL);
- totala=a.tv_sec*1000000+a.tv_usec;
- pthread_mutex_lock(&mutex);
- IceUtil::Time t = IceUtil::Time::now();
- reply->timeStamp.seconds = (long)t.toSeconds();
- reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
+ struct timeval a, b;
+ int cycle; // duración del ciclo
+ long totala;
+ long totalpre=0;
+ long diff;
- if (!m_colorFrame.isValid()){
- pthread_mutex_unlock(&mutex);
- continue;
- }
+ cycle=(float)(1/(float)fps)*1000000;
- //nite
- #ifdef WITH_NITE2
- const nite::UserMap& userLabels = userTrackerFrame.getUserMap();
- const nite::UserId* pLabels = userLabels.getPixels();
- #endif
+ while(!(_done)) {
+ gettimeofday(&a,NULL);
+ totala=a.tv_sec*1000000+a.tv_usec;
+ pthread_mutex_lock(&mutex);
+ IceUtil::Time t = IceUtil::Time::now();
+ reply->timeStamp.seconds = (long)t.toSeconds();
+ reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
- const openni::RGB888Pixel* pImageRow = (const openni::RGB888Pixel*)m_colorFrame.getData();
- int rowSize = m_colorFrame.getStrideInBytes() / sizeof(openni::RGB888Pixel);
+ if (!m_colorFrame.isValid()) {
+ pthread_mutex_unlock(&mutex);
+ continue;
+ }
- for (int y = 0; y < m_colorFrame.getHeight(); ++y)
- {
- const openni::RGB888Pixel* pImage = pImageRow;
- for (int x = 0; x < m_colorFrame.getWidth(); ++x, ++pImage)
- {
- switch(segmentation){
- case 0:
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 0] = pImage->r;
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 1] = pImage->g;
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 2] = pImage->b;
- break;
- case 1:
- #ifdef WITH_NITE2
- if (segmentation){
- pixelsID[(y*m_colorFrame.getWidth() + x)]= *pLabels;
- if (*pLabels!=0)
- {
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 0] = colors[*pLabels][0];
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 1] = colors[*pLabels][1];
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 2] = colors[*pLabels][2];
- }
- else{
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 0] = 0;
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 1] = 0;
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 2] = 0;
- }
- ++pLabels;
- }
- else{
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 0] = pImage->r;
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 1] = pImage->g;
- srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 2] = pImage->b;
- }
- #endif
- break;
- case 2:
+ //nite
- default:
- std::cout << "openniServer: Error segmentation not supported" << std::endl;
- break;
- }
+#ifdef WITH_NITE2
+ const nite::UserMap& userLabels = userTrackerFrame.getUserMap();
+ const nite::UserId* pLabels = userLabels.getPixels();
+#endif
+ const openni::RGB888Pixel* pImageRow = (const openni::RGB888Pixel*)m_colorFrame.getData();
+ int rowSize = m_colorFrame.getStrideInBytes() / sizeof(openni::RGB888Pixel);
- }
- pImageRow += rowSize;
- }
-
+ for (int y = 0; y < m_colorFrame.getHeight(); ++y)
+ {
+ const openni::RGB888Pixel* pImage = pImageRow;
+ for (int x = 0; x < m_colorFrame.getWidth(); ++x, ++pImage)
+ {
+ switch(segmentation) {
+ case 0:
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 0] = pImage->r;
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 1] = pImage->g;
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 2] = pImage->b;
+ break;
+ case 1:
+#ifdef WITH_NITE2
+ if (segmentation) {
+ pixelsID[(y*m_colorFrame.getWidth() + x)]= *pLabels;
+ if (*pLabels!=0)
+ {
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 0] = colors[*pLabels][0];
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 1] = colors[*pLabels][1];
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 2] = colors[*pLabels][2];
+ }
+ else {
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 0] = 0;
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 1] = 0;
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 2] = 0;
+ }
+ ++pLabels;
+ }
+ else {
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 0] = pImage->r;
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 1] = pImage->g;
+ srcRGB->data[(y*m_colorFrame.getWidth() + x)*3 + 2] = pImage->b;
+ }
+#endif
+ break;
+ case 2:
- //test
- //CalculateJoints();
+ default:
+ std::cout << "openniServer: Error segmentation not supported" << std::endl;
+ break;
+ }
-
- //cvFlip(srcRGB, NULL, 1);
- //std::cout << "camara:" << (int)mycameravga->imageDescription->width << "x" << (int) mycameravga->imageDescription->height << std::endl;
- //std::cout << "xtion: " << m_colorFrame.getWidth() << "x" << m_colorFrame.getHeight() << std::endl;
- if ((mycameravga->imageDescription->width != m_colorFrame.getWidth()) || (mycameravga->imageDescription->height != m_colorFrame.getHeight())){
- std::cout << "Assuming kinect device with resampled on device not working" << std::endl;
- resize(*srcRGB, dst_resize, srcRGB->size(), 0, 0, cv::INTER_LINEAR);
- memcpy(&(reply->pixelData[0]),(unsigned char *) dst_resize.data,dst_resize.cols*dst_resize.rows * 3);
- }
- else{
- memcpy(&(reply->pixelData[0]),(unsigned char *) srcRGB->data,srcRGB->rows*srcRGB->cols * 3);
- }
+ }
+ pImageRow += rowSize;
+ }
- {//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
+ if ((mycameravga->imageDescription->width != m_colorFrame.getWidth()) || (mycameravga->imageDescription->height != m_colorFrame.getHeight())) {
+ std::cout << "Assuming kinect device with resampled on device not working" << std::endl;
+ resize(*srcRGB, dst_resize, srcRGB->size(), 0, 0, cv::INTER_LINEAR);
+ memcpy(&(reply->pixelData[0]),(unsigned char *) dst_resize.data,dst_resize.cols*dst_resize.rows * 3);
+ }
+ else {
+ memcpy(&(reply->pixelData[0]),(unsigned char *) srcRGB->data,srcRGB->rows*srcRGB->cols * 3);
+ }
- pthread_mutex_unlock(&mutex);
- /*gettimeofday(&b,NULL);
- totalb=b.tv_sec*1000000+b.tv_usec;*/
- if (totalpre !=0){
- if ((totala - totalpre) > cycle ){
- std::cout<<"-------- openniServer: WARNING- RGB timeout-" << std::endl;
- }
- else{
- usleep(cycle - (totala - totalpre));
- }
- }
- /*if (totalpre !=0){
- std::cout << "rgb: " << 1000000/(totala-totalpre) << std::endl;
- }*/
- totalpre=totala;
- }
- }
- virtual void destroy(){
- this->_done=true;
- }
+ { //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
- private:
- CameraRGB* mycameravga;
- IceUtil::Mutex requestsMutex;
- std::list<jderobot::AMD_ImageProvider_getImageDataPtr> requests;
- int segmentation;
- int fps;
- bool _done;
+ pthread_mutex_unlock(&mutex);
+ if (totalpre !=0) {
+ if ((totala - totalpre) > cycle ) {
+ std::cout<<"-------- openniServer: WARNING- RGB timeout-" << std::endl;
+ }
+ else {
+ usleep(cycle - (totala - totalpre));
+ }
+ }
+
+ totalpre=totala;
+ }
+ }
+ virtual void destroy() {
+ this->_done=true;
+ }
+
+
+ private:
+ CameraRGB* mycameravga;
+ IceUtil::Mutex requestsMutex;
+ std::list<jderobot::AMD_ImageProvider_getImageDataPtr> requests;
+ int segmentation;
+ int fps;
+ bool _done;
+
};
typedef IceUtil::Handle<ReplyTask> ReplyTaskPtr;
@@ -624,254 +569,251 @@
IceUtil::ThreadControl control;
- };
+};
//*********************************************************************/
- class CameraDEPTH: virtual public jderobot::Camera {
+class CameraDEPTH: virtual public jderobot::Camera {
public:
- CameraDEPTH(std::string& propertyPrefix, const Ice::PropertiesPtr propIn)
- : prefix(propertyPrefix),
- imageFmt(),
- imageDescription(new jderobot::ImageDescription()),
- cameraDescription(new jderobot::CameraDescription()),
- replyTask()
- {
-
-
- Ice::PropertiesPtr prop = propIn;
+ CameraDEPTH(std::string& propertyPrefix, const Ice::PropertiesPtr propIn)
+ : prefix(propertyPrefix),
+ imageFmt(),
+ imageDescription(new jderobot::ImageDescription()),
+ cameraDescription(new jderobot::CameraDescription()),
+ replyTask()
+ {
- //fill cameraDescription
- cameraDescription->name = prop->getProperty(prefix+"Name");
- if (cameraDescription->name.size() == 0)
- std::cout << "Camera name not configured" << std::endl;
- cameraDescription->shortDescription = prop->getProperty(prefix+"ShortDescription");
+ Ice::PropertiesPtr prop = propIn;
- //fill imageDescription
- imageDescription->width = configWidth;
- int playerdetection = prop->getPropertyAsIntWithDefault(prefix+"PlayerDetection",0);
- #ifndef WITH_NITE2
- playerdetection=0;
- #endif
- imageDescription->height = configHeight;
- int fps = prop->getPropertyAsIntWithDefault(prefix+"fps",5);
- //we use formats acording to colorspaces
- std::string fmtStr = prop->getPropertyWithDefault(prefix+"Format","YUY2");//default format YUY2
- imageFmt = colorspaces::Image::Format::searchFormat(fmtStr);
- if (!imageFmt)
- std::cout << "Format " << fmtStr << " unknown" << std::endl;
- imageDescription->size = imageDescription->width * imageDescription->height * CV_ELEM_SIZE(imageFmt->cvType);
- imageDescription->format = imageFmt->name;
+ //fill cameraDescription
+ cameraDescription->name = prop->getProperty(prefix+"Name");
+ if (cameraDescription->name.size() == 0)
+ std::cout << "Camera name not configured" << std::endl;
- std::cout << "Starting thread for camera: " << cameraDescription->name << std::endl;
- replyTask = new ReplyTask(this, imageDescription->width, imageDescription->height,fps, playerdetection);
+ cameraDescription->shortDescription = prop->getProperty(prefix+"ShortDescription");
- this->control=replyTask->start();//my own thread
- }
+ //fill imageDescription
+ imageDescription->width = configWidth;
+ int playerdetection = prop->getPropertyAsIntWithDefault(prefix+"PlayerDetection",0);
+#ifndef WITH_NITE2
+ playerdetection=0;
+#endif
+ imageDescription->height = configHeight;
+ int fps = prop->getPropertyAsIntWithDefault(prefix+"fps",5);
+ //we use formats acording to colorspaces
+ std::string fmtStr = prop->getPropertyWithDefault(prefix+"Format","YUY2");//default format YUY2
+ imageFmt = colorspaces::Image::Format::searchFormat(fmtStr);
+ if (!imageFmt)
+ std::cout << "Format " << fmtStr << " unknown" << std::endl;
+ imageDescription->size = imageDescription->width * imageDescription->height * CV_ELEM_SIZE(imageFmt->cvType);
+ imageDescription->format = imageFmt->name;
- virtual ~CameraDEPTH(){
- std::cout << "Stopping and joining thread for camera: " << cameraDescription->name << std::endl;
- replyTask->destroy();
- this->control.join();
- depth.stop();
- depth.destroy();
- }
-
- virtual jderobot::ImageDescriptionPtr getImageDescription(const Ice::Current& c){
- return imageDescription;
- }
+ std::cout << "Starting thread for camera: " << cameraDescription->name << std::endl;
+ replyTask = new ReplyTask(this, imageDescription->width, imageDescription->height,fps, playerdetection);
- virtual jderobot::CameraDescriptionPtr getCameraDescription(const Ice::Current& c){
- return cameraDescription;
- }
+ this->control=replyTask->start();//my own thread
+ }
- virtual void getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr& cb,const Ice::Current& c){
- replyTask->pushJob(cb);
- }
+ virtual ~CameraDEPTH() {
+ std::cout << "Stopping and joining thread for camera: " << cameraDescription->name << std::endl;
+ replyTask->destroy();
+ this->control.join();
+ depth.stop();
+ depth.destroy();
+ }
- 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 jderobot::ImageDescriptionPtr getImageDescription(const Ice::Current& c) {
+ return imageDescription;
+ }
- virtual void stopCameraStreaming(const Ice::Current&) {
- std::cout << "Should be made anything to stop camera streaming: " << cameraDescription->name << std::endl;
- }
-
- virtual Ice::Int setCameraDescription(const jderobot::CameraDescriptionPtr&, const Ice::Current&){
- return 0;
- }
+ virtual jderobot::CameraDescriptionPtr getCameraDescription(const Ice::Current& c) {
+ return cameraDescription;
+ }
+ 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 Ice::Int setCameraDescription(const jderobot::CameraDescriptionPtr&, const Ice::Current&) {
+ return 0;
+ }
+
private:
- class ReplyTask: public IceUtil::Thread{
- public:
- ReplyTask(CameraDEPTH* camera, int width, int height, int fps, int playerDetection)
- :mycameradepth(camera),_done(false) {
- segmentation=playerDetection;
- this->fps=fps;
- this->minToTrain=15;
- }
-
+ class ReplyTask: public IceUtil::Thread {
+ public:
+ ReplyTask(CameraDEPTH* camera, int width, int height, int fps, int playerDetection)
+ :mycameradepth(camera),_done(false) {
+ segmentation=playerDetection;
+ this->fps=fps;
+ this->minToTrain=15;
+ }
- void pushJob(const jderobot::AMD_ImageProvider_getImageDataPtr& cb){
- IceUtil::Mutex::Lock sync(requestsMutex);
- requests.push_back(cb);
- }
- virtual void run(){
- int test;
-
+ void pushJob(const jderobot::AMD_ImageProvider_getImageDataPtr& cb) {
+ IceUtil::Mutex::Lock sync(requestsMutex);
+ requests.push_back(cb);
+ }
- jderobot::ImageDataPtr reply(new jderobot::ImageData);
- reply->description = mycameradepth->imageDescription;
- reply->pixelData.resize(mycameradepth->imageDescription->width*mycameradepth->imageDescription->height*3);
- cv::Mat dst_resize(cv::Size(mycameradepth->imageDescription->width, mycameradepth->imageDescription->height),CV_8UC3);
- cv::Mat src(cv::Size(mycameradepth->imageDescription->width, mycameradepth->imageDescription->height),CV_8UC3);
- struct timeval a, b;
- int cycle; // duración del ciclo
- long totalb,totala;
- long totalpre=0;
- long diff;
+ virtual void run() {
+ int test;
- //std::cout << "FPS depth: " << fps << std::endl;
- cycle=(float)(1/(float)fps)*1000000;
-
-
- while(!(_done)){
- gettimeofday(&a,NULL);
- totala=a.tv_sec*1000000+a.tv_usec;
- pthread_mutex_lock(&mutex);
- src=cv::Scalar(0, 0, 0);
+ jderobot::ImageDataPtr reply(new jderobot::ImageData);
+ reply->description = mycameradepth->imageDescription;
+ reply->pixelData.resize(mycameradepth->imageDescription->width*mycameradepth->imageDescription->height*3);
+ cv::Mat dst_resize(cv::Size(mycameradepth->imageDescription->width, mycameradepth->imageDescription->height),CV_8UC3);
+ cv::Mat src(cv::Size(mycameradepth->imageDescription->width, mycameradepth->imageDescription->height),CV_8UC3);
+ struct timeval a, b;
+ int cycle; // duración del ciclo
+ long totalb,totala;
+ long totalpre=0;
+ long diff;
- IceUtil::Time t = IceUtil::Time::now();
- reply->timeStamp.seconds = (long)t.toSeconds();
- reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
- if (!m_depthFrame.isValid()){
- pthread_mutex_unlock(&mutex);
- continue;
- }
+ //std::cout << "FPS depth: " << fps << std::endl;
+ cycle=(float)(1/(float)fps)*1000000;
- //cvZero(src);
- //nite
- #ifdef WITH_NITE2
- const nite::UserMap& userLabels = userTrackerFrame.getUserMap();
- const nite::UserId* pLabels = userLabels.getPixels();
- #endif
+ while(!(_done)) {
+ gettimeofday(&a,NULL);
+ totala=a.tv_sec*1000000+a.tv_usec;
+ pthread_mutex_lock(&mutex);
+ src=cv::Scalar(0, 0, 0);
- const openni::DepthPixel* pDepth = (const openni::DepthPixel*)m_depthFrame.getData();
- int restOfRow = m_depthFrame.getStrideInBytes() / sizeof(openni::DepthPixel) - m_depthFrame.getWidth();
+ IceUtil::Time t = IceUtil::Time::now();
+ reply->timeStamp.seconds = (long)t.toSeconds();
+ reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
+ if (!m_depthFrame.isValid()) {
+ pthread_mutex_unlock(&mutex);
+ continue;
+ }
- for (int y = 0; y < m_depthFrame.getHeight(); ++y)
- {
- for (int x = 0; x < m_depthFrame.getWidth(); ++x, ++pDepth)
- {
- switch(segmentation){
- case 0:
- distances[(y*m_depthFrame.getWidth() + x)] = *pDepth;
- if (*pDepth != 0)
- {
- src.data[(y*m_depthFrame.getWidth()+ x)*3+0] = (float(*pDepth)/(float)MAX_LENGHT)*255.;
- src.data[(y*m_depthFrame.getWidth()+ x)*3+1] = (*pDepth)>>8;
- src.data[(y*m_depthFrame.getWidth()+ x)*3+2] = (*pDepth)&0xff;
- }
- break;
- case 1:
- #ifdef WITH_NITE2
- if ((*pLabels!=0)||(!segmentation)){
- distances[(y*m_depthFrame.getWidth() + x)] = *pDepth;
- if (*pDepth != 0)
- {
- src.data[(y*m_depthFrame.getWidth()+ x)*3+0] = (float(*pDepth)/(float)MAX_LENGHT)*255.;
- src.data[(y*m_depthFrame.getWidth()+ x)*3+1] = (*pDepth)>>8;
- src.data[(y*m_depthFrame.getWidth()+ x)*3+2] = (*pDepth)&0xff;
+ //cvZero(src);
- }
- else{
- src.data[(y*m_depthFrame.getWidth()+ x)*3+0] = 0;
- src.data[(y*m_depthFrame.getWidth()+ x)*3+1] = 0;
- src.data[(y*m_depthFrame.getWidth()+ x)*3+2] = 0;
- }
- }
- else{
- src.data[(y*m_depthFrame.getWidth()+ x)*3+0] = 0;
- src.data[(y*m_depthFrame.getWidth()+ x)*3+1] = 0;
- src.data[(y*m_depthFrame.getWidth()+ x)*3+2] = 0;
- }
- ++pLabels;
- #endif
- break;
- case 2:
- break;
- default:
- std::cout << "openniServer: Error segmentation not supported" << std::endl;
- break;
- }
- }
+ //nite
+#ifdef WITH_NITE2
+ const nite::UserMap& userLabels = userTrackerFrame.getUserMap();
+ const nite::UserId* pLabels = userLabels.getPixels();
+#endif
- pDepth += restOfRow;
- }
- if ((mycameradepth->imageDescription->width != m_depthFrame.getWidth()) || (mycameradepth->imageDescription->height != m_depthFrame.getHeight())){
- //cv::resize(src,dst_resize);
- cv::resize(src, dst_resize, dst_resize.size(), 0, 0, cv::INTER_LINEAR);
- std::cout << "resize depth" << std::endl;
- memcpy(&(reply->pixelData[0]),(unsigned char *) dst_resize.data,dst_resize.cols*dst_resize.rows * 3);
- }
- else{
- memcpy(&(reply->pixelData[0]),(unsigned char *) src.data,src.cols*src.rows * 3);
- }
-
- {//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
- pthread_mutex_unlock(&mutex);
- /*gettimeofday(&b,NULL);
- totalb=b.tv_sec*1000000+b.tv_usec;*/
- if (totalpre !=0){
- if ((totala - totalpre) > cycle ){
- std::cout<<"-------- openniServer: WARNING- DEPTH timeout-" << std::endl;
- }
- else{
- usleep(cycle - (totala-totalpre));
- }
- }
- /*if (totalpre !=0){
- std::cout << "depth: " << 1000000/(totala-totalpre) << std::endl;
- }*/
- totalpre=totala;
-
- }
- }
-
- virtual void destroy(){
- this->_done=true;
- }
+ const openni::DepthPixel* pDepth = (const openni::DepthPixel*)m_depthFrame.getData();
+ int restOfRow = m_depthFrame.getStrideInBytes() / sizeof(openni::DepthPixel) - m_depthFrame.getWidth();
+ for (int y = 0; y < m_depthFrame.getHeight(); ++y)
+ {
+ for (int x = 0; x < m_depthFrame.getWidth(); ++x, ++pDepth)
+ {
+ switch(segmentation) {
+ case 0:
+ distances[(y*m_depthFrame.getWidth() + x)] = *pDepth;
+ if (*pDepth != 0)
+ {
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+0] = (float(*pDepth)/(float)MAX_LENGHT)*255.;
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+1] = (*pDepth)>>8;
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+2] = (*pDepth)&0xff;
+ }
+ break;
+ case 1:
+#ifdef WITH_NITE2
+ if ((*pLabels!=0)||(!segmentation)) {
+ distances[(y*m_depthFrame.getWidth() + x)] = *pDepth;
+ if (*pDepth != 0)
+ {
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+0] = (float(*pDepth)/(float)MAX_LENGHT)*255.;
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+1] = (*pDepth)>>8;
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+2] = (*pDepth)&0xff;
- private:
- CameraDEPTH* mycameradepth;
- IceUtil::Mutex requestsMutex;
- std::list<jderobot::AMD_ImageProvider_getImageDataPtr> requests;
+ }
+ else {
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+0] = 0;
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+1] = 0;
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+2] = 0;
+ }
+ }
+ else {
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+0] = 0;
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+1] = 0;
+ src.data[(y*m_depthFrame.getWidth()+ x)*3+2] = 0;
+ }
+ ++pLabels;
+#endif
+ break;
+ case 2:
+ break;
+ default:
+ std::cout << "openniServer: Error segmentation not supported" << std::endl;
+ break;
+ }
+ }
-
- int segmentation;
- int fps;
- int minToTrain;
- cv::BackgroundSubtractorMOG2 bg;
- cv::Mat fore;
- cv::Mat trainImage;
- bool _done;
-
-
+ pDepth += restOfRow;
+ }
+
+ if ((mycameradepth->imageDescription->width != m_depthFrame.getWidth()) || (mycameradepth->imageDescription->height != m_depthFrame.getHeight())) {
+ //cv::resize(src,dst_resize);
+ cv::resize(src, dst_resize, dst_resize.size(), 0, 0, cv::INTER_LINEAR);
+ std::cout << "resize depth" << std::endl;
+ memcpy(&(reply->pixelData[0]),(unsigned char *) dst_resize.data,dst_resize.cols*dst_resize.rows * 3);
+ }
+ else {
+ memcpy(&(reply->pixelData[0]),(unsigned char *) src.data,src.cols*src.rows * 3);
+ }
+
+ { //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
+ pthread_mutex_unlock(&mutex);
+
+ if (totalpre !=0) {
+ if ((totala - totalpre) > cycle ) {
+ std::cout<<"-------- openniServer: WARNING- DEPTH timeout-" << std::endl;
+ }
+ else {
+ usleep(cycle - (totala-totalpre));
+ }
+ }
+
+ totalpre=totala;
+
+ }
+ }
+
+ virtual void destroy() {
+ this->_done=true;
+ }
+
+
+ private:
+ CameraDEPTH* mycameradepth;
+ IceUtil::Mutex requestsMutex;
+ std::list<jderobot::AMD_ImageProvider_getImageDataPtr> requests;
+
+
+ int segmentation;
+ int fps;
+ int minToTrain;
+ cv::BackgroundSubtractorMOG2 bg;
+ cv::Mat fore;
+ cv::Mat trainImage;
+ bool _done;
+
+
};
typedef IceUtil::Handle<ReplyTask> ReplyTaskPtr;
@@ -883,181 +825,179 @@
ReplyTaskPtr replyTask;
IceUtil::ThreadControl control;
- };
+};
/**
* \brief Class wich contains all the functions and variables to serve point cloud interface
*/
- class pointCloudI: virtual public jderobot::pointCloud{
- public:
- pointCloudI (std::string& propertyPrefix, const Ice::PropertiesPtr propIn):
- prefix(propertyPrefix),data(new jderobot::pointCloudData()) {
- Ice::PropertiesPtr prop = propIn;
+class pointCloudI: virtual public jderobot::pointCloud {
+public:
+ pointCloudI (std::string& propertyPrefix, const Ice::PropertiesPtr propIn):
+ prefix(propertyPrefix),data(new jderobot::pointCloudData()) {
+ Ice::PropertiesPtr prop = propIn;
- int playerdetection = prop->getPropertyAsIntWithDefault("openniServer.PlayerDetection",0);
- int fps =prop->getPropertyAsIntWithDefault("openniServer.pointCloud.Fps",10);
- #ifndef WITH_NITE2
- playerdetection=0;
- #endif
- pthread_mutex_init(&this->localMutex, NULL);
- replyCloud = new ReplyCloud(this,prop->getProperty("openniServer.calibration"), playerdetection, configWidth, configHeight,fps);
- this->control=replyCloud->start();
- }
+ int playerdetection = prop->getPropertyAsIntWithDefault("openniServer.PlayerDetection",0);
+ int fps =prop->getPropertyAsIntWithDefault("openniServer.pointCloud.Fps",10);
+#ifndef WITH_NITE2
+ playerdetection=0;
+#endif
+ pthread_mutex_init(&this->localMutex, NULL);
+ replyCloud = new ReplyCloud(this,prop->getProperty("openniServer.calibration"), playerdetection, configWidth, configHeight,fps);
+ this->control=replyCloud->start();
+ }
- virtual ~pointCloudI(){
- std::cout << "Stopping and joining thread for pointCloud" << std::endl;
- replyCloud->destroy();
- this->control.join();
- }
-
+ virtual ~pointCloudI() {
+ std::cout << "Stopping and joining thread for pointCloud" << std::endl;
+ replyCloud->destroy();
+ this->control.join();
+ }
- virtual jderobot::pointCloudDataPtr getCloudData(const Ice::Current&){
- data=replyCloud->getCloud();
- return data;
- };
-
- private:
- class ReplyCloud :public IceUtil::Thread{
- public:
- ReplyCloud (pointCloudI* pcloud, std::string filepath, int playerDetection, int widthIn, int heightIn, int fpsIn) : data(new jderobot::pointCloudData()), data2(new jderobot::pointCloudData()), _done(false)
- {
- path=filepath;
- segmentation=playerDetection;
- cWidth = widthIn;
- cHeight = heightIn;
- fps=fpsIn;
- myCloud=pcloud;
- mypro=NULL;
- }
-
- void run()
- {
- mypro= new openniServer::myprogeo();
- mypro->load_cam((char*)path.c_str(),0, cWidth, cHeight);
-
- struct timeval a, b;
- int cycle; // duración del ciclo
- long totala;
- long totalpre=0;
+ virtual jderobot::pointCloudDataPtr getCloudData(const Ice::Current&) {
+ data=replyCloud->getCloud();
+ return data;
+ };
- cycle=(float)(1/(float)fps)*1000000;
+private:
+ class ReplyCloud :public IceUtil::Thread {
+ public:
+ ReplyCloud (pointCloudI* pcloud, std::string filepath, int playerDetection, int widthIn, int heightIn, int fpsIn) : data(new jderobot::pointCloudData()), data2(new jderobot::pointCloudData()), _done(false)
+ {
+ path=filepath;
+ segmentation=playerDetection;
+ cWidth = widthIn;
+ cHeight = heightIn;
+ fps=fpsIn;
+ myCloud=pcloud;
+ mypro=NULL;
- while(!(_done)){
- float distance;
- gettimeofday(&a,NULL);
- totala=a.tv_sec*1000000+a.tv_usec;
- pthread_mutex_lock(&mutex);
- //creamos una copia local de la imagen de color y de las distancias.
- cv::Mat localRGB;
- srcRGB->copyTo(localRGB);
- std::vector<int> localDistance(distances);
- pthread_mutex_unlock(&mutex);
- pthread_mutex_lock(&(this->myCloud->localMutex));
- data2->p.clear();
- for( unsigned int i = 0 ; (i < cWidth*cHeight)&&(distances.size()>0); i=i+9) {
- distance=(float)localDistance[i];
- if (distance!=0){
- //if (((unsigned char)srcRGB->data[3*i]!=0) && ((unsigned char)srcRGB->data[3*i+1]!=0) && ((unsigned char)srcRGB->data[3*i+2]!=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;
- float t;
- float Fx,Fy,Fz;
-
- mypro->mybackproject(i % cWidth, i / cWidth, &xp, &yp, &zp, &camx, &camy, &camz,0);
-
- //vector unitario
- float modulo;
-
- modulo = sqrt(1/(((camx-xp)*(camx-xp))+((camy-yp)*(camy-yp))+((camz-zp)*(camz-zp))));
- mypro->mygetcamerafoa(&c1x, &c1y, &c1z, 0);
-
- fmod = sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
- fx = (c1x - camx)*fmod;
- fy = (c1y - camy)*fmod;
- fz = (c1z - camz) * fmod;
- ux = (xp-camx)*modulo;
- uy = (yp-camy)*modulo;
- uz = (zp-camz)*modulo;
- Fx= distance*fx + camx;
- Fy= distance*fy + camy;
- Fz= distance*fz + camz;
- // calculamos el punto real
- t = (-(fx*camx) + (fx*Fx) - (fy*camy) + (fy*Fy) - (fz*camz) + (fz*Fz))/((fx*ux) + (fy*uy) + (fz*uz));
- auxP.x=t*ux + camx;
- auxP.y=t*uy+ camy;
- auxP.z=t*uz + camz;
- if ( segmentation){
- auxP.id=pixelsID[i];
- }
- auxP.r=(float)(int) (unsigned char)localRGB.data[3*i];
- auxP.g=(float)(int) (unsigned char)localRGB.data[3*i+1];
- auxP.b=(float)(int) (unsigned char)localRGB.data[3*i+2];
- data2->p.push_back(auxP);
- }
- //}
- }
- pthread_mutex_unlock(&(this->myCloud->localMutex));
- if (totalpre !=0){
- if ((totala - totalpre) > cycle ){
- std::cout<<"-------- openniServer: WARNING- POINTCLOUD timeout-" << std::endl;
- }
- else{
- usleep(cycle - (totala - totalpre));
- }
- }
- /*if (totalpre !=0){
- std::cout << "cloud: " << 1000000/(totala-totalpre) << std::endl;
- }*/
- totalpre=totala;
- }
- }
-
+ }
- jderobot::pointCloudDataPtr getCloud()
- {
- pthread_mutex_lock(&(this->myCloud->localMutex));
- data->p=data2->p;
- pthread_mutex_unlock(&(this->myCloud->localMutex));
- return data;
- }
+ void run()
+ {
+ mypro= new openniServer::myprogeo();
+ mypro->load_cam((char*)path.c_str(),0, cWidth, cHeight);
- virtual void destroy(){
- this->_done=true;
- }
+ struct timeval a, b;
+ int cycle; // duración del ciclo
+ long totala;
+ long totalpre=0;
+ cycle=(float)(1/(float)fps)*1000000;
+ while(!(_done)) {
+ float distance;
+ gettimeofday(&a,NULL);
+ totala=a.tv_sec*1000000+a.tv_usec;
+ pthread_mutex_lock(&mutex);
+ //creamos una copia local de la imagen de color y de las distancias.
+ cv::Mat localRGB;
+ srcRGB->copyTo(localRGB);
+ std::vector<int> localDistance(distances);
+ pthread_mutex_unlock(&mutex);
+ pthread_mutex_lock(&(this->myCloud->localMutex));
+ data2->p.clear();
+ for( unsigned int i = 0 ; (i < cWidth*cHeight)&&(distances.size()>0); i=i+9) {
+ distance=(float)localDistance[i];
+ if (distance!=0) {
- private:
- myprogeo *mypro;
- int cWidth;
- int cHeight;
- int fps;
- jderobot::pointCloudDataPtr data, data2;
- jderobot::RGBPoint auxP;
- std::string path;
- int segmentation;
- pointCloudI* myCloud;
- bool _done;
-
- };
+ 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;
+ float t;
+ float Fx,Fy,Fz;
- typedef IceUtil::Handle<ReplyCloud> ReplyCloudPtr;
- ReplyCloudPtr replyCloud;
- std::string prefix;
- jderobot::pointCloudDataPtr data;
- pthread_mutex_t localMutex;
- IceUtil::ThreadControl control;
+ mypro->mybackproject(i % cWidth, i / cWidth, &xp, &yp, &zp, &camx, &camy, &camz,0);
-
-
- };
+ //vector unitario
+ float modulo;
+
+ modulo = sqrt(1/(((camx-xp)*(camx-xp))+((camy-yp)*(camy-yp))+((camz-zp)*(camz-zp))));
+ mypro->mygetcamerafoa(&c1x, &c1y, &c1z, 0);
+
+ fmod = sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
+ fx = (c1x - camx)*fmod;
+ fy = (c1y - camy)*fmod;
+ fz = (c1z - camz) * fmod;
+ ux = (xp-camx)*modulo;
+ uy = (yp-camy)*modulo;
+ uz = (zp-camz)*modulo;
+ Fx= distance*fx + camx;
+ Fy= distance*fy + camy;
+ Fz= distance*fz + camz;
+ // calculamos el punto real
+ t = (-(fx*camx) + (fx*Fx) - (fy*camy) + (fy*Fy) - (fz*camz) + (fz*Fz))/((fx*ux) + (fy*uy) + (fz*uz));
+ auxP.x=t*ux + camx;
+ auxP.y=t*uy+ camy;
+ auxP.z=t*uz + camz;
+ if ( segmentation) {
+ auxP.id=pixelsID[i];
+ }
+ auxP.r=(float)(int) (unsigned char)localRGB.data[3*i];
+ auxP.g=(float)(int) (unsigned char)localRGB.data[3*i+1];
+ auxP.b=(float)(int) (unsigned char)localRGB.data[3*i+2];
+ data2->p.push_back(auxP);
+ }
+ //}
+ }
+ pthread_mutex_unlock(&(this->myCloud->localMutex));
+ if (totalpre !=0) {
+ if ((totala - totalpre) > cycle ) {
+ std::cout<<"-------- openniServer: WARNING- POINTCLOUD timeout-" << std::endl;
+ }
+ else {
+ usleep(cycle - (totala - totalpre));
+ }
+ }
+
+ totalpre=totala;
+ }
+ }
+
+
+ jderobot::pointCloudDataPtr getCloud()
+ {
+ pthread_mutex_lock(&(this->myCloud->localMutex));
+ data->p=data2->p;
+ pthread_mutex_unlock(&(this->myCloud->localMutex));
+ return data;
+ }
+
+ virtual void destroy() {
+ this->_done=true;
+ }
+
+
+
+ private:
+ myprogeo *mypro;
+ int cWidth;
+ int cHeight;
+ int fps;
+ jderobot::pointCloudDataPtr data, data2;
+ jderobot::RGBPoint auxP;
+ std::string path;
+ int segmentation;
+ pointCloudI* myCloud;
+ bool _done;
+
+ };
+
+ typedef IceUtil::Handle<ReplyCloud> ReplyCloudPtr;
+ ReplyCloudPtr replyCloud;
+ std::string prefix;
+ jderobot::pointCloudDataPtr data;
+ pthread_mutex_t localMutex;
+ IceUtil::ThreadControl control;
+
+
+
+};
} //namespace
@@ -1067,175 +1007,175 @@
openniServer::CameraDEPTH *camDEPTH;
openniServer::pointCloudI *pc1;
-void exitApplication(int s){
+void exitApplication(int s) {
- killed=true;
- componentAlive=false;
+ killed=true;
+ componentAlive=false;
- if (camRGB!= NULL)
- delete camRGB;
- if (camDEPTH != NULL)
- delete camDEPTH;
- if (pc1 != NULL){
- delete pc1;
- }
- ic->shutdown();
+ if (camRGB!= NULL)
+ delete camRGB;
+ if (camDEPTH != NULL)
+ delete camDEPTH;
+ if (pc1 != NULL) {
+ delete pc1;
+ }
+ ic->shutdown();
- pthread_join(updateThread, NULL);
+ pthread_join(updateThread, NULL);
- m_device.close();
- openni::OpenNI::shutdown();
- exit(1);
+ m_device.close();
+ openni::OpenNI::shutdown();
+ exit(1);
}
-int main(int argc, char** argv){
+int main(int argc, char** argv) {
- componentAlive=true;
- killed=false;
- struct sigaction sigIntHandler;
+ componentAlive=true;
+ killed=false;
+ struct sigaction sigIntHandler;
- sigIntHandler.sa_handler = exitApplication;
- sigemptyset(&sigIntHandler.sa_mask);
- sigIntHandler.sa_flags = 0;
+ sigIntHandler.sa_handler = exitApplication;
+ sigemptyset(&sigIntHandler.sa_mask);
+ sigIntHandler.sa_flags = 0;
- sigaction(SIGINT, &sigIntHandler, NULL);
+ sigaction(SIGINT, &sigIntHandler, NULL);
- Ice::ObjectPtr Pose3DMotors1;
- Ice::ObjectPtr kinectleds1;
- Ice::ObjectPtr pointcloud1;
- Ice::PropertiesPtr prop;
+ Ice::ObjectPtr Pose3DMotors1;
+ Ice::ObjectPtr kinectleds1;
+ Ice::ObjectPtr pointcloud1;
+ Ice::PropertiesPtr prop;
- try{
- ic = Ice::initialize(argc,argv);
- prop = ic->getProperties();
- }
- catch (const Ice::Exception& ex) {
- std::cerr << ex << std::endl;
- return 1;
- }
- catch (const char* msg) {
- std::cerr <<"Error :" << msg << std::endl;
- return 1;
- }
- std::string componentPrefix("openniServer");
+ try {
+ ic = Ice::initialize(argc,argv);
+ prop = ic->getProperties();
+ }
+ catch (const Ice::Exception& ex) {
+ std::cerr << ex << std::endl;
+ return 1;
+ }
+ catch (const char* msg) {
+ std::cerr <<"Error :" << msg << std::endl;
+ return 1;
+ }
+ std::string componentPrefix("openniServer");
- cameraR = prop->getPropertyAsIntWithDefault(componentPrefix + ".CameraRGB",0);
- cameraD = prop->getPropertyAsIntWithDefault(componentPrefix + ".CameraDEPTH",0);
- int motors = prop->getPropertyAsIntWithDefault(componentPrefix + ".Pose3DMotorsActive",0);
- int leds = prop->getPropertyAsIntWithDefault(componentPrefix + ".KinectLedsActive",0);
- int pointCloud = prop->getPropertyAsIntWithDefault(componentPrefix + ".pointCloudActive",0);
- openniServer::segmentationType= prop->getPropertyAsIntWithDefault(componentPrefix + ".PlayerDetection",0);
+ cameraR = prop->getPropertyAsIntWithDefault(componentPrefix + ".CameraRGB",0);
+ cameraD = prop->getPropertyAsIntWithDefault(componentPrefix + ".CameraDEPTH",0);
+ int motors = prop->getPropertyAsIntWithDefault(componentPrefix + ".Pose3DMotorsActive",0);
+ int leds = prop->getPropertyAsIntWithDefault(componentPrefix + ".KinectLedsActive",0);
+ int pointCloud = prop->getPropertyAsIntWithDefault(componentPrefix + ".pointCloudActive",0);
+ openniServer::segmentationType= prop->getPropertyAsIntWithDefault(componentPrefix + ".PlayerDetection",0);
- configWidth=prop->getPropertyAsIntWithDefault(componentPrefix + ".Width", 640);
- configHeight=prop->getPropertyAsIntWithDefault(componentPrefix+ ".Height",480);
- configFps=prop->getPropertyAsIntWithDefault(componentPrefix + ".Fps",30);
- std::string Endpoints = prop->getProperty(componentPrefix + ".Endpoints");
- Ice::ObjectAdapterPtr adapter =ic->createObjectAdapterWithEndpoints(componentPrefix, Endpoints);
+ configWidth=prop->getPropertyAsIntWithDefault(componentPrefix + ".Width", 640);
+ configHeight=prop->getPropertyAsIntWithDefault(componentPrefix+ ".Height",480);
+ configFps=prop->getPropertyAsIntWithDefault(componentPrefix + ".Fps",30);
+ std::string Endpoints = prop->getProperty(componentPrefix + ".Endpoints");
+ Ice::ObjectAdapterPtr adapter =ic->createObjectAdapterWithEndpoints(componentPrefix, Endpoints);
- if (openniServer::segmentationType){
- cameraR=1;
- cameraD=1;
- }
+ if (openniServer::segmentationType) {
+ cameraR=1;
+ cameraD=1;
+ }
- SELCAM = prop->getPropertyAsIntWithDefault(componentPrefix + ".deviceId",0);
- std::cout << "Selected device: " << SELCAM << std::endl;
- int nCameras=0;
+ SELCAM = prop->getPropertyAsIntWithDefault(componentPrefix + ".deviceId",0);
+ std::cout << "Selected device: " << SELCAM << std::endl;
+ int nCameras=0;
- /*COLORS*/
- colors[0][0]=0;
- colors[0][1]=0;
- colors[0][2]=255;
- colors[1][0]=0;
- colors[1][1]=255;
- colors[1][2]=255;
- colors[2][0]=255;
- colors[2][1]=255;
- colors[2][2]=0;
- colors[3][0]=255;
- colors[3][1]=0;
- colors[3][2]=0;
- colors[4][0]=0;
- colors[4][1]=255;
- colors[4][2]=0;
- colors[5][0]=255;
- colors[5][1]=255;
- colors[5][2]=0;
- colors[6][0]=0;
- colors[6][1]=0;
- colors[6][2]=0;
- colors[7][0]=150;
- colors[7][1]=150;
- colors[7][2]=0;
- colors[8][0]=150;
- colors[8][1]=150;
- colors[8][2]=150;
- colors[9][0]=0;
- colors[9][1]=150;
- colors[9][2]=150;
+ /*COLORS*/
+ colors[0][0]=0;
+ colors[0][1]=0;
+ colors[0][2]=255;
+ colors[1][0]=0;
+ colors[1][1]=255;
+ colors[1][2]=255;
+ colors[2][0]=255;
+ colors[2][1]=255;
+ colors[2][2]=0;
+ colors[3][0]=255;
+ colors[3][1]=0;
+ colors[3][2]=0;
+ colors[4][0]=0;
+ colors[4][1]=255;
+ colors[4][2]=0;
+ colors[5][0]=255;
+ colors[5][1]=255;
+ colors[5][2]=0;
+ colors[6][0]=0;
+ colors[6][1]=0;
+ colors[6][2]=0;
+ colors[7][0]=150;
+ colors[7][1]=150;
+ colors[7][2]=0;
+ colors[8][0]=150;
+ colors[8][1]=150;
+ colors[8][2]=150;
+ colors[9][0]=0;
+ colors[9][1]=150;
+ colors[9][2]=150;
- nCameras=cameraR + cameraD;
- //g_context = new xn::Context;
- std::cout << "NCAMERAS = " << nCameras << std::endl;
- pthread_mutex_init(&mutex, NULL);
- if ((nCameras>0)||(pointCloud)){
+ nCameras=cameraR + cameraD;
+ //g_context = new xn::Context;
+ std::cout << "NCAMERAS = " << nCameras << std::endl;
+ pthread_mutex_init(&mutex, NULL);
+ if ((nCameras>0)||(pointCloud)) {
- pthread_create(&updateThread, NULL, &openniServer::updateThread, NULL);
+ pthread_create(&updateThread, NULL, &openniServer::updateThread, NULL);
- }
+ }
- if (cameraR){
- std::string objPrefix(componentPrefix + ".CameraRGB.");
- std::string cameraName = prop->getProperty(objPrefix + "Name");
- if (cameraName.size() == 0){//no name specified, we create one using the index
- cameraName = "cameraR";
- prop->setProperty(objPrefix + "Name",cameraName);//set the value
- }
- std::cout << "Creating camera " << cameraName << std::endl;
- camRGB = new openniServer::CameraRGB(objPrefix,prop);
- adapter->add(camRGB, ic->stringToIdentity(cameraName));
- std::cout<<" -------- openniServer: Component: CameraRGB created successfully --------" << std::endl;
- }
- if (cameraD){
- std::string objPrefix(componentPrefix + ".CameraDEPTH.");
- std::string cameraName = prop->getProperty(objPrefix + "Name");
- if (cameraName.size() == 0){//no name specified, we create one using the index
- cameraName = "cameraD";
- prop->setProperty(objPrefix + "Name",cameraName);//set the value
- }
- std::cout << "Creating camera " << cameraName << std::endl;
- camDEPTH = new openniServer::CameraDEPTH(objPrefix,prop);
- adapter->add(camDEPTH, ic->stringToIdentity(cameraName));
- //test camera ok
- std::cout<<" -------- openniServer: Component: CameraDEPTH created successfully --------" << std::endl;
- }
- if (pointCloud){
- std::string objPrefix(componentPrefix + ".PointCloud.");
- std::string Name = prop->getProperty(objPrefix + "Name");
- std::cout << "Creating pointcloud1 " << Name << std::endl;
- pc1 = new openniServer::pointCloudI(objPrefix,prop);
- adapter->add(pc1 , ic->stringToIdentity(Name));
- adapter->add(pointcloud1, ic->stringToIdentity(Name));
- std::cout<<" -------- openniServer: Component: PointCloud created successfully --------" << std::endl;
- }
+ if (cameraR) {
+ std::string objPrefix(componentPrefix + ".CameraRGB.");
+ std::string cameraName = prop->getProperty(objPrefix + "Name");
+ if (cameraName.size() == 0) { //no name specified, we create one using the index
+ cameraName = "cameraR";
+ prop->setProperty(objPrefix + "Name",cameraName);//set the value
+ }
+ std::cout << "Creating camera " << cameraName << std::endl;
+ camRGB = new openniServer::CameraRGB(objPrefix,prop);
+ adapter->add(camRGB, ic->stringToIdentity(cameraName));
+ std::cout<<" -------- openniServer: Component: CameraRGB created successfully --------" << std::endl;
+ }
+ if (cameraD) {
+ std::string objPrefix(componentPrefix + ".CameraDEPTH.");
+ std::string cameraName = prop->getProperty(objPrefix + "Name");
+ if (cameraName.size() == 0) { //no name specified, we create one using the index
+ cameraName = "cameraD";
+ prop->setProperty(objPrefix + "Name",cameraName);//set the value
+ }
+ std::cout << "Creating camera " << cameraName << std::endl;
+ camDEPTH = new openniServer::CameraDEPTH(objPrefix,prop);
+ adapter->add(camDEPTH, ic->stringToIdentity(cameraName));
+ //test camera ok
+ std::cout<<" -------- openniServer: Component: CameraDEPTH created successfully --------" << std::endl;
+ }
+ if (pointCloud) {
+ std::string objPrefix(componentPrefix + ".PointCloud.");
+ std::string Name = prop->getProperty(objPrefix + "Name");
+ std::cout << "Creating pointcloud1 " << Name << std::endl;
+ pc1 = new openniServer::pointCloudI(objPrefix,prop);
+ adapter->add(pc1 , ic->stringToIdentity(Name));
+ adapter->add(pointcloud1, ic->stringToIdentity(Name));
+ std::cout<<" -------- openniServer: Component: PointCloud created successfully --------" << std::endl;
+ }
- adapter->activate();
- ic->waitForShutdown();
+ adapter->activate();
+ ic->waitForShutdown();
- if (!killed)
- exitApplication(0);
- return 0;
+ if (!killed)
+ exitApplication(0);
+ return 0;
}
More information about the Jderobot-admin
mailing list