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