Skip to content

Commit 36a5fb3

Browse files
hakuturu583hakuturu583
authored
Merge pull request #1147 from RobotecAI/feature/test-geometry
Feature/test geometry
2 parents c101075 + 5a50ead commit 36a5fb3

18 files changed

+2503
-517
lines changed

‎common/math/geometry/test/CMakeLists.txt

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,26 @@
1+
add_subdirectory(vector3)
2+
13
ament_add_gtest(test_bounding_box test_bounding_box.cpp)
24
ament_add_gtest(test_catmull_rom_spline test_catmull_rom_spline.cpp)
5+
ament_add_gtest(test_catmull_rom_subspline test_catmull_rom_subspline.cpp)
36
ament_add_gtest(test_collision test_collision.cpp)
47
ament_add_gtest(test_distance test_distance.cpp)
58
ament_add_gtest(test_hermite_curve test_hermite_curve.cpp)
6-
ament_add_gtest(test_line_segment test_line_segment.cpp)
79
ament_add_gtest(test_linear_algebra test_linear_algebra.cpp)
810
ament_add_gtest(test_polygon test_polygon.cpp)
911
ament_add_gtest(test_polynomial_solver test_polynomial_solver.cpp)
12+
ament_add_gtest(test_transform test_transform.cpp)
13+
ament_add_gtest(test_intersection test_intersection.cpp)
14+
ament_add_gtest(test_line_segment test_line_segment.cpp)
1015
target_link_libraries(test_bounding_box geometry)
1116
target_link_libraries(test_catmull_rom_spline geometry)
17+
target_link_libraries(test_catmull_rom_subspline geometry)
1218
target_link_libraries(test_collision geometry)
1319
target_link_libraries(test_distance geometry)
1420
target_link_libraries(test_hermite_curve geometry)
15-
target_link_libraries(test_line_segment geometry)
1621
target_link_libraries(test_linear_algebra geometry)
1722
target_link_libraries(test_polygon geometry)
1823
target_link_libraries(test_polynomial_solver geometry)
24+
target_link_libraries(test_transform geometry)
25+
target_link_libraries(test_intersection geometry)
26+
target_link_libraries(test_line_segment geometry)

‎common/math/geometry/test/expect_eq_macros.hpp

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,21 +28,50 @@
2828
EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \
2929
EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z);
3030

31+
#define EXPECT_BOOST_POINT_2D_AND_POINT_EQ(DATA0, DATA1) \
32+
EXPECT_DOUBLE_EQ(DATA0.x(), DATA1.x); \
33+
EXPECT_DOUBLE_EQ(DATA0.y(), DATA1.y);
34+
35+
#define EXPECT_POINT_NEAR(DATA0, DATA1, TOLERANCE) \
36+
EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \
37+
EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \
38+
EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE);
39+
40+
#define EXPECT_POINT_NAN(DATA) \
41+
EXPECT_TRUE(std::isnan(DATA.x)); \
42+
EXPECT_TRUE(std::isnan(DATA.y)); \
43+
EXPECT_TRUE(std::isnan(DATA.z));
44+
3145
#define EXPECT_VECTOR3_EQ(DATA0, DATA1) \
3246
EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \
3347
EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \
3448
EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z);
3549

50+
#define EXPECT_VECTOR3_NEAR(DATA0, DATA1, TOLERANCE) \
51+
EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \
52+
EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \
53+
EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE);
54+
3655
#define EXPECT_QUATERNION_EQ(DATA0, DATA1) \
3756
EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \
3857
EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \
3958
EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); \
4059
EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w);
4160

61+
#define EXPECT_QUATERNION_NEAR(DATA0, DATA1, TOLERANCE) \
62+
EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \
63+
EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \
64+
EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); \
65+
EXPECT_NEAR(DATA0.w, DATA1.w, TOLERANCE);
66+
4267
#define EXPECT_POSE_EQ(DATA0, DATA1) \
4368
EXPECT_POINT_EQ(DATA0.position, DATA1.position); \
4469
EXPECT_QUATERNION_EQ(DATA0.orientation, DATA1.orientation);
4570

