pcd_to_pointcloud.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id: pcd_to_pointcloud.cpp 33238 2010-03-11 00:46:58Z rusu $
35  *
36  */
37 
46 // ROS core
47 #include <ros/ros.h>
48 #include <pcl/io/io.h>
49 #include <pcl/io/pcd_io.h>
50 #include <pcl/point_types.h>
52 
53 #include "pcl_ros/publisher.h"
54 
55 using namespace std;
56 
58 {
59  protected:
60  string tf_frame_;
63  public:
64 
65  // ROS messages
66  sensor_msgs::PointCloud2 cloud_;
67 
68  string file_name_, cloud_topic_;
69  double wait_;
70 
72 
74  PCDGenerator () : tf_frame_ ("/base_link"), private_nh_("~")
75  {
76  // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
77 
78  cloud_topic_ = "cloud_pcd";
79  pub_.advertise (nh_, cloud_topic_.c_str (), 1);
80  private_nh_.param("frame_id", tf_frame_, std::string("/base_link"));
81  ROS_INFO ("Publishing data on topic %s with frame_id %s.", nh_.resolveName (cloud_topic_).c_str (), tf_frame_.c_str());
82  }
83 
85  // Start
86  int
87  start ()
88  {
89  if (file_name_ == "" || pcl::io::loadPCDFile (file_name_, cloud_) == -1)
90  return (-1);
91  cloud_.header.frame_id = tf_frame_;
92  return (0);
93  }
94 
96  // Spin (!)
97  bool spin ()
98  {
99  int nr_points = cloud_.width * cloud_.height;
100  string fields_list = pcl::getFieldsList (cloud_);
101  double interval = wait_ * 1e+6;
102  while (nh_.ok ())
103  {
104  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 ());
105  cloud_.header.stamp = ros::Time::now ();
106 
107  if (pub_.getNumSubscribers () > 0)
108  {
109  ROS_DEBUG ("Publishing data to %d subscribers.", pub_.getNumSubscribers ());
110  pub_.publish (cloud_);
111  }
112  else
113  {
114  // check once a second if there is any subscriber
115  ros::Duration (1).sleep ();
116  continue;
117  }
118 
119  usleep (interval);
120 
121  if (interval == 0) // We only publish once if a 0 seconds interval is given
122  {
123  // Give subscribers 3 seconds until point cloud decays... a little ugly!
124  ros::Duration (3.0).sleep ();
125  break;
126  }
127  }
128  return (true);
129  }
130 
131 
132 };
133 
134 /* ---[ */
135 int
136  main (int argc, char** argv)
137 {
138  if (argc < 2)
139  {
140  std::cerr << "Syntax is: " << argv[0] << " <file.pcd> [publishing_interval (in seconds)]" << std::endl;
141  return (-1);
142  }
143 
144  ros::init (argc, argv, "pcd_to_pointcloud");
145 
146  PCDGenerator c;
147  c.file_name_ = string (argv[1]);
148  // check if publishing interval is given
149  if (argc == 2)
150  {
151  c.wait_ = 0;
152  }
153  else
154  {
155  c.wait_ = atof (argv[2]);
156  }
157 
158  if (c.start () == -1)
159  {
160  ROS_ERROR ("Could not load file %s. Exiting.", argv[1]);
161  return (-1);
162  }
163  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 ());
164  c.spin ();
165 
166  return (0);
167 }
168 /* ]--- */
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
std::string resolveName(const std::string &name, bool remap=true) const
pcl_ros::Publisher< sensor_msgs::PointCloud2 > pub_
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_DEBUG_ONCE(...)
sensor_msgs::PointCloud2 cloud_
int main(int argc, char **argv)
ros::NodeHandle nh_
#define ROS_INFO(...)
int loadPCDFile(const std::string &file_name, sensor_msgs::PointCloud2 &cloud)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const sensor_msgs::PointCloud2Ptr &point_cloud) const
Definition: publisher.h:135
static Time now()
uint32_t getNumSubscribers() const
Definition: publisher.h:72
bool ok() const
ros::NodeHandle private_nh_
#define ROS_ERROR(...)
void advertise(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size)
Definition: publisher.h:60
#define ROS_DEBUG(...)


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Mon Jun 10 2019 14:19:18