Go to the documentation of this file.00001 #include "ros/ros.h"
00002 #include "costmap_2d/costmap_2d.h"
00003 #include "frontier_exploration/geometry_tools.h"
00004 #include "boost/foreach.hpp"
00005
00006 #include <gtest/gtest.h>
00007
00008 class PointInPolygonTest : public::testing::Test{
00009 protected:
00010 virtual void SetUp(){
00011
00012 geometry_msgs::Point32 point;
00013 point.x = -1; point.y = 1;
00014 polygon_.points.push_back(point);
00015 point.x = 1; point.y = 1;
00016 polygon_.points.push_back(point);
00017 point.x = -1; point.y = -1;
00018 polygon_.points.push_back(point);
00019 point.x = 1; point.y = -1;
00020 polygon_.points.push_back(point);
00021 }
00022 geometry_msgs::Polygon polygon_;
00023 };
00024
00025 TEST_F(PointInPolygonTest, outside){
00026 geometry_msgs::Point point;
00027 point.x = 0.5; point.y = 0;
00028 ASSERT_FALSE(frontier_exploration::pointInPolygon(point, polygon_));
00029 point.x = -0.5; point.y = 0;
00030 ASSERT_FALSE(frontier_exploration::pointInPolygon(point, polygon_));
00031 point.x = 0; point.y = 1.1;
00032 ASSERT_FALSE(frontier_exploration::pointInPolygon(point, polygon_));
00033 point.x = 0; point.y = -1.1;
00034 ASSERT_FALSE(frontier_exploration::pointInPolygon(point, polygon_));
00035 }
00036
00037 TEST_F(PointInPolygonTest, inside){
00038 geometry_msgs::Point point;
00039 point.x = 0; point.y = 0.5;
00040 ASSERT_TRUE(frontier_exploration::pointInPolygon(point, polygon_));
00041 point.x = 0; point.y = 0.5;
00042 ASSERT_TRUE(frontier_exploration::pointInPolygon(point, polygon_));
00043 }
00044
00045 TEST(PointsAdjacentTest, different)
00046 {
00047 geometry_msgs::Point a, b;
00048 a.x = 1;
00049 ASSERT_FALSE(frontier_exploration::pointsNearby(a,b,0));
00050 ASSERT_FALSE(frontier_exploration::pointsNearby(a,b,0.1));
00051 ASSERT_TRUE(frontier_exploration::pointsNearby(a,b,1));
00052 }
00053
00054 TEST(PointsAdjacentTest, identical)
00055 {
00056 geometry_msgs::Point a, b;
00057 a.x = 1;
00058 b.x = 1;
00059 ASSERT_TRUE(frontier_exploration::pointsNearby(a,b,0));
00060 ASSERT_TRUE(frontier_exploration::pointsNearby(a,b,0.1));
00061 ASSERT_TRUE(frontier_exploration::pointsNearby(a,b,1));
00062 }
00063
00064 int main(int argc, char **argv){
00065 testing::InitGoogleTest(&argc, argv);
00066 return RUN_ALL_TESTS();
00067 }