Storing kinect depth data from turtlebot in an array

Question

I'm trying to write a ros node that subscribes to the turtlebot kinect data streams and can retrieve a distance at a specific point in the image (for starters, it will just be the centre centre point) I've tried following the ros_bridge tutorials but I get errors when compiling and I'm not sure that that is the best route to take.

If it helps, the whole process I'm trying is to set up the turtlebot, have it decide on it target, get the distance to that target. Pass the observation into the pomdp node, and get an action out.

So far the pomdp node and action's are working, I just cant get data that I can manipulate out of the kinect depth topic.

---

Originally posted by Alkaros on ROS Answers with karma: 103 on 2013-10-07

Post score: 0

Answer 1

This function should do the trick and accounts for many cases, such as endianness and the raw and rectified depth image.

Of course, if you know the endianness of your system (and the image endianness doesn't change) as well as which depth image you are subscribed to (raw or rect) then this can be simplified a lot.

Note: the function returns the depth in millimeters.

typedef union U_FloatParse {
    float float_data;
    unsigned char byte_data[4];
} U_FloatConvert;

int ReadDepthData(unsigned int height_pos, unsigned int width_pos, sensor_msgs::ImageConstPtr depth_image)
{
    // If position is invalid
    if ((height_pos >= depth_image->height) || (width_pos >= depth_image->width))
        return -1;
    int index = (height_pos*depth_image->step) + (width_pos*(depth_image->step/depth_image->width));
    // If data is 4 byte floats (rectified depth image)
    if ((depth_image->step/depth_image->width) == 4) {
        U_FloatConvert depth_data;
        int i, endian_check = 1;
        // If big endian
        if ((depth_image->is_bigendian && (*(char*)&endian_check != 1)) ||  // Both big endian
           ((!depth_image->is_bigendian) && (*(char*)&endian_check == 1))) { // Both lil endian
            for (i = 0; i < 4; i++)
                depth_data.byte_data[i] = depth_image->data[index + i];
            // Make sure data is valid (check if NaN)
            if (depth_data.float_data == depth_data.float_data)
                return int(depth_data.float_data*1000);
            return -1;  // If depth data invalid
        }
        // else, one little endian, one big endian
        for (i = 0; i < 4; i++) 
            depth_data.byte_data[i] = depth_image->data[3 + index - i];
        // Make sure data is valid (check if NaN)
        if (depth_data.float_data == depth_data.float_data)
            return int(depth_data.float_data*1000);
        return -1;  // If depth data invalid
    }
    // Otherwise, data is 2 byte integers (raw depth image)
   int temp_val;
   // If big endian
   if (depth_image->is_bigendian)
       temp_val = (depth_image->data[index] << 8) + depth_image->data[index + 1];
   // If little endian
   else
       temp_val = depth_image->data[index] + (depth_image->data[index + 1] << 8);
   // Make sure data is valid (check if NaN)
   if (temp_val == temp_val)
       return temp_val;
   return -1;  // If depth data invalid
}

Hopefully this helps.

---

Originally posted by jdorich with karma: 211 on 2013-10-08

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by Alkaros on 2013-10-08:
For this, I just need to subscribe to the image stream and pass the desired coordinate and the image message in the function. No need for any opencv stuff?

Comment by jdorich on 2013-10-08:
Yes, if you just need the depth at a single coordinate in the depth image, this will work fine.