$search
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: pcd_to_pointcloud.cpp 33238 2010-03-11 00:46:58Z rusu $ 00035 * 00036 */ 00037 00046 // ROS core 00047 #include <ros/ros.h> 00048 #include <pcl/io/io.h> 00049 #include <pcl/io/pcd_io.h> 00050 #include <pcl/point_types.h> 00051 00052 #include "pcl_ros/publisher.h" 00053 00054 using namespace std; 00055 00056 class PCDGenerator 00057 { 00058 protected: 00059 string tf_frame_; 00060 ros::NodeHandle nh_; 00061 ros::NodeHandle private_nh_; 00062 public: 00063 00064 // ROS messages 00065 sensor_msgs::PointCloud2 cloud_; 00066 00067 string file_name_, cloud_topic_; 00068 double rate_; 00069 00070 pcl_ros::Publisher<sensor_msgs::PointCloud2> pub_; 00071 00073 PCDGenerator () : tf_frame_ ("/base_link"), private_nh_("~") 00074 { 00075 // Maximum number of outgoing messages to be queued for delivery to subscribers = 1 00076 00077 cloud_topic_ = "cloud_pcd"; 00078 pub_.advertise (nh_, cloud_topic_.c_str (), 1); 00079 private_nh_.param("frame_id", tf_frame_, std::string("/base_link")); 00080 ROS_INFO ("Publishing data on topic %s with frame_id %s.", nh_.resolveName (cloud_topic_).c_str (), tf_frame_.c_str()); 00081 } 00082 00084 // Start 00085 int 00086 start () 00087 { 00088 if (file_name_ == "" || pcl::io::loadPCDFile (file_name_, cloud_) == -1) 00089 return (-1); 00090 cloud_.header.frame_id = tf_frame_; 00091 return (0); 00092 } 00093 00095 // Spin (!) 00096 bool spin () 00097 { 00098 int nr_points = cloud_.width * cloud_.height; 00099 string fields_list = pcl::getFieldsList (cloud_); 00100 double interval = rate_ * 1e+6; 00101 while (nh_.ok ()) 00102 { 00103 ROS_DEBUG_ONCE ("Publishing data with %d points (%s) on topic %s in frame %s.", nr_points, fields_list.c_str (), nh_.resolveName (cloud_topic_).c_str (), cloud_.header.frame_id.c_str ()); 00104 cloud_.header.stamp = ros::Time::now (); 00105 00106 if (pub_.getNumSubscribers () > 0) 00107 { 00108 ROS_DEBUG ("Publishing data to %d subscribers.", pub_.getNumSubscribers ()); 00109 pub_.publish (cloud_); 00110 } 00111 else 00112 { 00113 ros::Duration (0.001).sleep (); 00114 continue; 00115 } 00116 00117 usleep (interval); 00118 00119 if (interval == 0) // We only publish once if a 0 seconds interval is given 00120 break; 00121 } 00122 00123 ros::Duration (3.0).sleep (); 00124 return (true); 00125 } 00126 00127 00128 }; 00129 00130 /* ---[ */ 00131 int 00132 main (int argc, char** argv) 00133 { 00134 if (argc < 3) 00135 { 00136 std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" << std::endl; 00137 return (-1); 00138 } 00139 00140 ros::init (argc, argv, "pcd_to_pointcloud"); 00141 00142 PCDGenerator c; 00143 c.file_name_ = string (argv[1]); 00144 c.rate_ = atof (argv[2]); 00145 00146 if (c.start () == -1) 00147 { 00148 ROS_ERROR ("Could not load file %s. Exiting.", argv[1]); 00149 return (-1); 00150 } 00151 ROS_INFO ("Loaded a point cloud with %d points (total size is %zu) and the following channels: %s.", c.cloud_.width * c.cloud_.height, c.cloud_.data.size (), pcl::getFieldsList (c.cloud_).c_str ()); 00152 c.spin (); 00153 00154 return (0); 00155 } 00156 /* ]--- */