bag_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: bag_to_pcd.cpp 35812 2011-02-08 00:05:03Z rusu $
35  *
36  */
37 
47 #include <sstream>
48 #include <boost/filesystem.hpp>
49 #include <rosbag/bag.h>
50 #include <rosbag/view.h>
51 #include <pcl/io/io.h>
52 #include <pcl/io/pcd_io.h>
54 #include "pcl_ros/transforms.h"
55 #include <tf/transform_listener.h>
57 
58 typedef sensor_msgs::PointCloud2 PointCloud;
59 typedef PointCloud::Ptr PointCloudPtr;
60 typedef PointCloud::ConstPtr PointCloudConstPtr;
61 
62 /* ---[ */
63 int
64  main (int argc, char** argv)
65 {
66  ros::init (argc, argv, "bag_to_pcd");
67  if (argc < 4)
68  {
69  std::cerr << "Syntax is: " << argv[0] << " <file_in.bag> <topic> <output_directory> [<target_frame>]" << std::endl;
70  std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" << std::endl;
71  return (-1);
72  }
73 
74  // TF
75  tf::TransformListener tf_listener;
76  tf::TransformBroadcaster tf_broadcaster;
77 
78  rosbag::Bag bag;
79  rosbag::View view;
80  rosbag::View::iterator view_it;
81 
82  try
83  {
84  bag.open (argv[1], rosbag::bagmode::Read);
85  }
86  catch (rosbag::BagException)
87  {
88  std::cerr << "Error opening file " << argv[1] << std::endl;
89  return (-1);
90  }
91 
92  view.addQuery (bag, rosbag::TypeQuery ("sensor_msgs/PointCloud2"));
93  view.addQuery (bag, rosbag::TypeQuery ("tf/tfMessage"));
94  view.addQuery (bag, rosbag::TypeQuery ("tf2_msgs/TFMessage"));
95  view_it = view.begin ();
96 
97  std::string output_dir = std::string (argv[3]);
98  boost::filesystem::path outpath (output_dir);
99  if (!boost::filesystem::exists (outpath))
100  {
101  if (!boost::filesystem::create_directories (outpath))
102  {
103  std::cerr << "Error creating directory " << output_dir << std::endl;
104  return (-1);
105  }
106  std::cerr << "Creating directory " << output_dir << std::endl;
107  }
108 
109  // Add the PointCloud2 handler
110  std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << argv[2] << " to " << output_dir << std::endl;
111 
112  PointCloud cloud_t;
113  ros::Duration r (0.001);
114  // Loop over the whole bag file
115  while (view_it != view.end ())
116  {
117  // Handle TF messages first
118  tf::tfMessage::ConstPtr tf = view_it->instantiate<tf::tfMessage> ();
119  if (tf != NULL)
120  {
121  tf_broadcaster.sendTransform (tf->transforms);
122  ros::spinOnce ();
123  r.sleep ();
124  }
125  else
126  {
127  // Get the PointCloud2 message
128  PointCloudConstPtr cloud = view_it->instantiate<PointCloud> ();
129  if (cloud == NULL)
130  {
131  ++view_it;
132  continue;
133  }
134 
135  // If a target_frame was specified
136  if(argc > 4)
137  {
138  // Transform it
139  if (!pcl_ros::transformPointCloud (argv[4], *cloud, cloud_t, tf_listener))
140  {
141  ++view_it;
142  continue;
143  }
144  }
145  else
146  {
147  // Else, don't transform it
148  cloud_t = *cloud;
149  }
150 
151  std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << cloud_t.header.frame_id << " with the following fields: " << pcl::getFieldsList (cloud_t) << std::endl;
152 
153  std::stringstream ss;
154  ss << output_dir << "/" << cloud_t.header.stamp << ".pcd";
155  std::cerr << "Data saved to " << ss.str () << std::endl;
156  pcl::io::savePCDFile (ss.str (), cloud_t, Eigen::Vector4f::Zero (),
157  Eigen::Quaternionf::Identity (), true);
158  }
159  // Increment the iterator
160  ++view_it;
161  }
162 
163  return (0);
164 }
165 /* ]--- */
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
void open(std::string const &filename, uint32_t mode=bagmode::Read)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
sensor_msgs::PointCloud2 PointCloud
Definition: bag_to_pcd.cpp:58
void addQuery(Bag const &bag, ros::Time const &start_time=ros::TIME_MIN, ros::Time const &end_time=ros::TIME_MAX)
PointCloud::Ptr PointCloudPtr
Definition: bag_to_pcd.cpp:59
PointCloud::ConstPtr PointCloudConstPtr
Definition: bag_to_pcd.cpp:60
void sendTransform(const StampedTransform &transform)
int main(int argc, char **argv)
Definition: bag_to_pcd.cpp:64
iterator end()
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
Apply a rigid transform defined by a 3D offset and a quaternion.
Definition: transforms.hpp:71
iterator begin()
ROSCPP_DECL void spinOnce()
int savePCDFile(const std::string &file_name, const sensor_msgs::PointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), const bool binary_mode=false)


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