How to publish images from rosserial windows

asked 2016-11-24 04:51:56 -0600

anflores gravatar image

Hi all!, I am working on a project which goal is to publish in ros speech commands, skeleton and color and depth images from the kinect v1 using the SDK of windows. This already work for speech and skeleton joints via rosserial in windows: http://wiki.ros.org/rosserial_windows...

Now I am trying to publish the color images from the kinect v1 via sensor_msgs/Image. The problem is that rosserial does not provide the cv_bridge normally used in ros for images, so I have to create the sensor_msgs/Image from scratch. Im not sure how to deal with this (maybe there is a easier way, any suggestion?)

This is the code that I have so far. It actually publishes the Image, but in a wrong way since I am not able to visualizate it in rviz.

    img.header.stamp = nh->now();
    img.header.frame_id = "kinect1_ref";
    img.height = height;
    img.width = width;
    img.encoding = "rgb8";
    img.is_bigendian = false;
    img.step = width*3;


    const BYTE* start = (const BYTE*) lockedRect.pBits;
    img.data_length = width*3*height;

    img.data = new uint8_t[width*3*height];

    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            // Determine rgb color for each pixel
            const BYTE* curr = start + (x + width*y)*4;
            for(int n=0; n<3; n++){      // Loop for RGB channel
                long it = y*width*3 + x*3 + n;
                img.data[it] =  (uint8_t) curr[2-n]/1.0f;
            }
        }
    }

I am not familiar with the way of encoding images. In the code, img is the sensor_msgs/Image message. On the other hand, lockedRect.pBits is a pointer to the first byte of the image of the kinect. BYTE is defined as a typedef unsigned char. I do not know exactly how to convert BYTE to uint_t which is actually another unsigned char. The image provided by the SDK of the kinect, as far as I read, is stored row wise, in top-to-bottom left-to-right order, with each pixel being represented by 4 sequential bytes representing a padding byte then R, G and B follwing it.

Any help would be really appreciated! Thank you

edit retag flag offensive close merge delete