dense_laser_assembler: dense_laser_assembler: dense_laser

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include "dense_laser_assembler/dense_laser_assembler.h"
00036 
00037 using namespace std;
00038 using namespace dense_laser_assembler;
00039 
00040 DenseLaserAssembler::DenseLaserAssembler(const unsigned int cache_size)
00041   : cache_(&settlerlib::SortedDeque<sensor_msgs::LaserScanConstPtr>::getPtrStamp, "dense_laser_deque")
00042 {
00043   setCacheSize(cache_size);
00044 }
00045 
00046 void DenseLaserAssembler::setCacheSize(const unsigned int cache_size)
00047 {
00048   cache_.setMaxSize(cache_size);
00049 }
00050 
00051 void DenseLaserAssembler::add(const sensor_msgs::LaserScanConstPtr& laser_scan)
00052 {
00053   cache_.add(laser_scan);
00054 }
00055 
00056 bool DenseLaserAssembler::assembleSnapshot(const ros::Time& start, const ros::Time& end, calibration_msgs::DenseLaserSnapshot& snapshot)
00057 {
00058   vector< sensor_msgs::LaserScanConstPtr > scan_vec;
00059   ROS_DEBUG("Assembling snapshot:\n"
00060             "   from: %.2f\n"
00061             "   to:   %.2f\n"
00062             "   duration: %.3f\n",
00063             start.toSec(), end.toSec(), (end-start).toSec());
00064 
00065   scan_vec = cache_.getInterval(start, end);
00066   return flattenScanVec(scan_vec, snapshot);
00067 }
00068 
00069 bool DenseLaserAssembler::flattenScanVec(const std::vector< sensor_msgs::LaserScanConstPtr >& scans,
00070                                          calibration_msgs::DenseLaserSnapshot& snapshot)
00071 {
00072   if (scans.size() == 0)
00073   {
00074     ROS_WARN("Trying to build empty cloud") ;
00075     snapshot.angle_min       = 0.0;
00076     snapshot.angle_max       = 0.0;
00077     snapshot.angle_increment = 0.0;
00078     snapshot.time_increment  = 0.0;
00079     snapshot.range_min       = 0.0;
00080     snapshot.range_max       = 0.0;
00081     snapshot.readings_per_scan = 0;
00082     snapshot.num_scans       = 0;
00083     snapshot.ranges.clear();
00084     snapshot.intensities.clear();
00085     snapshot.scan_start.clear();
00086     return true;
00087   }
00088 
00089   // Not really sure what the overall stamp should be.  But for now, copy over the stamp from the last scan
00090   snapshot.header.stamp = scans[scans.size()-1]->header.stamp;
00091 
00092   // Fill in all the metadata
00093   snapshot.header.frame_id = scans[0]->header.frame_id;
00094   snapshot.angle_min       = scans[0]->angle_min;
00095   snapshot.angle_max       = scans[0]->angle_max;
00096   snapshot.angle_increment = scans[0]->angle_increment;
00097   snapshot.time_increment  = scans[0]->time_increment;
00098   snapshot.range_min       = scans[0]->range_min;
00099   snapshot.range_max       = scans[0]->range_max;
00100 
00101   // Define the data dimensions
00102   snapshot.readings_per_scan = scans[0]->ranges.size();
00103   snapshot.num_scans         = scans.size();
00104   const unsigned int& w = snapshot.readings_per_scan;
00105   const unsigned int& h = snapshot.num_scans;
00106 
00107   // Do a consistency check on the metadata
00108   for (unsigned int i=0; i<scans.size(); i++)
00109   {
00110     if (!verifyMetadata(snapshot, *scans[i]))
00111     {
00112       ROS_WARN("Metadata doesn't match. It is likely that someone just changed the laser's configuration");
00113       return false ;
00114     }
00115   }
00116 
00117   // Allocate data vectors
00118   snapshot.scan_start.resize(h) ;
00119   snapshot.ranges.resize(w*h) ;
00120   snapshot.intensities.resize(w*h) ;
00121 
00122   const unsigned int range_elem_size     = sizeof(scans[0]->ranges[0]) ;
00123   const unsigned int intensity_elem_size = sizeof(scans[0]->intensities[0]) ;
00124 
00125   // Make sure our sizes match before doing memcpy
00126   assert(sizeof(snapshot.ranges[0])      == sizeof(scans[0]->ranges[0]));
00127   assert(sizeof(snapshot.intensities[0]) == sizeof(scans[0]->intensities[0]));
00128 
00129   for (unsigned int i=0; i<h; i++)
00130   {
00131     memcpy(&snapshot.ranges[i*w],      &scans[i]->ranges[0],      w*range_elem_size) ;
00132     memcpy(&snapshot.intensities[i*w], &scans[i]->intensities[0], w*intensity_elem_size) ;
00133 
00134     // Copy time stamp
00135     snapshot.scan_start[i] = scans[i]->header.stamp ;
00136   }
00137 
00138   ROS_DEBUG("Done building snapshot that is [%u rows] x [%u cols]", h, w) ;
00139   ROS_DEBUG("  ranges.size      = %u", snapshot.ranges.size());
00140   ROS_DEBUG("  intensities.size = %u", snapshot.intensities.size());
00141 
00142   return true ;
00143 }
00144 
00145 static const double eps = 1e-9 ;
00146 
00147 #define CHECK(a) \
00148 { \
00149   if ( (snapshot.a - scan.a < -eps) || (snapshot.a - scan.a > eps)) \
00150     return false ; \
00151 }
00152 
00153 bool DenseLaserAssembler::verifyMetadata(const calibration_msgs::DenseLaserSnapshot& snapshot, const sensor_msgs::LaserScan& scan)
00154 {
00155   CHECK(angle_min) ;
00156   CHECK(angle_max) ;
00157   CHECK(angle_increment) ;
00158   CHECK(time_increment) ;
00159   CHECK(range_min) ;
00160   CHECK(range_max) ;
00161 
00162   if (snapshot.header.frame_id.compare(scan.header.frame_id) != 0)
00163     return false ;
00164 
00165   if (snapshot.readings_per_scan != scan.ranges.size())
00166     return false ;
00167   if (snapshot.readings_per_scan != scan.intensities.size())
00168     return false ;
00169 
00170 
00171   return true ;
00172 }
dense_laser_assembler.cpp
