polygon_tests.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 #include <gtest/gtest.h>
35 #include <nav_2d_utils/polygons.h>
36 #include <vector>
37 
38 using nav_2d_utils::parseVVD;
39 using nav_2d_msgs::Polygon2D;
40 using nav_2d_utils::polygonFromString;
41 using nav_2d_utils::polygonFromParallelArrays;
42 
43 TEST(array_parser, basic_operation)
44 {
45  std::vector<std::vector<double> > vvd;
46  vvd = parseVVD("[[1, 2.2], [.3, -4e4]]");
47  EXPECT_DOUBLE_EQ(2U, vvd.size());
48  EXPECT_DOUBLE_EQ(2U, vvd[0].size());
49  EXPECT_DOUBLE_EQ(2U, vvd[1].size());
50  EXPECT_DOUBLE_EQ(1.0, vvd[0][0]);
51  EXPECT_DOUBLE_EQ(2.2, vvd[0][1]);
52  EXPECT_DOUBLE_EQ(0.3, vvd[1][0]);
53  EXPECT_DOUBLE_EQ(-40000.0, vvd[1][1]);
54 }
55 
56 TEST(array_parser, missing_open)
57 {
58  EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]]"), nav_2d_utils::PolygonParseException);
59 }
60 
61 TEST(array_parser, missing_close)
62 {
63  EXPECT_THROW(parseVVD("[[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException);
64 }
65 
66 TEST(array_parser, wrong_depth)
67 {
68  EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException);
69 }
70 
71 TEST(Polygon2D, radius_param)
72 {
73  Polygon2D footprint = nav_2d_utils::polygonFromRadius(10.0);
74  // Circular robot has 16-point footprint auto-generated.
75  ASSERT_EQ(16U, footprint.points.size());
76 
77  // Check the first point
78  EXPECT_EQ(10.0, footprint.points[0].x);
79  EXPECT_EQ(0.0, footprint.points[0].y);
80 
81  // Check the 4th point, which should be 90 degrees around the circle from the first.
82  EXPECT_NEAR(0.0, footprint.points[4].x, 0.0001);
83  EXPECT_NEAR(10.0, footprint.points[4].y, 0.0001);
84 }
85 
86 TEST(Polygon2D, string_param)
87 {
88  Polygon2D footprint = polygonFromString("[[1, 1], [-1, 1], [-1, -1]]");
89  ASSERT_EQ(3U, footprint.points.size());
90 
91  EXPECT_EQ(1.0, footprint.points[ 0 ].x);
92  EXPECT_EQ(1.0, footprint.points[ 0 ].y);
93 
94  EXPECT_EQ(-1.0, footprint.points[ 1 ].x);
95  EXPECT_EQ(1.0, footprint.points[ 1 ].y);
96 
97  EXPECT_EQ(-1.0, footprint.points[ 2 ].x);
98  EXPECT_EQ(-1.0, footprint.points[ 2 ].y);
99 }
100 
101 TEST(Polygon2D, broken_string_param)
102 {
103  // Not enough points
104  EXPECT_THROW(polygonFromString("[[1, 1], [-1, 1]]"), nav_2d_utils::PolygonParseException);
105 
106  // Too many numbers in point
107  EXPECT_THROW(polygonFromString("[[1, 1, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException);
108 
109  // Unexpected character
110  EXPECT_THROW(polygonFromString("[[x, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException);
111 
112  // Empty String
113  EXPECT_THROW(polygonFromString(""), nav_2d_utils::PolygonParseException);
114 
115  // Empty List
116  EXPECT_THROW(polygonFromString("[]"), nav_2d_utils::PolygonParseException);
117 
118  // Empty Point
119  EXPECT_THROW(polygonFromString("[[]]"), nav_2d_utils::PolygonParseException);
120 }
121 
122 TEST(Polygon2D, arrays)
123 {
124  std::vector<double> xs = {1, -1, -1};
125  std::vector<double> ys = {1, 1, -1};
126  Polygon2D footprint = polygonFromParallelArrays(xs, ys);
127  ASSERT_EQ(3U, footprint.points.size());
128 
129  EXPECT_EQ(1.0, footprint.points[ 0 ].x);
130  EXPECT_EQ(1.0, footprint.points[ 0 ].y);
131 
132  EXPECT_EQ(-1.0, footprint.points[ 1 ].x);
133  EXPECT_EQ(1.0, footprint.points[ 1 ].y);
134 
135  EXPECT_EQ(-1.0, footprint.points[ 2 ].x);
136  EXPECT_EQ(-1.0, footprint.points[ 2 ].y);
137 }
138 
139 TEST(Polygon2D, broken_arrays)
140 {
141  std::vector<double> shorty = {1, -1};
142  std::vector<double> three = {1, 1, -1};
143  std::vector<double> four = {1, 1, -1, -1};
144  EXPECT_THROW(polygonFromParallelArrays(shorty, shorty), nav_2d_utils::PolygonParseException);
145  EXPECT_THROW(polygonFromParallelArrays(three, four), nav_2d_utils::PolygonParseException);
146 }
147 
148 TEST(Polygon2D, test_move)
149 {
150  Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]");
151  geometry_msgs::Pose2D pose;
152  Polygon2D square2 = nav_2d_utils::movePolygonToPose(square, pose);
153  EXPECT_TRUE(nav_2d_utils::equals(square, square2));
154  pose.x = 15;
155  pose.y = -10;
156  pose.theta = M_PI / 4;
157  Polygon2D diamond = nav_2d_utils::movePolygonToPose(square, pose);
158  ASSERT_EQ(4U, diamond.points.size());
159  double side = 1.0 / sqrt(2);
160 
161  EXPECT_DOUBLE_EQ(pose.x, diamond.points[ 0 ].x);
162  EXPECT_DOUBLE_EQ(pose.y + side, diamond.points[ 0 ].y);
163  EXPECT_DOUBLE_EQ(pose.x + side, diamond.points[ 1 ].x);
164  EXPECT_DOUBLE_EQ(pose.y, diamond.points[ 1 ].y);
165  EXPECT_DOUBLE_EQ(pose.x, diamond.points[ 2 ].x);
166  EXPECT_DOUBLE_EQ(pose.y - side, diamond.points[ 2 ].y);
167  EXPECT_DOUBLE_EQ(pose.x - side, diamond.points[ 3 ].x);
168  EXPECT_DOUBLE_EQ(pose.y, diamond.points[ 3 ].y);
169 }
170 
171 TEST(Polygon2D, inside)
172 {
173  Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]");
174  EXPECT_TRUE(nav_2d_utils::isInside(square, 0.00, 0.00));
175  EXPECT_TRUE(nav_2d_utils::isInside(square, 0.45, 0.45));
176  EXPECT_FALSE(nav_2d_utils::isInside(square, 0.50, 0.50));
177  EXPECT_FALSE(nav_2d_utils::isInside(square, 0.00, 0.50));
178  EXPECT_FALSE(nav_2d_utils::isInside(square, 0.55, 0.55));
179 }
180 
181 int main(int argc, char** argv)
182 {
183  testing::InitGoogleTest(&argc, argv);
184  return RUN_ALL_TESTS();
185 }
nav_2d_utils::parseVVD
std::vector< std::vector< double > > parseVVD(const std::string &input)
Parse a vector of vectors of doubles from a string. Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...].
Definition: polygons.cpp:49
nav_2d_utils::equals
bool equals(const nav_2d_msgs::Polygon2D &polygon0, const nav_2d_msgs::Polygon2D &polygon1)
check if two polygons are equal. Used in testing
Definition: polygons.cpp:350
nav_2d_utils::PolygonParseException
Exception to throw when Polygon doesn&#39;t load correctly.
Definition: polygons.h:52
nav_2d_utils::polygonFromRadius
nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points=16)
Create a "circular" polygon from a given radius.
Definition: polygons.cpp:112
TEST
TEST(array_parser, basic_operation)
Definition: polygon_tests.cpp:43
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
nav_2d_utils::polygonFromParallelArrays
nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector< double > &xs, const std::vector< double > &ys)
Create a polygon from two parallel arrays.
Definition: polygons.cpp:317
main
int main(int argc, char **argv)
Definition: polygon_tests.cpp:181
nav_2d_utils::polygonFromString
nav_2d_msgs::Polygon2D polygonFromString(const std::string &polygon_string)
Make a polygon from the given string. Format should be bracketed array of arrays of doubles...
Definition: polygons.cpp:127
nav_2d_utils::movePolygonToPose
nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D &polygon, const geometry_msgs::Pose2D &pose)
Translate and rotate a polygon to a new pose.
Definition: polygons.cpp:366
nav_2d_utils::isInside
bool isInside(const nav_2d_msgs::Polygon2D &polygon, const double x, const double y)
Check if a given point is inside of a polygon.
Definition: polygons.cpp:382

nav_2d_utils
Author(s):
autogenerated on Mon Feb 28 2022 23:32:46