diff --git a/src/tools/rgbdViewer/CMakeLists.txt b/src/tools/rgbdViewer/CMakeLists.txt index 58a199734..edf7c637e 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}) @@ -29,21 +33,21 @@ TARGET_LINK_LIBRARIES(rgbdViewer ${libglademm_LIBRARIES} ${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} + JderobotInterfaces + jderobotutil ) 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..a181895d9 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cpp +++ b/src/tools/rgbdViewer/rgbdViewer.cpp @@ -31,10 +31,14 @@ #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,26 @@ 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; + std::cout<< di.format << std::endl; + + } } - if (pcClient) + /*if (pcClient) pcClient->getData(cloud); - +*/ if ((rgb.rows!=0)&&(depth.rows!=0)){ rgbdViewergui_ptx->updateAll(rgb,depth, cloud); } @@ -93,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())); @@ -135,6 +147,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 +157,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 +166,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 +177,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 +198,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 +209,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 +220,50 @@ 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(); + while (rgbSize == cv::Size(0, 0) || rgbSize == cv::Size(3, 3)) { + 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(); + while (depthSize == cv::Size(0, 0) || depthSize == cv::Size(3, 3)) { + temp = camDEPTH->getImage(); + depthSize = temp.data.size(); + //depthSize = cv::Size(temp.width, temp.height); } } } - 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.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); 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..0d982c075 --- /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: "/camera/rgb/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: "/camera/depth_registered/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 diff --git a/src/tools/rgbdViewer/rgbdViewergui.cpp b/src/tools/rgbdViewer/rgbdViewergui.cpp index 90245565f..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() { @@ -185,13 +188,14 @@ rgbdViewergui::updateAll( cv::Mat imageRGB, cv::Mat imageDEPTH, std::vectorm_distance.lock(); + 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); + distance->at(y,x) = (((int)layers[1].at(y,x)<<8)|(int)layers[2].at(y,x)); } } @@ -215,8 +219,9 @@ rgbdViewergui::updateAll( cv::Mat imageRGB, cv::Mat imageDEPTH, std::vectormy_expose_event(); while (gtkmain.events_pending()) @@ -482,6 +487,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;