40 #include "sensor_msgs/PointCloud.h" 47 #include "laser_assembler/AssembleScans.h" 48 #include "laser_assembler/AssembleScans2.h" 50 #include "boost/thread.hpp" 73 void start(
const std::string& in_topic_name);
91 virtual void ConvertToCloud(
const std::string& fixed_frame_id,
const T& scan_in, sensor_msgs::PointCloud& cloud_out) = 0 ;
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) ;
141 double tf_cache_time_secs ;
143 if (tf_cache_time_secs < 0)
144 ROS_ERROR(
"Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs) ;
147 ROS_INFO(
"TF Cache Time: %f Seconds", tf_cache_time_secs) ;
150 const int default_max_scans = 400 ;
153 if (tmp_max_scans < 0)
155 ROS_ERROR(
"Parameter max_scans<0 (%i)", tmp_max_scans) ;
156 tmp_max_scans = default_max_scans ;
166 ROS_ERROR(
"Need to set parameter fixed_frame") ;
169 int tmp_downsample_factor ;
171 if (tmp_downsample_factor < 1)
173 ROS_ERROR(
"Parameter downsample_factor<1: %i", tmp_downsample_factor) ;
174 tmp_downsample_factor = 1 ;
195 ROS_DEBUG(
"Called start(string). Starting to listen on message_filter::Subscriber the input stream");
197 ROS_ERROR(
"assembler::start() was called twice!. This is bad, and could leak memory") ;
209 ROS_DEBUG(
"Called start(). Starting tf::MessageFilter, but not initializing Subscriber");
211 ROS_ERROR(
"assembler::start() was called twice!. This is bad, and could leak memory") ;
233 const T scan = *scan_ptr ;
235 sensor_msgs::PointCloud cur_cloud ;
244 ROS_WARN(
"Transform Exception %s", ex.what()) ;
267 ROS_WARN(
"Service 'build_cloud' is deprecated. Call 'assemble_scans' instead");
287 unsigned int start_index = i ;
289 unsigned int req_pts = 0 ;
297 unsigned int past_end_index = i ;
299 if (start_index == past_end_index)
302 resp.cloud.header.stamp = req.end ;
303 resp.cloud.points.resize (0) ;
304 resp.cloud.channels.resize (0) ;
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++)
315 resp.cloud.channels[i].name =
scan_hist_[start_index].channels[i].name ;
316 resp.cloud.channels[i].values.resize (req_pts) ;
320 unsigned int cloud_count = 0 ;
325 for (
unsigned int chan_ind = 0; chan_ind <
scan_hist_[i].channels.size(); chan_ind++)
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 ());
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 ;
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] ;
342 resp.cloud.header.stamp =
scan_hist_[i].header.stamp;
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 ()) ;
354 ROS_WARN(
"Service 'build_cloud' is deprecated. Call 'assemble_scans' instead");
361 AssembleScans::Request tmp_req;
362 AssembleScans::Response tmp_res;
363 tmp_req.begin = req.begin;
364 tmp_req.end = req.end;
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.
boost::mutex scan_hist_mutex_
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.
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.
ros::ServiceServer assemble_scans_server_
bool param(const std::string ¶m_name, T ¶m_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_
ros::NodeHandle private_ns_
std::string fixed_frame_
The frame to transform data into upon receipt.
Connection registerCallback(const C &callback)