Convert RAW depth data to depth image using cv_bridge and publish it using image transport

asked 2019-04-17 15:59:55 -0500

rishikhera gravatar image

updated 2019-04-17 17:48:35 -0500

I am using a custom package which gives me raw depth data of type const uint16_t* with values like 2027, 1980, 0, 0, 1678... I want to be able to convert this raw depth data into an image and publish it over some topic. So far, my code looks like this

m_DepthFrame = *m_DepthProvider->getDepthFrame();
    if(m_DepthFrame.getDepthData() != nullptr)
    {
        cv::Mat l_depthImage(m_DepthFrame.getheight(), m_DepthFrame.getwidth(), CV_16UC1, (void*)m_DepthFrame.getDepthData());
        sensor_msgs::ImagePtr l_DepthImageMsg = cv_bridge::CvImage(std_msgs::Header(), "mono16", l_depthImage).toImageMsg();
        m_depthImagePublisher.publish(l_DepthImageMsg);
    }

And my subscriber looks something like,

cv_bridge::CvImageConstPtr cv_ptr;
try
{
    cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::TYPE_16UC1);
}
catch (cv_bridge::Exception& e)
{
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
}
cv::imshow(OPENCV_WINDOW, cv_ptr->image);

But my issue is that all I can see is a dark screen, like totally black with some white spots on the top. How can I use cv_bridge to create a depth image from raw depth data? Thanks.

P.S. I used this another approach but somehow the frame rate seems slow and the depth image is too dull (darkish) to clearly see what is going on.

cv::Mat l_depthFrame(m_DepthFrame.getheight(), m_DepthFrame.getwidth(), CV_16UC1);
        memcpy(l_depthFrame.data, m_DepthFrame.getDepthData(), m_DepthFrame.getheight() * m_DepthFrame.getwidth() * sizeof(uint16_t));

        std_msgs::Header l_header;
        l_header.stamp = ros::Time::now();
        cv_bridge::CvImage l_depthImage;
        l_depthImage = cv_bridge::CvImage(l_header, sensor_msgs::image_encodings::TYPE_16UC1, l_depthFrame);

        sensor_msgs::Image l_depthImageMsg;
        l_depthImage.toImageMsg(l_depthImageMsg);
        m_depthFramePublisher.publish(l_depthImageMsg);

Also, would be nice to know to do the same for a RGB Image.

edit retag flag offensive close merge delete