merge_clouds.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, 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 the Willow Garage 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 
00044 #include <ros/ros.h>
00045 #include <sensor_msgs/PointCloud.h>
00046 #include <tf/message_filter.h>
00047 #include <tf/transform_listener.h>
00048 
00049 #include <boost/thread/mutex.hpp>
00050 #include <boost/shared_ptr.hpp>
00051 #include <boost/bind.hpp>
00052 
00053 #include <message_filters/subscriber.h>
00054 
00055 class MergeClouds
00056 {
00057 public:
00058 
00059   MergeClouds(void) :
00060     sub1_(nh_, "cloud_in1", 1),
00061     sub2_(nh_, "cloud_in1", 1)
00062   {
00063     cloudOut_ = nh_.advertise<sensor_msgs::PointCloud>("cloud_out", 1);
00064     nh_.param<std::string>("~output_frame", output_frame_, std::string());
00065     nh_.param<double>("~max_frequency", max_freq_, 0.0);
00066     newCloud1_ = newCloud2_ = false;
00067 
00068     if (output_frame_.empty())
00069       ROS_ERROR("No output frame specified for merging pointclouds");
00070 
00071     // make sure we don't publish too fast
00072     if (max_freq_ > 1000.0 || max_freq_ < 0.0)
00073       max_freq_ = 0.0;
00074 
00075     if (max_freq_ > 0.0)
00076     {
00077       timer_ = nh_.createTimer(ros::Duration(1.0/max_freq_), boost::bind(&MergeClouds::onTimer, this, _1));
00078       haveTimer_ = true;
00079     }
00080     else
00081       haveTimer_ = false;
00082 
00083     tf_filter1_.reset(new tf::MessageFilter<sensor_msgs::PointCloud>(sub1_, tf_, output_frame_, 1));
00084     tf_filter2_.reset(new tf::MessageFilter<sensor_msgs::PointCloud>(sub2_, tf_, output_frame_, 1));
00085 
00086     tf_filter1_->registerCallback(boost::bind(&MergeClouds::receiveCloud1, this, _1));
00087     tf_filter2_->registerCallback(boost::bind(&MergeClouds::receiveCloud2, this, _1));
00088   }
00089 
00090   ~MergeClouds(void)
00091   {
00092 
00093   }
00094 
00095 private:
00096 
00097   void onTimer(const ros::TimerEvent &e)
00098   {
00099     if (newCloud1_ && newCloud2_)
00100       publishClouds();
00101   }
00102 
00103   void publishClouds(void)
00104   {
00105     lock1_.lock();
00106     lock2_.lock();
00107 
00108     newCloud1_ = false;
00109     newCloud2_ = false;
00110 
00111     sensor_msgs::PointCloud out;
00112     if (cloud1_.header.stamp > cloud2_.header.stamp)
00113       out.header = cloud1_.header;
00114     else
00115       out.header = cloud2_.header;
00116 
00117     out.points.resize(cloud1_.points.size() + cloud2_.points.size());
00118 
00119     // copy points
00120     std::copy(cloud1_.points.begin(), cloud1_.points.end(), out.points.begin());
00121     std::copy(cloud2_.points.begin(), cloud2_.points.end(), out.points.begin() + cloud1_.points.size());
00122 
00123     // copy common channels
00124     for (unsigned int i = 0 ; i < cloud1_.channels.size() ; ++i)
00125       for (unsigned int j = 0 ; j < cloud2_.channels.size() ; ++j)
00126         if (cloud1_.channels[i].name == cloud2_.channels[j].name)
00127         {
00128           ROS_ASSERT(cloud1_.channels[i].values.size() == cloud1_.points.size());
00129           ROS_ASSERT(cloud2_.channels[j].values.size() == cloud2_.points.size());
00130           unsigned int oc = out.channels.size();
00131           out.channels.resize(oc + 1);
00132           out.channels[oc].name = cloud1_.channels[i].name;
00133           out.channels[oc].values.resize(cloud1_.channels[i].values.size() + cloud2_.channels[j].values.size());
00134           std::copy(cloud1_.channels[i].values.begin(), cloud1_.channels[i].values.end(), out.channels[oc].values.begin());
00135           std::copy(cloud2_.channels[j].values.begin(), cloud2_.channels[j].values.end(), out.channels[oc].values.begin() + cloud1_.channels[i].values.size());
00136           break;
00137         }
00138 
00139     lock1_.unlock();
00140     lock2_.unlock();
00141 
00142     cloudOut_.publish(out);
00143   }
00144 
00145   void receiveCloud1(const sensor_msgs::PointCloudConstPtr &cloud)
00146   {
00147     lock1_.lock();
00148     processCloud(cloud, cloud1_);
00149     lock1_.unlock();
00150     newCloud1_ = true;
00151     if (!haveTimer_ && newCloud2_)
00152       publishClouds();
00153   }
00154 
00155   void receiveCloud2(const sensor_msgs::PointCloudConstPtr &cloud)
00156   {
00157     lock2_.lock();
00158     processCloud(cloud, cloud2_);
00159     lock2_.unlock();
00160     newCloud2_ = true;
00161     if (!haveTimer_ && newCloud1_)
00162       publishClouds();
00163   }
00164 
00165   void processCloud(const sensor_msgs::PointCloudConstPtr &cloud, sensor_msgs::PointCloud &cloudOut)
00166   {
00167     if (output_frame_ != cloud->header.frame_id)
00168       tf_.transformPointCloud(output_frame_, *cloud, cloudOut);
00169     else
00170       cloudOut = *cloud;
00171   }
00172 
00173   ros::NodeHandle       nh_;
00174   tf::TransformListener tf_;
00175 
00176   ros::Timer            timer_;
00177   bool                  haveTimer_;
00178 
00179   ros::Publisher        cloudOut_;
00180   double                max_freq_;
00181   std::string           output_frame_;
00182 
00183   message_filters::Subscriber<sensor_msgs::PointCloud> sub1_;
00184   message_filters::Subscriber<sensor_msgs::PointCloud> sub2_;
00185   boost::shared_ptr<tf::MessageFilter<sensor_msgs::PointCloud> > tf_filter1_;
00186   boost::shared_ptr<tf::MessageFilter<sensor_msgs::PointCloud> > tf_filter2_;
00187 
00188   bool                    newCloud1_;
00189   bool                    newCloud2_;
00190   sensor_msgs::PointCloud cloud1_;
00191   sensor_msgs::PointCloud cloud2_;
00192   boost::mutex            lock1_;
00193   boost::mutex            lock2_;
00194 
00195 };
00196 
00197 
00198 int main(int argc, char **argv)
00199 {
00200   ros::init(argc, argv, "merge_clouds", ros::init_options::AnonymousName);
00201 
00202   ROS_WARN("laser_assembler/merge_clouds is deprecated. You should instead be using 3dnav_pr2/merge_clouds");
00203 
00204   MergeClouds mc;
00205   ros::spin();
00206 
00207   return 0;
00208 }


laser_assembler
Author(s): Vijay Pradeep
autogenerated on Mon Oct 6 2014 01:43:03