bag_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: bag_io.cpp 34896 2010-12-19 06:21:42Z rusu $
35  *
36  */
37 
39 #include "pcl_ros/io/bag_io.h"
40 
42 bool
43 pcl_ros::BAGReader::open (const std::string &file_name, const std::string &topic_name)
44 {
45  try
46  {
47  bag_.open (file_name, rosbag::bagmode::Read);
48  view_.addQuery (bag_, rosbag::TopicQuery (topic_name));
49 
50  if (view_.size () == 0)
51  return (false);
52 
53  it_ = view_.begin ();
54  }
55  catch (rosbag::BagException &e)
56  {
57  return (false);
58  }
59  return (true);
60 }
61 
63 void
65 {
67  pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
68  // ---[ Mandatory parameters
69  if (!pnh_->getParam ("file_name", file_name_))
70  {
71  NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!");
72  return;
73  }
74  if (!pnh_->getParam ("topic_name", topic_name_))
75  {
76  NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!");
77  return;
78  }
79  // ---[ Optional parameters
80  int max_queue_size = 1;
81  pnh_->getParam ("publish_rate", publish_rate_);
82  pnh_->getParam ("max_queue_size", max_queue_size);
83 
84  ros::Publisher pub_output = pnh_->advertise<sensor_msgs::PointCloud2> ("output", max_queue_size);
85 
86  NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n"
87  " - file_name : %s\n"
88  " - topic_name : %s",
89  file_name_.c_str (), topic_name_.c_str ());
90 
91  if (!open (file_name_, topic_name_))
92  return;
93  PointCloud output;
94  output_ = boost::make_shared<PointCloud> (output);
95  output_->header.stamp = ros::Time::now ();
96 
97  // Continous publishing enabled?
98  while (pnh_->ok ())
99  {
100  PointCloudConstPtr cloud = getNextCloud ();
101  NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, pnh_->resolveName ("output").c_str (), output_->header.frame_id.c_str ());
102  output_->header.stamp = ros::Time::now ();
103 
104  pub_output.publish (output_);
105 
107  ros::spinOnce ();
108  }
109 }
110 
113 
PointCloudPtr output_
The output point cloud dataset containing the points loaded from the file.
Definition: bag_io.h:120
#define NODELET_ERROR(...)
std::string topic_name_
The name of the topic that contains the PointCloud data.
Definition: bag_io.h:114
void publish(const boost::shared_ptr< M > &message) const
void open(std::string const &filename, uint32_t mode=bagmode::Read)
BAG PointCloud file format reader.
Definition: bag_io.h:52
bool sleep() const
BAGReader()
Empty constructor.
Definition: bag_io.h:60
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
std::string file_name_
The name of the BAG file that contains the PointCloud data.
Definition: bag_io.h:117
sensor_msgs::PointCloud2 PointCloud
Definition: bag_io.h:55
rosbag::Bag bag_
The BAG object.
Definition: bag_io.h:105
virtual void onInit()
Nodelet initialization routine.
Definition: bag_io.cpp:64
uint32_t size()
bool open(const std::string &file_name, const std::string &topic_name)
Open a BAG file for reading and select a specified topic.
Definition: bag_io.cpp:43
ros::NodeHandle & getMTPrivateNodeHandle() const
PointCloud::ConstPtr PointCloudConstPtr
Definition: bag_io.h:57
void addQuery(Bag const &bag, ros::Time const &start_time=ros::TIME_MIN, ros::Time const &end_time=ros::TIME_MAX)
rosbag::View view_
The BAG view object.
Definition: bag_io.h:108
PointCloudConstPtr getNextCloud()
Get the next point cloud dataset in the BAG file.
Definition: bag_io.h:74
iterator begin()
static Time now()
rosbag::View::iterator it_
The BAG view iterator object.
Definition: bag_io.h:111
ROSCPP_DECL void spinOnce()
#define NODELET_DEBUG(...)
double publish_rate_
The publishing interval in seconds. Set to 0 to publish once (default).
Definition: bag_io.h:102


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Wed Jun 5 2019 19:52:52