Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
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
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
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
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 ;
00170
00171
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
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
00189 cloud_srv_server_ = private_ns_.advertiseService("build_cloud", &BaseAssemblerSrv<T>::buildCloud, this);
00190
00191
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
00198
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
00235 try
00236 {
00237 ConvertToCloud(fixed_frame_, scan, cur_cloud) ;
00238 }
00239 catch(tf::TransformException& ex)
00240 {
00241 ROS_WARN("Transform Exception %s", ex.what()) ;
00242 return ;
00243 }
00244
00245
00246 scan_hist_mutex_.lock() ;
00247 if (scan_hist_.size() == max_scans_)
00248 {
00249 total_pts_ -= scan_hist_.front().points.size() ;
00250 scan_hist_.pop_front() ;
00251 }
00252 scan_hist_.push_back(cur_cloud) ;
00253 total_pts_ += cur_cloud.points.size() ;
00254
00255
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
00264
00265 scan_hist_mutex_.lock() ;
00266
00267 unsigned int i = 0 ;
00268
00269
00270 while ( i < scan_hist_.size() &&
00271 scan_hist_[i].header.stamp < req.begin )
00272 {
00273 i++ ;
00274 }
00275 unsigned int start_index = i ;
00276
00277 unsigned int req_pts = 0 ;
00278
00279 while ( i < scan_hist_.size() &&
00280 scan_hist_[i].header.stamp < req.end )
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
00297
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
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 }