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 "sensor_msgs/point_cloud_conversion.h"
00042 #include "message_filters/subscriber.h"
00043
00044 #include <deque>
00045
00046
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
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
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
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
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 ;
00161
00162
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
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
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
00187
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
00238 try
00239 {
00240 ConvertToCloud(fixed_frame_, scan, cur_cloud) ;
00241 }
00242 catch(tf::TransformException& ex)
00243 {
00244 ROS_WARN("Transform Exception %s", ex.what()) ;
00245 return ;
00246 }
00247
00248
00249 scan_hist_mutex_.lock() ;
00250 if (scan_hist_.size() == max_scans_)
00251 {
00252 total_pts_ -= scan_hist_.front().points.size () ;
00253 scan_hist_.pop_front() ;
00254 }
00255 scan_hist_.push_back(cur_cloud) ;
00256 total_pts_ += cur_cloud.points.size () ;
00257
00258
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
00276
00277 scan_hist_mutex_.lock() ;
00278
00279 unsigned int i = 0 ;
00280
00281
00282 while ( i < scan_hist_.size() &&
00283 scan_hist_[i].header.stamp < req.begin )
00284 {
00285 i++ ;
00286 }
00287 unsigned int start_index = i ;
00288
00289 unsigned int req_pts = 0 ;
00290
00291 while ( i < scan_hist_.size() &&
00292 scan_hist_[i].header.stamp < req.end )
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
00309
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
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
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 }