00001 #include "ros/ros.h"
00002 #include "costmap_2d/costmap_2d.h"
00003 #include "frontier_exploration/costmap_tools.h"
00004 #include "boost/foreach.hpp"
00005
00006 #include <gtest/gtest.h>
00007
00008
00009 class NeighborhoodFunctionTest : public ::testing::Test{
00010 protected:
00011 virtual void SetUp(){
00012
00013 costmap_.resizeMap(9,9,0.1,0,0);
00014 }
00015 costmap_2d::Costmap2D costmap_;
00016 };
00017
00018 TEST_F(NeighborhoodFunctionTest, middle)
00019 {
00020
00021 unsigned int idx = costmap_.getIndex(4,4);
00022 ASSERT_EQ(4,frontier_exploration::nhood4(idx,costmap_).size());
00023 ASSERT_EQ(8,frontier_exploration::nhood8(idx,costmap_).size());
00024
00025 }
00026
00027 TEST_F(NeighborhoodFunctionTest, edge)
00028 {
00029
00030 std::list<unsigned int> to_test;
00031 to_test.push_back(costmap_.getIndex(4,8));
00032 to_test.push_back(costmap_.getIndex(4,0));
00033 to_test.push_back(costmap_.getIndex(8,4));
00034 to_test.push_back(costmap_.getIndex(0,4));
00035 BOOST_FOREACH(unsigned int idx, to_test){
00036 ASSERT_EQ(3,frontier_exploration::nhood4(idx,costmap_).size());
00037 ASSERT_EQ(5,frontier_exploration::nhood8(idx,costmap_).size());
00038 }
00039 }
00040
00041
00042 TEST_F(NeighborhoodFunctionTest, corner)
00043 {
00044
00045 std::list<unsigned int> to_test;
00046 to_test.push_back(costmap_.getIndex(8,8));
00047 to_test.push_back(costmap_.getIndex(8,0));
00048 to_test.push_back(costmap_.getIndex(8,0));
00049 to_test.push_back(costmap_.getIndex(0,0));
00050 BOOST_FOREACH(unsigned int idx, to_test){
00051 ASSERT_EQ(2,frontier_exploration::nhood4(idx,costmap_).size());
00052 ASSERT_EQ(3,frontier_exploration::nhood8(idx,costmap_).size());
00053 }
00054
00055 }
00056
00057 TEST_F(NeighborhoodFunctionTest, offMap)
00058 {
00059
00060 unsigned int idx = costmap_.getIndex(12,12);
00061 ASSERT_EQ(0,frontier_exploration::nhood4(idx,costmap_).size());
00062 ASSERT_EQ(0,frontier_exploration::nhood8(idx,costmap_).size());
00063
00064 }
00065
00066 class NearestCellTest : public ::testing::Test{
00067 protected:
00068 virtual void SetUp(){
00069
00070 costmap_.resizeMap(9,9,0.1,0,0);
00071 unsigned char* map = costmap_.getCharMap();
00072 const unsigned int size_x = costmap_.getSizeInCellsX(), size_y = costmap_.getSizeInCellsY();
00073
00074 std::fill(map, map+ (size_x*size_y)/2, 0);
00075 std::fill(map+(size_x*size_y)/2 + 1, map+(size_x*size_y), 1);
00076 }
00077
00078 costmap_2d::Costmap2D costmap_;
00079 };
00080
00081 TEST_F(NearestCellTest, sameCell)
00082 {
00083 unsigned int input = 80;
00084 unsigned int result;
00085 ASSERT_TRUE(frontier_exploration::nearestCell(result,input,1,costmap_));
00086 ASSERT_EQ(input,result);
00087
00088 }
00089
00090 TEST_F(NearestCellTest, differentCell)
00091 {
00092 unsigned int input = 20;
00093 unsigned int result;
00094 ASSERT_TRUE(frontier_exploration::nearestCell(result,input,1,costmap_));
00095 ASSERT_NE(input,result);
00096
00097 }
00098
00099 TEST_F(NearestCellTest, offMap)
00100 {
00101 unsigned int input = std::numeric_limits<unsigned int>::max();
00102 unsigned int result;
00103 ASSERT_FALSE(frontier_exploration::nearestCell(result,input,1,costmap_));
00104
00105 }
00106
00107 int main(int argc, char **argv){
00108 testing::InitGoogleTest(&argc, argv);
00109 return RUN_ALL_TESTS();
00110 }