pcd_io.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_io.cpp 35812 2011-02-08 00:05:03Z rusu $
35  *
36  */
37 
39 #include <pcl_ros/io/pcd_io.h>
40 
42 void
44 {
46  // Provide a latched topic
47  ros::Publisher pub_output = pnh_->advertise<PointCloud2> ("output", max_queue_size_, true);
48 
49  pnh_->getParam ("publish_rate", publish_rate_);
50  pnh_->getParam ("tf_frame", tf_frame_);
51 
52  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
53  " - publish_rate : %f\n"
54  " - tf_frame : %s",
55  getName ().c_str (),
56  publish_rate_, tf_frame_.c_str ());
57 
58  PointCloud2Ptr output_new;
59  output_ = boost::make_shared<PointCloud2> ();
60  output_new = boost::make_shared<PointCloud2> ();
61 
62  // Wait in a loop until someone connects
63  do
64  {
65  ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ());
66  ros::spinOnce ();
67  ros::Duration (0.01).sleep ();
68  }
69  while (pnh_->ok () && pub_output.getNumSubscribers () == 0);
70 
71  std::string file_name;
72 
73  while (pnh_->ok ())
74  {
75  // Get the current filename parameter. If no filename set, loop
76  if (!pnh_->getParam ("filename", file_name_) && file_name_.empty ())
77  {
78  ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ());
79  ros::Duration (0.01).sleep ();
80  ros::spinOnce ();
81  continue;
82  }
83 
84  // If the filename parameter holds a different value than the last one we read
85  if (file_name_.compare (file_name) != 0 && !file_name_.empty ())
86  {
87  NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ());
88  file_name = file_name_;
89  pcl::PCLPointCloud2 cloud;
90  if (impl_.read (file_name_, cloud) < 0)
91  {
92  NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ());
93  return;
94  }
96  output_->header.stamp = ros::Time::now ();
97  output_->header.frame_id = tf_frame_;
98  }
99 
100  // We do not publish empty data
101  if (output_->data.size () == 0)
102  continue;
103 
104  if (publish_rate_ == 0)
105  {
106  if (output_ != output_new)
107  {
108  NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
109  pub_output.publish (output_);
110  output_new = output_;
111  }
112  ros::Duration (0.01).sleep ();
113  }
114  else
115  {
116  NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
117  output_->header.stamp = ros::Time::now ();
118  pub_output.publish (output_);
119 
121  }
122 
123  ros::spinOnce ();
124  // Update parameters from server
125  pnh_->getParam ("publish_rate", publish_rate_);
126  pnh_->getParam ("tf_frame", tf_frame_);
127  }
128 
130 }
131 
133 
135 void
137 {
139 
140  sub_input_ = pnh_->subscribe ("input", 1, &PCDWriter::input_callback, this);
141  // ---[ Optional parameters
142  pnh_->getParam ("filename", file_name_);
143  pnh_->getParam ("binary_mode", binary_mode_);
144 
145  NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
146  " - filename : %s\n"
147  " - binary_mode : %s",
148  getName ().c_str (),
149  file_name_.c_str (), (binary_mode_) ? "true" : "false");
150 
152 }
153 
155 void
157 {
158  if (!isValid (cloud))
159  return;
160 
161  pnh_->getParam ("filename", file_name_);
162 
163  NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
164 
165  std::string fname;
166  if (file_name_.empty ())
167  fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd";
168  else
169  fname = file_name_;
170  pcl::PCLPointCloud2 pcl_cloud;
171  // It is safe to remove the const here because we are the only subscriber callback.
172  pcl_conversions::moveToPCL(*(const_cast<PointCloud2*>(cloud.get())), pcl_cloud);
173  impl_.write (fname, pcl_cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_);
174 
175  NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ());
176 }
177 
182 
PointCloud2Ptr output_
The output point cloud dataset containing the points loaded from the file.
Definition: pcd_io.h:89
pcl_ros::PCDWriter PCDWriter
Definition: pcd_io.cpp:179
#define NODELET_ERROR(...)
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcl_nodelet.h:203
void publish(const boost::shared_ptr< M > &message) const
std::string file_name_
The name of the file that contains the PointCloud data.
Definition: pcd_io.h:86
std::string resolveName(const std::string &name, bool remap=true) const
bool sleep() const
const std::string & getName() const
Point Cloud Data (PCD) file format reader.
Definition: pcd_io.h:50
#define ROS_DEBUG_ONCE(...)
void input_callback(const PointCloud2ConstPtr &cloud)
Definition: pcd_io.cpp:156
double publish_rate_
The publishing interval in seconds. Set to 0 to only publish once (default).
Definition: pcd_io.h:80
bool isValid(const PointCloud2::ConstPtr &cloud, const std::string &topic_name="input")
Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-ze...
Definition: pcl_nodelet.h:140
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcd_io.cpp:136
ros::NodeHandle & getMTPrivateNodeHandle() const
sensor_msgs::PointCloud2 PointCloud2
Definition: pcd_io.h:53
PointCloud2::ConstPtr PointCloud2ConstPtr
Definition: pcd_io.h:109
PointCloud2::Ptr PointCloud2Ptr
Definition: pcd_io.h:54
boost::shared_ptr< ros::NodeHandle > pnh_
#define ROS_ERROR_ONCE(...)
#define NODELET_INFO(...)
std::string tf_frame_
The TF frame the data should be published in ("/base_link" by default).
Definition: pcd_io.h:83
uint32_t getNumSubscribers() const
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
Definition: pcd_io.cpp:43
PLUGINLIB_EXPORT_CLASS(PCDReader, nodelet::Nodelet)
int max_queue_size_
The maximum queue size (default: 3).
Definition: pcl_nodelet.h:127
static Time now()
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
Point Cloud Data (PCD) file format writer.
Definition: pcd_io.h:102
pcl::PCDReader impl_
The PCL implementation used.
Definition: pcd_io.h:93
ROSCPP_DECL void spinOnce()
#define NODELET_DEBUG(...)
void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
PCDReader()
Empty constructor.
Definition: pcd_io.h:58


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