pointcloud_to_pcd.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: pointcloud_to_pcd.cpp 33238 2010-03-11 00:46:58Z rusu $
35  *
36  */
37 
38 // ROS core
39 #include <ros/ros.h>
40 
41 #include <tf2_ros/buffer.h>
43 #include <tf2_eigen/tf2_eigen.h>
44 
45 // PCL includes
46 #include <pcl/io/pcd_io.h>
47 
49 
50 using namespace std;
51 
60 {
61  protected:
63 
64  private:
65  std::string prefix_;
66  std::string filename_;
67  bool binary_;
69  std::string fixed_frame_;
72 
73  public:
74  string cloud_topic_;
75 
77 
79  // Callback
80  void
82  {
83  if ((cloud->width * cloud->height) == 0)
84  return;
85 
86  ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
87  (int)cloud->width * cloud->height,
88  cloud->header.frame_id.c_str (),
89  pcl::getFieldsList (*cloud).c_str ());
90 
91  Eigen::Vector4f v = Eigen::Vector4f::Zero ();
92  Eigen::Quaternionf q = Eigen::Quaternionf::Identity ();
93  if (!fixed_frame_.empty ()) {
94  if (!tf_buffer_.canTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp), ros::Duration (3.0))) {
95  ROS_WARN("Could not get transform!");
96  return;
97  }
98 
99  Eigen::Affine3d transform;
100  transform = tf2::transformToEigen (tf_buffer_.lookupTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header.stamp)));
101  v = Eigen::Vector4f::Zero ();
102  v.head<3> () = transform.translation ().cast<float> ();
103  q = transform.rotation ().cast<float> ();
104  }
105 
106  std::stringstream ss;
107  if (filename_ != "")
108  {
109  ss << filename_ << ".pcd";
110  }
111  else
112  {
113  ss << prefix_ << cloud->header.stamp << ".pcd";
114  }
115  ROS_INFO ("Data saved to %s", ss.str ().c_str ());
116 
117  pcl::PCDWriter writer;
118  if(binary_)
119  {
120  if(compressed_)
121  {
122  writer.writeBinaryCompressed (ss.str (), *cloud, v, q);
123  }
124  else
125  {
126  writer.writeBinary (ss.str (), *cloud, v, q);
127  }
128  }
129  else
130  {
131  writer.writeASCII (ss.str (), *cloud, v, q, 8);
132  }
133 
134  }
135 
137  PointCloudToPCD () : binary_(false), compressed_(false), tf_listener_(tf_buffer_)
138  {
139  // Check if a prefix parameter is defined for output file names.
140  ros::NodeHandle priv_nh("~");
141  if (priv_nh.getParam ("prefix", prefix_))
142  {
143  ROS_INFO_STREAM ("PCD file prefix is: " << prefix_);
144  }
145  else if (nh_.getParam ("prefix", prefix_))
146  {
147  ROS_WARN_STREAM ("Non-private PCD prefix parameter is DEPRECATED: "
148  << prefix_);
149  }
150 
151  priv_nh.getParam ("fixed_frame", fixed_frame_);
152  priv_nh.getParam ("binary", binary_);
153  priv_nh.getParam ("compressed", compressed_);
154  priv_nh.getParam ("filename", filename_);
155  if(binary_)
156  {
157  if(compressed_)
158  {
159  ROS_INFO_STREAM ("Saving as binary compressed PCD");
160  }
161  else
162  {
163  ROS_INFO_STREAM ("Saving as binary uncompressed PCD");
164  }
165  }
166  else
167  {
168  ROS_INFO_STREAM ("Saving as ASCII PCD");
169  }
170 
171  if (filename_ != "")
172  {
173  ROS_INFO_STREAM ("Saving to fixed filename: " << filename_);
174  }
175 
176  cloud_topic_ = "input";
177 
178  sub_ = nh_.subscribe (cloud_topic_, 1, &PointCloudToPCD::cloud_cb, this);
179  ROS_INFO ("Listening for incoming data on topic %s",
180  nh_.resolveName (cloud_topic_).c_str ());
181  }
182 };
183 
184 /* ---[ */
185 int
186  main (int argc, char** argv)
187 {
188  ros::init (argc, argv, "pointcloud_to_pcd", ros::init_options::AnonymousName);
189 
190  PointCloudToPCD b;
191  ros::spin ();
192 
193  return (0);
194 }
195 /* ]--- */
ros::init_options::AnonymousName
AnonymousName
main
int main(int argc, char **argv)
Definition: pointcloud_to_pcd.cpp:186
PointCloudToPCD
Definition: pointcloud_to_pcd.cpp:59
PointCloudToPCD::binary_
bool binary_
Definition: pointcloud_to_pcd.cpp:67
boost::shared_ptr
pcl::getFieldsList
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
tf2_eigen.h
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
ros.h
PointCloudToPCD::cloud_cb
void cloud_cb(const boost::shared_ptr< const pcl::PCLPointCloud2 > &cloud)
Definition: pointcloud_to_pcd.cpp:81
PointCloudToPCD::compressed_
bool compressed_
Definition: pointcloud_to_pcd.cpp:68
PointCloudToPCD::prefix_
std::string prefix_
Definition: pointcloud_to_pcd.cpp:65
buffer.h
PointCloudToPCD::fixed_frame_
std::string fixed_frame_
Definition: pointcloud_to_pcd.cpp:69
PointCloudToPCD::tf_listener_
tf2_ros::TransformListener tf_listener_
Definition: pointcloud_to_pcd.cpp:71
tf2_ros::TransformListener
PCDWriter
pcl_ros::PCDWriter PCDWriter
Definition: pcd_io.cpp:179
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
PointCloudToPCD::sub_
ros::Subscriber sub_
Definition: pointcloud_to_pcd.cpp:76
tf2_ros::Buffer::canTransform
virtual bool canTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration timeout, std::string *errstr=NULL) const
pcl_conversions::fromPCL
void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
PointCloudToPCD::nh_
ros::NodeHandle nh_
Definition: pointcloud_to_pcd.cpp:62
ROS_WARN
#define ROS_WARN(...)
tf2_ros::Buffer
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
tf2::transformToEigen
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
transform_listener.h
std
PointCloudToPCD::filename_
std::string filename_
Definition: pointcloud_to_pcd.cpp:66
PointCloudToPCD::tf_buffer_
tf2_ros::Buffer tf_buffer_
Definition: pointcloud_to_pcd.cpp:70
ros::spin
ROSCPP_DECL void spin()
ROS_INFO
#define ROS_INFO(...)
ros::Duration
PointCloudToPCD::PointCloudToPCD
PointCloudToPCD()
Definition: pointcloud_to_pcd.cpp:137
tf2_ros::Buffer::lookupTransform
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration timeout) const
ros::NodeHandle
PointCloudToPCD::cloud_topic_
string cloud_topic_
Definition: pointcloud_to_pcd.cpp:74
ros::Subscriber
pcl_conversions.h


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Fri Jul 12 2024 02:54:40