Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
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
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
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_EXPORT_CLASS(BAGReader,nodelet::Nodelet);
00113