laser_assembler: laser_assembler: base

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