pointcloud2.cpp

 void
RingColors::convertPoints(const sensor_msgs::PointCloud2ConstPtr &inMsg)
{
  if (output_.getNumSubscribers() == 0)         // no one listening?
    return;                                     // do nothing

  // allocate an PointXYZRGB message with same time and frame ID as
  // input data
  sensor_msgs::PointCloud2::Ptr outMsg(new sensor_msgs::PointCloud2());
  sensor_msgs::PointCloud2Modifier modifier(*outMsg);
  modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
  modifier.resize(inMsg->height * inMsg->width);

  outMsg->header.stamp = inMsg->header.stamp;
  outMsg->header.frame_id = inMsg->header.frame_id;
  outMsg->height = 1;

  sensor_msgs::PointCloud2Iterator<float> out_x(*outMsg, "x");
  sensor_msgs::PointCloud2Iterator<float> out_y(*outMsg, "y");
  sensor_msgs::PointCloud2Iterator<float> out_z(*outMsg, "z");
  sensor_msgs::PointCloud2Iterator<uint8_t> out_r(*outMsg, "r");
  sensor_msgs::PointCloud2Iterator<uint8_t> out_g(*outMsg, "g");
  sensor_msgs::PointCloud2Iterator<uint8_t> out_b(*outMsg, "b");

  sensor_msgs::PointCloud2ConstIterator<float> in_x(*inMsg, "x");
  sensor_msgs::PointCloud2ConstIterator<float> in_y(*inMsg, "y");
  sensor_msgs::PointCloud2ConstIterator<float> in_z(*inMsg, "z");
  sensor_msgs::PointCloud2ConstIterator<uint16_t> in_ring(*inMsg, "ring");

  for (size_t i = 0; i < inMsg->height * inMsg->width; ++i, ++out_x, ++out_y, ++out_z, ++out_r, ++out_g, ++out_b,
    ++in_x, ++in_y, ++in_z, ++in_ring)
    {
      *out_x = *in_x;
      *out_y = *in_y;
      *out_z = *in_z;

      // color lasers with the rainbow array
      int color = *in_ring % N_COLORS;
      *out_r = (rainbow[color] >> 16) & 0x0000ff;
      *out_g = (rainbow[color] >> 8) & 0x0000ff;
      *out_b = rainbow[color] & 0x0000ff;
    }

  output_.publish(outMsg);
}

how to work with a PointCloud2 message without using PCL. I got it from ros-drivers/velodyne#49 from Vincent Rabaud.