HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

199
navigations/nav_2d_utils/test/bounds_test.cpp Executable file
View File

@@ -0,0 +1,199 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, 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/bounds.h>
#include <nav_grid/vector_nav_grid.h>
#include <vector>
using nav_2d_utils::divideBounds;
using nav_core2::UIntBounds;
/**
* @brief Count the values in a grid.
* @param[in] The grid
* @param[out] match Number of values == 1
* @param[out] missed Number of values == 0
* @param[out] multiple Number of other values
*/
void countValues(const nav_grid::VectorNavGrid<unsigned char>& grid,
unsigned int& match, unsigned int& missed, unsigned int& multiple)
{
match = 0;
missed = 0;
multiple = 0;
nav_grid::NavGridInfo info = grid.getInfo();
// No iterator to avoid tricky depenencies
for (unsigned int x = 0; x < info.width; x++)
{
for (unsigned int y = 0; y < info.height; y++)
{
switch (grid(x, y))
{
case 0:
missed++;
break;
case 1:
match++;
break;
default:
multiple++;
break;
}
}
}
}
TEST(DivideBounds, zeroes)
{
UIntBounds bounds(2, 2, 5, 5);
// Number of rows/cols has to be positive
EXPECT_THROW(divideBounds(bounds, 0, 2), std::invalid_argument);
EXPECT_THROW(divideBounds(bounds, 2, 0), std::invalid_argument);
EXPECT_THROW(divideBounds(bounds, 0, 0), std::invalid_argument);
EXPECT_NO_THROW(divideBounds(bounds, 2, 2));
bounds.reset();
// check for errors with empty bounds
EXPECT_NO_THROW(divideBounds(bounds, 2, 2));
}
/**
* This test is for the divideBounds method and takes grids of various sizes
* (cycled through with the outer two loops) and tries to divide them into subgrids of
* various sizes (cycled through with the next two loops). The resulting vector of
* bounds should cover every cell in the original grid, so each of the divided bounds is
* iterated over, adding one to each grid cell. If everything works perfectly, each cell
* should be touched exactly once.
*/
TEST(DivideBounds, iterative_tests)
{
nav_grid::VectorNavGrid<unsigned char> full_grid;
nav_grid::NavGridInfo info;
// count variables
unsigned int match, missed, multiple;
for (info.width = 1; info.width < 15; info.width++)
{
for (info.height = 1; info.height < 15; info.height++)
{
full_grid.setInfo(info);
UIntBounds full_bounds = nav_2d_utils::getFullUIntBounds(info);
for (unsigned int rows = 1; rows < 11u; rows++)
{
for (unsigned int cols = 1; cols < 11u; cols++)
{
full_grid.reset();
std::vector<UIntBounds> divided = divideBounds(full_bounds, cols, rows);
ASSERT_LE(divided.size(), rows * cols) << info.width << "x" << info.height << " " << rows << "x" << cols;
for (const UIntBounds& sub : divided)
{
EXPECT_FALSE(sub.isEmpty());
// Can't use nav_grid_iterator for circular dependencies
for (unsigned int x = sub.getMinX(); x <= sub.getMaxX(); x++)
{
for (unsigned int y = sub.getMinY(); y <= sub.getMaxY(); y++)
{
full_grid.setValue(x, y, full_grid(x, y) + 1);
}
}
}
countValues(full_grid, match, missed, multiple);
ASSERT_EQ(match, info.width * info.height) << "Full grid: " << info.width << "x" << info.height
<< " Requested divisions: " << rows << "x" << cols;
EXPECT_EQ(missed, 0u);
EXPECT_EQ(multiple, 0u);
}
}
}
}
}
/**
* This test is for the divideBounds method and calls it recursively to
* ensure that the method works when the minimum values in the original bounds
* are not zero.
*/
TEST(DivideBounds, recursive_tests)
{
nav_grid::VectorNavGrid<unsigned char> full_grid;
nav_grid::NavGridInfo info;
info.width = 100;
info.height = 100;
full_grid.setInfo(info);
UIntBounds full_bounds = nav_2d_utils::getFullUIntBounds(info);
std::vector<UIntBounds> level_one = divideBounds(full_bounds, 2, 2);
ASSERT_EQ(level_one.size(), 4u);
for (const UIntBounds& sub : level_one)
{
std::vector<UIntBounds> level_two = divideBounds(sub, 2, 2);
ASSERT_EQ(level_two.size(), 4u);
for (const UIntBounds& subsub : level_two)
{
EXPECT_GE(subsub.getMinX(), sub.getMinX());
EXPECT_LE(subsub.getMaxX(), sub.getMaxX());
EXPECT_GE(subsub.getMinY(), sub.getMinY());
EXPECT_LE(subsub.getMaxY(), sub.getMaxY());
// Can't use nav_grid_iterator for circular dependencies
for (unsigned int x = subsub.getMinX(); x <= subsub.getMaxX(); x++)
{
for (unsigned int y = subsub.getMinY(); y <= subsub.getMaxY(); y++)
{
full_grid.setValue(x, y, full_grid(x, y) + 1);
}
}
}
}
// Count values
unsigned int match = 0,
missed = 0,
multiple = 0;
countValues(full_grid, match, missed, multiple);
ASSERT_EQ(match, info.width * info.height);
EXPECT_EQ(missed, 0u);
EXPECT_EQ(multiple, 0u);
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

97
navigations/nav_2d_utils/test/compress_test.cpp Executable file
View File

@@ -0,0 +1,97 @@
/*
* 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/path_ops.h>
using nav_2d_utils::compressPlan;
using nav_2d_utils::addPose;
TEST(CompressTest, compress_test)
{
nav_2d_msgs::Path2D path;
// Dataset borrowed from https://karthaus.nl/rdp/
addPose(path, 24, 173);
addPose(path, 26, 170);
addPose(path, 24, 166);
addPose(path, 27, 162);
addPose(path, 37, 161);
addPose(path, 45, 157);
addPose(path, 48, 152);
addPose(path, 46, 143);
addPose(path, 40, 140);
addPose(path, 34, 137);
addPose(path, 26, 134);
addPose(path, 24, 130);
addPose(path, 24, 125);
addPose(path, 28, 121);
addPose(path, 36, 118);
addPose(path, 46, 117);
addPose(path, 63, 121);
addPose(path, 76, 125);
addPose(path, 82, 120);
addPose(path, 86, 111);
addPose(path, 88, 103);
addPose(path, 90, 91);
addPose(path, 95, 87);
addPose(path, 107, 89);
addPose(path, 107, 104);
addPose(path, 106, 117);
addPose(path, 109, 129);
addPose(path, 119, 131);
addPose(path, 131, 131);
addPose(path, 139, 134);
addPose(path, 138, 143);
addPose(path, 131, 152);
addPose(path, 119, 154);
addPose(path, 111, 149);
addPose(path, 105, 143);
addPose(path, 91, 139);
addPose(path, 80, 142);
addPose(path, 81, 152);
addPose(path, 76, 163);
addPose(path, 67, 161);
addPose(path, 59, 149);
addPose(path, 63, 138);
EXPECT_EQ(41U, compressPlan(path, 0.1).poses.size());
EXPECT_EQ(34U, compressPlan(path, 1.3).poses.size());
EXPECT_EQ(12U, compressPlan(path, 9.5).poses.size());
EXPECT_EQ(8U, compressPlan(path, 19.9).poses.size());
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

146
navigations/nav_2d_utils/test/param_tests.cpp Executable file
View File

@@ -0,0 +1,146 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* 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 Willow Garage, Inc. 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 OWNER 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.
*
* Author: Dave Hershberger
* David V. Lu!! (nav_2d_utils version)
*********************************************************************/
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <nav_2d_utils/polygons.h>
using nav_2d_utils::polygonFromParams;
using nav_2d_msgs::Polygon2D;
TEST(Polygon2D, unpadded_footprint_from_string_param)
{
ros::NodeHandle nh("~unpadded");
Polygon2D footprint = polygonFromParams(nh, "footprint");
ASSERT_EQ(3U, footprint.points.size());
EXPECT_EQ(1.0f, footprint.points[ 0 ].x);
EXPECT_EQ(1.0f, footprint.points[ 0 ].y);
EXPECT_EQ(-1.0f, footprint.points[ 1 ].x);
EXPECT_EQ(1.0f, footprint.points[ 1 ].y);
EXPECT_EQ(-1.0f, footprint.points[ 2 ].x);
EXPECT_EQ(-1.0f, footprint.points[ 2 ].y);
}
TEST(Polygon2D, check_search_capabilities)
{
ros::NodeHandle nh("~unpadded/unneccessarily/long_namespace");
Polygon2D footprint = polygonFromParams(nh, "footprint");
ASSERT_EQ(3U, footprint.points.size());
EXPECT_THROW(polygonFromParams(nh, "footprint", false), nav_2d_utils::PolygonParseException);
}
TEST(Polygon2D, footprint_from_xmlrpc_param)
{
ros::NodeHandle nh("~xmlrpc");
Polygon2D footprint = polygonFromParams(nh, "footprint");
ASSERT_EQ(4U, footprint.points.size());
EXPECT_FLOAT_EQ(0.1f, footprint.points[ 0 ].x);
EXPECT_FLOAT_EQ(0.1f, footprint.points[ 0 ].y);
EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 1 ].x);
EXPECT_FLOAT_EQ(0.1f, footprint.points[ 1 ].y);
EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 2 ].x);
EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 2 ].y);
EXPECT_FLOAT_EQ(0.1f, footprint.points[ 3 ].x);
EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 3 ].y);
Polygon2D footprint2 = polygonFromParams(nh, "footprint2");
ASSERT_TRUE(nav_2d_utils::equals(footprint, footprint2));
}
TEST(Polygon2D, footprint_from_same_level_param)
{
ros::NodeHandle nh("~same_level");
Polygon2D footprint = polygonFromParams(nh, "footprint");
ASSERT_EQ(3U, footprint.points.size());
EXPECT_EQ(1.0f, footprint.points[ 0 ].x);
EXPECT_EQ(2.0f, footprint.points[ 0 ].y);
EXPECT_EQ(3.0f, footprint.points[ 1 ].x);
EXPECT_EQ(4.0f, footprint.points[ 1 ].y);
EXPECT_EQ(5.0f, footprint.points[ 2 ].x);
EXPECT_EQ(6.0f, footprint.points[ 2 ].y);
}
TEST(Polygon2D, footprint_from_xmlrpc_param_failure)
{
ros::NodeHandle nh("~xmlrpc_fail");
EXPECT_THROW(polygonFromParams(nh, "footprint"), nav_2d_utils::PolygonParseException);
EXPECT_THROW(polygonFromParams(nh, "footprint2"), nav_2d_utils::PolygonParseException);
EXPECT_THROW(polygonFromParams(nh, "footprint3"), nav_2d_utils::PolygonParseException);
EXPECT_THROW(polygonFromParams(nh, "footprint4"), nav_2d_utils::PolygonParseException);
EXPECT_THROW(polygonFromParams(nh, "footprint5"), nav_2d_utils::PolygonParseException);
EXPECT_THROW(polygonFromParams(nh, "footprint6"), nav_2d_utils::PolygonParseException);
EXPECT_THROW(polygonFromParams(nh, "footprint7"), nav_2d_utils::PolygonParseException);
EXPECT_THROW(polygonFromParams(nh, "footprint8"), nav_2d_utils::PolygonParseException);
EXPECT_THROW(polygonFromParams(nh, "footprint9"), nav_2d_utils::PolygonParseException);
}
TEST(Polygon2D, footprint_empty)
{
ros::NodeHandle nh("~empty");
EXPECT_THROW(polygonFromParams(nh, "footprint"), nav_2d_utils::PolygonParseException);
}
TEST(Polygon2D, test_write)
{
ros::NodeHandle nh("~unpadded");
Polygon2D footprint = polygonFromParams(nh, "footprint");
nh.setParam("another_footprint", nav_2d_utils::polygonToXMLRPC(footprint));
Polygon2D another_footprint = polygonFromParams(nh, "another_footprint");
EXPECT_TRUE(nav_2d_utils::equals(footprint, another_footprint));
nh.setParam("third_footprint", nav_2d_utils::polygonToXMLRPC(footprint, false));
another_footprint = polygonFromParams(nh, "third_footprint");
EXPECT_TRUE(nav_2d_utils::equals(footprint, another_footprint));
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "param_tests");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

