From 6313176846ede4c53779f830df20ec8ee377e9e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Thu, 25 Jun 2015 19:31:55 +0200 Subject: [PATCH 1/2] added native sensor_msgs::PointCloud2 support without conversion and an organized mode --- CMakeLists.txt | 2 +- include/laser_assembler/base_assembler.h | 190 ++----- .../point_cloud2_base_assembler.h | 464 ++++++++++++++++++ .../point_cloud_base_assembler.h | 201 ++++++++ package.xml | 6 +- src/laser_scan_assembler.cpp | 10 +- src/point_cloud2_assembler.cpp | 114 +++-- src/point_cloud_assembler.cpp | 8 +- 8 files changed, 779 insertions(+), 216 deletions(-) create mode 100644 include/laser_assembler/point_cloud2_base_assembler.h create mode 100644 include/laser_assembler/point_cloud_base_assembler.h diff --git a/CMakeLists.txt b/CMakeLists.txt index ae36340..38ada4e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ project(laser_assembler) ############################################################################## set(THIS_PACKAGE_ROS_DEPS -tf sensor_msgs message_filters roscpp laser_geometry filters) +tf sensor_msgs message_filters roscpp laser_geometry filters tf2_ros) find_package(catkin REQUIRED COMPONENTS ${THIS_PACKAGE_ROS_DEPS} diff --git a/include/laser_assembler/base_assembler.h b/include/laser_assembler/base_assembler.h index 2481dd7..ec2b318 100644 --- a/include/laser_assembler/base_assembler.h +++ b/include/laser_assembler/base_assembler.h @@ -34,10 +34,12 @@ //! \author Vijay Pradeep +#ifndef LASER_ASSEMBLER_BASE_ASSEMBLER_H +#define LASER_ASSEMBLER_BASE_ASSEMBLER_H + #include "ros/ros.h" #include "tf/transform_listener.h" #include "tf/message_filter.h" -#include "sensor_msgs/PointCloud.h" #include "sensor_msgs/point_cloud_conversion.h" #include "message_filters/subscriber.h" @@ -56,7 +58,7 @@ namespace laser_assembler /** * \brief Maintains a history of point clouds and generates an aggregate point cloud upon request */ -template +template class BaseAssembler { public: @@ -88,7 +90,7 @@ class BaseAssembler * \param scan_in The scan that we want to convert * \param cloud_out The result of transforming scan_in into a cloud in frame fixed_frame_id */ - virtual void ConvertToCloud(const std::string& fixed_frame_id, const T& scan_in, sensor_msgs::PointCloud& cloud_out) = 0 ; + virtual void Convert(const std::string& fixed_frame_id, const T& scan_in, V& cloud_out) = 0 ; protected: tf::TransformListener* tf_ ; @@ -97,12 +99,18 @@ class BaseAssembler ros::NodeHandle private_ns_; ros::NodeHandle n_; + //! \brief Stores history of scans + std::deque scan_hist_ ; + boost::mutex scan_hist_mutex_ ; + + //! \brief The frame to transform data into upon receipt + std::string fixed_frame_ ; + + //! \brief Specify how much to downsample the data. A value of 1 preserves all the data. 3 would keep 1/3 of the data. + unsigned int downsample_factor_ ; + private: // ROS Input/Ouptut Handling - ros::ServiceServer build_cloud_server_; - ros::ServiceServer assemble_scans_server_; - ros::ServiceServer build_cloud_server2_; - ros::ServiceServer assemble_scans_server2_; message_filters::Subscriber scan_sub_; message_filters::Connection tf_filter_connection_; @@ -110,32 +118,13 @@ class BaseAssembler //void scansCallback(const tf::MessageNotifier::MessagePtr& scan_ptr, const T& testA) virtual void msgCallback(const boost::shared_ptr& scan_ptr) ; - //! \brief Service Callback function called whenever we need to build a cloud - bool buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) ; - bool assembleScans(AssembleScans::Request& req, AssembleScans::Response& resp) ; - bool buildCloud2(AssembleScans2::Request& req, AssembleScans2::Response& resp) ; - bool assembleScans2(AssembleScans2::Request& req, AssembleScans2::Response& resp) ; - - //! \brief Stores history of scans - std::deque scan_hist_ ; - boost::mutex scan_hist_mutex_ ; - - //! \brief The number points currently in the scan history - unsigned int total_pts_ ; - //! \brief The max number of scans to store in the scan history unsigned int max_scans_ ; - //! \brief The frame to transform data into upon receipt - std::string fixed_frame_ ; - - //! \brief Specify how much to downsample the data. A value of 1 preserves all the data. 3 would keep 1/3 of the data. - unsigned int downsample_factor_ ; - } ; -template -BaseAssembler::BaseAssembler(const std::string& max_size_param_name) : private_ns_("~") +template +BaseAssembler::BaseAssembler(const std::string& max_size_param_name) : private_ns_("~") { // **** Initialize TransformListener **** double tf_cache_time_secs ; @@ -157,7 +146,6 @@ BaseAssembler::BaseAssembler(const std::string& max_size_param_name) : privat } max_scans_ = tmp_max_scans ; ROS_INFO("Max Scans in History: %u", max_scans_) ; - total_pts_ = 0 ; // We're always going to start with no points in our history // ***** Set fixed_frame ***** private_ns_.param("fixed_frame", fixed_frame_, std::string("ERROR_NO_NAME")); @@ -176,21 +164,14 @@ BaseAssembler::BaseAssembler(const std::string& max_size_param_name) : privat downsample_factor_ = tmp_downsample_factor ; if (downsample_factor_ != 1) ROS_WARN("Downsample set to [%u]. Note that this is an unreleased/unstable feature", downsample_factor_); - - // ***** Start Services ***** - build_cloud_server_ = n_.advertiseService("build_cloud", &BaseAssembler::buildCloud, this); - assemble_scans_server_ = n_.advertiseService("assemble_scans", &BaseAssembler::assembleScans, this); - build_cloud_server2_ = n_.advertiseService("build_cloud2", &BaseAssembler::buildCloud2, this); - assemble_scans_server2_ = n_.advertiseService("assemble_scans2", &BaseAssembler::assembleScans2, this); - // ***** Start Listening to Data ***** // (Well, don't start listening just yet. Keep this as null until we actually start listening, when start() is called) tf_filter_ = NULL; } -template -void BaseAssembler::start(const std::string& in_topic_name) +template +void BaseAssembler::start(const std::string& in_topic_name) { ROS_DEBUG("Called start(string). Starting to listen on message_filter::Subscriber the input stream"); if (tf_filter_) @@ -199,12 +180,12 @@ void BaseAssembler::start(const std::string& in_topic_name) { scan_sub_.subscribe(n_, in_topic_name, 10); tf_filter_ = new tf::MessageFilter(scan_sub_, *tf_, fixed_frame_, 10); - tf_filter_->registerCallback( boost::bind(&BaseAssembler::msgCallback, this, _1) ); + tf_filter_->registerCallback( boost::bind(&BaseAssembler::msgCallback, this, _1) ); } } -template -void BaseAssembler::start() +template +void BaseAssembler::start() { ROS_DEBUG("Called start(). Starting tf::MessageFilter, but not initializing Subscriber"); if (tf_filter_) @@ -213,12 +194,12 @@ void BaseAssembler::start() { scan_sub_.subscribe(n_, "bogus", 10); tf_filter_ = new tf::MessageFilter(scan_sub_, *tf_, fixed_frame_, 10); - tf_filter_->registerCallback( boost::bind(&BaseAssembler::msgCallback, this, _1) ); + tf_filter_->registerCallback( boost::bind(&BaseAssembler::msgCallback, this, _1) ); } } -template -BaseAssembler::~BaseAssembler() +template +BaseAssembler::~BaseAssembler() { if (tf_filter_) delete tf_filter_; @@ -226,18 +207,18 @@ BaseAssembler::~BaseAssembler() delete tf_ ; } -template -void BaseAssembler::msgCallback(const boost::shared_ptr& scan_ptr) +template +void BaseAssembler::msgCallback(const boost::shared_ptr& scan_ptr) { ROS_DEBUG("starting msgCallback"); const T scan = *scan_ptr ; - sensor_msgs::PointCloud cur_cloud ; + V cur_cloud ; // Convert the scan data into a universally known datatype: PointCloud try { - ConvertToCloud(fixed_frame_, scan, cur_cloud) ; // Convert scan into a point cloud + Convert(fixed_frame_, scan, cur_cloud) ; // Convert scan into V } catch(tf::TransformException& ex) { @@ -249,125 +230,14 @@ void BaseAssembler::msgCallback(const boost::shared_ptr& scan_ptr) scan_hist_mutex_.lock() ; if (scan_hist_.size() == max_scans_) // Is our deque full? { - total_pts_ -= scan_hist_.front().points.size () ; // We're removing an elem, so this reduces our total point count scan_hist_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it } scan_hist_.push_back(cur_cloud) ; // Add the newest scan to the back of the deque - total_pts_ += cur_cloud.points.size () ; // Add the new scan to the running total of points - - //printf("Scans: %4u Points: %10u\n", scan_hist_.size(), total_pts_) ; scan_hist_mutex_.unlock() ; ROS_DEBUG("done with msgCallback"); } -template -bool BaseAssembler::buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) -{ - ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); - return assembleScans(req, resp); -} - - -template -bool BaseAssembler::assembleScans(AssembleScans::Request& req, AssembleScans::Response& resp) -{ - //printf("Starting Service Request\n") ; - - scan_hist_mutex_.lock() ; - // Determine where in our history we actually are - unsigned int i = 0 ; - - // Find the beginning of the request. Probably should be a search - while ( i < scan_hist_.size() && // Don't go past end of deque - scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time - { - i++ ; - } - unsigned int start_index = i ; - - unsigned int req_pts = 0 ; // Keep a total of the points in the current request - // Find the end of the request - while ( i < scan_hist_.size() && // Don't go past end of deque - scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request - { - req_pts += (scan_hist_[i].points.size ()+downsample_factor_-1)/downsample_factor_ ; - i += downsample_factor_ ; - } - unsigned int past_end_index = i ; - - if (start_index == past_end_index) - { - resp.cloud.header.frame_id = fixed_frame_ ; - resp.cloud.header.stamp = req.end ; - resp.cloud.points.resize (0) ; - resp.cloud.channels.resize (0) ; - } - else - { - // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen - // Allocate space for the cloud - resp.cloud.points.resize (req_pts); - const unsigned int num_channels = scan_hist_[start_index].channels.size (); - resp.cloud.channels.resize (num_channels) ; - for (i = 0; i%u. BufferSize: %lu. Points in cloud: %u", start_index, past_end_index, scan_hist_.size(), (int)resp.cloud.points.size ()) ; - return true ; -} - -template -bool BaseAssembler::buildCloud2(AssembleScans2::Request& req, AssembleScans2::Response& resp) -{ - ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); - return assembleScans2(req, resp); } -template -bool BaseAssembler::assembleScans2(AssembleScans2::Request& req, AssembleScans2::Response& resp) -{ - AssembleScans::Request tmp_req; - AssembleScans::Response tmp_res; - tmp_req.begin = req.begin; - tmp_req.end = req.end; - bool ret = assembleScans(tmp_req, tmp_res); - - if ( ret ) - { - sensor_msgs::convertPointCloudToPointCloud2(tmp_res.cloud, resp.cloud); - } - return ret; -} -} +#endif /* base_assembler.h */ diff --git a/include/laser_assembler/point_cloud2_base_assembler.h b/include/laser_assembler/point_cloud2_base_assembler.h new file mode 100644 index 0000000..a2f60ba --- /dev/null +++ b/include/laser_assembler/point_cloud2_base_assembler.h @@ -0,0 +1,464 @@ +/* + * Software License Agreement (BSD License) + * + * Robot Operating System code by the University of Osnabrück + * Copyright (c) 2015, University of Osnabrück + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above + * copyright notice, this list of conditions and the following + * disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED + * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * + * point_cloud2_base_assembler.h + * + * Created on: 01.07.2015 + * Authors: Sebastian Pütz + */ + +#ifndef LASER_ASSEMBLER_POINT_CLOUD2_BASE_ASSEMBLER_H +#define LASER_ASSEMBLER_POINT_CLOUD2_BASE_ASSEMBLER_H + +//! \author Sebastian Puetz (spuetz@uni-osnabrueck.de) + +#include "base_assembler.h" +#include "sensor_msgs/PointCloud2.h" +#include "sensor_msgs/point_field_conversion.h" + +// Service +#include "laser_assembler/AssembleScans2.h" + +namespace laser_assembler +{ + + /** + * \brief Maintains a history of point clouds and generates an aggregate point cloud upon request + */ + template + class PointCloud2BaseAssembler : public BaseAssembler + { + public: + PointCloud2BaseAssembler(const std::string& max_size_param_name); + ~PointCloud2BaseAssembler() ; + + /** \brief Returns the number of points in the current scan + * \param scan The scan for for which we want to know the number of points + * \return the number of points in scan + */ + virtual unsigned int GetPointsInScan(const T& scan) = 0 ; + + /** \brief Converts the current scan into a cloud in the specified fixed frame + * + * Note: Once implemented, ConvertToCloud should NOT catch TF exceptions. These exceptions are caught by + * BaseAssembler, and will be counted for diagnostic information + * \param fixed_frame_id The name of the frame in which we want cloud_out to be in + * \param scan_in The scan that we want to convert + * \param cloud_out The result of transforming scan_in into a cloud in frame fixed_frame_id + */ + virtual void Convert(const std::string& fixed_frame_id, const T& scan_in, sensor_msgs::PointCloud2& cloud_out) = 0 ; + + private: + // ROS Input/Ouptut Handling + ros::ServiceServer build_cloud_server2_; + ros::ServiceServer assemble_scans_server2_; + + //! \brief Service Callback function called whenever we need to build a cloud + bool buildCloud2(AssembleScans2::Request& req, AssembleScans2::Response& resp) ; + bool assembleScans2(AssembleScans2::Request& req, AssembleScans2::Response& resp) ; + bool assembleScans2Organized(AssembleScans2::Request& req, AssembleScans2::Response& resp) ; + bool assembleScans2Unorganized(AssembleScans2::Request& req, AssembleScans2::Response& resp) ; + + + sensor_msgs::PointField* getPointFieldByName(std::vector& fields, const std::string& name); + + /** \brief Fills-up index holes in an organized point cloud + * + * \param data_count The current index of the bytebuffer of the point cloud 2, the value will be modified + * \param pc The output PointCloud2 object + * \param index the current scan ray index which will be insertet, the value will be modified + * \param previous_index the previous read scan ray index + * \param point_step The step of one combined field in bytes in the buffer + * \param fields A vector of all fields + * \param idx_offset The byte offset of one scan ray index + * \param idx_datatype The type of one scan ray index + */ + inline void fillUpIndexHoles(unsigned int& data_count, + sensor_msgs::PointCloud2 &pc, + int& previous_index, + int index, + const unsigned int point_step, + const std::vector& fields, + const unsigned int idx_offset, + const unsigned int idx_datatype); + + /** \brief checks the equality of two scans headers + * + * \param first_scan The first scan to assemble + * \param current_scan The current scan to assemble + * \return true, if the two scans are equal + */ + bool hasEqualScanHeader(sensor_msgs::PointCloud2 &first_scan, sensor_msgs::PointCloud2 ¤t_scan); + + //! \enables the organized mode. + bool organized; + } ; + + template + PointCloud2BaseAssembler::PointCloud2BaseAssembler(const std::string& max_size_param_name) + : BaseAssembler(max_size_param_name) + { + this->private_ns_.param("organized", organized, false); + + // ***** Start Services ***** + std::string build_cloud2_service_name = ros::names::resolve("build_cloud2"); + std::string assemble_scans2_service_name = ros::names::resolve("assemble_scans2"); + build_cloud_server2_ = BaseAssembler::n_.advertiseService("build_cloud2", &PointCloud2BaseAssembler::buildCloud2, this); + assemble_scans_server2_ = BaseAssembler::n_.advertiseService("assemble_scans2", &PointCloud2BaseAssembler::assembleScans2, this); + } + + template + PointCloud2BaseAssembler::~PointCloud2BaseAssembler() + { + } + + template + bool PointCloud2BaseAssembler::hasEqualScanHeader(sensor_msgs::PointCloud2 &first_scan, sensor_msgs::PointCloud2 ¤t_scan){ + bool malformed = false; + for (unsigned int chan_ind = 0; chan_ind < first_scan.fields.size(); chan_ind++){ + std::string field_name = first_scan.fields[chan_ind].name; + sensor_msgs::PointField* field = getPointFieldByName(first_scan.fields, field_name); + + if (!field){ + ROS_FATAL("field for name \"%s\", existing in the first scan, not found in the other scan!", field_name.c_str()); + return false; + } + if (field->offset != first_scan.fields[chan_ind].offset){ + ROS_FATAL("Field \"%s\" has not the same offset as the first scan!", field_name.c_str()); + malformed = true; + } + if (field->datatype != first_scan.fields[chan_ind].datatype){ + ROS_FATAL("Field \"%s\" has not the same datatype as the first scan!", field_name.c_str()); + malformed = true; + } + if (field->count != first_scan.fields[chan_ind].count){ + ROS_FATAL("Field \"%s\" has not the same number of elements as the first scan!", field_name.c_str()); + malformed = true; + } + } + return !malformed; + } + + template + bool PointCloud2BaseAssembler::buildCloud2(AssembleScans2::Request& req, AssembleScans2::Response& resp) + { + ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); + return assembleScans2(req, resp); + } + + template + sensor_msgs::PointField* PointCloud2BaseAssembler::getPointFieldByName(std::vector& fields, const std::string& name){ + for(unsigned int i=0; i + void PointCloud2BaseAssembler::fillUpIndexHoles(unsigned int& data_count, sensor_msgs::PointCloud2 &pc, int& previous_index, int index, const unsigned int point_step, const std::vector& fields, const unsigned int idx_offset, const unsigned int idx_type) + { + + // fill up for not existing indices + while(previous_index + this->downsample_factor_ < index){ + for (unsigned int chan_ind = 0; chan_ind < fields.size(); chan_ind++) + { + const sensor_msgs::PointField field = fields[chan_ind]; + switch(field.datatype){ + case sensor_msgs::PointField::FLOAT32 : + // write NaN values for float datatypes + sensor_msgs::writePointCloud2BufferValue( + &(pc.data[data_count+field.offset]), nanf("")) ; + break ; + case sensor_msgs::PointField::FLOAT64 : + // write NaN values for double datatypes + sensor_msgs::writePointCloud2BufferValue( + &(pc.data[data_count+field.offset]), nan("")) ; + break ; + default: + // write zero values for other datatypes + sensor_msgs::writePointCloud2BufferValue( + &(pc.data[data_count+field.offset]), field.datatype, 0) ; + break; + } + } + previous_index += this->downsample_factor_ ; + // write index value + sensor_msgs::writePointCloud2BufferValue( + &(pc.data[data_count+idx_offset]), idx_type, previous_index); + data_count+=point_step; + } + } + + template + bool PointCloud2BaseAssembler::assembleScans2Organized(AssembleScans2::Request& req, AssembleScans2::Response& resp) + { + this->scan_hist_mutex_.lock(); + // Determine where in our history we actually are + unsigned int i = 0 ; + + // Find the beginning of the request. Probably should be a search + while ( i < BaseAssembler::scan_hist_.size() && // Don't go past end of deque + BaseAssembler::scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time + { + i++ ; + } + unsigned int start_index = i ; + sensor_msgs::PointCloud2& first_scan = this->scan_hist_[start_index]; + const unsigned int point_step = first_scan.point_step; + const unsigned int num_fields = first_scan.fields.size(); + + // Find the end of the request + + int max_index = -1 ; + int min_index = INT_MAX ; + int count_scans = 0; + const std::string index_field_name("index"); + + while ( i < this->scan_hist_.size() && // Don't go past end of deque + this->scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request + { + sensor_msgs::PointCloud2& current_scan = this->scan_hist_[i]; + unsigned int pts_count = this->GetPointsInScan(current_scan) ; + if(pts_count>0) + { + unsigned int pts_step = current_scan.point_step ; + sensor_msgs::PointField* index_field = getPointFieldByName(current_scan.fields, index_field_name) ; + if(!index_field){ + ROS_ERROR("No index information found in cloud! The point field \"index\" is needed to build an organized point cloud 2"); + return false; + } + unsigned char* first_index_entry = &(current_scan.data[0+index_field->offset]) ; + unsigned char* last_index_entry = &(current_scan.data[(pts_count-1)*pts_step+index_field->offset]) ; + int first_index_value = sensor_msgs::readPointCloud2BufferValue(first_index_entry, index_field->datatype) ; + int last_index_value = sensor_msgs::readPointCloud2BufferValue(last_index_entry, index_field->datatype) ; + if(max_indexfirst_index_value) + min_index = first_index_value ; + i += this->downsample_factor_ ; + count_scans++; + } + } + + int index_range = max_index - min_index + 1; + int index_range_downsampled = (index_range+this->downsample_factor_-1)/this->downsample_factor_; + unsigned int past_end_index = i ; + + + if (start_index == past_end_index) + { + resp.cloud.header.frame_id = this->fixed_frame_ ; + resp.cloud.header.stamp = req.end ; + resp.cloud.data.resize (0) ; + resp.cloud.fields.resize (0) ; + resp.cloud.height = 0; + resp.cloud.width = 0; + } + else + { + // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen + // Allocate space for the cloud + resp.cloud.data.resize (count_scans*index_range_downsampled*point_step); + resp.cloud.fields.resize (num_fields) ; + resp.cloud.point_step = point_step ; + resp.cloud.is_bigendian = first_scan.is_bigendian; + + for (i = 0; ifixed_frame_ ; + + unsigned int cloud_count = 0 ; + unsigned int data_count = 0; + bool is_dense = true; + + for (i=start_index; idownsample_factor_) + { + sensor_msgs::PointCloud2& current_scan = this->scan_hist_[i]; + + is_dense &= current_scan.is_dense ; + + if(!hasEqualScanHeader(first_scan, current_scan)){ + return false; + } + + sensor_msgs::PointField* index_field = getPointFieldByName(current_scan.fields, index_field_name) ; + std::vector fields = current_scan.fields; + int previous_index = min_index-this->downsample_factor_; + + for(unsigned int j=0; j< this->GetPointsInScan(current_scan) * point_step; j+= point_step) + { + // read the next index in the cloud + int index = sensor_msgs::readPointCloud2BufferValue(&(current_scan.data[j+index_field->offset]), index_field->datatype); + // if the index is smaller than the step size (downsample_factor_), skip it + if(index < previous_index + this->downsample_factor_){ + continue; + } + fillUpIndexHoles(data_count, resp.cloud, previous_index, index, point_step, fields, index_field->offset, index_field->datatype); + if(previous_index + this->downsample_factor_ == index){ + for(int p_idx=0; p_idx < point_step; p_idx++){ + resp.cloud.data[data_count++] = current_scan.data[j+p_idx] ; + } + previous_index = index; + } + + } + int max_index_ext = max_index + 1; + fillUpIndexHoles(data_count, resp.cloud, previous_index, max_index_ext, point_step, fields, index_field->offset, index_field->datatype); + resp.cloud.header.stamp = current_scan.header.stamp ; + cloud_count ++ ; + } + + resp.cloud.height = cloud_count ; + resp.cloud.width = index_range_downsampled ; + resp.cloud.row_step = index_range_downsampled * point_step ; + resp.cloud.is_dense = is_dense ; + } + this->scan_hist_mutex_.unlock() ; + + ROS_DEBUG("Point Cloud Results: Aggregated from index %u->%u. BufferSize: %lu. Points in cloud: %u", start_index, past_end_index, this->scan_hist_.size(), resp.cloud.height*resp.cloud.width) ; + return true ; + } + + template + bool PointCloud2BaseAssembler::assembleScans2Unorganized(AssembleScans2::Request& req, AssembleScans2::Response& resp) + { + this->scan_hist_mutex_.lock(); + // Determine where in our history we actually are + unsigned int i = 0 ; + + // Find the beginning of the request. Probably should be a search + while ( i < BaseAssembler::scan_hist_.size() && // Don't go past end of deque + BaseAssembler::scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time + { + i++ ; + } + unsigned int start_index = i ; + sensor_msgs::PointCloud2& first_scan = this->scan_hist_[start_index]; + + unsigned int req_pts = 0 ; // Keep a total of the points in the current request + // Find the end of the request + + while ( i < this->scan_hist_.size() && // Don't go past end of deque + this->scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request + { + sensor_msgs::PointCloud2& current_scan = this->scan_hist_[i]; + unsigned int pts_count = this->GetPointsInScan(current_scan) ; + unsigned int pts_downsampled = (pts_count+this->downsample_factor_-1)/this->downsample_factor_ ; + req_pts += pts_downsampled; + i += this->downsample_factor_ ; + } + unsigned int past_end_index = i ; + + if (start_index == past_end_index) + { + resp.cloud.header.frame_id = this->fixed_frame_ ; + resp.cloud.header.stamp = req.end ; + resp.cloud.data.resize (0) ; + resp.cloud.fields.resize (0) ; + resp.cloud.height = 0; + resp.cloud.width = 0; + } + else + { + // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen + // Allocate space for the cloud + const unsigned int point_step = first_scan.point_step; + const unsigned int num_fields = first_scan.fields.size(); + resp.cloud.data.resize (req_pts * point_step); + resp.cloud.fields.resize (num_fields) ; + resp.cloud.point_step = point_step ; + resp.cloud.is_bigendian = first_scan.is_bigendian; + resp.cloud.header.frame_id = this->fixed_frame_ ; + resp.cloud.is_dense = true; + resp.cloud.height = 1 ; + resp.cloud.width = req_pts ; + resp.cloud.row_step = req_pts * point_step ; + + for (i = 0; idownsample_factor_) + { + sensor_msgs::PointCloud2& current_scan = this->scan_hist_[i]; + if(!hasEqualScanHeader(first_scan, current_scan)){ + return false; + } + for(unsigned int j=0; j< this->GetPointsInScan(current_scan) * point_step; j+= (this->downsample_factor_ * point_step)) + { + for(unsigned int p_idx=0; p_idx < point_step; p_idx++){ + resp.cloud.data[data_count++] = current_scan.data[j+p_idx] ; + } + } + resp.cloud.header.stamp = current_scan.header.stamp ; + resp.cloud.is_dense &= current_scan.is_dense ; + } + } + this->scan_hist_mutex_.unlock() ; + + ROS_DEBUG("Point Cloud Results: Aggregated from index %u->%u. BufferSize: %lu. Points in cloud: %u", start_index, past_end_index, this->scan_hist_.size(), req_pts) ; + return true ; + } + + template + bool PointCloud2BaseAssembler::assembleScans2(AssembleScans2::Request& req, AssembleScans2::Response& resp) + { + if(organized){ + return assembleScans2Organized(req, resp); + }else{ + return assembleScans2Unorganized(req, resp); + } + } +} + +#endif /* point_cloud2_base_assembler.h */ diff --git a/include/laser_assembler/point_cloud_base_assembler.h b/include/laser_assembler/point_cloud_base_assembler.h new file mode 100644 index 0000000..2055103 --- /dev/null +++ b/include/laser_assembler/point_cloud_base_assembler.h @@ -0,0 +1,201 @@ +/* + * Software License Agreement (BSD License) + * + * Robot Operating System code by the University of Osnabrück + * Copyright (c) 2015, University of Osnabrück + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above + * copyright notice, this list of conditions and the following + * disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED + * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * + * point_cloud_base_assembler.h + * + * Created on: 01.07.2015 + * Authors: Sebastian Pütz + */ + +#ifndef LASER_ASSEMBLER_POINT_CLOUD_BASE_ASSEMBLER_H +#define LASER_ASSEMBLER_POINT_CLOUD_BASE_ASSEMBLER_H + +//! \author Sebastian Puetz + +#include "base_assembler.h" +#include "sensor_msgs/PointCloud.h" + +// Service +#include "laser_assembler/AssembleScans.h" + +namespace laser_assembler +{ + +/** + * \brief Maintains a history of point clouds and generates an aggregate point cloud upon request + */ +template +class PointCloudBaseAssembler : public BaseAssembler +{ +public: + PointCloudBaseAssembler(const std::string& max_size_param_name); + ~PointCloudBaseAssembler() ; + + /** \brief Returns the number of points in the current scan + * \param scan The scan for for which we want to know the number of points + * \return the number of points in scan + */ + virtual unsigned int GetPointsInScan(const T& scan) = 0 ; + + /** \brief Converts the current scan into a cloud in the specified fixed frame + * + * Note: Once implemented, ConvertToCloud should NOT catch TF exceptions. These exceptions are caught by + * BaseAssembler, and will be counted for diagnostic information + * \param fixed_frame_id The name of the frame in which we want cloud_out to be in + * \param scan_in The scan that we want to convert + * \param cloud_out The result of transforming scan_in into a cloud in frame fixed_frame_id + */ + virtual void Convert(const std::string& fixed_frame_id, const T& scan_in, sensor_msgs::PointCloud& cloud_out) = 0 ; + +private: + // ROS Input/Ouptut Handling + ros::ServiceServer build_cloud_server_; + ros::ServiceServer assemble_scans_server_; + + //! \brief Service Callback function called whenever we need to build a cloud + bool buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) ; + bool assembleScans(AssembleScans::Request& req, AssembleScans::Response& resp) ; +} ; + +template +PointCloudBaseAssembler::PointCloudBaseAssembler(const std::string& max_size_param_name) + : BaseAssembler(max_size_param_name) +{ + // ***** Start Services ***** + build_cloud_server_ = PointCloudBaseAssembler::n_.advertiseService("build_cloud", &PointCloudBaseAssembler::buildCloud, this); + assemble_scans_server_ = PointCloudBaseAssembler::n_.advertiseService("assemble_scans", &PointCloudBaseAssembler::assembleScans, this); +} + +template +PointCloudBaseAssembler::~PointCloudBaseAssembler() +{ + +} + +template +bool PointCloudBaseAssembler::buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) +{ + ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); + return assembleScans(req, resp); +} + + +template +bool PointCloudBaseAssembler::assembleScans(AssembleScans::Request& req, AssembleScans::Response& resp) +{ + this->scan_hist_mutex_.lock() ; + // Determine where in our history we actually are + unsigned int i = 0 ; + + // Find the beginning of the request. Probably should be a search + while ( i < BaseAssembler::scan_hist_.size() && // Don't go past end of deque + BaseAssembler::scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time + { + i++ ; + } + unsigned int start_index = i ; + + unsigned int req_pts = 0 ; // Keep a total of the points in the current request + // Find the end of the request + while ( i < BaseAssembler::scan_hist_.size() && // Don't go past end of deque + BaseAssembler::scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request + { + req_pts += (BaseAssembler::scan_hist_[i].points.size ()+BaseAssembler::downsample_factor_-1)/ BaseAssembler::downsample_factor_ ; + i += BaseAssembler::downsample_factor_ ; + } + unsigned int past_end_index = i ; + + if (start_index == past_end_index) + { + resp.cloud.header.frame_id = this->fixed_frame_ ; + resp.cloud.header.stamp = req.end ; + resp.cloud.points.resize (0) ; + resp.cloud.channels.resize (0) ; + } + else + { + // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen + // Allocate space for the cloud + resp.cloud.points.resize (req_pts); + const unsigned int num_channels = this->scan_hist_[start_index].channels.size (); + resp.cloud.channels.resize (num_channels) ; + for (i = 0; iscan_hist_[start_index].channels[i].name ; + resp.cloud.channels[i].values.resize (req_pts) ; + } + //resp.cloud.header.stamp = req.end ; + resp.cloud.header.frame_id = this->fixed_frame_ ; + unsigned int cloud_count = 0 ; + for (i=start_index; i::downsample_factor_) + { + + // Sanity check: Each channel should be the same length as the points vector + for (unsigned int chan_ind = 0; chan_ind < BaseAssembler::scan_hist_[i].channels.size(); chan_ind++) + { + if (this->scan_hist_[i].points.size () != this->scan_hist_[i].channels[chan_ind].values.size()) + ROS_FATAL("Trying to add a malformed point cloud. Cloud has %u points, but channel %u has %u elems", + (int)this->scan_hist_[i].points.size (), chan_ind, (int)this->scan_hist_[i].channels[chan_ind].values.size ()); + } + + for(unsigned int j=0; jscan_hist_[i].points.size (); j+=this->downsample_factor_) + { + resp.cloud.points[cloud_count].x = this->scan_hist_[i].points[j].x ; + resp.cloud.points[cloud_count].y = this->scan_hist_[i].points[j].y ; + resp.cloud.points[cloud_count].z = this->scan_hist_[i].points[j].z ; + + for (unsigned int k=0; kscan_hist_[i].channels[k].values[j] ; + + cloud_count++ ; + } + resp.cloud.header.stamp = this->scan_hist_[i].header.stamp; + } + } + this->scan_hist_mutex_.unlock() ; + + ROS_DEBUG("Point Cloud Results: Aggregated from index %u->%u. BufferSize: %lu. Points in cloud: %u", start_index, past_end_index, this->scan_hist_.size(), (int)resp.cloud.points.size ()) ; + return true ; +} + +} + +#endif /* point_cloud_base_assembler */ diff --git a/package.xml b/package.xml index 775d151..91b81e7 100644 --- a/package.xml +++ b/package.xml @@ -24,7 +24,9 @@ filters laser_geometry pluginlib - + tf2_ros + tf2_sensor_msgs + message_runtime sensor_msgs message_filters @@ -33,6 +35,8 @@ filters laser_geometry pluginlib + tf2_ros + tf2_sensor_msgs diff --git a/src/laser_scan_assembler.cpp b/src/laser_scan_assembler.cpp index 6d76e78..8656857 100644 --- a/src/laser_scan_assembler.cpp +++ b/src/laser_scan_assembler.cpp @@ -35,7 +35,7 @@ #include "laser_geometry/laser_geometry.h" #include "sensor_msgs/LaserScan.h" -#include "laser_assembler/base_assembler.h" +#include "laser_assembler/point_cloud_base_assembler.h" #include "filters/filter_chain.h" using namespace laser_geometry; @@ -47,10 +47,10 @@ namespace laser_assembler /** * \brief Maintains a history of laser scans and generates a point cloud upon request */ -class LaserScanAssembler : public BaseAssembler +class LaserScanAssembler : public PointCloudBaseAssembler { public: - LaserScanAssembler() : BaseAssembler("max_scans"), filter_chain_("sensor_msgs::LaserScan") + LaserScanAssembler() : PointCloudBaseAssembler("max_scans"), filter_chain_("sensor_msgs::LaserScan") { // ***** Set Laser Projection Method ***** private_ns_.param("ignore_laser_skew", ignore_laser_skew_, true); @@ -78,7 +78,7 @@ class LaserScanAssembler : public BaseAssembler return (scan.ranges.size ()); } - void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& cloud_out) + void Convert(const string& fixed_frame_id, const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& cloud_out) { // apply filters on laser scan filter_chain_.update (scan_in, scan_filtered_); @@ -88,7 +88,7 @@ class LaserScanAssembler : public BaseAssembler { projector_.projectLaser(scan_filtered_, cloud_out); if (cloud_out.header.frame_id != fixed_frame_id) - tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out); + this->tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out); } else // Do it the slower (more accurate) way { diff --git a/src/point_cloud2_assembler.cpp b/src/point_cloud2_assembler.cpp index 7f9f166..44b731c 100644 --- a/src/point_cloud2_assembler.cpp +++ b/src/point_cloud2_assembler.cpp @@ -1,40 +1,53 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - - -#include "laser_assembler/base_assembler.h" -#include +/* + * Software License Agreement (BSD License) + * + * Robot Operating System code by the University of Osnabrück + * Copyright (c) 2015, University of Osnabrück + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above + * copyright notice, this list of conditions and the following + * disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED + * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * + * point_cloud2_assembler.cpp + * + * Created on: 01.07.2015 + * Authors: Sebastian Pütz + */ + + +#include "laser_assembler/point_cloud2_base_assembler.h" +#include +#include + using namespace std ; @@ -47,12 +60,11 @@ namespace laser_assembler * params * * (Several params are inherited from BaseAssemblerSrv) */ -class PointCloud2Assembler : public BaseAssembler +class PointCloud2Assembler : public PointCloud2BaseAssembler { public: - PointCloud2Assembler() : BaseAssembler("max_clouds") + PointCloud2Assembler() : PointCloud2BaseAssembler("max_clouds") { - } ~PointCloud2Assembler() @@ -65,12 +77,24 @@ class PointCloud2Assembler : public BaseAssembler return (scan.width * scan.height); } - void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::PointCloud2& scan_in, sensor_msgs::PointCloud& cloud_out) + void Convert(const string& fixed_frame_id, const + sensor_msgs::PointCloud2& cloud_in, sensor_msgs::PointCloud2& cloud_out) { - sensor_msgs::PointCloud cloud_in; - sensor_msgs::convertPointCloud2ToPointCloud(scan_in, cloud_in); - tf_->transformPointCloud(fixed_frame_id, cloud_in, cloud_out) ; - return ; + + std::string error_msg; + bool success = this->tf_->waitForTransform(fixed_frame_id, cloud_in.header.frame_id, cloud_in.header.stamp, + ros::Duration(0.1), ros::Duration(0.01), &error_msg); + if (!success) + { + ROS_WARN("Could not get transform! %s", error_msg.c_str()); + return; + } + + tf::StampedTransform transform; + this->tf_->lookupTransform(fixed_frame_id, cloud_in.header.frame_id, cloud_in.header.stamp, transform); + geometry_msgs::TransformStamped tf_msg; + tf::transformStampedTFToMsg(transform, tf_msg); + tf2::doTransform(cloud_in, cloud_out, tf_msg); } private: diff --git a/src/point_cloud_assembler.cpp b/src/point_cloud_assembler.cpp index db20979..3a32fb0 100644 --- a/src/point_cloud_assembler.cpp +++ b/src/point_cloud_assembler.cpp @@ -33,7 +33,7 @@ *********************************************************************/ -#include "laser_assembler/base_assembler.h" +#include "laser_assembler/point_cloud_base_assembler.h" using namespace std ; @@ -47,10 +47,10 @@ namespace laser_assembler * params * * (Several params are inherited from BaseAssemblerSrv) */ -class PointCloudAssembler : public BaseAssembler +class PointCloudAssembler : public PointCloudBaseAssembler { public: - PointCloudAssembler() : BaseAssembler("max_clouds") + PointCloudAssembler() : PointCloudBaseAssembler("max_clouds") { } @@ -65,7 +65,7 @@ class PointCloudAssembler : public BaseAssembler return (scan.points.size ()); } - void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::PointCloud& scan_in, sensor_msgs::PointCloud& cloud_out) + void Convert(const string& fixed_frame_id, const sensor_msgs::PointCloud& scan_in, sensor_msgs::PointCloud& cloud_out) { tf_->transformPointCloud(fixed_frame_id, scan_in, cloud_out) ; return ; From 62890b88f1a24234240c9e355190c506410ca89a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Mon, 1 Feb 2016 18:09:40 +0100 Subject: [PATCH 2/2] added optional scan line skipping --- include/laser_assembler/base_assembler.h | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/include/laser_assembler/base_assembler.h b/include/laser_assembler/base_assembler.h index ec2b318..72a5220 100644 --- a/include/laser_assembler/base_assembler.h +++ b/include/laser_assembler/base_assembler.h @@ -120,6 +120,9 @@ class BaseAssembler //! \brief The max number of scans to store in the scan history unsigned int max_scans_ ; + + //! \brief The number of scans to skip continuously + unsigned int skip_scans_, skip_; } ; @@ -132,6 +135,11 @@ BaseAssembler::BaseAssembler(const std::string& max_size_param_name) : pri if (tf_cache_time_secs < 0) ROS_ERROR("Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs) ; + int tmp_skip_scans; + private_ns_.param("skip_scans", tmp_skip_scans, 0); + skip_scans_ = tmp_skip_scans; + skip_ = skip_scans_; + tf_ = new tf::TransformListener(n_, ros::Duration(tf_cache_time_secs)); ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs) ; @@ -210,6 +218,17 @@ BaseAssembler::~BaseAssembler() template void BaseAssembler::msgCallback(const boost::shared_ptr& scan_ptr) { + + // skip a certain number of scans + if(skip_scans_ > 0){ + if(skip_ > 0){ + skip_--; + return; + }else{ + skip_ = skip_scans_; + } + } + ROS_DEBUG("starting msgCallback"); const T scan = *scan_ptr ;