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;
}
Ejemplo n.º 2
0
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);
    }

}