34
navigations/nav_2d_utils/test/param_tests.launch Executable file
View File

@@ -0,0 +1,34 @@
<launch>
<test time-limit="10" test-name="param_tests" pkg="nav_2d_utils" type="param_tests">
<param name="unpadded/footprint" value="[[1, 1], [-1, 1], [-1, -1]]" />
<rosparam ns="xmlrpc">
footprint: [[0.1, 0.1], [-0.1, 0.1], [-0.1, -0.1], [0.1, -0.1]]
footprint2:
x: [0.1, -0.1, -0.1, 0.1]
y: [0.1, 0.1, -0.1, -0.1]
</rosparam>
<rosparam ns="xmlrpc_fail"> <!-- Footprint includes a 3-value point, which should make it fail. -->
footprint: [[0.1, 0.1], [-0.1, 0.1, 77.0], [-0.1, -0.1], [0.1, -0.1]]
footprint2: 1.0
footprint3: false
footprint4: [[0.1, 0.1], [0.1, 0.1]]
footprint5: ['x', 'y', 'z']
footprint6: [['x', 'y'], ['a', 'b'], ['c'], ['d']]
footprint7:
x: [0.1, -0.1, -0.1, 0.1]
footprint8:
x: 0
y: 0
footprint9:
x: ['a', 'b', 'c']
y: [d, e, f]
</rosparam>
<param name="same_level/footprint" value="[[1, 2], [3, 4], [5, 6]]" />
<param name="empty/empty" value="0" /> <!-- just so you can see there are no real params under "empty". -->
</test>
</launch>

