bool USBCameraDriverRgb::getImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& image)
{
if( (image.width() != _width) || (image.height() != _height) )
image.resize(_width, _height);
deviceRgb->getRgbBuffer(image.getRawImage());
return true;
}
bool yarp::dev::GazeboYarpMultiCameraDriver::getImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& _image)
{
for (unsigned int i = 0; i < m_camera_count; ++i) {
m_dataMutex[i]->wait();
}
if (m_vertical) {
_image.resize(m_max_width, m_max_height * m_camera_count);
} else {
_image.resize(m_max_width * m_camera_count, m_max_height);
}
unsigned char *pBuffer = _image.getRawImage();
memset(pBuffer, 0, _image.getRawImageSize());
for (unsigned int i = 0; i < m_camera_count; ++i) {
int rowsize = 3 * m_width[i];
int rowoffset = i * 3 * m_max_width;
if (m_vertical) {
if (m_max_width == m_width[i] && m_max_height == m_height[i]) {
memcpy(pBuffer + (3 * m_max_width * m_max_height * i), m_imageBuffer[i], m_bufferSize[i]);
} else {
for (int r = 0; r < m_height[i]; r++) {
memcpy(pBuffer + (3 * m_max_width) * (m_max_height * i + r), m_imageBuffer[i] + rowsize * r, rowsize);
}
}
} else {
for (int r = 0; r < m_height[i]; r++) {
memcpy(pBuffer + (3 * m_max_width * m_camera_count * r) + rowoffset, m_imageBuffer[i] + rowsize * r, rowsize);
}
}
}
if (m_display_time_box) {
m_counter = (++m_counter % 10);
for (int c = m_counter*30; c < 30 + m_counter*30; c++) {
for (int r=0; r<30; r++) {
for (unsigned int i = 0; i < m_camera_count; ++i) {
unsigned char *pixel;
if (m_vertical) {
pixel = _image.getPixelAddress(m_width[i]-c-1, m_max_height * i + m_height[i]-r-1);
} else {
pixel = _image.getPixelAddress(m_max_width * i + m_width[i]-c-1, m_height[i]-r-1);
}
pixel[0] = (m_counter % 2 == 0) ? 255 : 0;
pixel[1] = (m_counter % 2 == 0) ? 0 : 255;
pixel[2] = 0;
}
}
}
}
for (unsigned int i = 0; i < m_camera_count; ++i) {
m_dataMutex[i]->post();
}
return true;
}
void InputCallback::onRead(yarp::sig::ImageOf<yarp::sig::PixelRgba> &img)
#endif
{
uchar *tmpBuf;
QSize s = (QSize(img.width(),img.height()));
#if QT_VERSION >= 0x050302
int imgSize = img.getRawImageSize();
#else
int imgSize = s.width() * s.height() * img.getPixelSize();
#endif
// Allocate a QVideoFrame
QVideoFrame frame(imgSize,
s,
#if QT_VERSION >= 0x050302
img.getRowSize(),
#else
s.width() * img.getPixelSize(),
#endif
QVideoFrame::Format_RGB32);
// Maps the buffer
frame.map(QAbstractVideoBuffer::WriteOnly);
// Takes the ownership of the buffer in write only mode
tmpBuf = frame.bits();
unsigned char *rawImg = img.getRawImage();
//int j = 0;
// Inverts the planes because Qt Wants an image in RGB format instead of BGR
/* for(int i=0; i<imgSize; i++){
tmpBuf[j+2] = rawImg[i];
i++;
tmpBuf[j+1] = rawImg[i];
i++;
tmpBuf[j] = rawImg[i];
tmpBuf[j+3] = 0;
j+=4;
}*/
#if QT_VERSION >= 0x050302
memcpy(tmpBuf,rawImg,imgSize);
#else
for(int x =0; x < s.height(); x++) {
memcpy(tmpBuf + x * (img.width() * img.getPixelSize()),
rawImg + x * (img.getRowSize()),
img.width() * img.getPixelSize());
}
#endif
//unmap the buffer
frame.unmap();
if(sigHandler){
sigHandler->sendVideoFrame(frame);
}
}
bool realsense2Driver::getImage(yarp::sig::ImageOf<yarp::sig::PixelMono>& image)
{
if (!m_stereoMode)
{
yError()<<"realsense2Driver: infrared stereo stream not enabled";
return false;
}
image.resize(width(), height());
std::lock_guard<std::mutex> guard(m_mutex);
rs2::frameset data = m_pipeline.wait_for_frames();
rs2::video_frame frm1 = data.get_infrared_frame(1);
rs2::video_frame frm2 = data.get_infrared_frame(2);
int pixCode = pixFormatToCode(frm1.get_profile().format());
if (pixCode != VOCAB_PIXEL_MONO && pixCode != VOCAB_PIXEL_MONO16)
{
yError() << "realsense2Driver: expecting Pixel Format MONO or MONO16";
return false;
}
size_t singleImage_rowSizeByte = image.getRowSize()/2;
unsigned char * pixelLeft = (unsigned char*) (frm1.get_data());
unsigned char * pixelRight = (unsigned char*) (frm2.get_data());
unsigned char * pixelOutLeft = image.getRawImage();
unsigned char * pixelOutRight = image.getRawImage() + singleImage_rowSizeByte;
for (size_t h=0; h< image.height(); h++)
{
memcpy(pixelOutLeft, pixelLeft, singleImage_rowSizeByte);
memcpy(pixelOutRight, pixelRight, singleImage_rowSizeByte);
pixelOutLeft += 2*singleImage_rowSizeByte;
pixelOutRight += 2*singleImage_rowSizeByte;
pixelLeft += singleImage_rowSizeByte;
pixelRight += singleImage_rowSizeByte;
}
return true;
}