Skip to content

Commit

Permalink
[issue JdeRobot#803] Solved bug in JdeRobotComm camera client
Browse files Browse the repository at this point in the history
  • Loading branch information
@aitormf
aitormf committed May 10, 2017
1 parent 728b692 commit b30d440
Show file tree
Hide file tree
Showing 5 changed files with 27 additions and 150 deletions.
19 changes: 16 additions & 3 deletions src/libs/jderobotcomm_cpp/CMakeLists.txt
View file
Expand Up @@ -29,7 +29,7 @@ include_directories(
${LIBS_DIR}
${catkin_INCLUDE_DIRS}
${roscpp_INCLUDE_DIRS}
${ice_interfaces_INCLUDE_DIRS}
${INTERFACES_CPP_DIR}
)


Expand Down Expand Up @@ -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)


Expand Down
1 change: 1 addition & 0 deletions src/libs/jderobotcomm_cpp/include/jderobot/comm/ice/cameraIceClient.hpp
View file
Expand Up @@ -31,6 +31,7 @@
#include <jderobot/types/image.h>
#include <jderobot/comm/interfaces/cameraClient.hpp>
#include <zlib.h>
#include <jderobotutil/CameraUtils.h>

namespace JdeRobotComm {

Expand Down
146 changes: 4 additions & 142 deletions src/libs/jderobotcomm_cpp/src/ice/cameraIceClient.cpp
View file
Expand Up @@ -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<std::string>::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);

Expand Down Expand Up @@ -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);

}
Expand Down
3 changes: 2 additions & 1 deletion src/tools/cameraview/CMakeLists.txt
View file
Expand Up @@ -39,7 +39,8 @@ TARGET_LINK_LIBRARIES(cameraview
${catkin_LIBRARIES}
${jderobotcomm_LIBRARIES}
${GLOG_LIBRARIES}

JderobotInterfaces
jderobotutil
)

install(TARGETS cameraview
Expand Down
8 changes: 4 additions & 4 deletions src/tools/kobukiViewer/CMakeLists.txt
View file
Expand Up @@ -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}
Expand All @@ -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/
Expand Down

0 comments on commit b30d440

Please sign in to comment.