185
navigations/nav_2d_utils/test/polygon_tests.cpp Executable file
View File

@@ -0,0 +1,185 @@
/*
* 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();
}

78
navigations/nav_2d_utils/test/resolution_test.cpp Executable file
View File

@@ -0,0 +1,78 @@
/*
* 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/path_ops.h>
using nav_2d_utils::adjustPlanResolution;
using nav_2d_utils::addPose;
TEST(ResolutionTest, simple_example)
{
nav_2d_msgs::Path2D path;
// Space between points is one meter
addPose(path, 0.0, 0.0);
addPose(path, 0.0, 1.0);
// resolution>=1, path won't change
EXPECT_EQ(2U, adjustPlanResolution(path, 2.0).poses.size());
EXPECT_EQ(2U, adjustPlanResolution(path, 1.0).poses.size());
// 0.5 <= resolution < 1.0, one point should be added in the middle
EXPECT_EQ(3U, adjustPlanResolution(path, 0.8).poses.size());
EXPECT_EQ(3U, adjustPlanResolution(path, 0.5).poses.size());
// 0.333 <= resolution < 0.5, two points need to be added
EXPECT_EQ(4U, adjustPlanResolution(path, 0.34).poses.size());
// 0.25 <= resolution < 0.333, three points need to be added
EXPECT_EQ(5U, adjustPlanResolution(path, 0.32).poses.size());
}
TEST(ResolutionTest, real_example)
{
// This test is based on a real-world example
nav_2d_msgs::Path2D path;
addPose(path, 17.779193, -0.972024);
addPose(path, 17.799171, -0.950775);
addPose(path, 17.851942, -0.903709);
EXPECT_EQ(3U, adjustPlanResolution(path, 0.2).poses.size());
EXPECT_EQ(4U, adjustPlanResolution(path, 0.05).poses.size());
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}