interp_snapshot_unittest.cpp
Go to the documentation of this file.
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 #include <laser_joint_processor/joint_image_interpolator.h>
00037 
00038 using namespace std;
00039 using namespace laser_joint_processor;
00040 
00041 static const float eps = 1e-6;
00042 
00043 class InterpSnapshot_EasyTests : public ::testing::Test
00044 {
00045 protected:
00046   virtual void SetUp()
00047   {
00048     snapshot_.angle_min = 10.0;
00049     snapshot_.angle_increment = 1.0;
00050     snapshot_.readings_per_scan = 4;
00051     snapshot_.num_scans = 3;
00052     snapshot_.ranges.resize(12);
00053     for (unsigned int i=0; i<snapshot_.ranges.size(); i++)
00054       snapshot_.ranges[i] = i*100;
00055 
00056     points_.resize(7);
00057 
00058     // Boundaries
00059     points_[0] = Point(0,0);
00060     points_[1] = Point(3,0);
00061     points_[2] = Point(0,2);
00062     points_[3] = Point(3,2);
00063 
00064     points_[4] = Point(2.5, .5);
00065     points_[5] = Point(2,    1);
00066     points_[6] = Point(1.25,  1.5);
00067 
00068     bool result;
00069     result = interpSnapshot(points_, snapshot_, angles_, ranges_);
00070     ASSERT_TRUE(result);
00071   }
00072 
00073   geometry_msgs::Point Point(float x, float y)
00074   {
00075     geometry_msgs::Point point;
00076     point.x = x;
00077     point.y = y;
00078     return point;
00079   }
00080 
00081   std::vector <geometry_msgs::Point> points_;
00082   vector<float> angles_;
00083   vector<float> ranges_;
00084   calibration_msgs::DenseLaserSnapshot snapshot_;
00085 };
00086 
00087 TEST_F(InterpSnapshot_EasyTests, angles)
00088 {
00089   EXPECT_NEAR(angles_[0], 10.0, eps);
00090   EXPECT_NEAR(angles_[1], 13.0, eps);
00091   EXPECT_NEAR(angles_[2], 10.0, eps);
00092   EXPECT_NEAR(angles_[3], 13.0, eps);
00093   EXPECT_NEAR(angles_[4], 12.5, eps);
00094   EXPECT_NEAR(angles_[5], 12.0, eps);
00095   EXPECT_NEAR(angles_[6], 11.25,eps);
00096 }
00097 
00098 
00099 /*
00100  *   0  100  200  300
00101  * 400  500  600  700
00102  * 800  900 1000 1100
00103  */
00104 
00105 TEST_F(InterpSnapshot_EasyTests, ranges)
00106 {
00107   EXPECT_NEAR(ranges_[0],    0.0, eps);
00108   EXPECT_NEAR(ranges_[1],  300.0, eps);
00109   EXPECT_NEAR(ranges_[2],  800.0, eps);
00110   EXPECT_NEAR(ranges_[3], 1100.0, eps);
00111   EXPECT_NEAR(ranges_[4],  450.0, eps);
00112   EXPECT_NEAR(ranges_[5],  600.0, eps);
00113   EXPECT_NEAR(ranges_[6],  725.0, eps);
00114 }
00115 
00116 TEST_F(InterpSnapshot_EasyTests, out_of_bounds)
00117 {
00118   points_[6] = Point(-1.0, 0);
00119   EXPECT_FALSE(interpSnapshot(points_, snapshot_, angles_, ranges_));
00120 
00121   points_[6] = Point(0, -1.0);
00122   EXPECT_FALSE(interpSnapshot(points_, snapshot_, angles_, ranges_));
00123 
00124   points_[6] = Point(3.1, 1.9);
00125   EXPECT_FALSE(interpSnapshot(points_, snapshot_, angles_, ranges_));
00126 
00127   points_[6] = Point(2.9, 2.1);
00128   EXPECT_FALSE(interpSnapshot(points_, snapshot_, angles_, ranges_));
00129 
00130   // Control Test
00131   points_[6] = Point(1, 1);
00132   EXPECT_TRUE(interpSnapshot(points_, snapshot_, angles_, ranges_));
00133 }
00134 
00135 
00136 
00137 int main(int argc, char **argv){
00138   testing::InitGoogleTest(&argc, argv);
00139   return RUN_ALL_TESTS();
00140 }


laser_joint_processor
Author(s): Vijay Pradeep
autogenerated on Wed Apr 3 2019 02:59:32