point cloud publisher

pcl_publisher.cpp

#include <ros/ros.h>

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>

#include <iostream>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

// camera parameters
const double camera_factor = 700;
const double camera_cx = 423.2039489746094;
const double camera_cy = 241.249267578125;
const double camera_fx = 422.557861328125;
const double camera_fy = 422.557861328125;

class ImageServer
{
private:
    sensor_msgs::Image _depth_data;
    cv::Mat _depth_image;

public:
    size_t _height = 640;
    size_t _width = 480;


    void depth_callback(const sensor_msgs::Image &depth);

    sensor_msgs::Image get_depth_data() { return _depth_data; };
    cv::Mat get_depth_image() { return _depth_image; }

    void set_depth_data(sensor_msgs::Image depth_data) { _depth_data = depth_data; }
    void set_depth_image(cv::Mat depth_image) { _depth_image = depth_image; }
};

void ImageServer::depth_callback(const sensor_msgs::Image &msg)
{
    ImageServer::set_depth_data(msg);

    cv::Mat depth_image;
    cv_bridge::CvImagePtr cv_ptr;

    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg);
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    _depth_image = cv_ptr->image;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pcl_publisher_node");
    ros::NodeHandle n;

    ImageServer imgobject;

    sensor_msgs::Image depth_d;
    cv::Mat depth_i;

    pcl::PointCloud<pcl::PointXYZ> cloud;
    sensor_msgs::PointCloud2 output;

    ros::Subscriber sub_depth = n.subscribe("/camera/depth/image_raw", 1, &ImageServer::depth_callback, &imgobject);

    ros::Publisher pcl_pub = n.advertise<sensor_msgs::PointCloud2>("/camera/depth/points", 1);

    while (ros::ok())
    {
        ros::spinOnce();

        depth_i = imgobject.get_depth_image();

        PointCloud::Ptr cloud(new PointCloud);

        for (int m = 0; m < depth_i.rows; m++)
            for (int n = 0; n < depth_i.cols; n++)
            {
                ushort d = depth_i.ptr<ushort>(m)[n];

                if (d == 0)
                    continue;

                PointT p;

                p.z = double(d) / camera_factor;
                p.x = (n - camera_cx) * p.z / camera_fx;
                p.y = (m - camera_cy) * p.z / camera_fy;

                cloud->points.push_back(p);
            }

        cloud->height = 1; // because unordered point_cloud
        cloud->width = cloud->points.size();
        cloud->is_dense = false;

        cloud->sensor_origin_.setZero();
        cloud->sensor_orientation_.w() = 1.0f;
        cloud->sensor_orientation_.x() = 0.0f;
        cloud->sensor_orientation_.y() = 0.0f;
        cloud->sensor_orientation_.z() = 1.0f;

        pcl::toROSMsg(*cloud, output);

        output.header.frame_id = "camera_link";
        output.header.stamp = ros::Time::now();

        pcl_pub.publish(output);

        cloud->points.clear();
    }

    return 0;
}