convert.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
00003  *  Copyright (C) 2011 Jesse Vera
00004  *  Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id$
00008  */
00009 
00016 #include "convert.h"
00017 
00018 namespace velodyne_pointcloud
00019 {
00021   Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh):
00022     data_(new velodyne_rawdata::RawData())
00023   {
00024     data_->setup(private_nh);
00025 
00026     // advertise output point cloud (before subscribing to input data)
00027     output_ =
00028       node.advertise<sensor_msgs::PointCloud2>("velodyne_points", 10);
00029 
00030     // subscribe to VelodyneScan packets
00031     velodyne_scan_ =
00032       node.subscribe("velodyne_packets", 10,
00033                      &Convert::processScan, (Convert *) this,
00034                      ros::TransportHints().tcpNoDelay(true));
00035   }
00036 
00038   void Convert::processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg)
00039   {
00040     if (output_.getNumSubscribers() == 0)         // no one listening?
00041       return;                                     // avoid much work
00042 
00043     // allocate a point cloud with same time and frame ID as raw data
00044     velodyne_rawdata::VPointCloud::Ptr
00045       outMsg(new velodyne_rawdata::VPointCloud());
00046     outMsg->header.stamp = scanMsg->header.stamp;
00047     outMsg->header.frame_id = scanMsg->header.frame_id;
00048     outMsg->height = 1;
00049 
00050     // process each packet provided by the driver
00051     for (size_t i = 0; i < scanMsg->packets.size(); ++i)
00052       {
00053         data_->unpack(scanMsg->packets[i], *outMsg);
00054       }
00055 
00056     // publish the accumulated cloud message
00057     ROS_DEBUG_STREAM("Publishing " << outMsg->height * outMsg->width
00058                      << " Velodyne points, time: " << outMsg->header.stamp);
00059     output_.publish(outMsg);
00060   }
00061 
00062 } // namespace velodyne_pointcloud


velodyne_pointcloud
Author(s): Jack O'Quin, Piyush Khandelwal, Jesse Vera
autogenerated on Fri Jan 3 2014 12:11:12