pointcloud_to_pcd_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_pcl_ros_utils/pointcloud_to_pcd.h"
00037 #include <sensor_msgs/PointCloud2.h>
00038 #include "jsk_recognition_utils/pcl_conversion_util.h"
00039 #include <jsk_recognition_utils/tf_listener_singleton.h>
00040 
00041 
00042 #include <pcl/io/io.h>
00043 #include <Eigen/Geometry>
00044 
00045 namespace jsk_pcl_ros_utils
00046 {
00047     PointCloudToPCD::~PointCloudToPCD()
00048   {
00049     timer_.stop();
00050   }
00051 
00052   void PointCloudToPCD::timerCallback (const ros::TimerEvent& event)
00053   {
00054     pcl::PCLPointCloud2::ConstPtr cloud;
00055     cloud = ros::topic::waitForMessage<pcl::PCLPointCloud2>("input", *pnh_);
00056     if ((cloud->width * cloud->height) == 0)
00057     {
00058       return;
00059     }
00060   
00061     ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
00062             (int)cloud->width * cloud->height,
00063             cloud->header.frame_id.c_str (),
00064             pcl::getFieldsList (*cloud).c_str ());
00065   
00066     Eigen::Vector4f v = Eigen::Vector4f::Zero ();
00067     Eigen::Quaternionf q = Eigen::Quaternionf::Identity ();
00068     if (!fixed_frame_.empty ()) {
00069         if (!tf_listener_->waitForTransform (fixed_frame_, cloud->header.frame_id, pcl_conversions::fromPCL (cloud->header).stamp, ros::Duration (duration_))) {
00070         ROS_WARN("Could not get transform!");
00071         return;
00072       }
00073       tf::StampedTransform transform_stamped;
00074       tf_listener_->lookupTransform (fixed_frame_, cloud->header.frame_id,  pcl_conversions::fromPCL (cloud->header).stamp, transform_stamped); 
00075       Eigen::Affine3d transform;
00076       tf::transformTFToEigen(transform_stamped, transform);
00077       v = Eigen::Vector4f::Zero ();
00078       v.head<3> () = transform.translation ().cast<float> ();
00079       q = transform.rotation ().cast<float> ();
00080     }
00081   
00082     std::stringstream ss;
00083     ss << prefix_ << cloud->header.stamp << ".pcd";
00084     ROS_INFO ("Data saved to %s", ss.str ().c_str ());
00085   
00086     pcl::PCDWriter writer;
00087     if(binary_)
00088     {
00089       if(compressed_)
00090       {
00091         writer.writeBinaryCompressed (ss.str (), *cloud, v, q);
00092       }
00093       else
00094       {
00095         writer.writeBinary (ss.str (), *cloud, v, q);
00096       }
00097     }
00098     else
00099     {
00100       writer.writeASCII (ss.str (), *cloud, v, q, 8);
00101     }
00102   }
00103   
00104   void PointCloudToPCD::onInit()
00105   {
00106     PCLNodelet::onInit();
00107     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00108     dynamic_reconfigure::Server<Config>::CallbackType f = boost::bind(&PointCloudToPCD::configCallback, this, _1, _2);
00109     srv_->setCallback (f);
00110     tf_listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance();
00111     if(binary_)
00112     {
00113       if(compressed_)
00114       {
00115         ROS_INFO_STREAM ("Saving as binary compressed PCD");
00116       }
00117       else
00118       {
00119         ROS_INFO_STREAM ("Saving as binary PCD");
00120       }
00121     }
00122     else
00123     {
00124       ROS_INFO_STREAM ("Saving as binary PCD");
00125     }
00126   }
00127 
00128   void PointCloudToPCD::configCallback(Config &config, uint32_t level)
00129   {
00130     boost::mutex::scoped_lock lock(mutex_);
00131     prefix_ = config.prefix;
00132     binary_ = config.binary;
00133     compressed_ = config.compressed;
00134     fixed_frame_ = config.fixed_frame;
00135     duration_ = config.duration;
00136     timer_ = pnh_->createTimer(ros::Duration(duration_), boost::bind(&PointCloudToPCD::timerCallback, this, _1));
00137   }
00138 }
00139 
00140 #include <pluginlib/class_list_macros.h>
00141 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PointCloudToPCD, nodelet::Nodelet);


jsk_pcl_ros_utils
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:05