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 <boost/filesystem.hpp>
48 #include <rosbag/bag.h>
49 #include <rosbag/view.h>
51 #include "pcl_ros/transforms.h"
52 #include <tf/transform_listener.h>
54 
55 typedef sensor_msgs::PointCloud2 PointCloud;
56 typedef PointCloud::Ptr PointCloudPtr;
57 typedef PointCloud::ConstPtr PointCloudConstPtr;
58 
59 /* ---[ */
60 int
61  main (int argc, char** argv)
62 {
63  ros::init (argc, argv, "bag_to_pcd");
64  if (argc < 4)
65  {
66  std::cerr << "Syntax is: " << argv[0] << " <file_in.bag> <topic> <output_directory> [<target_frame>]" << std::endl;
67  std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" << std::endl;
68  return (-1);
69  }
70 
71  // TF
72  tf::TransformListener tf_listener;
73  tf::TransformBroadcaster tf_broadcaster;
74 
75  rosbag::Bag bag;
76  rosbag::View view;
77  rosbag::View::iterator view_it;
78 
79  try
80  {
81  bag.open (argv[1], rosbag::bagmode::Read);
82  }
83  catch (const rosbag::BagException&)
84  {
85  std::cerr << "Error opening file " << argv[1] << std::endl;
86  return (-1);
87  }
88 
89  // check that target topic exists in the bag file:
90  rosbag::View topic_list_view(bag);
91  std::string target_topic;
92  std::map<std::string, std::string> topic_list;
93  for(rosbag::ConnectionInfo const *ci: topic_list_view.getConnections() )
94  {
95  topic_list[ci->topic] = ci->datatype;
96  if (ci->topic == argv[2])
97  {
98  if (ci->datatype == std::string("sensor_msgs/PointCloud2"))
99  {
100  target_topic = std::string (argv[2]);
101  view.addQuery (bag, rosbag::TopicQuery (target_topic));
102  }
103  else
104  {
105  std::cerr << "Provided topic '" << argv[2] << "' is in the bag file, but is not of type sensor_msgs/PointCloud2 (type: " << ci->datatype << ")" << std::endl;
106  }
107  }
108  }
109  if (target_topic.empty())
110  {
111  std::cerr << "Could not find a sensor_msgs/PointCloud2 type on topic '" << argv[2] << "' in bag file " << argv[1] << std::endl;
112  std::cerr << "Topics found in the bag file:" << std::endl;
113  for (std::map<std::string, std::string>::iterator it=topic_list.begin(); it!=topic_list.end(); ++it)
114  std::cout << " " << it->first << " (" << it->second << ")" << std::endl;
115  return (-1);
116  }
117 
118  view.addQuery (bag, rosbag::TypeQuery ("tf/tfMessage"));
119  view.addQuery (bag, rosbag::TypeQuery ("tf2_msgs/TFMessage"));
120  view_it = view.begin ();
121 
122  std::string output_dir = std::string (argv[3]);
123  boost::filesystem::path outpath (output_dir);
124  if (!boost::filesystem::exists (outpath))
125  {
126  if (!boost::filesystem::create_directories (outpath))
127  {
128  std::cerr << "Error creating directory " << output_dir << std::endl;
129  return (-1);
130  }
131  std::cerr << "Creating directory " << output_dir << std::endl;
132  }
133 
134  // Add the PointCloud2 handler
135  std::cerr << "Saving recorded sensor_msgs::PointCloud2 messages on topic " << target_topic << " to " << output_dir << std::endl;
136 
137  PointCloud cloud_t;
138  ros::Duration r (0.001);
139  // Loop over the whole bag file
140  while (view_it != view.end ())
141  {
142  // Handle TF messages first
143  tf::tfMessage::ConstPtr tf = view_it->instantiate<tf::tfMessage> ();
144  if (tf != NULL)
145  {
146  tf_broadcaster.sendTransform (tf->transforms);
147  ros::spinOnce ();
148  r.sleep ();
149  }
150  else
151  {
152  // Get the PointCloud2 message
153  PointCloudConstPtr cloud = view_it->instantiate<PointCloud> ();
154  if (cloud == NULL)
155  {
156  ++view_it;
157  continue;
158  }
159 
160  // If a target_frame was specified
161  if(argc > 4)
162  {
163  // Transform it
164  if (!pcl_ros::transformPointCloud (argv[4], *cloud, cloud_t, tf_listener))
165  {
166  ++view_it;
167  continue;
168  }
169  }
170  else
171  {
172  // Else, don't transform it
173  cloud_t = *cloud;
174  }
175 
176  std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << cloud_t.header.frame_id << " on topic " << view_it->getTopic() << " with the following fields: " << pcl::getFieldsList (cloud_t) << std::endl;
177 
178  std::stringstream ss;
179  ss << output_dir << "/" << cloud_t.header.stamp << ".pcd";
180  std::cerr << "Data saved to " << ss.str () << std::endl;
181  pcl::io::savePCDFile (ss.str (), cloud_t, Eigen::Vector4f::Zero (),
182  Eigen::Quaternionf::Identity (), true);
183  }
184  // Increment the iterator
185  ++view_it;
186  }
187 
188  return (0);
189 }
190 /* ]--- */
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
std::vector< const ConnectionInfo *> getConnections()
void open(std::string const &filename, uint32_t mode=bagmode::Read)
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:55
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:56
PointCloud::ConstPtr PointCloudConstPtr
Definition: bag_to_pcd.cpp:57
void sendTransform(const StampedTransform &transform)
int main(int argc, char **argv)
Definition: bag_to_pcd.cpp:61
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:85
iterator begin()
bool sleep() const
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 Thu Nov 28 2019 03:13:48