base_assembler.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
36 
37 #include "ros/ros.h"
38 #include "tf/transform_listener.h"
39 #include "tf/message_filter.h"
40 #include "sensor_msgs/PointCloud.h"
43 
44 #include <deque>
45 
46 // Service
47 #include "laser_assembler/AssembleScans.h"
48 #include "laser_assembler/AssembleScans2.h"
49 
50 #include "boost/thread.hpp"
51 #include "math.h"
52 
53 namespace laser_assembler
54 {
55 
59 template<class T>
61 {
62 public:
63  BaseAssembler(const std::string& max_size_param_name);
64  ~BaseAssembler() ;
65 
73  void start(const std::string& in_topic_name);
74  void start();
75 
76 
81  virtual unsigned int GetPointsInScan(const T& scan) = 0 ;
82 
91  virtual void ConvertToCloud(const std::string& fixed_frame_id, const T& scan_in, sensor_msgs::PointCloud& cloud_out) = 0 ;
92 
93 protected:
96 
99 
100 private:
101  // ROS Input/Ouptut Handling
108 
110  //void scansCallback(const tf::MessageNotifier<T>::MessagePtr& scan_ptr, const T& testA)
111  virtual void msgCallback(const boost::shared_ptr<const T>& scan_ptr) ;
112 
114  bool buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) ;
115  bool assembleScans(AssembleScans::Request& req, AssembleScans::Response& resp) ;
116  bool buildCloud2(AssembleScans2::Request& req, AssembleScans2::Response& resp) ;
117  bool assembleScans2(AssembleScans2::Request& req, AssembleScans2::Response& resp) ;
118 
120  std::deque<sensor_msgs::PointCloud> scan_hist_ ;
121  boost::mutex scan_hist_mutex_ ;
122 
124  unsigned int total_pts_ ;
125 
127  unsigned int max_scans_ ;
128 
130  std::string fixed_frame_ ;
131 
133  unsigned int downsample_factor_ ;
134 
135 } ;
136 
137 template <class T>
138 BaseAssembler<T>::BaseAssembler(const std::string& max_size_param_name) : private_ns_("~")
139 {
140  // **** Initialize TransformListener ****
141  double tf_cache_time_secs ;
142  private_ns_.param("tf_cache_time_secs", tf_cache_time_secs, 10.0) ;
143  if (tf_cache_time_secs < 0)
144  ROS_ERROR("Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs) ;
145 
146  tf_ = new tf::TransformListener(n_, ros::Duration(tf_cache_time_secs));
147  ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs) ;
148 
149  // ***** Set max_scans *****
150  const int default_max_scans = 400 ;
151  int tmp_max_scans ;
152  private_ns_.param(max_size_param_name, tmp_max_scans, default_max_scans);
153  if (tmp_max_scans < 0)
154  {
155  ROS_ERROR("Parameter max_scans<0 (%i)", tmp_max_scans) ;
156  tmp_max_scans = default_max_scans ;
157  }
158  max_scans_ = tmp_max_scans ;
159  ROS_INFO("Max Scans in History: %u", max_scans_) ;
160  total_pts_ = 0 ; // We're always going to start with no points in our history
161 
162  // ***** Set fixed_frame *****
163  private_ns_.param("fixed_frame", fixed_frame_, std::string("ERROR_NO_NAME"));
164  ROS_INFO("Fixed Frame: %s", fixed_frame_.c_str()) ;
165  if (fixed_frame_ == "ERROR_NO_NAME")
166  ROS_ERROR("Need to set parameter fixed_frame") ;
167 
168  // ***** Set downsample_factor *****
169  int tmp_downsample_factor ;
170  private_ns_.param("downsample_factor", tmp_downsample_factor, 1);
171  if (tmp_downsample_factor < 1)
172  {
173  ROS_ERROR("Parameter downsample_factor<1: %i", tmp_downsample_factor) ;
174  tmp_downsample_factor = 1 ;
175  }
176  downsample_factor_ = tmp_downsample_factor ;
177  if (downsample_factor_ != 1)
178  ROS_WARN("Downsample set to [%u]. Note that this is an unreleased/unstable feature", downsample_factor_);
179 
180  // ***** Start Services *****
185 
186  // ***** Start Listening to Data *****
187  // (Well, don't start listening just yet. Keep this as null until we actually start listening, when start() is called)
188  tf_filter_ = NULL;
189 
190 }
191 
192 template <class T>
193 void BaseAssembler<T>::start(const std::string& in_topic_name)
194 {
195  ROS_DEBUG("Called start(string). Starting to listen on message_filter::Subscriber the input stream");
196  if (tf_filter_)
197  ROS_ERROR("assembler::start() was called twice!. This is bad, and could leak memory") ;
198  else
199  {
200  scan_sub_.subscribe(n_, in_topic_name, 10);
202  tf_filter_->registerCallback( boost::bind(&BaseAssembler<T>::msgCallback, this, _1) );
203  }
204 }
205 
206 template <class T>
208 {
209  ROS_DEBUG("Called start(). Starting tf::MessageFilter, but not initializing Subscriber");
210  if (tf_filter_)
211  ROS_ERROR("assembler::start() was called twice!. This is bad, and could leak memory") ;
212  else
213  {
214  scan_sub_.subscribe(n_, "bogus", 10);
216  tf_filter_->registerCallback( boost::bind(&BaseAssembler<T>::msgCallback, this, _1) );
217  }
218 }
219 
220 template <class T>
222 {
223  if (tf_filter_)
224  delete tf_filter_;
225 
226  delete tf_ ;
227 }
228 
229 template <class T>
231 {
232  ROS_DEBUG("starting msgCallback");
233  const T scan = *scan_ptr ;
234 
235  sensor_msgs::PointCloud cur_cloud ;
236 
237  // Convert the scan data into a universally known datatype: PointCloud
238  try
239  {
240  ConvertToCloud(fixed_frame_, scan, cur_cloud) ; // Convert scan into a point cloud
241  }
242  catch(tf::TransformException& ex)
243  {
244  ROS_WARN("Transform Exception %s", ex.what()) ;
245  return ;
246  }
247 
248  // Add the current scan (now of type PointCloud) into our history of scans
249  scan_hist_mutex_.lock() ;
250  if (scan_hist_.size() == max_scans_) // Is our deque full?
251  {
252  total_pts_ -= scan_hist_.front().points.size () ; // We're removing an elem, so this reduces our total point count
253  scan_hist_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
254  }
255  scan_hist_.push_back(cur_cloud) ; // Add the newest scan to the back of the deque
256  total_pts_ += cur_cloud.points.size () ; // Add the new scan to the running total of points
257 
258  //printf("Scans: %4u Points: %10u\n", scan_hist_.size(), total_pts_) ;
259 
260  scan_hist_mutex_.unlock() ;
261  ROS_DEBUG("done with msgCallback");
262 }
263 
264 template <class T>
265 bool BaseAssembler<T>::buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp)
266 {
267  ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead");
268  return assembleScans(req, resp);
269 }
270 
271 
272 template <class T>
273 bool BaseAssembler<T>::assembleScans(AssembleScans::Request& req, AssembleScans::Response& resp)
274 {
275  //printf("Starting Service Request\n") ;
276 
277  scan_hist_mutex_.lock() ;
278  // Determine where in our history we actually are
279  unsigned int i = 0 ;
280 
281  // Find the beginning of the request. Probably should be a search
282  while ( i < scan_hist_.size() && // Don't go past end of deque
283  scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time
284  {
285  i++ ;
286  }
287  unsigned int start_index = i ;
288 
289  unsigned int req_pts = 0 ; // Keep a total of the points in the current request
290  // Find the end of the request
291  while ( i < scan_hist_.size() && // Don't go past end of deque
292  scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request
293  {
294  req_pts += (scan_hist_[i].points.size ()+downsample_factor_-1)/downsample_factor_ ;
295  i += downsample_factor_ ;
296  }
297  unsigned int past_end_index = i ;
298 
299  if (start_index == past_end_index)
300  {
301  resp.cloud.header.frame_id = fixed_frame_ ;
302  resp.cloud.header.stamp = req.end ;
303  resp.cloud.points.resize (0) ;
304  resp.cloud.channels.resize (0) ;
305  }
306  else
307  {
308  // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen
309  // Allocate space for the cloud
310  resp.cloud.points.resize (req_pts);
311  const unsigned int num_channels = scan_hist_[start_index].channels.size ();
312  resp.cloud.channels.resize (num_channels) ;
313  for (i = 0; i<num_channels; i++)
314  {
315  resp.cloud.channels[i].name = scan_hist_[start_index].channels[i].name ;
316  resp.cloud.channels[i].values.resize (req_pts) ;
317  }
318  //resp.cloud.header.stamp = req.end ;
319  resp.cloud.header.frame_id = fixed_frame_ ;
320  unsigned int cloud_count = 0 ;
321  for (i=start_index; i<past_end_index; i+=downsample_factor_)
322  {
323 
324  // Sanity check: Each channel should be the same length as the points vector
325  for (unsigned int chan_ind = 0; chan_ind < scan_hist_[i].channels.size(); chan_ind++)
326  {
327  if (scan_hist_[i].points.size () != scan_hist_[i].channels[chan_ind].values.size())
328  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 ());
329  }
330 
331  for(unsigned int j=0; j<scan_hist_[i].points.size (); j+=downsample_factor_)
332  {
333  resp.cloud.points[cloud_count].x = scan_hist_[i].points[j].x ;
334  resp.cloud.points[cloud_count].y = scan_hist_[i].points[j].y ;
335  resp.cloud.points[cloud_count].z = scan_hist_[i].points[j].z ;
336 
337  for (unsigned int k=0; k<num_channels; k++)
338  resp.cloud.channels[k].values[cloud_count] = scan_hist_[i].channels[k].values[j] ;
339 
340  cloud_count++ ;
341  }
342  resp.cloud.header.stamp = scan_hist_[i].header.stamp;
343  }
344  }
345  scan_hist_mutex_.unlock() ;
346 
347  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 ()) ;
348  return true ;
349 }
350 
351 template <class T>
352 bool BaseAssembler<T>::buildCloud2(AssembleScans2::Request& req, AssembleScans2::Response& resp)
353 {
354  ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead");
355  return assembleScans2(req, resp);
356 }
357 
358 template <class T>
359 bool BaseAssembler<T>::assembleScans2(AssembleScans2::Request& req, AssembleScans2::Response& resp)
360 {
361  AssembleScans::Request tmp_req;
362  AssembleScans::Response tmp_res;
363  tmp_req.begin = req.begin;
364  tmp_req.end = req.end;
365  bool ret = assembleScans(tmp_req, tmp_res);
366 
367  if ( ret )
368  {
369  sensor_msgs::convertPointCloudToPointCloud2(tmp_res.cloud, resp.cloud);
370  }
371  return ret;
372 }
373 }
unsigned int downsample_factor_
Specify how much to downsample the data. A value of 1 preserves all the data. 3 would keep 1/3 of the...
bool buildCloud(AssembleScans::Request &req, AssembleScans::Response &resp)
Service Callback function called whenever we need to build a cloud.
#define ROS_FATAL(...)
ros::ServiceServer build_cloud_server2_
std::vector< double > values
virtual unsigned int GetPointsInScan(const T &scan)=0
Returns the number of points in the current scan.
virtual void msgCallback(const boost::shared_ptr< const T > &scan_ptr)
Callback function for every time we receive a new scan.
bool assembleScans2(AssembleScans2::Request &req, AssembleScans2::Response &resp)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::deque< sensor_msgs::PointCloud > scan_hist_
Stores history of scans.
#define ROS_WARN(...)
virtual void ConvertToCloud(const std::string &fixed_frame_id, const T &scan_in, sensor_msgs::PointCloud &cloud_out)=0
Converts the current scan into a cloud in the specified fixed frame.
Maintains a history of point clouds and generates an aggregate point cloud upon request.
#define ROS_INFO(...)
ros::ServiceServer assemble_scans_server_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
unsigned int total_pts_
The number points currently in the scan history.
tf::MessageFilter< T > * tf_filter_
message_filters::Connection tf_filter_connection_
ros::ServiceServer assemble_scans_server2_
BaseAssembler(const std::string &max_size_param_name)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
static bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
message_filters::Subscriber< T > scan_sub_
bool buildCloud2(AssembleScans2::Request &req, AssembleScans2::Response &resp)
unsigned int max_scans_
The max number of scans to store in the scan history.
bool assembleScans(AssembleScans::Request &req, AssembleScans::Response &resp)
tf::TransformListener * tf_
ros::ServiceServer build_cloud_server_
#define ROS_ERROR(...)
std::string fixed_frame_
The frame to transform data into upon receipt.
Connection registerCallback(const C &callback)
#define ROS_DEBUG(...)


laser_assembler
Author(s): Vijay Pradeep
autogenerated on Thu Jun 27 2019 19:47:56