base_assembler.h
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 
00036 
00037 #include "ros/ros.h"
00038 #include "tf/transform_listener.h"
00039 #include "tf/message_filter.h"
00040 #include "sensor_msgs/PointCloud.h"
00041 #include "sensor_msgs/point_cloud_conversion.h"
00042 #include "message_filters/subscriber.h"
00043 
00044 #include <deque>
00045 
00046 // Service
00047 #include "laser_assembler/AssembleScans.h"
00048 #include "laser_assembler/AssembleScans2.h"
00049 
00050 #include "boost/thread.hpp"
00051 #include "math.h"
00052 
00053 namespace laser_assembler
00054 {
00055 
00059 template<class T>
00060 class BaseAssembler
00061 {
00062 public:
00063   BaseAssembler(const std::string& max_size_param_name);
00064   ~BaseAssembler() ;
00065 
00073   void start(const std::string& in_topic_name);
00074   void start();
00075 
00076 
00081   virtual unsigned int GetPointsInScan(const T& scan) = 0 ;
00082 
00091   virtual void ConvertToCloud(const std::string& fixed_frame_id, const T& scan_in, sensor_msgs::PointCloud& cloud_out) = 0 ;
00092 
00093 protected:
00094   tf::TransformListener* tf_ ;
00095   tf::MessageFilter<T>* tf_filter_;
00096 
00097   ros::NodeHandle private_ns_;
00098   ros::NodeHandle n_;
00099 
00100 private:
00101   // ROS Input/Ouptut Handling
00102   ros::ServiceServer build_cloud_server_;
00103   ros::ServiceServer assemble_scans_server_;
00104   ros::ServiceServer build_cloud_server2_;
00105   ros::ServiceServer assemble_scans_server2_;
00106   message_filters::Subscriber<T> scan_sub_;
00107   message_filters::Connection tf_filter_connection_;
00108 
00110   //void scansCallback(const tf::MessageNotifier<T>::MessagePtr& scan_ptr, const T& testA)
00111   virtual void msgCallback(const boost::shared_ptr<const T>& scan_ptr) ;
00112 
00114   bool buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) ;
00115   bool assembleScans(AssembleScans::Request& req, AssembleScans::Response& resp) ;
00116   bool buildCloud2(AssembleScans2::Request& req, AssembleScans2::Response& resp) ;
00117   bool assembleScans2(AssembleScans2::Request& req, AssembleScans2::Response& resp) ;
00118 
00120   std::deque<sensor_msgs::PointCloud> scan_hist_ ;
00121   boost::mutex scan_hist_mutex_ ;
00122 
00124   unsigned int total_pts_ ;
00125 
00127   unsigned int max_scans_ ;
00128 
00130   std::string fixed_frame_ ;
00131 
00133   unsigned int downsample_factor_ ;
00134 
00135 } ;
00136 
00137 template <class T>
00138 BaseAssembler<T>::BaseAssembler(const std::string& max_size_param_name) : private_ns_("~")
00139 {
00140   // **** Initialize TransformListener ****
00141   double tf_cache_time_secs ;
00142   private_ns_.param("tf_cache_time_secs", tf_cache_time_secs, 10.0) ;
00143   if (tf_cache_time_secs < 0)
00144     ROS_ERROR("Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs) ;
00145 
00146   tf_ = new tf::TransformListener(n_, ros::Duration(tf_cache_time_secs));
00147   ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs) ;
00148 
00149   // ***** Set max_scans *****
00150   const int default_max_scans = 400 ;
00151   int tmp_max_scans ;
00152   private_ns_.param(max_size_param_name, tmp_max_scans, default_max_scans);
00153   if (tmp_max_scans < 0)
00154   {
00155     ROS_ERROR("Parameter max_scans<0 (%i)", tmp_max_scans) ;
00156     tmp_max_scans = default_max_scans ;
00157   }
00158   max_scans_ = tmp_max_scans ;
00159   ROS_INFO("Max Scans in History: %u", max_scans_) ;
00160   total_pts_ = 0 ;    // We're always going to start with no points in our history
00161 
00162   // ***** Set fixed_frame *****
00163   private_ns_.param("fixed_frame", fixed_frame_, std::string("ERROR_NO_NAME"));
00164   ROS_INFO("Fixed Frame: %s", fixed_frame_.c_str()) ;
00165   if (fixed_frame_ == "ERROR_NO_NAME")
00166     ROS_ERROR("Need to set parameter fixed_frame") ;
00167 
00168   // ***** Set downsample_factor *****
00169   int tmp_downsample_factor ;
00170   private_ns_.param("downsample_factor", tmp_downsample_factor, 1);
00171   if (tmp_downsample_factor < 1)
00172   {
00173     ROS_ERROR("Parameter downsample_factor<1: %i", tmp_downsample_factor) ;
00174     tmp_downsample_factor = 1 ;
00175   }
00176   downsample_factor_ = tmp_downsample_factor ;
00177   if (downsample_factor_ != 1)
00178     ROS_WARN("Downsample set to [%u]. Note that this is an unreleased/unstable feature", downsample_factor_);
00179 
00180   // ***** Start Services *****
00181   build_cloud_server_    = n_.advertiseService("build_cloud",    &BaseAssembler<T>::buildCloud,    this);
00182   assemble_scans_server_ = n_.advertiseService("assemble_scans", &BaseAssembler<T>::assembleScans, this);
00183   build_cloud_server2_    = n_.advertiseService("build_cloud2",    &BaseAssembler<T>::buildCloud2,    this);
00184   assemble_scans_server2_ = n_.advertiseService("assemble_scans2", &BaseAssembler<T>::assembleScans2, this);
00185 
00186   // ***** Start Listening to Data *****
00187   // (Well, don't start listening just yet. Keep this as null until we actually start listening, when start() is called)
00188   tf_filter_ = NULL;
00189 
00190 }
00191 
00192 template <class T>
00193 void BaseAssembler<T>::start(const std::string& in_topic_name)
00194 {
00195   ROS_DEBUG("Called start(string). Starting to listen on message_filter::Subscriber the input stream");
00196   if (tf_filter_)
00197     ROS_ERROR("assembler::start() was called twice!. This is bad, and could leak memory") ;
00198   else
00199   {
00200     scan_sub_.subscribe(n_, in_topic_name, 10);
00201     tf_filter_ = new tf::MessageFilter<T>(scan_sub_, *tf_, fixed_frame_, 10);
00202     tf_filter_->registerCallback( boost::bind(&BaseAssembler<T>::msgCallback, this, _1) );
00203   }
00204 }
00205 
00206 template <class T>
00207 void BaseAssembler<T>::start()
00208 {
00209   ROS_DEBUG("Called start(). Starting tf::MessageFilter, but not initializing Subscriber");
00210   if (tf_filter_)
00211     ROS_ERROR("assembler::start() was called twice!. This is bad, and could leak memory") ;
00212   else
00213   {
00214     scan_sub_.subscribe(n_, "bogus", 10);
00215     tf_filter_ = new tf::MessageFilter<T>(scan_sub_, *tf_, fixed_frame_, 10);
00216     tf_filter_->registerCallback( boost::bind(&BaseAssembler<T>::msgCallback, this, _1) );
00217   }
00218 }
00219 
00220 template <class T>
00221 BaseAssembler<T>::~BaseAssembler()
00222 {
00223   if (tf_filter_)
00224     delete tf_filter_;
00225 
00226   delete tf_ ;
00227 }
00228 
00229 template <class T>
00230 void BaseAssembler<T>::msgCallback(const boost::shared_ptr<const T>& scan_ptr)
00231 {
00232   ROS_DEBUG("starting msgCallback");
00233   const T scan = *scan_ptr ;
00234 
00235   sensor_msgs::PointCloud cur_cloud ;
00236 
00237   // Convert the scan data into a universally known datatype: PointCloud
00238   try
00239   {
00240     ConvertToCloud(fixed_frame_, scan, cur_cloud) ;              // Convert scan into a point cloud
00241   }
00242   catch(tf::TransformException& ex)
00243   {
00244     ROS_WARN("Transform Exception %s", ex.what()) ;
00245     return ;
00246   }
00247 
00248   // Add the current scan (now of type PointCloud) into our history of scans
00249   scan_hist_mutex_.lock() ;
00250   if (scan_hist_.size() == max_scans_)                           // Is our deque full?
00251   {
00252     total_pts_ -= scan_hist_.front().points.size () ;            // We're removing an elem, so this reduces our total point count
00253     scan_hist_.pop_front() ;                                     // The front of the deque has the oldest elem, so we can get rid of it
00254   }
00255   scan_hist_.push_back(cur_cloud) ;                              // Add the newest scan to the back of the deque
00256   total_pts_ += cur_cloud.points.size () ;                       // Add the new scan to the running total of points
00257 
00258   //printf("Scans: %4u  Points: %10u\n", scan_hist_.size(), total_pts_) ;
00259 
00260   scan_hist_mutex_.unlock() ;
00261   ROS_DEBUG("done with msgCallback");
00262 }
00263 
00264 template <class T>
00265 bool BaseAssembler<T>::buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp)
00266 {
00267   ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead");
00268   return assembleScans(req, resp);
00269 }
00270 
00271 
00272 template <class T>
00273 bool BaseAssembler<T>::assembleScans(AssembleScans::Request& req, AssembleScans::Response& resp)
00274 {
00275   //printf("Starting Service Request\n") ;
00276 
00277   scan_hist_mutex_.lock() ;
00278   // Determine where in our history we actually are
00279   unsigned int i = 0 ;
00280 
00281   // Find the beginning of the request. Probably should be a search
00282   while ( i < scan_hist_.size() &&                                                    // Don't go past end of deque
00283           scan_hist_[i].header.stamp < req.begin )                                    // Keep stepping until we've exceeded the start time
00284   {
00285     i++ ;
00286   }
00287   unsigned int start_index = i ;
00288 
00289   unsigned int req_pts = 0 ;                                                          // Keep a total of the points in the current request
00290   // Find the end of the request
00291   while ( i < scan_hist_.size() &&                                                    // Don't go past end of deque
00292           scan_hist_[i].header.stamp < req.end )                                      // Don't go past the end-time of the request
00293   {
00294     req_pts += (scan_hist_[i].points.size ()+downsample_factor_-1)/downsample_factor_ ;
00295     i += downsample_factor_ ;
00296   }
00297   unsigned int past_end_index = i ;
00298 
00299   if (start_index == past_end_index)
00300   {
00301     resp.cloud.header.frame_id = fixed_frame_ ;
00302     resp.cloud.header.stamp = req.end ;
00303     resp.cloud.points.resize (0) ;
00304     resp.cloud.channels.resize (0) ;
00305   }
00306   else
00307   {
00308     // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen
00309     // Allocate space for the cloud
00310     resp.cloud.points.resize (req_pts);
00311     const unsigned int num_channels = scan_hist_[start_index].channels.size ();
00312     resp.cloud.channels.resize (num_channels) ;
00313     for (i = 0; i<num_channels; i++)
00314     {
00315       resp.cloud.channels[i].name = scan_hist_[start_index].channels[i].name ;
00316       resp.cloud.channels[i].values.resize (req_pts) ;
00317     }
00318     //resp.cloud.header.stamp = req.end ;
00319     resp.cloud.header.frame_id = fixed_frame_ ;
00320     unsigned int cloud_count = 0 ;
00321     for (i=start_index; i<past_end_index; i+=downsample_factor_)
00322     {
00323 
00324       // Sanity check: Each channel should be the same length as the points vector
00325       for (unsigned int chan_ind = 0; chan_ind < scan_hist_[i].channels.size(); chan_ind++)
00326       {
00327         if (scan_hist_[i].points.size () != scan_hist_[i].channels[chan_ind].values.size())
00328           ROS_FATAL("Trying to add a malformed point cloud. Cloud has %u points, but channel %u has %u elems", (int)scan_hist_[i].points.size (), chan_ind, (int)scan_hist_[i].channels[chan_ind].values.size ());
00329       }
00330 
00331       for(unsigned int j=0; j<scan_hist_[i].points.size (); j+=downsample_factor_)
00332       {
00333         resp.cloud.points[cloud_count].x = scan_hist_[i].points[j].x ;
00334         resp.cloud.points[cloud_count].y = scan_hist_[i].points[j].y ;
00335         resp.cloud.points[cloud_count].z = scan_hist_[i].points[j].z ;
00336 
00337         for (unsigned int k=0; k<num_channels; k++)
00338           resp.cloud.channels[k].values[cloud_count] = scan_hist_[i].channels[k].values[j] ;
00339 
00340         cloud_count++ ;
00341       }
00342       resp.cloud.header.stamp = scan_hist_[i].header.stamp;
00343     }
00344   }
00345   scan_hist_mutex_.unlock() ;
00346 
00347   ROS_DEBUG("Point Cloud Results: Aggregated from index %u->%u. BufferSize: %lu. Points in cloud: %u", start_index, past_end_index, scan_hist_.size(), (int)resp.cloud.points.size ()) ;
00348   return true ;
00349 }
00350 
00351 template <class T>
00352 bool BaseAssembler<T>::buildCloud2(AssembleScans2::Request& req, AssembleScans2::Response& resp)
00353 {
00354   ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead");
00355   return assembleScans2(req, resp);
00356 }
00357 
00358 template <class T>
00359 bool BaseAssembler<T>::assembleScans2(AssembleScans2::Request& req, AssembleScans2::Response& resp)
00360 {
00361   AssembleScans::Request tmp_req;
00362   AssembleScans::Response tmp_res;
00363   tmp_req.begin = req.begin;
00364   tmp_req.end = req.end;
00365   bool ret = assembleScans(tmp_req, tmp_res);
00366 
00367   if ( ret )
00368   {
00369     sensor_msgs::convertPointCloudToPointCloud2(tmp_res.cloud, resp.cloud);
00370   }
00371   return ret;
00372 }
00373 }


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