From b30d440b3bfd7dc9c07b63bf4902ee605b5353ed Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Wed, 10 May 2017 18:20:27 +0200 Subject: [PATCH] [issue #803] Solved bug in JdeRobotComm camera client --- src/libs/jderobotcomm_cpp/CMakeLists.txt | 19 ++- .../jderobot/comm/ice/cameraIceClient.hpp | 1 + .../src/ice/cameraIceClient.cpp | 146 +----------------- src/tools/cameraview/CMakeLists.txt | 3 +- src/tools/kobukiViewer/CMakeLists.txt | 8 +- 5 files changed, 27 insertions(+), 150 deletions(-) diff --git a/src/libs/jderobotcomm_cpp/CMakeLists.txt b/src/libs/jderobotcomm_cpp/CMakeLists.txt index c7cf0d1e0..9bae3786b 100644 --- a/src/libs/jderobotcomm_cpp/CMakeLists.txt +++ b/src/libs/jderobotcomm_cpp/CMakeLists.txt @@ -29,7 +29,7 @@ include_directories( ${LIBS_DIR} ${catkin_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS} - ${ice_interfaces_INCLUDE_DIRS} + ${INTERFACES_CPP_DIR} ) @@ -84,12 +84,25 @@ ENDIF() ## Adding shared library for common usage add_library(${PROJECT_NAME} SHARED ${SOURCES} ${HEADERS}) -target_link_libraries(${PROJECT_NAME} ${Ice_LIBRARIES} logger ${Boost_LIBRARIES} colorspacesmm JderobotInterfaces ${catkin_LIBRARIES}) + +target_link_libraries(${PROJECT_NAME} + ${Ice_LIBRARIES} + logger + ${Boost_LIBRARIES} + colorspacesmm + ${catkin_LIBRARIES}) ## Adding static library for single .so configurations # since target is a shared library, -fPIC must be provided add_library(${PROJECT_NAME}-embedded STATIC ${SOURCES} ${HEADERS}) -target_link_libraries(${PROJECT_NAME}-embedded ${Ice_LIBRARIES} logger ${Boost_LIBRARIES} colorspacesmm JderobotInterfaces) + +target_link_libraries(${PROJECT_NAME}-embedded + ${Ice_LIBRARIES} + logger + ${Boost_LIBRARIES} + colorspacesmm + JderobotInterfaces + ${catkin_LIBRARIES}) set_property(TARGET ${PROJECT_NAME}-embedded PROPERTY POSITION_INDEPENDENT_CODE 1) diff --git a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/cameraIceClient.hpp b/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/cameraIceClient.hpp index 355a353aa..9783dac56 100644 --- a/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/cameraIceClient.hpp +++ b/src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/cameraIceClient.hpp @@ -31,6 +31,7 @@ #include #include #include +#include namespace JdeRobotComm { diff --git a/src/libs/jderobotcomm_cpp/src/ice/cameraIceClient.cpp b/src/libs/jderobotcomm_cpp/src/ice/cameraIceClient.cpp index 90f02a590..4c541607b 100644 --- a/src/libs/jderobotcomm_cpp/src/ice/cameraIceClient.cpp +++ b/src/libs/jderobotcomm_cpp/src/ice/cameraIceClient.cpp @@ -63,44 +63,7 @@ CameraIceClient::CameraIceClient(Ice::CommunicatorPtr ic, std::string prefix) { //check if default format is defined std::string definedFormat=prop->getPropertyWithDefault(prefix+".Format", "RGB8"); - // Discover what format are supported. - jderobot::ImageFormat formats = this->prx->getImageFormat(); - - std::vector::iterator it; - it = std::find(formats.begin(), formats.end(), definedFormat); - if (it==formats.end()){ - it = std::find(formats.begin(), formats.end(), colorspaces::ImageRGB8::FORMAT_RGB8.get()->name); - - if (it != formats.end()) - { - this->mImageFormat = colorspaces::ImageRGB8::FORMAT_RGB8.get()->name; - it = std::find(formats.begin(), formats.end(), colorspaces::ImageRGB8::FORMAT_RGB8_Z.get()->name); - if (it != formats.end()) - this->mImageFormat = colorspaces::ImageRGB8::FORMAT_RGB8_Z.get()->name; - } - else - { - it = std::find(formats.begin(), formats.end(), colorspaces::ImageRGB8::FORMAT_DEPTH8_16.get()->name); - if (it != formats.end()) - { - this->mImageFormat = colorspaces::ImageRGB8::FORMAT_DEPTH8_16.get()->name; - it = std::find(formats.begin(), formats.end(), colorspaces::ImageRGB8::FORMAT_DEPTH8_16_Z.get()->name); - if (it != formats.end()) - this->mImageFormat = colorspaces::ImageRGB8::FORMAT_DEPTH8_16_Z.get()->name; - } - else{ - this->mImageFormat = colorspaces::ImageGRAY8::FORMAT_GRAY8.get()->name; - it = std::find(formats.begin(), formats.end(), colorspaces::ImageGRAY8::FORMAT_GRAY8_Z.get()->name); - if (it != formats.end()) - this->mImageFormat = colorspaces::ImageGRAY8::FORMAT_GRAY8_Z.get()->name; - } - } - } - else{ - this->mImageFormat = definedFormat; - } - - LOG(INFO) << "Negotiated format " + this->mImageFormat + " for camera " + this->prx->getCameraDescription()->name; + this->mImageFormat = CameraUtils::negotiateDefaultFormat(this->prx,definedFormat); jderobot::ImageDataPtr data = this->prx->getImageData(this->mImageFormat); @@ -167,121 +130,20 @@ CameraIceClient::run(){ dataPtr = this->prx->getImageData(this->mImageFormat); - fmt = colorspaces::Image::Format::searchFormat(dataPtr->description->format); - if (!fmt) - throw "Format not supported"; + // Putting image data + img.data = CameraUtils::getImageFromCameraProxy(dataPtr); img.format = dataPtr->description->format; img.width = dataPtr->description->width; img.height = dataPtr->description->height; img.timeStamp = dataPtr->timeStamp.seconds + dataPtr->timeStamp.useconds * 1e-6; - //creating image cv::mat - if (dataPtr->description->format == colorspaces::ImageRGB8::FORMAT_RGB8_Z.get()->name || - dataPtr->description->format == colorspaces::ImageRGB8::FORMAT_DEPTH8_16_Z.get()->name ) - { - - size_t dest_len = dataPtr->description->width*dataPtr->description->height*3; - size_t source_len = dataPtr->pixelData.size(); - - unsigned char* origin_buf = (uchar*) malloc(dest_len); - - int r = uncompress((Bytef *) origin_buf, (uLongf *) &dest_len, (const Bytef *) &(dataPtr->pixelData[0]), (uLong)source_len); - - if(r != Z_OK) { - fprintf(stderr, "[CMPR] Error:\n"); - switch(r) { - case Z_MEM_ERROR: - fprintf(stderr, "[CMPR] Error: Not enough memory to compress.\n"); - break; - case Z_BUF_ERROR: - fprintf(stderr, "[CMPR] Error: Target buffer too small.\n"); - break; - case Z_STREAM_ERROR: // Invalid compression level - fprintf(stderr, "[CMPR] Error: Invalid compression level.\n"); - break; - } - } - else - { - colorspaces::Image imageRGB(dataPtr->description->width,dataPtr->description->height,colorspaces::ImageRGB8::FORMAT_RGB8,&(origin_buf[0])); - colorspaces::ImageRGB8 img_rgb888(imageRGB);//conversion will happen if needed - cv::Mat(cvSize(img_rgb888.width,img_rgb888.height), CV_8UC3, img_rgb888.data).copyTo(img.data); - - img_rgb888.release(); - } - - - if (origin_buf) - free(origin_buf); - - } - else if (dataPtr->description->format == colorspaces::ImageRGB8::FORMAT_RGB8.get()->name || - dataPtr->description->format == colorspaces::ImageRGB8::FORMAT_DEPTH8_16.get()->name ) - { - colorspaces::Image imageRGB(dataPtr->description->width,dataPtr->description->height,colorspaces::ImageRGB8::FORMAT_RGB8,&(dataPtr->pixelData[0])); - colorspaces::ImageRGB8 img_rgb888(imageRGB);//conversion will happen if needed - cv::Mat(cvSize(img_rgb888.width,img_rgb888.height), CV_8UC3, img_rgb888.data).copyTo(img.data); - - img_rgb888.release(); - } - else if (dataPtr->description->format == colorspaces::ImageGRAY8::FORMAT_GRAY8_Z.get()->name) { - //gay compressed - size_t dest_len = dataPtr->description->width*dataPtr->description->height; - size_t source_len = dataPtr->pixelData.size(); - - unsigned char* origin_buf = (uchar*) malloc(dest_len); - - int r = uncompress((Bytef *) origin_buf, (uLongf *) &dest_len, (const Bytef *) &(dataPtr->pixelData[0]), (uLong)source_len); - - if(r != Z_OK) { - fprintf(stderr, "[CMPR] Error:\n"); - switch(r) { - case Z_MEM_ERROR: - fprintf(stderr, "[CMPR] Error: Not enough memory to compress.\n"); - break; - case Z_BUF_ERROR: - fprintf(stderr, "[CMPR] Error: Target buffer too small.\n"); - break; - case Z_STREAM_ERROR: // Invalid compression level - fprintf(stderr, "[CMPR] Error: Invalid compression level.\n"); - break; - } - } - else - { - colorspaces::Image imageGray(dataPtr->description->width,dataPtr->description->height,colorspaces::ImageGRAY8::FORMAT_GRAY8,&(origin_buf[0])); - colorspaces::ImageGRAY8 img_gray8(imageGray);//conversion will happen if needed - - cv::Mat(cvSize(img_gray8.width,img_gray8.height), CV_8UC1, img_gray8.data).copyTo(img.data); - - img_gray8.release(); - } - - - if (origin_buf) - free(origin_buf); - } - else if (dataPtr->description->format == colorspaces::ImageGRAY8::FORMAT_GRAY8.get()->name){ - colorspaces::Image imageGray(dataPtr->description->width,dataPtr->description->height,colorspaces::ImageGRAY8::FORMAT_GRAY8,&(dataPtr->pixelData[0])); - colorspaces::ImageGRAY8 img_gray8(imageGray);//conversion will happen if needed - - cv::Mat(cvSize(img_gray8.width,img_gray8.height), CV_8UC1, img_gray8.data).copyTo(img.data); - - img_gray8.release(); - } - else{ - //TODO raise exception - } - - //end image cv::mat - } catch(...){ - LOG(INFO) << prefix +"error during request (connection error)"; + LOG(WARNING) << prefix +"error during request (connection error)"; usleep(50000); } diff --git a/src/tools/cameraview/CMakeLists.txt b/src/tools/cameraview/CMakeLists.txt index f71ba92e0..15c6a2266 100644 --- a/src/tools/cameraview/CMakeLists.txt +++ b/src/tools/cameraview/CMakeLists.txt @@ -39,7 +39,8 @@ TARGET_LINK_LIBRARIES(cameraview ${catkin_LIBRARIES} ${jderobotcomm_LIBRARIES} ${GLOG_LIBRARIES} - + JderobotInterfaces + jderobotutil ) install(TARGETS cameraview diff --git a/src/tools/kobukiViewer/CMakeLists.txt b/src/tools/kobukiViewer/CMakeLists.txt index 2305aec97..c411d57f4 100644 --- a/src/tools/kobukiViewer/CMakeLists.txt +++ b/src/tools/kobukiViewer/CMakeLists.txt @@ -38,6 +38,7 @@ if (${QT5_COMPILE} AND ${roscpp_FOUND}) ${jderobottypes_INCLUDE_DIRS} ${jderobotcomm_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS} + ${ZLIB_INCLUDE_DIRS} ) link_directories(${JDE_LIBS} @@ -57,13 +58,12 @@ if (${QT5_COMPILE} AND ${roscpp_FOUND}) ${easyiceconfig_LIBRARIES} ${jderobotcomm_LIBRARIES} ${ZeroCIce_LIBRARIES} - ${Ice_LIBRARIES} - ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} JderobotInterfaces jderobotutil ${catkin_LIBRARIES} - ${GLOG_LIBRARIES} - + ${GLOG_LIBRARIES} + ${ZLIB_LIBRARIES} + colorspacesmm ) install(TARGETS kobukiViewer DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/