test_voxel_grid.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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 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 /* Author: Mrinal Kalakrishnan, Ken Anderson */
00036 
00037 #include <gtest/gtest.h>
00038 
00039 #include <moveit/distance_field/voxel_grid.h>
00040 #include <ros/ros.h>
00041 
00042 using namespace distance_field;
00043 
00044 TEST(TestVoxelGrid, TestReadWrite)
00045 {
00046   int i;
00047   int def = -100;
00048   VoxelGrid<int> vg(0.02, 0.02, 0.02, 0.01, 0, 0, 0, def);
00049 
00050   int numX = vg.getNumCells(DIM_X);
00051   int numY = vg.getNumCells(DIM_Y);
00052   int numZ = vg.getNumCells(DIM_Z);
00053 
00054   // Check dimensions
00055   EXPECT_EQ(numX, 2);
00056   EXPECT_EQ(numY, 2);
00057   EXPECT_EQ(numZ, 2);
00058 
00059   // check initial values
00060   vg.reset(0);
00061 
00062   i = 0;
00063   for (int x = 0; x < numX; x++)
00064     for (int y = 0; y < numY; y++)
00065       for (int z = 0; z < numZ; z++)
00066       {
00067         EXPECT_EQ(vg.getCell(x, y, z), 0);
00068         i++;
00069       }
00070 
00071   // Check out-of-bounds query    // FIXME-- this test fails!!
00072   // EXPECT_EQ( vg.getCell(999,9999,999), def );
00073   // EXPECT_EQ( vg.getCell(numX+1,0,0), def);
00074   // EXPECT_EQ( vg.getCell(0,numY+1,0), def);
00075   // EXPECT_EQ( vg.getCell(0,0,numZ+1), def);
00076 
00077   // Set values
00078   i = 0;
00079   for (int x = 0; x < numX; x++)
00080     for (int y = 0; y < numY; y++)
00081       for (int z = 0; z < numZ; z++)
00082       {
00083         vg.getCell(x, y, z) = i;
00084         i++;
00085       }
00086 
00087   // check reset values
00088   i = 0;
00089   for (int x = 0; x < numX; x++)
00090     for (int y = 0; y < numY; y++)
00091       for (int z = 0; z < numZ; z++)
00092       {
00093         EXPECT_EQ(i, vg.getCell(x, y, z));
00094         i++;
00095       }
00096 }
00097 
00098 int main(int argc, char** argv)
00099 {
00100   testing::InitGoogleTest(&argc, argv);
00101   return RUN_ALL_TESTS();
00102 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49