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;
}
Originally posted by Alkaros with karma: 103 on 2013-10-15
This answer was ACCEPTED on the original site
Post score: 4
Original comments
Comment by PKumars on 2016-02-17:
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.