Ask Your Question
4

Get depth from Kinect sensor in gazebo simulator

asked 2013-10-14 05:54:15 -0600

maysamsh gravatar image

updated 2016-10-24 09:10:05 -0600

ngrennan gravatar image

I'm trying to find a specific pixel's depth of /depth/image_raw topic that is published by Kinect sensor mounted on Turtlebot robot.

It's what I get: www.uppic.com/do.php?img=97090) (Sorry I have not enough karma to upload image.

It's what I see in rviz: www.uppic.com/do.php?img=97092

How can I fix it to get depth of pixels?

edit retag flag offensive close merge delete

Comments

Hi, I am having trouble understanding your question. What does the first picture actually show and what do you want to do with the depth information? The second picture looks goot btw, so I think your sensor does work correct.

psei gravatar imagepsei ( 2013-10-15 01:20:25 -0600 )edit

I want to find distance of specific pixels

maysamsh gravatar imagemaysamsh ( 2013-10-15 03:43:28 -0600 )edit

I guess then you should take a look at the pointcloud topic that comes from the camera. It should contain distance information for every pixel.

psei gravatar imagepsei ( 2013-10-15 03:46:42 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
4

answered 2013-10-15 13:52:26 -0600

Alkaros gravatar image

updated 2013-10-15 13:55:59 -0600

Heres a function someone else on this board someone gave me. I'd also added the code that I used for my node.

The node subscribes to the image stream and for every image, prints the depth at 100, 320. It's not the most beautiful code but it works :)

#include <image_transport/image_transport.h>

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
}

// Image Callback
void imageCallback(const sensor_msgs::ImageConstPtr& image) {
    int depth = ReadDepthData(100, 320, image); // Width = 640, Height = 480
    ROS_INFO("Depth: %d", depth);
}

//*** Main ***//
int main(int argc, char **argv)
{
    ros::init(argc, argv, "imagegrab");
    ros::NodeHandle n;
    printf("READY to get image\n");
    image_transport::ImageTransport it(n);
    image_transport::Subscriber sub = it.subscribe("/camera/depth/image_raw", 1, imageCallback);
    ros::spin();
    return 0;
}
edit flag offensive delete link more

Comments

what if I want to subscribe both RGB and depth image at a time and then process both images simultaneously to get x,y,z in real-time? I'm trying to get position of a ball using Kinect in ROS_Indigo.

PKumars gravatar imagePKumars ( 2016-02-17 11:27:37 -0600 )edit
0

answered 2016-08-01 22:00:07 -0600

JunJun gravatar image

this fuction ReadDepthData return integer depth (mm) value ??

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-14 05:54:15 -0600

Seen: 6,288 times

Last updated: Aug 01 '16