colors.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2012 Austin Robot Technology
00003  *  License: Modified BSD Software License Agreement
00004  *
00005  *  $Id$
00006  */
00007 
00017 #include "colors.h"
00018 #include <velodyne_pointcloud/point_types.h>
00019 
00021 #include <pcl/io/pcd_io.h>
00022 #include <pcl/point_cloud.h>
00023 
00024 namespace
00025 {
00026   // RGB color values
00027   const int color_red =       0xff0000;
00028   const int color_orange =    0xff8800;
00029   const int color_yellow =    0xffff00;
00030   const int color_green =     0x00ff00;
00031   const int color_blue =      0x0000ff;
00032   const int color_violet =    0xff00ff;
00033 
00034   const int N_COLORS = 6;
00035   int rainbow[N_COLORS] = {color_red, color_orange, color_yellow,
00036                            color_green, color_blue, color_violet};
00037 }
00038 
00039 namespace velodyne_pointcloud
00040 {
00041 
00043   typedef pcl::PointXYZRGB RGBPoint;
00044   typedef pcl::PointCloud<RGBPoint> RGBPointCloud;
00045 
00047   RingColors::RingColors(ros::NodeHandle node, ros::NodeHandle private_nh)
00048   {
00049     // advertise output point cloud (before subscribing to input data)
00050     output_ =
00051       node.advertise<sensor_msgs::PointCloud2>("velodyne_rings", 10);
00052 
00053     // subscribe to VelodyneScan packets
00054     input_ =
00055       node.subscribe("velodyne_points", 10,
00056                      &RingColors::convertPoints, this,
00057                      ros::TransportHints().tcpNoDelay(true));
00058   }
00059 
00060 
00062   void
00063     RingColors::convertPoints(const VPointCloud::ConstPtr &inMsg)
00064   {
00065     if (output_.getNumSubscribers() == 0)         // no one listening?
00066       return;                                     // do nothing
00067 
00068     // allocate an PointXYZRGB message with same time and frame ID as
00069     // input data
00070     RGBPointCloud::Ptr outMsg(new RGBPointCloud());
00071     outMsg->header.stamp = inMsg->header.stamp;
00072     outMsg->header.frame_id = inMsg->header.frame_id;
00073     outMsg->height = 1;
00074 
00075     for (size_t i = 0; i < inMsg->points.size(); ++i)
00076       {
00077         RGBPoint p;
00078         p.x = inMsg->points[i].x;
00079         p.y = inMsg->points[i].y;
00080         p.z = inMsg->points[i].z;
00081 
00082         // color lasers with the rainbow array
00083         int color = inMsg->points[i].ring % N_COLORS;
00084         p.rgb = *reinterpret_cast<float*>(&rainbow[color]);
00085 
00086         outMsg->points.push_back(p);
00087         ++outMsg->width;
00088       }
00089 
00090     output_.publish(outMsg);
00091   }
00092 
00093 } // namespace velodyne_pcl


velodyne_pointcloud
Author(s): Jack O'Quin, Piyush Khandelwal, Jesse Vera
autogenerated on Mon Oct 6 2014 08:36:38