From 892303232abcbaf55c82a1df5cc6789881bc8b9c Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Wed, 17 Jan 2018 16:19:26 +0100 Subject: [PATCH 1/4] [#1000] added comm to rgbdViewer --- src/tools/rgbdViewer/CMakeLists.txt | 14 ++-- src/tools/rgbdViewer/rgbdViewer.cfg | 27 ------- src/tools/rgbdViewer/rgbdViewer.cpp | 120 ++++++++++++++-------------- src/tools/rgbdViewer/rgbdViewer.yml | 52 ++++++++++++ 4 files changed, 121 insertions(+), 92 deletions(-) delete mode 100644 src/tools/rgbdViewer/rgbdViewer.cfg create mode 100644 src/tools/rgbdViewer/rgbdViewer.yml diff --git a/src/tools/rgbdViewer/CMakeLists.txt b/src/tools/rgbdViewer/CMakeLists.txt index 58a199734..7276c67f6 100644 --- a/src/tools/rgbdViewer/CMakeLists.txt +++ b/src/tools/rgbdViewer/CMakeLists.txt @@ -9,12 +9,16 @@ include_directories( ${libglademm_INCLUDE_DIRS} ${gtkglextmm_INCLUDE_DIRS} ${resourcelocator_INCLUDE_DIRS} - ${easyiceconfig_INCLUDE_DIRS} + ${jderobottypes_INCLUDE_DIRS} + ${comm_INCLUDE_DIRS} + ${config_INCLUDE_DIRS} + ${roscpp_INCLUDE_DIRS} ) link_directories( ${resourcelocator_LIBRARY_DIRS} - ${easyiceconfig_LIBRARY_DIRS} + ${comm_LIBRARY_DIRS} + ${config_LIBRARY_DIRS} ) add_executable (rgbdViewer ${SOURCE_FILES}) @@ -30,20 +34,20 @@ TARGET_LINK_LIBRARIES(rgbdViewer ${gtkglextmm_LIBRARIES} colorspacesmm JderobotInterfaces - parallelIce jderobotutil progeo geometry pioneer ${gsl_LIBRARIES} - ${easyiceconfig_LIBRARIES} ${ZeroCIce_LIBRARIES} ${ZLIB_LIBRARIES} ${resourcelocator_LIBRARIES} ${GLOG_LIBRARIES} + ${comm_LIBRARIES} + ${config_LIBRARIES} ) INSTALL (TARGETS rgbdViewer DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/ COMPONENT rgbdViewer) INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/rgbdViewergui.glade DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/glade COMPONENT rgbdViewer) -INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/rgbdViewer.cfg DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT rgbdViewer) +INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/rgbdViewer.yml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT rgbdViewer) INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/camera-0.cfg DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT rgbdViewer) diff --git a/src/tools/rgbdViewer/rgbdViewer.cfg b/src/tools/rgbdViewer/rgbdViewer.cfg deleted file mode 100644 index 57c218358..000000000 --- a/src/tools/rgbdViewer/rgbdViewer.cfg +++ /dev/null @@ -1,27 +0,0 @@ -rgbdViewer.CameraRGBActive=1 -rgbdViewer.CameraDEPTHActive=1 -rgbdViewer.RGBDActive=0 - -rgbdViewer.RGBD.Proxy=rgbd1:tcp -h localhost -p 9999 - -#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.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.pointCloudActive=0 -#rgbdViewer.pointCloud.Proxy=pointcloud1:tcp -h 193.147.14.20 -p 9998 -rgbdViewer.pointCloud.Fps=10 -rgbdViewer.pointCloud.Proxy=pointcloud1:tcp -h localhost -p 9999 -rgbdViewer.Pose3DMotorsActive=0 -rgbdViewer.Pose3DMotors.Proxy=Pose3DMotors1:tcp -h 193.147.14.20 -p 9999 -rgbdViewer.KinectLedsActive=0 -rgbdViewer.KinectLeds.Proxy=kinectleds1:tcp -h 193.147.14.20 -p 9999 -rgbdViewer.WorldFile=./config/fempsa/fempsa.cfg -#rgbdViewer.camRGB=./config/joseMaria/CameraAEsquina.xml -#rgbdViewer.camIR=./config/joseMaria/CameraAEsquina.xml -rgbdViewer.Width=640 -rgbdViewer.Height=480 -rgbdViewer.Fps=15 -rgbdViewer.Debug=1 diff --git a/src/tools/rgbdViewer/rgbdViewer.cpp b/src/tools/rgbdViewer/rgbdViewer.cpp index 704bca39a..a3c918658 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cpp +++ b/src/tools/rgbdViewer/rgbdViewer.cpp @@ -26,15 +26,19 @@ #include #include #include -#include +//#include #include #include #include "rgbdViewergui.h" #include "pthread.h" -#include "parallelIce/cameraClient.h" -#include "parallelIce/pointcloudClient.h" +#include +#include +#include +#include +#include +#include +#include -#include "easyiceconfig/EasyIce.h" @@ -42,10 +46,12 @@ rgbdViewer::rgbdViewergui* rgbdViewergui_ptx; -jderobot::CameraClientPtr camRGB; -jderobot::CameraClientPtr camDEPTH; -jderobot::PointcloudClientPtr pcClient; -jderobot::rgbdPrx rgbClient; + +//jderobot::PointcloudClientPtr pcClient; + +Comm::RgbdClient* rgbClient; +Comm::CameraClient* camRGB; +Comm::CameraClient* camDEPTH; bool cameraRGBDActive=false; @@ -65,20 +71,24 @@ void *gui_thread(void* arg){ while(rgbdViewergui_ptx->isVisible() && ! rgbdViewergui_ptx->isClosed()){ if (cameraRGBDActive) { - auto data = rgbClient->getData(); - rgb = CameraUtils::getImageFromCameraProxy(data.color); - depth = CameraUtils::getImageFromCameraProxy(data.depth); + JdeRobotTypes::Rgbd data = rgbClient->getRgbd(); + rgb = data.color.data; + depth = data.depth.data; } else { - if (camRGB) - camRGB->getImage(rgb); - if (camDEPTH) - camDEPTH->getImage(depth); + if (camRGB){ + JdeRobotTypes::Image rgbi = camRGB->getImage(); + rgb = rgbi.data; + } + if (camDEPTH){ + JdeRobotTypes::Image di = camDEPTH->getImage(); + depth = di.data; + } } - if (pcClient) + /*if (pcClient) pcClient->getData(cloud); - +*/ if ((rgb.rows!=0)&&(depth.rows!=0)){ rgbdViewergui_ptx->updateAll(rgb,depth, cloud); } @@ -135,6 +145,9 @@ int main(int argc, char** argv){ bool cameraRGBActive=false; bool cameraDepthActive=false; + Config::Properties cfg = Config::load(argc, argv); + Comm::Communicator* jdrc = new Comm::Communicator(cfg); + @@ -142,8 +155,7 @@ int main(int argc, char** argv){ pthread_attr_init(&attr); pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE); try{ - ic = EasyIce::initialize(argc,argv); - prop = ic->getProperties(); + ic = Ice::initialize(argc, argv); }catch (const Ice::Exception& ex) { std::cerr << ex << std::endl; return 1; @@ -152,9 +164,9 @@ int main(int argc, char** argv){ std::cerr <<"Error :" << msg << std::endl; return 1; } - cameraRGBActive=(bool)prop->getPropertyAsIntWithDefault("rgbdViewer.CameraRGBActive",0); - cameraDepthActive=(bool)prop->getPropertyAsIntWithDefault("rgbdViewer.CameraDEPTHActive",0); - cameraRGBDActive=(bool)prop->getPropertyAsIntWithDefault("rgbdViewer.RGBDActive",0); + cameraRGBActive=(bool)cfg.asIntWithDefault("rgbdViewer.CameraRGB.Server",0); + cameraDepthActive=(bool)cfg.asIntWithDefault("rgbdViewer.CameraDEPTH.Server",0); + cameraRGBDActive=(bool)cfg.asIntWithDefault("rgbdViewer.RGBD.Server",0); if (cameraRGBDActive && (cameraRGBActive || cameraDepthActive)){ LOG(ERROR) << "RGBD and single cameras cannot be selected at the same time"; @@ -163,34 +175,20 @@ int main(int argc, char** argv){ if (cameraRGBDActive) { - std::string prefix = "rgbdViewer.RGBD."; - try{ - auto base = ic->propertyToProxy(prefix+"Proxy"); - if (0==base){ - throw prefix + "Could not create proxy with RGBD"; - } - else { - rgbClient = jderobot::rgbdPrx::checkedCast(base); - if (0==rgbClient) - throw "Invalid " + prefix + ".Proxy"; + rgbClient = Comm::getRgbdClient(jdrc, "rgbdViewer.RGBD"); + if (rgbClient != NULL) { + rgbCamSelected = true; + create_gui = true; + depthCamSelected=true; + } else { + throw "rgbdViewer: failed to load RGBD interface"; } - }catch (const Ice::Exception& ex) { - std::cerr << ex << std::endl; - } - catch (const char* msg) { - std::cerr << msg << std::endl; - LOG(FATAL) << prefix + " Not camera provided"; - } - create_gui = true; - rgbCamSelected=true; - depthCamSelected=true; } else{ if (cameraRGBActive) { - camRGB = jderobot::CameraClientPtr(new jderobot::cameraClient(ic, "rgbdViewer.CameraRGB.")); + camRGB = Comm::getCameraClient(jdrc, "rgbdViewer.CameraRGB"); if (camRGB != NULL) { rgbCamSelected = true; - camRGB->start(); create_gui = true; } else { throw "rgbdViewer: failed to load RGB Camera"; @@ -198,10 +196,9 @@ int main(int argc, char** argv){ } if (cameraDepthActive) { - camDEPTH =jderobot::CameraClientPtr( new jderobot::cameraClient(ic, "rgbdViewer.CameraDEPTH.")); + camDEPTH =Comm::getCameraClient(jdrc, "rgbdViewer.CameraDEPTH"); if (camDEPTH != NULL) { depthCamSelected = true; - camDEPTH->start(); create_gui = true; } else { throw "rgbdViewer: failed to load DEPTH Camera"; @@ -210,7 +207,7 @@ int main(int argc, char** argv){ } - if (prop->getPropertyAsIntWithDefault("rgbdViewer.pointCloudActive",0)){ + /*if (prop->getPropertyAsIntWithDefault("rgbdViewer.pointCloudActive",0)){ pcClient = jderobot::PointcloudClientPtr(new jderobot::pointcloudClient(ic,"rgbdViewer.pointCloud.")); if (pcClient!= NULL){ pcClient->start(); @@ -221,44 +218,47 @@ int main(int argc, char** argv){ throw "rgbdViewer: failed to load pointCloud"; } } - +*/ //set the sizes of each image cv::Size rgbSize(0,0); cv::Size depthSize(0,0); + JdeRobotTypes::Image temp; //rgb if (cameraRGBDActive) { while ((rgbSize == cv::Size(0, 0)) && (depthSize == cv::Size(0, 0))) { - auto data = rgbClient->getData(); - rgbSize = CameraUtils::getImageFromCameraProxy(data.color).size(); - depthSize = CameraUtils::getImageFromCameraProxy(data.depth).size(); + JdeRobotTypes::Rgbd data = rgbClient->getRgbd(); + rgbSize = data.color.data.size(); + depthSize = data.depth.data.size(); } } else { if (rgbCamSelected) { while (rgbSize == cv::Size(0, 0)) { - cv::Mat temp; - camRGB->getImage(temp, true); - rgbSize = temp.size(); + temp = camRGB->getImage(); + rgbSize = temp.data.size(); } } //depth if (depthCamSelected) { while (depthSize == cv::Size(0, 0)) { - cv::Mat temp; - camDEPTH->getImage(temp, true); - depthSize = temp.size(); + temp = camDEPTH->getImage(); + depthSize = temp.data.size();; } } } - debug=prop->getPropertyAsIntWithDefault("rgbdViewer.Debug",320); - int fps=prop->getPropertyAsIntWithDefault("rgbdViewer.Fps",0); + debug=cfg.asIntWithDefault("rgbdViewer.Debug",320); + int fps=cfg.asIntWithDefault("rgbdViewer.Fps",0); float cycle=(float)(1/(float)fps)*1000000; - rgbdViewergui_ptx = new rgbdViewer::rgbdViewergui(rgbCamSelected,depthCamSelected, pointCloudSelected, prop->getProperty("rgbdViewer.WorldFile"), prop->getProperty("rgbdViewer.camRGB"), prop->getProperty("rgbdViewer.camIR"),rgbSize,depthSize, cycle); + std::string worldfile = cfg.asString("rgbdViewer.WorldFile"); + std::string camRGBFile = cfg.asString("rgbdViewer.camRGB"); + std::string camIRFile = cfg.asString("rgbdViewer.camIR"); + + rgbdViewergui_ptx = new rgbdViewer::rgbdViewergui(rgbCamSelected,depthCamSelected, pointCloudSelected, worldfile, camRGBFile, camIRFile,rgbSize,depthSize, cycle); std::cout << create_gui << std::endl; if (create_gui){ diff --git a/src/tools/rgbdViewer/rgbdViewer.yml b/src/tools/rgbdViewer/rgbdViewer.yml new file mode 100644 index 000000000..3cc1f4c9e --- /dev/null +++ b/src/tools/rgbdViewer/rgbdViewer.yml @@ -0,0 +1,52 @@ +rgbdViewer: + CameraRGB: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "cameraA:tcp -h localhost -p 9999" + Format: RGB8 + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: cameraA + Fps: 30 + + CameraDEPTH: + Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "cameraB:tcp -h localhost -p 9999" + Format: RGB8 + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: cameraB + Fps: 30 + + PointCloud: + Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "pointcloud1:tcp -h localhost -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: pointcloud + Fps: 30 + + RGBD: + Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "rgbd1:tcp -h localhost -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: RGBD + Fps: 30 + + Pose3DMotors: + Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "Pose3DMotors1:tcp -h 193.147.14.20 -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: Pose3DMotors + + KinectLeds: + Server: 0 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS + Proxy: "kinectleds1:tcp -h 193.147.14.20 -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: KinectLeds + + + NodeName: rgbdViewer + #camRGB: "./config/joseMaria/CameraAEsquina.xml" + #camIR: "./config/joseMaria/CameraAEsquina.xml" + WorldFile: "./config/fempsa/fempsa.cfg" + Width: 640 + Height: 480 + Fps: 15 + Debug: 1 From 5a46dc75e65d021a7c4421af749895207f7b52b9 Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Mon, 29 Jan 2018 12:39:25 +0100 Subject: [PATCH 2/4] [#1000] Updated CMakelists --- src/tools/rgbdViewer/CMakeLists.txt | 4 ++-- src/tools/rgbdViewer/rgbdViewer.cpp | 15 ++++++++++----- src/tools/rgbdViewer/rgbdViewergui.cpp | 23 ++++++++++++++++++----- 3 files changed, 30 insertions(+), 12 deletions(-) diff --git a/src/tools/rgbdViewer/CMakeLists.txt b/src/tools/rgbdViewer/CMakeLists.txt index 7276c67f6..edf7c637e 100644 --- a/src/tools/rgbdViewer/CMakeLists.txt +++ b/src/tools/rgbdViewer/CMakeLists.txt @@ -33,8 +33,6 @@ TARGET_LINK_LIBRARIES(rgbdViewer ${libglademm_LIBRARIES} ${gtkglextmm_LIBRARIES} colorspacesmm - JderobotInterfaces - jderobotutil progeo geometry pioneer @@ -45,6 +43,8 @@ TARGET_LINK_LIBRARIES(rgbdViewer ${GLOG_LIBRARIES} ${comm_LIBRARIES} ${config_LIBRARIES} + JderobotInterfaces + jderobotutil ) INSTALL (TARGETS rgbdViewer DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/ COMPONENT rgbdViewer) diff --git a/src/tools/rgbdViewer/rgbdViewer.cpp b/src/tools/rgbdViewer/rgbdViewer.cpp index a3c918658..b0d49e4e3 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cpp +++ b/src/tools/rgbdViewer/rgbdViewer.cpp @@ -26,7 +26,7 @@ #include #include #include -//#include +#include #include #include #include "rgbdViewergui.h" @@ -83,6 +83,8 @@ void *gui_thread(void* arg){ if (camDEPTH){ JdeRobotTypes::Image di = camDEPTH->getImage(); depth = di.data; + std::cout<< di.format << std::endl; + } } /*if (pcClient) @@ -103,7 +105,7 @@ void *gui_thread(void* arg){ } if (((IceUtil::Time::now().toMicroSeconds() - lastIT.toMicroSeconds()) > rgbdViewergui_ptx->getCycle() )){ if (debug) - std::cout<<"-------- rgbdViewer: timeout-" << std::endl; + ;//std::cout<<"-------- rgbdViewer: timeout-" << std::endl; } else{ usleep(rgbdViewergui_ptx->getCycle() - (IceUtil::Time::now().toMicroSeconds() - lastIT.toMicroSeconds())); @@ -233,7 +235,7 @@ int main(int argc, char** argv){ } else { if (rgbCamSelected) { - while (rgbSize == cv::Size(0, 0)) { + while (rgbSize == cv::Size(0, 0) || rgbSize == cv::Size(3, 3)) { temp = camRGB->getImage(); rgbSize = temp.data.size(); @@ -241,15 +243,18 @@ int main(int argc, char** argv){ } //depth if (depthCamSelected) { - while (depthSize == cv::Size(0, 0)) { + while (depthSize == cv::Size(0, 0) || depthSize == cv::Size(3, 3)) { temp = camDEPTH->getImage(); - depthSize = temp.data.size();; + depthSize = temp.data.size(); + //depthSize = cv::Size(temp.width, temp.height); } } } + + std::cout << std::endl<< std::endl<< std::endl<< depthSize << " " << rgbSize << std::endl<< std::endl<< std::endl<< std::endl<< std::endl; debug=cfg.asIntWithDefault("rgbdViewer.Debug",320); int fps=cfg.asIntWithDefault("rgbdViewer.Fps",0); float cycle=(float)(1/(float)fps)*1000000; diff --git a/src/tools/rgbdViewer/rgbdViewergui.cpp b/src/tools/rgbdViewer/rgbdViewergui.cpp index 90245565f..5ddc08c50 100644 --- a/src/tools/rgbdViewer/rgbdViewergui.cpp +++ b/src/tools/rgbdViewer/rgbdViewergui.cpp @@ -185,13 +185,19 @@ rgbdViewergui::updateAll( cv::Mat imageRGB, cv::Mat imageDEPTH, std::vectorm_distance.lock(); + //this->distance = new cv::Mat(imageDEPTH.size(),CV_32FC1,cv::Scalar(0,0,0)); + //cv::resize(*distance, *distance, imageDEPTH.size()); for (int x=0; x< layers[1].cols ; x++) { for (int y=0; yat(y,x) = ((int)layers[1].at(y,x)<<8)|(int)layers[2].at(y,x); + //std::cout << distance->cols << " " << distance->rows << std::endl; + //std::cout << layers[1].cols << " " << layers[1].rows << std::endl; + distance->at(y,x) = (((int)layers[1].at(y,x)<<8)|(int)layers[2].at(y,x))/1000; + //std::cout << (int)layers[1].at(y,x) << " " << (int)layers[2].at(y,x) << std::endl; + //std::cout << (((int)layers[1].at(y,x)<<8)|(int)layers[2].at(y,x)) << std::endl; } } @@ -482,6 +488,7 @@ void rgbdViewergui::add_cameras_position() { void rgbdViewergui::on_reconstruct_depth() { + std::cout << std::endl<< std::endl<< std::endl<< w_reconstruct->get_active() << std::endl<< std::endl<< std::endl<< std::endl; if (w_reconstruct->get_active()) { reconstruct_depth_activate=true; world->draw_kinect_points=true; @@ -558,10 +565,16 @@ rgbdViewergui::add_depth_pointsImage(cv::Mat imageRGB, cv::Mat distance) { if (w_realDistance->get_active()){ - world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,color(0),color(1),color(2)); + //world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,color(0),color(1),color(2)); + for (float i=0; i< 1000000; i+=1){ + world->add_kinect_point(1,1,i,0,0,0); + } } else{ - world->add_kinect_point(d*ux + camx,d*uy+ camy,d*uz + camz,color(0),color(1),color(2)); + //world->add_kinect_point(d*ux + camx,d*uy+ camy,d*uz + camz,color(0),color(1),color(2)); + for (float i=0; i< 1000000; i+=1){ + world->add_kinect_point(1,1,i,0,0,0); + } } } From f552600261036954d7a6e015fad9e3400b6e8a93 Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Wed, 31 Jan 2018 16:46:22 +0100 Subject: [PATCH 3/4] [#1000] solved bug with 3d reconstruction of rgbdViewer --- src/tools/rgbdViewer/rgbdViewer.cpp | 8 ++++---- src/tools/rgbdViewer/rgbdViewer.yml | 2 +- src/tools/rgbdViewer/rgbdViewergui.cpp | 25 +++++++++---------------- 3 files changed, 14 insertions(+), 21 deletions(-) diff --git a/src/tools/rgbdViewer/rgbdViewer.cpp b/src/tools/rgbdViewer/rgbdViewer.cpp index b0d49e4e3..a181895d9 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cpp +++ b/src/tools/rgbdViewer/rgbdViewer.cpp @@ -254,14 +254,14 @@ int main(int argc, char** argv){ - std::cout << std::endl<< std::endl<< std::endl<< depthSize << " " << rgbSize << std::endl<< std::endl<< std::endl<< std::endl<< std::endl; debug=cfg.asIntWithDefault("rgbdViewer.Debug",320); int fps=cfg.asIntWithDefault("rgbdViewer.Fps",0); float cycle=(float)(1/(float)fps)*1000000; - std::string worldfile = cfg.asString("rgbdViewer.WorldFile"); - std::string camRGBFile = cfg.asString("rgbdViewer.camRGB"); - std::string camIRFile = cfg.asString("rgbdViewer.camIR"); + std::string worldfile = cfg.asStringWithDefault("rgbdViewer.WorldFile", ""); + std::string camRGBFile = cfg.asStringWithDefault("rgbdViewer.camRGB", ""); + std::string camIRFile = cfg.asStringWithDefault("rgbdViewer.camIR", ""); + rgbdViewergui_ptx = new rgbdViewer::rgbdViewergui(rgbCamSelected,depthCamSelected, pointCloudSelected, worldfile, camRGBFile, camIRFile,rgbSize,depthSize, cycle); diff --git a/src/tools/rgbdViewer/rgbdViewer.yml b/src/tools/rgbdViewer/rgbdViewer.yml index 3cc1f4c9e..cee40061d 100644 --- a/src/tools/rgbdViewer/rgbdViewer.yml +++ b/src/tools/rgbdViewer/rgbdViewer.yml @@ -45,7 +45,7 @@ rgbdViewer: NodeName: rgbdViewer #camRGB: "./config/joseMaria/CameraAEsquina.xml" #camIR: "./config/joseMaria/CameraAEsquina.xml" - WorldFile: "./config/fempsa/fempsa.cfg" + #WorldFile: "./config/fempsa/fempsa.cfg" Width: 640 Height: 480 Fps: 15 diff --git a/src/tools/rgbdViewer/rgbdViewergui.cpp b/src/tools/rgbdViewer/rgbdViewergui.cpp index 5ddc08c50..0d05d2c5b 100644 --- a/src/tools/rgbdViewer/rgbdViewergui.cpp +++ b/src/tools/rgbdViewer/rgbdViewergui.cpp @@ -151,6 +151,9 @@ rgbdViewergui::rgbdViewergui(bool rgb, bool depth,bool pointCloud , std::string mainwindow->show(); this->distance = new cv::Mat(cv::Size(myDepthSize.width, myDepthSize.height),CV_32FC1,cv::Scalar(0,0,0)); + /*for (float i=0; i< 10000; i+=1){ + world->add_kinect_point(1,1,i,0,0,0); + }*/ } rgbdViewergui::~rgbdViewergui() { @@ -189,15 +192,10 @@ rgbdViewergui::updateAll( cv::Mat imageRGB, cv::Mat imageDEPTH, std::vectorm_distance.lock(); - //this->distance = new cv::Mat(imageDEPTH.size(),CV_32FC1,cv::Scalar(0,0,0)); - //cv::resize(*distance, *distance, imageDEPTH.size()); + for (int x=0; x< layers[1].cols ; x++) { for (int y=0; ycols << " " << distance->rows << std::endl; - //std::cout << layers[1].cols << " " << layers[1].rows << std::endl; - distance->at(y,x) = (((int)layers[1].at(y,x)<<8)|(int)layers[2].at(y,x))/1000; - //std::cout << (int)layers[1].at(y,x) << " " << (int)layers[2].at(y,x) << std::endl; - //std::cout << (((int)layers[1].at(y,x)<<8)|(int)layers[2].at(y,x)) << std::endl; + distance->at(y,x) = (((int)layers[1].at(y,x)<<8)|(int)layers[2].at(y,x)); } } @@ -221,8 +219,9 @@ rgbdViewergui::updateAll( cv::Mat imageRGB, cv::Mat imageDEPTH, std::vectormy_expose_event(); while (gtkmain.events_pending()) @@ -565,16 +564,10 @@ rgbdViewergui::add_depth_pointsImage(cv::Mat imageRGB, cv::Mat distance) { if (w_realDistance->get_active()){ - //world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,color(0),color(1),color(2)); - for (float i=0; i< 1000000; i+=1){ - world->add_kinect_point(1,1,i,0,0,0); - } + world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,color(0),color(1),color(2)); } else{ - //world->add_kinect_point(d*ux + camx,d*uy+ camy,d*uz + camz,color(0),color(1),color(2)); - for (float i=0; i< 1000000; i+=1){ - world->add_kinect_point(1,1,i,0,0,0); - } + world->add_kinect_point(d*ux + camx,d*uy+ camy,d*uz + camz,color(0),color(1),color(2)); } } From 4f85900d97c88ba75aa6e0bb6a39b6f790917921 Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Fri, 2 Feb 2018 09:50:21 +0100 Subject: [PATCH 4/4] [#1000] Umpdated rgbViewer.yml with rostopics --- src/tools/rgbdViewer/rgbdViewer.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/tools/rgbdViewer/rgbdViewer.yml b/src/tools/rgbdViewer/rgbdViewer.yml index cee40061d..0d982c075 100644 --- a/src/tools/rgbdViewer/rgbdViewer.yml +++ b/src/tools/rgbdViewer/rgbdViewer.yml @@ -3,7 +3,7 @@ rgbdViewer: Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS Proxy: "cameraA:tcp -h localhost -p 9999" Format: RGB8 - Topic: "/TurtlebotROS/cameraL/image_raw" + Topic: "/camera/rgb/image_raw" Name: cameraA Fps: 30 @@ -11,7 +11,7 @@ rgbdViewer: Server: 1 # 0 -> Deactivate, 1 -> Ice , 2 -> ROS Proxy: "cameraB:tcp -h localhost -p 9999" Format: RGB8 - Topic: "/TurtlebotROS/cameraL/image_raw" + Topic: "/camera/depth_registered/image_raw" Name: cameraB Fps: 30