laser_assembler: laser_assembler: base_assembler

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 "message_filters/subscriber.h"
00042 
00043 #include <deque>
00044 
00045 // Service
00046 #include "laser_assembler/AssembleScans.h"
00047 
00048 #include "boost/thread.hpp"
00049 #include "math.h"
00050 
00051 namespace laser_assembler
00052 {
00053 
00071 template<class T>
00072 class BaseAssemblerSrv
00073 {
00074 public:
00075   BaseAssemblerSrv() ;
00076   ~BaseAssemblerSrv() ;
00077 
00085   void start() ;
00086 
00087 
00092   virtual unsigned int GetPointsInScan(const T& scan) = 0 ;
00093 
00102   virtual void ConvertToCloud(const std::string& fixed_frame_id, const T& scan_in, sensor_msgs::PointCloud& cloud_out) = 0 ;
00103 
00104 protected:
00105   tf::TransformListener* tf_ ;
00106 
00107   ros::NodeHandle private_ns_;
00108   ros::NodeHandle n_;
00109 
00110 private:
00111   // ROS Input/Ouptut Handling
00112   ros::ServiceServer cloud_srv_server_;
00113   message_filters::Subscriber<T> scan_sub_;
00114   tf::MessageFilter<T>* tf_filter_;
00115   message_filters::Connection tf_filter_connection_;
00116 
00118   //void scansCallback(const tf::MessageNotifier<T>::MessagePtr& scan_ptr, const T& testA)
00119   void scansCallback(const boost::shared_ptr<const T>& scan_ptr) ;
00120 
00122   bool buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) ;
00123 
00124 
00126   std::deque<sensor_msgs::PointCloud> scan_hist_ ;
00127   boost::mutex scan_hist_mutex_ ;
00128 
00130   unsigned int total_pts_ ;
00131 
00133   unsigned int max_scans_ ;
00134 
00136   std::string fixed_frame_ ;
00137 
00139   double tf_tolerance_secs_ ;
00140 
00142   unsigned int downsample_factor_ ;
00143 
00144 } ;
00145 
00146 template <class T>
00147 BaseAssemblerSrv<T>::BaseAssemblerSrv() : private_ns_("~")
00148 {
00149   // **** Initialize TransformListener ****
00150   double tf_cache_time_secs ;
00151   private_ns_.param("tf_cache_time_secs", tf_cache_time_secs, 10.0) ;
00152   if (tf_cache_time_secs < 0)
00153     ROS_ERROR("Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs) ;
00154 
00155   tf_ = new tf::TransformListener(n_, ros::Duration(tf_cache_time_secs));
00156   ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs) ;
00157 
00158   // ***** Set max_scans *****
00159   const int default_max_scans = 400 ;
00160   int tmp_max_scans ;
00161   private_ns_.param("max_scans", tmp_max_scans, default_max_scans);
00162   if (tmp_max_scans < 0)
00163   {
00164     ROS_ERROR("Parameter max_scans<0 (%i)", tmp_max_scans) ;
00165     tmp_max_scans = default_max_scans ;
00166   }
00167   max_scans_ = tmp_max_scans ;
00168   ROS_INFO("Max Scans in History: %u", max_scans_) ;
00169   total_pts_ = 0 ;    // We're always going to start with no points in our history
00170 
00171   // ***** Set fixed_frame *****
00172   private_ns_.param("fixed_frame", fixed_frame_, std::string("ERROR_NO_NAME"));
00173   ROS_INFO("Fixed Frame: %s", fixed_frame_.c_str()) ;
00174   if (fixed_frame_ == "ERROR_NO_NAME")
00175     ROS_ERROR("Need to set parameter fixed_frame") ;
00176 
00177   // ***** Set downsample_factor *****
00178   int tmp_downsample_factor ;
00179   private_ns_.param("downsample_factor", tmp_downsample_factor, 1);
00180   if (tmp_downsample_factor < 1)
00181   {
00182     ROS_ERROR("Parameter downsample_factor<1: %i", tmp_downsample_factor) ;
00183     tmp_downsample_factor = 1 ;
00184   }
00185   downsample_factor_ = tmp_downsample_factor ;
00186   ROS_INFO("Downsample Factor: %u", downsample_factor_) ;
00187 
00188   // ***** Start Services *****
00189   cloud_srv_server_ = private_ns_.advertiseService("build_cloud", &BaseAssemblerSrv<T>::buildCloud, this);
00190 
00191   // **** Get the TF Notifier Tolerance ****
00192   private_ns_.param("tf_tolerance_secs", tf_tolerance_secs_, 0.0);
00193   if (tf_tolerance_secs_ < 0)
00194     ROS_ERROR("Parameter tf_tolerance_secs<0 (%f)", tf_tolerance_secs_) ;
00195   ROS_INFO("tf Tolerance: %f seconds", tf_tolerance_secs_) ;
00196 
00197   // ***** Start Listening to Data *****
00198   // (Well, don't start listening just yet. Keep this as null until we actually start listening, when start() is called)
00199   tf_filter_ = NULL;
00200 
00201 }
00202 
00203 template <class T>
00204 void BaseAssemblerSrv<T>::start()
00205 {
00206   ROS_INFO("Starting to listen on the input stream") ;
00207   if (tf_filter_)
00208     ROS_ERROR("assembler::start() was called twice!. This is bad, and could leak memory") ;
00209   else
00210   {
00211     scan_sub_.subscribe(n_, "scan_in", 10);
00212     tf_filter_ = new tf::MessageFilter<T>(scan_sub_, *tf_, fixed_frame_, 10);
00213     tf_filter_->setTolerance(ros::Duration(tf_tolerance_secs_));
00214     tf_filter_->registerCallback( boost::bind(&BaseAssemblerSrv<T>::scansCallback, this, _1) );
00215   }
00216 }
00217 
00218 template <class T>
00219 BaseAssemblerSrv<T>::~BaseAssemblerSrv()
00220 {
00221   if (tf_filter_)
00222     delete tf_filter_;
00223 
00224   delete tf_ ;
00225 }
00226 
00227 template <class T>
00228 void BaseAssemblerSrv<T>::scansCallback(const boost::shared_ptr<const T>& scan_ptr)
00229 {
00230   const T scan = *scan_ptr ;
00231 
00232   sensor_msgs::PointCloud cur_cloud ;
00233 
00234   // Convert the scan data into a universally known datatype: PointCloud
00235   try
00236   {
00237     ConvertToCloud(fixed_frame_, scan, cur_cloud) ;              // Convert scan into a point cloud
00238   }
00239   catch(tf::TransformException& ex)
00240   {
00241     ROS_WARN("Transform Exception %s", ex.what()) ;
00242     return ;
00243   }
00244 
00245   // Add the current scan (now of type PointCloud) into our history of scans
00246   scan_hist_mutex_.lock() ;
00247   if (scan_hist_.size() == max_scans_)                           // Is our deque full?
00248   {
00249     total_pts_ -= scan_hist_.front().points.size() ;            // We're removing an elem, so this reduces our total point count
00250     scan_hist_.pop_front() ;                                     // The front of the deque has the oldest elem, so we can get rid of it
00251   }
00252   scan_hist_.push_back(cur_cloud) ;                              // Add the newest scan to the back of the deque
00253   total_pts_ += cur_cloud.points.size() ;                       // Add the new scan to the running total of points
00254 
00255   //printf("Scans: %4u  Points: %10u\n", scan_hist_.size(), total_pts_) ;
00256 
00257   scan_hist_mutex_.unlock() ;
00258 }
00259 
00260 template <class T>
00261 bool BaseAssemblerSrv<T>::buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp)
00262 {
00263   //printf("Starting Service Request\n") ;
00264 
00265   scan_hist_mutex_.lock() ;
00266   // Determine where in our history we actually are
00267   unsigned int i = 0 ;
00268 
00269   // Find the beginning of the request. Probably should be a search
00270   while ( i < scan_hist_.size() &&                                                    // Don't go past end of deque
00271           scan_hist_[i].header.stamp < req.begin )                                    // Keep stepping until we've exceeded the start time
00272   {
00273     i++ ;
00274   }
00275   unsigned int start_index = i ;
00276 
00277   unsigned int req_pts = 0 ;                                                          // Keep a total of the points in the current request
00278   // Find the end of the request
00279   while ( i < scan_hist_.size() &&                                                    // Don't go past end of deque
00280           scan_hist_[i].header.stamp < req.end )                                      // Don't go past the end-time of the request
00281   {
00282     req_pts += (scan_hist_[i].points.size()+downsample_factor_-1)/downsample_factor_ ;
00283     i += downsample_factor_ ;
00284   }
00285   unsigned int past_end_index = i ;
00286 
00287   if (start_index == past_end_index)
00288   {
00289     resp.cloud.header.frame_id = fixed_frame_ ;
00290     resp.cloud.header.stamp = req.end ;
00291     resp.cloud.points.resize(0) ;
00292     resp.cloud.channels.resize(0) ;
00293   }
00294   else
00295   {
00296     // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen
00297     // Allocate space for the cloud
00298     resp.cloud.points.resize( req_pts ) ;
00299     const unsigned int num_channels = scan_hist_[start_index].channels.size() ;
00300     resp.cloud.channels.resize(num_channels) ;
00301     for (i = 0; i<num_channels; i++)
00302     {
00303       resp.cloud.channels[i].name = scan_hist_[start_index].channels[i].name ;
00304       resp.cloud.channels[i].values.resize(req_pts) ;
00305     }
00306     //resp.cloud.header.stamp = req.end ;
00307     resp.cloud.header.frame_id = fixed_frame_ ;
00308     unsigned int cloud_count = 0 ;
00309     for (i=start_index; i<past_end_index; i+=downsample_factor_)
00310     {
00311       for(unsigned int j=0; j<scan_hist_[i].points.size(); j+=downsample_factor_)
00312       {
00313         resp.cloud.points[cloud_count].x = scan_hist_[i].points[j].x ;
00314         resp.cloud.points[cloud_count].y = scan_hist_[i].points[j].y ;
00315         resp.cloud.points[cloud_count].z = scan_hist_[i].points[j].z ;
00316         for (unsigned int k=0; k<num_channels; k++)
00317           resp.cloud.channels[k].values[cloud_count] = scan_hist_[i].channels[k].values[j] ;
00318 
00319         cloud_count++ ;
00320       }
00321       resp.cloud.header.stamp = scan_hist_[i].header.stamp;
00322     }
00323   }
00324   scan_hist_mutex_.unlock() ;
00325 
00326   ROS_DEBUG("Point Cloud Results: Aggregated from index %u->%u. BufferSize: %lu. Points in cloud: %lu", start_index, past_end_index, scan_hist_.size(), resp.cloud.points.size()) ;
00327   return true ;
00328 }
00329 
00330 }
base_assembler_srv.h
