dense_laser_assembler: dense_laser_assembler: dense_laser_assembler

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 <gtest/gtest.h>
00036 
00037 #include <sensor_msgs/LaserScan.h>
00038 #include <dense_laser_assembler/dense_laser_assembler.h>
00039 
00040 using namespace std;
00041 using namespace dense_laser_assembler;
00042 
00043 sensor_msgs::LaserScanConstPtr buildScan(const ros::Time& stamp, double start_angle, double start_range, double start_intensity, const unsigned int N)
00044 {
00045   sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan);
00046 
00047   scan->header.stamp = stamp;
00048 
00049   scan->angle_increment = 1.0;
00050   scan->angle_min = start_angle;
00051   scan->angle_max = start_angle + (N-1)*scan->angle_increment;
00052   scan->time_increment = 1.0;
00053   scan->scan_time = 0.0;
00054   scan->range_min = 0.0;
00055   scan->range_max = 1000.0;
00056   scan->ranges.resize(N);
00057   scan->intensities.resize(N);
00058   for (unsigned int i=0; i<N; i++)
00059   {
00060     scan->ranges[i] = start_range + 1.0*i;
00061     scan->intensities[i] = start_intensity + 1.0*i;
00062     //printf("%.3f ", scan->ranges[i]);
00063   }
00064   //printf("\n");
00065   return scan;
00066 }
00067 
00068 static const unsigned int NUM_SCANS = 20;
00069 static const unsigned int RAYS_PER_SCAN = 50;
00070 static const double eps = 1e-8;
00071 
00072 void addScans1(const ros::Time& start_stamp, double start_angle, DenseLaserAssembler& assembler)
00073 {
00074 
00075   for (unsigned int i=0; i<NUM_SCANS; i++)
00076   {
00077     sensor_msgs::LaserScanConstPtr scan = buildScan(start_stamp + ros::Duration(i,0), start_angle, 100*i, 1000*i, RAYS_PER_SCAN);
00078     assembler.add(scan);
00079   }
00080 }
00081 
00082 TEST(DenseLaserAssembler, easy)
00083 {
00084   DenseLaserAssembler assembler;
00085   addScans1(ros::Time(10,0), 0.0, assembler);
00086 
00087   calibration_msgs::DenseLaserSnapshot snapshot;
00088   bool result;
00089   result = assembler.assembleSnapshot(ros::Time(9,0), ros::Time(31,0), snapshot);
00090   ASSERT_TRUE(result);
00091   ASSERT_EQ(snapshot.ranges.size(),      NUM_SCANS * RAYS_PER_SCAN);
00092   ASSERT_EQ(snapshot.intensities.size(), NUM_SCANS * RAYS_PER_SCAN);
00093   ASSERT_EQ(snapshot.readings_per_scan, RAYS_PER_SCAN);
00094   ASSERT_EQ(snapshot.num_scans, NUM_SCANS);
00095   ASSERT_EQ(snapshot.scan_start.size(), NUM_SCANS);
00096 
00097   // Check start time
00098   EXPECT_EQ(snapshot.scan_start[0],  ros::Time(10,0));
00099   EXPECT_EQ(snapshot.scan_start[5],  ros::Time(15,0));
00100   EXPECT_EQ(snapshot.scan_start[19], ros::Time(29,0));
00101 
00102   // Check ranges
00103   EXPECT_NEAR(snapshot.ranges[0],      0, eps);
00104   EXPECT_NEAR(snapshot.ranges[70],   120, eps);
00105   EXPECT_NEAR(snapshot.ranges[999], 1949, eps);
00106 
00107   // Check intensities
00108   EXPECT_NEAR(snapshot.intensities[0],       0, eps);
00109   EXPECT_NEAR(snapshot.intensities[70],   1020, eps);
00110   EXPECT_NEAR(snapshot.intensities[999], 19049, eps);
00111 }
00112 
00113 TEST(DenseLaserAssembler, mismatchedMetadata)
00114 {
00115   DenseLaserAssembler assembler;
00116   addScans1(ros::Time( 10,0), 0.0, assembler);
00117   addScans1(ros::Time(110,0), 1.0, assembler);
00118 
00119   calibration_msgs::DenseLaserSnapshot snapshot;
00120   bool result;
00121   result = assembler.assembleSnapshot(ros::Time(9,0), ros::Time(31,0), snapshot);
00122   EXPECT_TRUE(result);
00123   EXPECT_EQ(snapshot.ranges.size(), NUM_SCANS * RAYS_PER_SCAN);
00124 
00125   result = assembler.assembleSnapshot(ros::Time(109,0), ros::Time(131,0), snapshot);
00126   EXPECT_TRUE(result);
00127   EXPECT_EQ(snapshot.ranges.size(), NUM_SCANS * RAYS_PER_SCAN);
00128 
00129   result = assembler.assembleSnapshot(ros::Time(9,0), ros::Time(131,0), snapshot);
00130   EXPECT_FALSE(result);
00131 }
00132 
00133 
00134 int main(int argc, char **argv){
00135   testing::InitGoogleTest(&argc, argv);
00136   return RUN_ALL_TESTS();
00137 }
dense_laser_assembler_unittest.cpp
