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.