71+
#define EXPECT_POSE_NEAR(DATA0, DATA1, TOLERANCE) \
72+
EXPECT_POINT_NEAR(DATA0.position, DATA1.position, TOLERANCE); \
73+
EXPECT_QUATERNION_NEAR(DATA0.orientation, DATA1.orientation, TOLERANCE);
74+
4675
#define EXPECT_LANELET_POSE_EQ(DATA0, DATA1) \
4776
EXPECT_EQ(DATA0.lanelet_id, DATA1.lanelet_id); \
4877
EXPECT_DOUBLE_EQ(DATA0.s, DATA1.s); \

‎common/math/geometry/test/test_bounding_box.cpp

Lines changed: 97 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -13,40 +13,115 @@
1313
// limitations under the License.
1414

1515
#include <gtest/gtest.h>
16+
#include <quaternion_operation/quaternion_operation.h>
1617

1718
#include <geometry/bounding_box.hpp>
1819
#include <scenario_simulator_exception/exception.hpp>
1920

20-
TEST(BoundingBox, GetPolygonDistanceWithCollision)
21+
#include "expect_eq_macros.hpp"
22+
#include "test_utils.hpp"
23+
24+
TEST(BoundingBox, getPointsFromBboxDefault)
25+
{
26+
geometry_msgs::msg::Pose pose;
27+
traffic_simulator_msgs::msg::BoundingBox bbox;
28+
std::vector<geometry_msgs::msg::Point> points = math::geometry::getPointsFromBbox(bbox);
29+
EXPECT_EQ(points.size(), size_t(4));
30+
EXPECT_POINT_EQ(points[0], makePoint(0.0, 0.0, 0.0));
31+
EXPECT_POINT_EQ(points[1], makePoint(0.0, 0.0, 0.0));
32+
EXPECT_POINT_EQ(points[2], makePoint(0.0, 0.0, 0.0));
33+
EXPECT_POINT_EQ(points[3], makePoint(0.0, 0.0, 0.0));
34+
}
35+
36+
TEST(BoundingBox, getPointsFromBboxCustom)
37+
{
38+
geometry_msgs::msg::Pose pose;
39+
traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(5.0, 2.0, 2.0, 1.0);
40+
std::vector<geometry_msgs::msg::Point> points =
41+
math::geometry::getPointsFromBbox(bbox, 1.0, 2.0, 3.0, 4.0);
42+
EXPECT_EQ(points.size(), size_t(4));
43+
EXPECT_POINT_EQ(points[0], makePoint(6.5, 3.0, 1.0));
44+
EXPECT_POINT_EQ(points[1], makePoint(-5.5, 3.0, 1.0));
45+
EXPECT_POINT_EQ(points[2], makePoint(-5.5, -2.0, 1.0));
46+
EXPECT_POINT_EQ(points[3], makePoint(6.5, -2.0, 1.0));
47+
}
48+
49+
TEST(BoundingBox, get2DPolygonZeroPose)
50+
{
51+
geometry_msgs::msg::Pose pose;
52+
traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0);
53+
boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double>> poly =
54+
math::geometry::get2DPolygon(pose, bbox);
55+
EXPECT_EQ(poly.inners().size(), size_t(0));
56+
EXPECT_EQ(poly.outer().size(), size_t(5));
57+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(1.0, 1.0));
58+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(-1.0, 1.0));
59+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(-1.0, -1.0));
60+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(1.0, -1.0));
61+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(1.0, 1.0));
62+
}
63+
64+
TEST(BoundingBox, get2DPolygonOnlyTranslation)
65+
{
66+
geometry_msgs::msg::Pose pose = makePose(1.0, 2.0);
67+
traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0);
68+
boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double>> poly =
69+
math::geometry::get2DPolygon(pose, bbox);
70+
EXPECT_EQ(poly.inners().size(), size_t(0));
71+
EXPECT_EQ(poly.outer().size(), size_t(5));
72+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(2.0, 3.0));
73+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(0.0, 3.0));
74+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(0.0, 1.0));
75+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(2.0, 1.0));
76+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(2.0, 3.0));
77+
}
78+
79+
TEST(BoundingBox, get2DPolygonFullPose)
80+
{
81+
geometry_msgs::msg::Pose pose = makePose(1.0, 2.0);
82+
pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(
83+
geometry_msgs::build<geometry_msgs::msg::Vector3>().x(0.0).y(0.0).z(30.0 * M_PI / 180.0));
84+
traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0);
85+
boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double>> poly =
86+
math::geometry::get2DPolygon(pose, bbox);
87+
EXPECT_EQ(poly.inners().size(), size_t(0));
88+
EXPECT_EQ(poly.outer().size(), size_t(5));
89+
const double x = std::sqrt(2.0) * std::cos((30.0 + 45.0) * M_PI / 180.0);
90+
const double y = std::sqrt(2.0) * std::sin((30.0 + 45.0) * M_PI / 180.0);
91+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(x + 1.0, y + 2.0));
92+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(-y + 1.0, x + 2.0));
93+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(-x + 1.0, -y + 2.0));
94+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(y + 1.0, -x + 2.0));
95+
EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(x + 1.0, y + 2.0));
96+
}
97+
98+
TEST(BoundingBox, getPolygonDistanceWithCollision)
2199
{
22100
geometry_msgs::msg::Pose pose0;
23-
traffic_simulator_msgs::msg::BoundingBox bbox0;
24-
bbox0.dimensions.x = 3;
25-
bbox0.dimensions.y = 3;
26-
bbox0.dimensions.z = 3;
101+
traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(3.0, 3.0, 3.0);
27102
geometry_msgs::msg::Pose pose1;
28-
traffic_simulator_msgs::msg::BoundingBox bbox1;
29-
bbox1.dimensions.x = 1;
30-
bbox1.dimensions.y = 1;
31-
bbox1.dimensions.z = 1;
103+
traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(1.0, 1.0, 1.0);
32104
EXPECT_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1), std::nullopt);
33105
}
34106

