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/pcd_io.h>
00040
00042 void
00043 pcl_ros::PCDReader::onInit ()
00044 {
00045 ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
00046
00047 ros::Publisher pub_output = private_nh.advertise<PointCloud2> ("output", max_queue_size_, true);
00048
00049 private_nh.getParam ("publish_rate", publish_rate_);
00050 private_nh.getParam ("tf_frame", tf_frame_);
00051
00052 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00053 " - publish_rate : %f\n"
00054 " - tf_frame : %s",
00055 getName ().c_str (),
00056 publish_rate_, tf_frame_.c_str ());
00057
00058 PointCloud2Ptr output_new;
00059 output_ = boost::make_shared<PointCloud2> ();
00060 output_new = boost::make_shared<PointCloud2> ();
00061
00062
00063 do
00064 {
00065 ROS_DEBUG_ONCE ("[%s::onInit] Waiting for a client to connect...", getName ().c_str ());
00066 ros::spinOnce ();
00067 ros::Duration (0.01).sleep ();
00068 }
00069 while (private_nh.ok () && pub_output.getNumSubscribers () == 0);
00070
00071 std::string file_name;
00072
00073 while (private_nh.ok ())
00074 {
00075
00076 if (!private_nh.getParam ("filename", file_name_) && file_name_.empty ())
00077 {
00078 ROS_ERROR_ONCE ("[%s::onInit] Need a 'filename' parameter to be set before continuing!", getName ().c_str ());
00079 ros::Duration (0.01).sleep ();
00080 ros::spinOnce ();
00081 continue;
00082 }
00083
00084
00085 if (file_name_.compare (file_name) != 0 && !file_name_.empty ())
00086 {
00087 NODELET_INFO ("[%s::onInit] New file given: %s", getName ().c_str (), file_name_.c_str ());
00088 file_name = file_name_;
00089 PointCloud2 cloud;
00090 if (impl_.read (file_name_, cloud) < 0)
00091 {
00092 NODELET_ERROR ("[%s::onInit] Error reading %s !", getName ().c_str (), file_name_.c_str ());
00093 return;
00094 }
00095 output_ = boost::make_shared<PointCloud2> (cloud);
00096 output_->header.stamp = ros::Time::now ();
00097 output_->header.frame_id = tf_frame_;
00098 }
00099
00100
00101 if (output_->data.size () == 0)
00102 continue;
00103
00104 if (publish_rate_ == 0)
00105 {
00106 if (output_ != output_new)
00107 {
00108 NODELET_DEBUG ("Publishing data once (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
00109 pub_output.publish (output_);
00110 output_new = output_;
00111 }
00112 ros::Duration (0.01).sleep ();
00113 }
00114 else
00115 {
00116 NODELET_DEBUG ("Publishing data (%d points) on topic %s in frame %s.", output_->width * output_->height, getMTPrivateNodeHandle ().resolveName ("output").c_str (), output_->header.frame_id.c_str ());
00117 output_->header.stamp = ros::Time::now ();
00118 pub_output.publish (output_);
00119
00120 ros::Duration (publish_rate_).sleep ();
00121 }
00122
00123 ros::spinOnce ();
00124
00125 private_nh.getParam ("publish_rate", publish_rate_);
00126 private_nh.getParam ("tf_frame", tf_frame_);
00127 }
00128 }
00129
00131
00133 void
00134 pcl_ros::PCDWriter::onInit ()
00135 {
00136 ros::NodeHandle pnh = getMTPrivateNodeHandle ();
00137 sub_input_ = pnh.subscribe ("input", 1, &PCDWriter::input_callback, this);
00138
00139 pnh.getParam ("filename", file_name_);
00140 pnh.getParam ("binary_mode", binary_mode_);
00141
00142 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00143 " - filename : %s\n"
00144 " - binary_mode : %s",
00145 getName ().c_str (),
00146 file_name_.c_str (), (binary_mode_) ? "true" : "false");
00147
00148 }
00149
00151 void
00152 pcl_ros::PCDWriter::input_callback (const PointCloud2ConstPtr &cloud)
00153 {
00154 if (!isValid (cloud))
00155 return;
00156
00157 getMTPrivateNodeHandle ().getParam ("filename", file_name_);
00158
00159 NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
00160
00161 std::string fname;
00162 if (file_name_.empty ())
00163 fname = boost::lexical_cast<std::string> (cloud->header.stamp.toSec ()) + ".pcd";
00164 else
00165 fname = file_name_;
00166 impl_.write (fname, cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary_mode_);
00167
00168 NODELET_DEBUG ("[%s::input_callback] Data saved to %s", getName ().c_str (), fname.c_str ());
00169 }
00170
00171 typedef pcl_ros::PCDReader PCDReader;
00172 typedef pcl_ros::PCDWriter PCDWriter;
00173 PLUGINLIB_EXPORT_CLASS(PCDReader,nodelet::Nodelet);
00174 PLUGINLIB_EXPORT_CLASS(PCDWriter,nodelet::Nodelet);
00175