bag_io.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: bag_io.cpp 34896 2010-12-19 06:21:42Z rusu $
00035  *
00036  */
00037 
00038 #include <pluginlib/class_list_macros.h>
00039 #include "pcl_ros/io/bag_io.h"
00040 
00042 bool 
00043 pcl_ros::BAGReader::open (const std::string &file_name, const std::string &topic_name)
00044 {
00045   try
00046   {
00047     bag_.open (file_name, rosbag::bagmode::Read);
00048     view_.addQuery (bag_, rosbag::TopicQuery (topic_name));
00049 
00050     if (view_.size () == 0)
00051       return (false);
00052 
00053     it_ = view_.begin ();
00054   }
00055   catch (rosbag::BagException &e)
00056   {
00057     return (false);
00058   }
00059   return (true);
00060 }
00061 
00063 void
00064 pcl_ros::BAGReader::onInit ()
00065 {
00066   boost::shared_ptr<ros::NodeHandle> pnh_;
00067   pnh_.reset (new ros::NodeHandle (getMTPrivateNodeHandle ()));
00068   // ---[ Mandatory parameters
00069   if (!pnh_->getParam ("file_name", file_name_))
00070   {
00071     NODELET_ERROR ("[onInit] Need a 'file_name' parameter to be set before continuing!");
00072     return;
00073   }
00074    if (!pnh_->getParam ("topic_name", topic_name_))
00075   {
00076     NODELET_ERROR ("[onInit] Need a 'topic_name' parameter to be set before continuing!"); 
00077     return;
00078   }
00079   // ---[ Optional parameters
00080   int max_queue_size = 1;
00081   pnh_->getParam ("publish_rate", publish_rate_);
00082   pnh_->getParam ("max_queue_size", max_queue_size);
00083 
00084   ros::Publisher pub_output = pnh_->advertise<sensor_msgs::PointCloud2> ("output", max_queue_size);
00085 
00086   NODELET_DEBUG ("[onInit] Nodelet successfully created with the following parameters:\n"
00087                  " - file_name    : %s\n"
00088                  " - topic_name   : %s",
00089                  file_name_.c_str (), topic_name_.c_str ());
00090 
00091   if (!open (file_name_, topic_name_))
00092     return;
00093   PointCloud output;
00094   output_ = boost::make_shared<PointCloud> (output);
00095   output_->header.stamp    = ros::Time::now ();
00096 
00097   // Continous publishing enabled?
00098   while (pnh_->ok ())
00099   {
00100     PointCloudConstPtr cloud = getNextCloud ();
00101     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 ());
00102     output_->header.stamp = ros::Time::now ();
00103 
00104     pub_output.publish (output_);
00105 
00106     ros::Duration (publish_rate_).sleep ();
00107     ros::spinOnce ();
00108   }
00109 }
00110 
00111 typedef pcl_ros::BAGReader BAGReader;
00112 PLUGINLIB_DECLARE_CLASS (pcl, BAGReader, BAGReader, nodelet::Nodelet);
00113 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


pcl_ros
Author(s): Maintained by Radu Bogdan Rusu
autogenerated on Tue Mar 5 2013 13:54:40