Go to the documentation of this file.00001 #include "ros/ros.h"
00002 #include "costmap_2d/costmap_2d.h"
00003 #include "heatmap/geometry_tools.h"
00004 #include "boost/foreach.hpp"
00005
00006 #include <gtest/gtest.h>
00007
00008 class PointInPolygonTest : public ::testing::Test
00009 {
00010 protected:
00011 virtual void SetUp()
00012 {
00013
00014 geometry_msgs::Point32 point;
00015 point.x = -1;
00016 point.y = 1;
00017 polygon_.points.push_back(point);
00018 point.x = 1;
00019 point.y = 1;
00020 polygon_.points.push_back(point);
00021 point.x = -1;
00022 point.y = -1;
00023 polygon_.points.push_back(point);
00024 point.x = 1;
00025 point.y = -1;
00026 polygon_.points.push_back(point);
00027 }
00028 geometry_msgs::Polygon polygon_;
00029 };
00030
00031 class BottomTopPointPolygonTest : public ::testing::Test
00032 {
00033 protected:
00034 virtual void SetUp()
00035 {
00036
00037 geometry_msgs::Point32 point;
00038 point.x = -1;
00039 point.y = 1;
00040 polygon_.points.push_back(point);
00041 point.x = 1;
00042 point.y = 1;
00043 polygon_.points.push_back(point);
00044 point.x = -1;
00045 point.y = -1;
00046 polygon_.points.push_back(point);
00047 point.x = 1;
00048 point.y = -1;
00049 polygon_.points.push_back(point);
00050 }
00051 geometry_msgs::Polygon polygon_;
00052 };
00053
00054 TEST_F(PointInPolygonTest, outside)
00055 {
00056 geometry_msgs::Point point;
00057 point.x = 0.5;
00058 point.y = 0;
00059 ASSERT_FALSE(heatmap::pointInPolygon(point, polygon_));
00060 point.x = -0.5;
00061 point.y = 0;
00062 ASSERT_FALSE(heatmap::pointInPolygon(point, polygon_));
00063 point.x = 0;
00064 point.y = 1.1;
00065 ASSERT_FALSE(heatmap::pointInPolygon(point, polygon_));
00066 point.x = 0;
00067 point.y = -1.1;
00068 ASSERT_FALSE(heatmap::pointInPolygon(point, polygon_));
00069 }
00070
00071 TEST_F(PointInPolygonTest, inside)
00072 {
00073 geometry_msgs::Point point;
00074 point.x = 0;
00075 point.y = 0.5;
00076 ASSERT_TRUE(heatmap::pointInPolygon(point, polygon_));
00077 point.x = 0;
00078 point.y = 0.5;
00079 ASSERT_TRUE(heatmap::pointInPolygon(point, polygon_));
00080 }
00081
00082 TEST_F(BottomTopPointPolygonTest, bottom)
00083 {
00084 geometry_msgs::Point32 p;
00085 p.x = -1;
00086 p.y = -1;
00087
00088 ASSERT_EQ(heatmap::bottomLeftPointPolygon<geometry_msgs::Point32>(polygon_).x, p.x);
00089 ASSERT_EQ(heatmap::bottomLeftPointPolygon<geometry_msgs::Point32>(polygon_).y, p.y);
00090 }
00091
00092 TEST_F(BottomTopPointPolygonTest, top)
00093 {
00094 geometry_msgs::Point32 p;
00095 p.x = 1;
00096 p.y = 1;
00097
00098 ASSERT_EQ(heatmap::topRightPointPolygon<geometry_msgs::Point32>(polygon_).x, p.x);
00099 ASSERT_EQ(heatmap::topRightPointPolygon<geometry_msgs::Point32>(polygon_).y, p.y);
00100 }
00101
00102 TEST(PointsAdjacentTest, different)
00103 {
00104 geometry_msgs::Point a, b;
00105 a.x = 1;
00106 ASSERT_FALSE(heatmap::pointsNearby(a, b, 0));
00107 ASSERT_FALSE(heatmap::pointsNearby(a, b, 0.1));
00108 ASSERT_TRUE(heatmap::pointsNearby(a, b, 1));
00109 }
00110
00111 TEST(PointsAdjacentTest, identical)
00112 {
00113 geometry_msgs::Point a, b;
00114 a.x = 1;
00115 b.x = 1;
00116 ASSERT_TRUE(heatmap::pointsNearby(a, b, 0));
00117 ASSERT_TRUE(heatmap::pointsNearby(a, b, 0.1));
00118 ASSERT_TRUE(heatmap::pointsNearby(a, b, 1));
00119 }
00120
00121 int main(int argc, char **argv)
00122 {
00123 testing::InitGoogleTest(&argc, argv);
00124 return RUN_ALL_TESTS();
00125 }