Ask Your Question
0

Storing kinect depth data from turtlebot in an array.

asked 2013-10-07 17:02:03 -0500

Alkaros gravatar image

updated 2016-10-24 09:01:14 -0500

ngrennan gravatar image

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.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2013-10-08 13:21:46 -0500

jdorich gravatar image

updated 2013-10-08 13:23:49 -0500

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.

edit flag offensive delete link more

Comments

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?

Alkaros gravatar image Alkaros  ( 2013-10-08 13:29:05 -0500 )edit

Yes, if you just need the depth at a single coordinate in the depth image, this will work fine.

jdorich gravatar image jdorich  ( 2013-10-08 13:46:50 -0500 )edit
0

answered 2013-10-07 20:03:40 -0500

jorge gravatar image

Unless someone else gives you a better answer, I would take a look at the depthimage_to_laserscan package. It basically does what you need but it takes a central slide of the depth image instead of the central point, and generates a laser scan.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2013-10-07 17:02:03 -0500

Seen: 808 times

Last updated: Oct 08 '13