35-
TEST(BoundingBox, GetPolygonDistanceWithoutCollision)
107+
TEST(BoundingBox, getPolygonDistanceTouch)
36108
{
37109
geometry_msgs::msg::Pose pose0;
38-
traffic_simulator_msgs::msg::BoundingBox bbox0;
39-
bbox0.dimensions.x = 3;
40-
bbox0.dimensions.y = 3;
41-
bbox0.dimensions.z = 3;
42-
geometry_msgs::msg::Pose pose1;
43-
pose1.position.y = 5;
44-
traffic_simulator_msgs::msg::BoundingBox bbox1;
45-
bbox1.dimensions.x = 1;
46-
bbox1.dimensions.y = 1;
47-
bbox1.dimensions.z = 1;
48-
EXPECT_TRUE(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1));
49-
EXPECT_DOUBLE_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1).value(), 3.0);
110+
traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(4.0, 4.0, 4.0);
111+
geometry_msgs::msg::Pose pose1 = makePose(3.0, 3.0, 3.0);
112+
traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(2.0, 2.0, 2.0);
113+
EXPECT_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1), std::nullopt);
114+
}
115+
116+
TEST(BoundingBox, getPolygonDistanceWithoutCollision)
117+
{
118+
geometry_msgs::msg::Pose pose0;
119+
traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(3.0, 3.0, 3.0);
120+
geometry_msgs::msg::Pose pose1 = makePose(0.0, 5.0);
121+
traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(1.0, 1.0, 1.0);
122+
const auto ans = math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1);
123+
EXPECT_TRUE(ans);
124+
EXPECT_DOUBLE_EQ(ans.value(), 3.0);
50125
}
51126

52127
int main(int argc, char ** argv)

0 commit comments

Comments
 (0)