Coloring of pcd-pointcloud in Rviz

edit

I think ROS does not really implement rgb data publish in pcd_to_pointcloud package. This is an example to change the PointCloud type from PointXYZ to PointXYZRGB

#include <iostream>
#include <string>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/io/pcd_io.h>

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
using namespace std;

int main(int argc, char** argv)
{
    PointCloud::Ptr cloud(new PointCloud);
    ros::init (argc, argv, "my_pcl_tutorial");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<PointCloud> ("points", 1);
    pcl::io::loadPCDFile<PointT> ("yourpcd.pcd", *cloud);

    PointCloud::Ptr msg(new PointCloud);
    msg->header.frame_id = "map";
    msg->height = cloud->height;
    msg->width = cloud->width;

    PointT p;
    for (size_t i = 0; i < cloud->size(); i++) {
        p.x = cloud->points[i].x;
        p.y = cloud->points[i].y;
        p.z = cloud->points[i].z;
        p.r = cloud->points[i].r;
        p.g = cloud->points[i].g;
        p.b = cloud->points[i].b;
        msg->points.push_back(p);
    }

    ros::Rate loop_rate(4);

    while (nh.ok())
    {
        pub.publish (msg);
        ros::spinOnce ();
        loop_rate.sleep ();
    }
}