186 lines
6.4 KiB
C++
Executable File
186 lines
6.4 KiB
C++
Executable File
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Copyright (c) 2018, Locus Robotics
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of the copyright holder nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
#include <gtest/gtest.h>
|
|
#include <nav_2d_utils/polygons.h>
|
|
#include <vector>
|
|
|
|
using nav_2d_utils::parseVVD;
|
|
using nav_2d_msgs::Polygon2D;
|
|
using nav_2d_utils::polygonFromString;
|
|
using nav_2d_utils::polygonFromParallelArrays;
|
|
|
|
TEST(array_parser, basic_operation)
|
|
{
|
|
std::vector<std::vector<double> > vvd;
|
|
vvd = parseVVD("[[1, 2.2], [.3, -4e4]]");
|
|
EXPECT_DOUBLE_EQ(2U, vvd.size());
|
|
EXPECT_DOUBLE_EQ(2U, vvd[0].size());
|
|
EXPECT_DOUBLE_EQ(2U, vvd[1].size());
|
|
EXPECT_DOUBLE_EQ(1.0, vvd[0][0]);
|
|
EXPECT_DOUBLE_EQ(2.2, vvd[0][1]);
|
|
EXPECT_DOUBLE_EQ(0.3, vvd[1][0]);
|
|
EXPECT_DOUBLE_EQ(-40000.0, vvd[1][1]);
|
|
}
|
|
|
|
TEST(array_parser, missing_open)
|
|
{
|
|
EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]]"), nav_2d_utils::PolygonParseException);
|
|
}
|
|
|
|
TEST(array_parser, missing_close)
|
|
{
|
|
EXPECT_THROW(parseVVD("[[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException);
|
|
}
|
|
|
|
TEST(array_parser, wrong_depth)
|
|
{
|
|
EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException);
|
|
}
|
|
|
|
TEST(Polygon2D, radius_param)
|
|
{
|
|
Polygon2D footprint = nav_2d_utils::polygonFromRadius(10.0);
|
|
// Circular robot has 16-point footprint auto-generated.
|
|
ASSERT_EQ(16U, footprint.points.size());
|
|
|
|
// Check the first point
|
|
EXPECT_EQ(10.0, footprint.points[0].x);
|
|
EXPECT_EQ(0.0, footprint.points[0].y);
|
|
|
|
// Check the 4th point, which should be 90 degrees around the circle from the first.
|
|
EXPECT_NEAR(0.0, footprint.points[4].x, 0.0001);
|
|
EXPECT_NEAR(10.0, footprint.points[4].y, 0.0001);
|
|
}
|
|
|
|
TEST(Polygon2D, string_param)
|
|
{
|
|
Polygon2D footprint = polygonFromString("[[1, 1], [-1, 1], [-1, -1]]");
|
|
ASSERT_EQ(3U, footprint.points.size());
|
|
|
|
EXPECT_EQ(1.0, footprint.points[ 0 ].x);
|
|
EXPECT_EQ(1.0, footprint.points[ 0 ].y);
|
|
|
|
EXPECT_EQ(-1.0, footprint.points[ 1 ].x);
|
|
EXPECT_EQ(1.0, footprint.points[ 1 ].y);
|
|
|
|
EXPECT_EQ(-1.0, footprint.points[ 2 ].x);
|
|
EXPECT_EQ(-1.0, footprint.points[ 2 ].y);
|
|
}
|
|
|
|
TEST(Polygon2D, broken_string_param)
|
|
{
|
|
// Not enough points
|
|
EXPECT_THROW(polygonFromString("[[1, 1], [-1, 1]]"), nav_2d_utils::PolygonParseException);
|
|
|
|
// Too many numbers in point
|
|
EXPECT_THROW(polygonFromString("[[1, 1, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException);
|
|
|
|
// Unexpected character
|
|
EXPECT_THROW(polygonFromString("[[x, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException);
|
|
|
|
// Empty String
|
|
EXPECT_THROW(polygonFromString(""), nav_2d_utils::PolygonParseException);
|
|
|
|
// Empty List
|
|
EXPECT_THROW(polygonFromString("[]"), nav_2d_utils::PolygonParseException);
|
|
|
|
// Empty Point
|
|
EXPECT_THROW(polygonFromString("[[]]"), nav_2d_utils::PolygonParseException);
|
|
}
|
|
|
|
TEST(Polygon2D, arrays)
|
|
{
|
|
std::vector<double> xs = {1, -1, -1};
|
|
std::vector<double> ys = {1, 1, -1};
|
|
Polygon2D footprint = polygonFromParallelArrays(xs, ys);
|
|
ASSERT_EQ(3U, footprint.points.size());
|
|
|
|
EXPECT_EQ(1.0, footprint.points[ 0 ].x);
|
|
EXPECT_EQ(1.0, footprint.points[ 0 ].y);
|
|
|
|
EXPECT_EQ(-1.0, footprint.points[ 1 ].x);
|
|
EXPECT_EQ(1.0, footprint.points[ 1 ].y);
|
|
|
|
EXPECT_EQ(-1.0, footprint.points[ 2 ].x);
|
|
EXPECT_EQ(-1.0, footprint.points[ 2 ].y);
|
|
}
|
|
|
|
TEST(Polygon2D, broken_arrays)
|
|
{
|
|
std::vector<double> shorty = {1, -1};
|
|
std::vector<double> three = {1, 1, -1};
|
|
std::vector<double> four = {1, 1, -1, -1};
|
|
EXPECT_THROW(polygonFromParallelArrays(shorty, shorty), nav_2d_utils::PolygonParseException);
|
|
EXPECT_THROW(polygonFromParallelArrays(three, four), nav_2d_utils::PolygonParseException);
|
|
}
|
|
|
|
TEST(Polygon2D, test_move)
|
|
{
|
|
Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]");
|
|
geometry_msgs::Pose2D pose;
|
|
Polygon2D square2 = nav_2d_utils::movePolygonToPose(square, pose);
|
|
EXPECT_TRUE(nav_2d_utils::equals(square, square2));
|
|
pose.x = 15;
|
|
pose.y = -10;
|
|
pose.theta = M_PI / 4;
|
|
Polygon2D diamond = nav_2d_utils::movePolygonToPose(square, pose);
|
|
ASSERT_EQ(4U, diamond.points.size());
|
|
double side = 1.0 / sqrt(2);
|
|
|
|
EXPECT_DOUBLE_EQ(pose.x, diamond.points[ 0 ].x);
|
|
EXPECT_DOUBLE_EQ(pose.y + side, diamond.points[ 0 ].y);
|
|
EXPECT_DOUBLE_EQ(pose.x + side, diamond.points[ 1 ].x);
|
|
EXPECT_DOUBLE_EQ(pose.y, diamond.points[ 1 ].y);
|
|
EXPECT_DOUBLE_EQ(pose.x, diamond.points[ 2 ].x);
|
|
EXPECT_DOUBLE_EQ(pose.y - side, diamond.points[ 2 ].y);
|
|
EXPECT_DOUBLE_EQ(pose.x - side, diamond.points[ 3 ].x);
|
|
EXPECT_DOUBLE_EQ(pose.y, diamond.points[ 3 ].y);
|
|
}
|
|
|
|
TEST(Polygon2D, inside)
|
|
{
|
|
Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]");
|
|
EXPECT_TRUE(nav_2d_utils::isInside(square, 0.00, 0.00));
|
|
EXPECT_TRUE(nav_2d_utils::isInside(square, 0.45, 0.45));
|
|
EXPECT_FALSE(nav_2d_utils::isInside(square, 0.50, 0.50));
|
|
EXPECT_FALSE(nav_2d_utils::isInside(square, 0.00, 0.50));
|
|
EXPECT_FALSE(nav_2d_utils::isInside(square, 0.55, 0.55));
|
|
}
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
testing::InitGoogleTest(&argc, argv);
|
|
return RUN_ALL_TESTS();
|
|
}
|