Commit bd32b053 authored by Dinesh Thakur's avatar Dinesh Thakur
Browse files

copy correct depth bits from royale depthImage

parent 1fc2e182
......@@ -1222,7 +1222,7 @@ void PerCameraMgr::InitializePointCloudMessage(int imageHeight, int imageWidth,
// Defines the format of the depthConfidence channel.
m_pointCloudMsg.fields[index].name = "confidence";
m_pointCloudMsg.fields[index].offset = m_pointCloudMsg.fields[index - 1].offset + sizeof(float);
m_pointCloudMsg.fields[index].offset = m_pointCloudMsg.fields[index - 1].offset + sizeof(uint32_t);
m_pointCloudMsg.fields[index].datatype = sensor_msgs::PointField::UINT32;
m_pointCloudMsg.fields[index].count = 1;
......@@ -1297,7 +1297,15 @@ void PerCameraMgr::PublishDepthImage(const void* pData)
uint32_t totalBytes = m_depthImageMsg.height * m_depthImageMsg.step; // 2 because depth image is 16bytes per pixel
m_depthImageMsg.data.resize(totalBytes);
memcpy(&m_depthImageMsg.data[0], (uint8_t *)(pDepthImageData->cdData.data()), totalBytes);
uint16_t* pDepthImage = (uint16_t*)&m_depthImageMsg.data[0];
uint16_t* pRoyaleImage = (uint16_t*)(pDepthImageData->cdData.data());
for(size_t i=0; i < pDepthImageData->cdData.size(); i++)
{
*pDepthImage = ((*pRoyaleImage) & 0x1FFF);
pDepthImage++;
pRoyaleImage++;
}
m_depthImageMsg.is_bigendian = false;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment