base_assembler_srv.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"
42 
43 #include <deque>
44 
45 // Service
46 #include "laser_assembler/AssembleScans.h"
47 
48 #include "boost/thread.hpp"
49 #include "math.h"
50 
51 namespace laser_assembler
52 {
53 
71 template<class T>
73 {
74 public:
77 
85  void start() ;
86 
87 
92  virtual unsigned int GetPointsInScan(const T& scan) = 0 ;
93 
102  virtual void ConvertToCloud(const std::string& fixed_frame_id, const T& scan_in, sensor_msgs::PointCloud& cloud_out) = 0 ;
103 
104 protected:
106 
109 
110 private:
111  // ROS Input/Ouptut Handling
116 
118  //void scansCallback(const tf::MessageNotifier<T>::MessagePtr& scan_ptr, const T& testA)
119  void scansCallback(const boost::shared_ptr<const T>& scan_ptr) ;
120 
122  bool buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) ;
123 
124 
126  std::deque<sensor_msgs::PointCloud> scan_hist_ ;
127  boost::mutex scan_hist_mutex_ ;
128 
130  unsigned int total_pts_ ;
131 
133  unsigned int max_scans_ ;
134 
136  std::string fixed_frame_ ;
137 
140 
142  unsigned int downsample_factor_ ;
143 
144 } ;
145 
146 template <class T>
148 {
149  // **** Initialize TransformListener ****
150  double tf_cache_time_secs ;
151  private_ns_.param("tf_cache_time_secs", tf_cache_time_secs, 10.0) ;
152  if (tf_cache_time_secs < 0)
153  ROS_ERROR("Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs) ;
154 
155  tf_ = new tf::TransformListener(n_, ros::Duration(tf_cache_time_secs));
156  ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs) ;
157 
158  // ***** Set max_scans *****
159  const int default_max_scans = 400 ;
160  int tmp_max_scans ;
161  private_ns_.param("max_scans", tmp_max_scans, default_max_scans);
162  if (tmp_max_scans < 0)
163  {
164  ROS_ERROR("Parameter max_scans<0 (%i)", tmp_max_scans) ;
165  tmp_max_scans = default_max_scans ;
166  }
167  max_scans_ = tmp_max_scans ;
168  ROS_INFO("Max Scans in History: %u", max_scans_) ;
169  total_pts_ = 0 ; // We're always going to start with no points in our history
170 
171  // ***** Set fixed_frame *****
172  private_ns_.param("fixed_frame", fixed_frame_, std::string("ERROR_NO_NAME"));
173  ROS_INFO("Fixed Frame: %s", fixed_frame_.c_str()) ;
174  if (fixed_frame_ == "ERROR_NO_NAME")
175  ROS_ERROR("Need to set parameter fixed_frame") ;
176 
177  // ***** Set downsample_factor *****
178  int tmp_downsample_factor ;
179  private_ns_.param("downsample_factor", tmp_downsample_factor, 1);
180  if (tmp_downsample_factor < 1)
181  {
182  ROS_ERROR("Parameter downsample_factor<1: %i", tmp_downsample_factor) ;
183  tmp_downsample_factor = 1 ;
184  }
185  downsample_factor_ = tmp_downsample_factor ;
186  ROS_INFO("Downsample Factor: %u", downsample_factor_) ;
187 
188  // ***** Start Services *****
190 
191  // **** Get the TF Notifier Tolerance ****
192  private_ns_.param("tf_tolerance_secs", tf_tolerance_secs_, 0.0);
193  if (tf_tolerance_secs_ < 0)
194  ROS_ERROR("Parameter tf_tolerance_secs<0 (%f)", tf_tolerance_secs_) ;
195  ROS_INFO("tf Tolerance: %f seconds", tf_tolerance_secs_) ;
196 
197  // ***** Start Listening to Data *****
198  // (Well, don't start listening just yet. Keep this as null until we actually start listening, when start() is called)
199  tf_filter_ = NULL;
200 
201 }
202 
203 template <class T>
205 {
206  ROS_INFO("Starting to listen on the input stream") ;
207  if (tf_filter_)
208  ROS_ERROR("assembler::start() was called twice!. This is bad, and could leak memory") ;
209  else
210  {
211  scan_sub_.subscribe(n_, "scan_in", 10);
215  }
216 }
217 
218 template <class T>
220 {
221  if (tf_filter_)
222  delete tf_filter_;
223 
224  delete tf_ ;
225 }
226 
227 template <class T>
229 {
230  const T scan = *scan_ptr ;
231 
232  sensor_msgs::PointCloud cur_cloud ;
233 
234  // Convert the scan data into a universally known datatype: PointCloud
235  try
236  {
237  ConvertToCloud(fixed_frame_, scan, cur_cloud) ; // Convert scan into a point cloud
238  }
239  catch(tf::TransformException& ex)
240  {
241  ROS_WARN("Transform Exception %s", ex.what()) ;
242  return ;
243  }
244 
245  // Add the current scan (now of type PointCloud) into our history of scans
246  scan_hist_mutex_.lock() ;
247  if (scan_hist_.size() == max_scans_) // Is our deque full?
248  {
249  total_pts_ -= scan_hist_.front().points.size() ; // We're removing an elem, so this reduces our total point count
250  scan_hist_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
251  }
252  scan_hist_.push_back(cur_cloud) ; // Add the newest scan to the back of the deque
253  total_pts_ += cur_cloud.points.size() ; // Add the new scan to the running total of points
254 
255  //printf("Scans: %4u Points: %10u\n", scan_hist_.size(), total_pts_) ;
256 
257  scan_hist_mutex_.unlock() ;
258 }
259 
260 template <class T>
261 bool BaseAssemblerSrv<T>::buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp)
262 {
263  //printf("Starting Service Request\n") ;
264 
265  scan_hist_mutex_.lock() ;
266  // Determine where in our history we actually are
267  unsigned int i = 0 ;
268 
269  // Find the beginning of the request. Probably should be a search
270  while ( i < scan_hist_.size() && // Don't go past end of deque
271  scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time
272  {
273  i++ ;
274  }
275  unsigned int start_index = i ;
276 
277  unsigned int req_pts = 0 ; // Keep a total of the points in the current request
278  // Find the end of the request
279  while ( i < scan_hist_.size() && // Don't go past end of deque
280  scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request
281  {
282  req_pts += (scan_hist_[i].points.size()+downsample_factor_-1)/downsample_factor_ ;
283  i += downsample_factor_ ;
284  }
285  unsigned int past_end_index = i ;
286 
287  if (start_index == past_end_index)
288  {
289  resp.cloud.header.frame_id = fixed_frame_ ;
290  resp.cloud.header.stamp = req.end ;
291  resp.cloud.points.resize(0) ;
292  resp.cloud.channels.resize(0) ;
293  }
294  else
295  {
296  // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen
297  // Allocate space for the cloud
298  resp.cloud.points.resize( req_pts ) ;
299  const unsigned int num_channels = scan_hist_[start_index].channels.size() ;
300  resp.cloud.channels.resize(num_channels) ;
301  for (i = 0; i<num_channels; i++)
302  {
303  resp.cloud.channels[i].name = scan_hist_[start_index].channels[i].name ;
304  resp.cloud.channels[i].values.resize(req_pts) ;
305  }
306  //resp.cloud.header.stamp = req.end ;
307  resp.cloud.header.frame_id = fixed_frame_ ;
308  unsigned int cloud_count = 0 ;
309  for (i=start_index; i<past_end_index; i+=downsample_factor_)
310  {
311  for(unsigned int j=0; j<scan_hist_[i].points.size(); j+=downsample_factor_)
312  {
313  resp.cloud.points[cloud_count].x = scan_hist_[i].points[j].x ;
314  resp.cloud.points[cloud_count].y = scan_hist_[i].points[j].y ;
315  resp.cloud.points[cloud_count].z = scan_hist_[i].points[j].z ;
316  for (unsigned int k=0; k<num_channels; k++)
317  resp.cloud.channels[k].values[cloud_count] = scan_hist_[i].channels[k].values[j] ;
318 
319  cloud_count++ ;
320  }
321  resp.cloud.header.stamp = scan_hist_[i].header.stamp;
322  }
323  }
324  scan_hist_mutex_.unlock() ;
325 
326  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()) ;
327  return true ;
328 }
329 
330 }
virtual unsigned int GetPointsInScan(const T &scan)=0
Returns the number of points in the current scan.
unsigned int total_pts_
The number points currently in the scan history.
void scansCallback(const boost::shared_ptr< const T > &scan_ptr)
Callback function for every time we receive a new scan.
Maintains a history of point clouds and generates an aggregate point cloud upon request.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
#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.
std::deque< sensor_msgs::PointCloud > scan_hist_
Stores history of scans.
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
unsigned int max_scans_
The max number of scans to store in the scan history.
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)
bool buildCloud(AssembleScans::Request &req, AssembleScans::Response &resp)
Service Callback function called whenever we need to build a cloud.
message_filters::Subscriber< T > scan_sub_
std::string fixed_frame_
The frame to transform data into upon receipt.
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...
double tf_tolerance_secs_
How long we should wait before processing the input data. Very useful for laser scans.
void setTolerance(const ros::Duration &tolerance)
#define ROS_ERROR(...)
message_filters::Connection tf_filter_connection_
tf::MessageFilter< T > * tf_filter_
void start()
Tells the assembler to start listening to input data It is possible that a derived class needs to ini...
Connection registerCallback(const C &callback)
#define ROS_DEBUG(...)


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