diff --git a/common/math/geometry/test/CMakeLists.txt b/common/math/geometry/test/CMakeLists.txt index 416d1d32f35..71741d743ea 100644 --- a/common/math/geometry/test/CMakeLists.txt +++ b/common/math/geometry/test/CMakeLists.txt @@ -1,18 +1,26 @@ +add_subdirectory(vector3) + ament_add_gtest(test_bounding_box test_bounding_box.cpp) ament_add_gtest(test_catmull_rom_spline test_catmull_rom_spline.cpp) +ament_add_gtest(test_catmull_rom_subspline test_catmull_rom_subspline.cpp) ament_add_gtest(test_collision test_collision.cpp) ament_add_gtest(test_distance test_distance.cpp) ament_add_gtest(test_hermite_curve test_hermite_curve.cpp) -ament_add_gtest(test_line_segment test_line_segment.cpp) ament_add_gtest(test_linear_algebra test_linear_algebra.cpp) ament_add_gtest(test_polygon test_polygon.cpp) ament_add_gtest(test_polynomial_solver test_polynomial_solver.cpp) +ament_add_gtest(test_transform test_transform.cpp) +ament_add_gtest(test_intersection test_intersection.cpp) +ament_add_gtest(test_line_segment test_line_segment.cpp) target_link_libraries(test_bounding_box geometry) target_link_libraries(test_catmull_rom_spline geometry) +target_link_libraries(test_catmull_rom_subspline geometry) target_link_libraries(test_collision geometry) target_link_libraries(test_distance geometry) target_link_libraries(test_hermite_curve geometry) -target_link_libraries(test_line_segment geometry) target_link_libraries(test_linear_algebra geometry) target_link_libraries(test_polygon geometry) target_link_libraries(test_polynomial_solver geometry) +target_link_libraries(test_transform geometry) +target_link_libraries(test_intersection geometry) +target_link_libraries(test_line_segment geometry) diff --git a/common/math/geometry/test/expect_eq_macros.hpp b/common/math/geometry/test/expect_eq_macros.hpp index c89552dcd88..5b452f67385 100644 --- a/common/math/geometry/test/expect_eq_macros.hpp +++ b/common/math/geometry/test/expect_eq_macros.hpp @@ -28,21 +28,50 @@ EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); +#define EXPECT_BOOST_POINT_2D_AND_POINT_EQ(DATA0, DATA1) \ + EXPECT_DOUBLE_EQ(DATA0.x(), DATA1.x); \ + EXPECT_DOUBLE_EQ(DATA0.y(), DATA1.y); + +#define EXPECT_POINT_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); + +#define EXPECT_POINT_NAN(DATA) \ + EXPECT_TRUE(std::isnan(DATA.x)); \ + EXPECT_TRUE(std::isnan(DATA.y)); \ + EXPECT_TRUE(std::isnan(DATA.z)); + #define EXPECT_VECTOR3_EQ(DATA0, DATA1) \ EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); +#define EXPECT_VECTOR3_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); + #define EXPECT_QUATERNION_EQ(DATA0, DATA1) \ EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); \ EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w); +#define EXPECT_QUATERNION_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); \ + EXPECT_NEAR(DATA0.w, DATA1.w, TOLERANCE); + #define EXPECT_POSE_EQ(DATA0, DATA1) \ EXPECT_POINT_EQ(DATA0.position, DATA1.position); \ EXPECT_QUATERNION_EQ(DATA0.orientation, DATA1.orientation); +#define EXPECT_POSE_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_POINT_NEAR(DATA0.position, DATA1.position, TOLERANCE); \ + EXPECT_QUATERNION_NEAR(DATA0.orientation, DATA1.orientation, TOLERANCE); + #define EXPECT_LANELET_POSE_EQ(DATA0, DATA1) \ EXPECT_EQ(DATA0.lanelet_id, DATA1.lanelet_id); \ EXPECT_DOUBLE_EQ(DATA0.s, DATA1.s); \ diff --git a/common/math/geometry/test/test_bounding_box.cpp b/common/math/geometry/test/test_bounding_box.cpp index 13243e503ee..5184b787dcc 100644 --- a/common/math/geometry/test/test_bounding_box.cpp +++ b/common/math/geometry/test/test_bounding_box.cpp @@ -13,40 +13,115 @@ // limitations under the License. #include +#include #include #include -TEST(BoundingBox, GetPolygonDistanceWithCollision) +#include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +TEST(BoundingBox, getPointsFromBboxDefault) +{ + geometry_msgs::msg::Pose pose; + traffic_simulator_msgs::msg::BoundingBox bbox; + std::vector points = math::geometry::getPointsFromBbox(bbox); + EXPECT_EQ(points.size(), size_t(4)); + EXPECT_POINT_EQ(points[0], makePoint(0.0, 0.0, 0.0)); + EXPECT_POINT_EQ(points[1], makePoint(0.0, 0.0, 0.0)); + EXPECT_POINT_EQ(points[2], makePoint(0.0, 0.0, 0.0)); + EXPECT_POINT_EQ(points[3], makePoint(0.0, 0.0, 0.0)); +} + +TEST(BoundingBox, getPointsFromBboxCustom) +{ + geometry_msgs::msg::Pose pose; + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(5.0, 2.0, 2.0, 1.0); + std::vector points = + math::geometry::getPointsFromBbox(bbox, 1.0, 2.0, 3.0, 4.0); + EXPECT_EQ(points.size(), size_t(4)); + EXPECT_POINT_EQ(points[0], makePoint(6.5, 3.0, 1.0)); + EXPECT_POINT_EQ(points[1], makePoint(-5.5, 3.0, 1.0)); + EXPECT_POINT_EQ(points[2], makePoint(-5.5, -2.0, 1.0)); + EXPECT_POINT_EQ(points[3], makePoint(6.5, -2.0, 1.0)); +} + +TEST(BoundingBox, get2DPolygonZeroPose) +{ + geometry_msgs::msg::Pose pose; + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0); + boost::geometry::model::polygon> poly = + math::geometry::get2DPolygon(pose, bbox); + EXPECT_EQ(poly.inners().size(), size_t(0)); + EXPECT_EQ(poly.outer().size(), size_t(5)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(1.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(-1.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(-1.0, -1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(1.0, -1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(1.0, 1.0)); +} + +TEST(BoundingBox, get2DPolygonOnlyTranslation) +{ + geometry_msgs::msg::Pose pose = makePose(1.0, 2.0); + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0); + boost::geometry::model::polygon> poly = + math::geometry::get2DPolygon(pose, bbox); + EXPECT_EQ(poly.inners().size(), size_t(0)); + EXPECT_EQ(poly.outer().size(), size_t(5)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(2.0, 3.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(0.0, 3.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(0.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(2.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(2.0, 3.0)); +} + +TEST(BoundingBox, get2DPolygonFullPose) +{ + geometry_msgs::msg::Pose pose = makePose(1.0, 2.0); + pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( + geometry_msgs::build().x(0.0).y(0.0).z(30.0 * M_PI / 180.0)); + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0); + boost::geometry::model::polygon> poly = + math::geometry::get2DPolygon(pose, bbox); + EXPECT_EQ(poly.inners().size(), size_t(0)); + EXPECT_EQ(poly.outer().size(), size_t(5)); + const double x = std::sqrt(2.0) * std::cos((30.0 + 45.0) * M_PI / 180.0); + const double y = std::sqrt(2.0) * std::sin((30.0 + 45.0) * M_PI / 180.0); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(x + 1.0, y + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(-y + 1.0, x + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(-x + 1.0, -y + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(y + 1.0, -x + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(x + 1.0, y + 2.0)); +} + +TEST(BoundingBox, getPolygonDistanceWithCollision) { geometry_msgs::msg::Pose pose0; - traffic_simulator_msgs::msg::BoundingBox bbox0; - bbox0.dimensions.x = 3; - bbox0.dimensions.y = 3; - bbox0.dimensions.z = 3; + traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(3.0, 3.0, 3.0); geometry_msgs::msg::Pose pose1; - traffic_simulator_msgs::msg::BoundingBox bbox1; - bbox1.dimensions.x = 1; - bbox1.dimensions.y = 1; - bbox1.dimensions.z = 1; + traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(1.0, 1.0, 1.0); EXPECT_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1), std::nullopt); } -TEST(BoundingBox, GetPolygonDistanceWithoutCollision) +TEST(BoundingBox, getPolygonDistanceTouch) { geometry_msgs::msg::Pose pose0; - traffic_simulator_msgs::msg::BoundingBox bbox0; - bbox0.dimensions.x = 3; - bbox0.dimensions.y = 3; - bbox0.dimensions.z = 3; - geometry_msgs::msg::Pose pose1; - pose1.position.y = 5; - traffic_simulator_msgs::msg::BoundingBox bbox1; - bbox1.dimensions.x = 1; - bbox1.dimensions.y = 1; - bbox1.dimensions.z = 1; - EXPECT_TRUE(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1)); - EXPECT_DOUBLE_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1).value(), 3.0); + traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(4.0, 4.0, 4.0); + geometry_msgs::msg::Pose pose1 = makePose(3.0, 3.0, 3.0); + traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(2.0, 2.0, 2.0); + EXPECT_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1), std::nullopt); +} + +TEST(BoundingBox, getPolygonDistanceWithoutCollision) +{ + geometry_msgs::msg::Pose pose0; + traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(3.0, 3.0, 3.0); + geometry_msgs::msg::Pose pose1 = makePose(0.0, 5.0); + traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(1.0, 1.0, 1.0); + const auto ans = math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1); + EXPECT_TRUE(ans); + EXPECT_DOUBLE_EQ(ans.value(), 3.0); } int main(int argc, char ** argv) diff --git a/common/math/geometry/test/test_catmull_rom_spline.cpp b/common/math/geometry/test/test_catmull_rom_spline.cpp index dbe97a86955..6245ca1bce8 100644 --- a/common/math/geometry/test/test_catmull_rom_spline.cpp +++ b/common/math/geometry/test/test_catmull_rom_spline.cpp @@ -18,6 +18,46 @@ #include #include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +constexpr double EPS = 1e-6; + +/// @brief Helper function generating line: p(0,0)-> p(1,3) -> p(2,6) +math::geometry::CatmullRomSpline makeLine() +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 3.0), makePoint(2.0, 6.0)}; + return math::geometry::CatmullRomSpline(points); +} + +/// @brief Helper function generating curve: p(0,0)-> p(1,1)-> p(2,0) +math::geometry::CatmullRomSpline makeCurve() +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 1.0), makePoint(2.0, 0.0)}; + return math::geometry::CatmullRomSpline(points); +} + +/// @brief Helper function generating curve: p(0,0)-> p(1,1)-> p(0,2) +math::geometry::CatmullRomSpline makeCurve2() +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 1.0), makePoint(0.0, 2.0)}; + return math::geometry::CatmullRomSpline(points); +} + +/** + * Add an offset to the given point in a specified direction. + * + * @param point The point to which the offset will be added. + * @param offset The value of the offset. + * @param theta The angle in radians representing the direction. + */ +void addOffset(geometry_msgs::msg::Point & point, const double offset, const double theta) +{ + point.x += std::cos(theta) * offset; + point.y += std::sin(theta) * offset; +} /// @brief Testing the `CatmullRomSpline::getCollisionPointIn2D` function works valid. /// In this test case, number of the control points of the catmull-rom spline (variable name `spline`) is 2, so the shape of the value `spline` is line segment. @@ -136,220 +176,359 @@ TEST(CatmullRomSpline, GetCollisionWith1ControlPoint) TEST(CatmullRomSpline, GetCollisionPointIn2D) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - geometry_msgs::msg::Point p2; - p2.x = 2; - auto points = {p0, p1, p2}; - auto spline = math::geometry::CatmullRomSpline(points); - EXPECT_DOUBLE_EQ(spline.getLength(), 2); - geometry_msgs::msg::Point start; - start.x = 0.1; - start.y = 1.0; - geometry_msgs::msg::Point goal; - goal.x = 0.1; - goal.y = -1.0; - auto collision_s = spline.getCollisionPointIn2D(start, goal, false); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.1); + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 0.0), makePoint(2.0, 0.0)}; + const math::geometry::CatmullRomSpline spline(points); + + EXPECT_NEAR(spline.getLength(), 2.0, EPS); + geometry_msgs::msg::Point start = makePoint(0.1, 1.0), goal = makePoint(0.1, -1.0); + + const auto collision_s0 = spline.getCollisionPointIn2D(start, goal, false); + EXPECT_TRUE(collision_s0); + if (collision_s0) { + EXPECT_NEAR(collision_s0.value(), 0.1, EPS); } - collision_s = spline.getCollisionPointIn2D({start, goal}, false); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.1); + const auto collision_s1 = spline.getCollisionPointIn2D({start, goal}, false); + EXPECT_TRUE(collision_s1); + if (collision_s1) { + EXPECT_NEAR(collision_s1.value(), 0.1, EPS); } - collision_s = spline.getCollisionPointIn2D(start, goal, true); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.1); + const auto collision_s2 = spline.getCollisionPointIn2D(start, goal, true); + EXPECT_TRUE(collision_s2); + if (collision_s2) { + EXPECT_NEAR(collision_s2.value(), 0.1, EPS); } - collision_s = spline.getCollisionPointIn2D({start, goal}, true); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.1); + const auto collision_s3 = spline.getCollisionPointIn2D({start, goal}, true); + EXPECT_TRUE(collision_s3); + if (collision_s3) { + EXPECT_NEAR(collision_s3.value(), 0.1, EPS); } } -TEST(CatmullRomSpline, Maximum2DCurvature) +TEST(CatmullRomSpline, getCollisionPointIn2DNoCollision) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - p1.y = 3; - geometry_msgs::msg::Point p2; - p2.x = 2; - p2.y = 6; - auto points = {p0, p1, p2}; + const math::geometry::CatmullRomSpline spline = makeCurve(); + + const geometry_msgs::msg::Point start = makePoint(0.5, 0.0), goal = makePoint(1.5, 0.0); + + const auto collision_s0 = spline.getCollisionPointIn2D(start, goal, false); + EXPECT_FALSE(collision_s0); + + const auto collision_s1 = spline.getCollisionPointIn2D({start, goal}, false); + EXPECT_FALSE(collision_s1); + + const auto collision_s2 = spline.getCollisionPointIn2D(start, goal, true); + EXPECT_FALSE(collision_s2); + + const auto collision_s3 = spline.getCollisionPointIn2D({start, goal}, true); + EXPECT_FALSE(collision_s3); +} + +TEST(CatmullRomSpline, getCollisionPointIn2DEmpty) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + const std::vector polygon; + + EXPECT_THROW(spline.getCollisionPointIn2D(polygon), common::SimulationError); + EXPECT_THROW(spline.getCollisionPointIn2D(polygon, true), common::SimulationError); +} + +TEST(CatmullRomSpline, getMaximum2DCurvatureLine) +{ + const math::geometry::CatmullRomSpline spline = makeLine(); + EXPECT_DOUBLE_EQ(spline.getMaximum2DCurvature(), 0.0); +} + +TEST(CatmullRomSpline, getPolygon) +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 0.0), makePoint(2.0, 0.0), makePoint(3.0, 0.0), + makePoint(4.0, 0.0)}; + math::geometry::CatmullRomSpline spline(points); + std::vector polygon = spline.getPolygon(1.0, 4); + + EXPECT_EQ(polygon.size(), static_cast(24)); + // pair of triangles + EXPECT_POINT_NEAR(polygon[0], makePoint(0.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[1], makePoint(0.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[2], makePoint(1.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[3], makePoint(0.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[4], makePoint(1.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[5], makePoint(1.0, 0.5), EPS); + // pair of triangles + EXPECT_POINT_NEAR(polygon[6], makePoint(1.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[7], makePoint(1.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[8], makePoint(2.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[9], makePoint(1.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[10], makePoint(2.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[11], makePoint(2.0, 0.5), EPS); + // pair of triangles + EXPECT_POINT_NEAR(polygon[12], makePoint(2.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[13], makePoint(2.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[14], makePoint(3.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[15], makePoint(2.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[16], makePoint(3.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[17], makePoint(3.0, 0.5), EPS); + // pair of triangles + EXPECT_POINT_NEAR(polygon[18], makePoint(3.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[19], makePoint(3.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[20], makePoint(4.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[21], makePoint(3.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[22], makePoint(4.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[23], makePoint(4.0, 0.5), EPS); +} + +TEST(CatmullRomSpline, initializationLine) +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 3.0), makePoint(2.0, 6.0)}; + EXPECT_NO_THROW(auto spline = math::geometry::CatmullRomSpline(points)); auto spline = math::geometry::CatmullRomSpline(points); - EXPECT_DOUBLE_EQ(spline.getMaximum2DCurvature(), 0); + EXPECT_DOUBLE_EQ(spline.getMaximum2DCurvature(), 0.0); } -TEST(CatmullRomSpline, Interpolate3Points) +TEST(CatmullRomSpline, initializationCurve) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - p1.y = 3; - geometry_msgs::msg::Point p2; - p2.x = 2; - p2.y = 5; - geometry_msgs::msg::Point p3; - p3.x = 4; - p3.y = 6; - auto points = {p0, p1, p2, p3}; + const std::vector points{ + + makePoint(0.0, 0.0), makePoint(1.0, 3.0), makePoint(2.0, 5.0), makePoint(4.0, 6.0), + makePoint(4.0, 1.0)}; EXPECT_NO_THROW(auto spline = math::geometry::CatmullRomSpline(points)); } -TEST(CatmullRomSpline, Interpolate4Points) +TEST(CatmullRomSpline, getLengthLine) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - p1.y = 3; - geometry_msgs::msg::Point p2; - p2.x = 2; - p2.y = 5; - geometry_msgs::msg::Point p3; - p3.x = 4; - p3.y = 6; - geometry_msgs::msg::Point p4; - p4.x = 4; - p4.y = 10; - auto points = {p0, p1, p2, p3, p4}; - EXPECT_NO_THROW(auto spline = math::geometry::CatmullRomSpline(points)); + const math::geometry::CatmullRomSpline spline = makeLine(); + EXPECT_NEAR(spline.getLength(), std::hypot(2.0, 6.0), EPS); } -TEST(CatmullRomSpline, GetPoint) +TEST(CatmullRomSpline, getLengthCurve) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - geometry_msgs::msg::Point p2; - p2.x = 2; - geometry_msgs::msg::Point p3; - p3.x = 3; - geometry_msgs::msg::Point p4; - p4.x = 4; - auto points = {p0, p1, p2, p3, p4}; - auto spline = math::geometry::CatmullRomSpline(points); - EXPECT_DOUBLE_EQ(spline.getLength(), 4); - auto point = spline.getPoint(3); - EXPECT_DOUBLE_EQ(point.x, 3); - EXPECT_DOUBLE_EQ(point.y, 0); - EXPECT_DOUBLE_EQ(point.z, 0); + const math::geometry::CatmullRomSpline spline = makeCurve(); + constexpr double eps = 0.1; + EXPECT_NEAR(spline.getLength(), 3.0, eps); } -TEST(CatmullRomSpline, GetSValue) +TEST(CatmullRomSpline, getPointLine) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - geometry_msgs::msg::Point p2; - p2.x = 2; - geometry_msgs::msg::Point p3; - p3.x = 4; - auto points = {p0, p1, p2, p3}; - auto spline = math::geometry::CatmullRomSpline(points); - geometry_msgs::msg::Pose p; - p.position.x = 0.1; - p.position.y = 0; - p.position.z = 0; - const auto result = spline.getSValue(p); + const math::geometry::CatmullRomSpline spline = makeLine(); + auto point = spline.getPoint(std::hypot(1.5, 4.5)); + EXPECT_POINT_NEAR(point, makePoint(1.5, 4.5), EPS); +} + +TEST(CatmullRomSpline, getPointCurve) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto point = spline.getPoint(1.5); + constexpr double eps = 0.15; + EXPECT_POINT_NEAR(point, makePoint(1.0, 1.0), eps); +} + +TEST(CatmullRomSpline, getTangentVectorLine) +{ + const math::geometry::CatmullRomSpline spline = makeLine(); + const auto vec = spline.getTangentVector(1.0); + const double norm_vec = std::hypot(vec.x, vec.y, vec.z); + const auto ans = makeVector(1.5, 4.5); + const double norm_ans = std::hypot(ans.x, ans.y, ans.z); + EXPECT_NEAR(vec.x / norm_vec, ans.x / norm_ans, EPS); + EXPECT_NEAR(vec.y / norm_vec, ans.y / norm_ans, EPS); + EXPECT_NEAR(vec.z / norm_vec, ans.z / norm_ans, EPS); +} + +TEST(CatmullRomSpline, getTangentVectorCurve) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto vec = spline.getTangentVector(1.5); + const double norm_vec = std::hypot(vec.x, vec.y, vec.z); + const auto ans = makeVector(1.0, 0.0); + constexpr double eps = 0.15; + EXPECT_NEAR(vec.x / norm_vec, ans.x, eps); + EXPECT_NEAR(vec.y / norm_vec, ans.y, eps); + EXPECT_NEAR(vec.z / norm_vec, ans.z, eps); +} + +TEST(CatmullRomSpline, getNormalVectorLine) +{ + const math::geometry::CatmullRomSpline spline = makeLine(); + const auto vec = spline.getNormalVector(1.0); + const double norm_vec = std::hypot(vec.x, vec.y, vec.z); + const auto ans = makeVector(-4.5, 1.5); + const double norm_ans = std::hypot(ans.x, ans.y, ans.z); + EXPECT_NEAR(vec.x / norm_vec, ans.x / norm_ans, EPS); + EXPECT_NEAR(vec.y / norm_vec, ans.y / norm_ans, EPS); + EXPECT_NEAR(vec.z / norm_vec, ans.z / norm_ans, EPS); +} + +TEST(CatmullRomSpline, getNormalVectorCurve) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto vec = spline.getNormalVector(1.5); + const double norm_vec = std::hypot(vec.x, vec.y, vec.z); + const auto ans = makeVector(0.0, 1.0); + constexpr double eps = 0.15; + EXPECT_NEAR(vec.x / norm_vec, ans.x, eps); + EXPECT_NEAR(vec.y / norm_vec, ans.y, eps); + EXPECT_NEAR(vec.z / norm_vec, ans.z, eps); +} + +TEST(CatmullRomSpline, getPoseLine) +{ + const math::geometry::CatmullRomSpline spline = makeLine(); + const auto pose = spline.getPose(std::hypot(1.5, 4.5)); + EXPECT_POINT_NEAR(pose.position, makePoint(1.5, 4.5), EPS); + EXPECT_QUATERNION_NEAR( + pose.orientation, + quaternion_operation::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, std::atan(3.0))), EPS); +} + +TEST(CatmullRomSpline, getPoseCurve) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto pose = spline.getPose(1.5); + constexpr double eps = 0.02; + EXPECT_POINT_NEAR(pose.position, makePoint(1.0, 1.0), eps); + EXPECT_QUATERNION_NEAR(pose.orientation, geometry_msgs::msg::Quaternion(), eps); +} + +TEST(CatmullRomSpline, getSValue1) +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 0.0), makePoint(2.0, 0.0), makePoint(4.0, 0.0)}; + const auto spline = math::geometry::CatmullRomSpline(points); + const auto result = spline.getSValue(makePose(0.1, 0.0)); EXPECT_TRUE(result); - EXPECT_TRUE(result.value() > 0.099); - EXPECT_TRUE(result.value() < 0.101); - p.position.x = 10; - p.position.y = 0; - p.position.z = 0; - EXPECT_FALSE(spline.getSValue(p, 3)); + EXPECT_NEAR(result.value(), 0.1, EPS); + EXPECT_FALSE(spline.getSValue(makePose(10.0, 0.0), 3.0)); } -TEST(CatmullRomSpline, GetSValue2) +TEST(CatmullRomSpline, getSValue2) { - geometry_msgs::msg::Point p0; - p0.x = 89122.2; - p0.y = 43364.1; - p0.z = 3.13364; - geometry_msgs::msg::Point p1; - p1.x = 89122.5; - p1.y = 43363.8; - p1.z = 3.13364; - geometry_msgs::msg::Point p2; - p2.x = 89122.8; - p2.y = 43363.4; - p2.z = 3.13364; - geometry_msgs::msg::Point p3; - p3.x = 89123.1; - p3.y = 43363.0; - p3.z = 3.13364; - geometry_msgs::msg::Point p4; - p4.x = 89123.4; - p4.y = 43362.6; - p4.z = 3.13364; - auto points = {p0, p1, p2, p3, p4}; + std::vector points{ + makePoint(89122.2, 43364.1, 3.13364), makePoint(89122.5, 43363.8, 3.13364), + makePoint(89122.8, 43363.4, 3.13364), makePoint(89123.1, 43363.0, 3.13364), + makePoint(89123.4, 43362.6, 3.13364)}; auto spline = math::geometry::CatmullRomSpline(points); - geometry_msgs::msg::Pose p; - p.position.x = 89122.8; - p.position.y = 43363.4; - p.position.z = 3.13364; - p.orientation.x = -0.0159808; - p.orientation.y = -0.00566353; - p.orientation.z = -0.453507; - p.orientation.w = 0.891092; - { - const auto result = spline.getSValue(p); - EXPECT_TRUE(result); - if (result) { - EXPECT_DOUBLE_EQ(result.value(), 0.92433178422155371); - } - } - p.position.x = 89122.5; - p.position.y = 43363.8; - p.position.z = 3.13364; - p.orientation.x = 0.0159365; - p.orientation.y = -0.00578704; - p.orientation.z = -0.446597; - p.orientation.w = 0.894575; - { - const auto result = spline.getSValue(p); - EXPECT_TRUE(result); - if (result) { - EXPECT_DOUBLE_EQ(result.value(), 0.42440442127906564); - } - } + + geometry_msgs::msg::Pose pose0; + pose0.position.x = 89122.8; + pose0.position.y = 43363.4; + pose0.position.z = 3.13364; + pose0.orientation.x = -0.0159808; + pose0.orientation.y = -0.00566353; + pose0.orientation.z = -0.453507; + pose0.orientation.w = 0.891092; + const auto result0 = spline.getSValue(pose0); + EXPECT_TRUE(result0); + EXPECT_DOUBLE_EQ(result0.value(), 0.92433178422155371); + + geometry_msgs::msg::Pose pose1; + pose1.position.x = 89122.5; + pose1.position.y = 43363.8; + pose1.position.z = 3.13364; + pose1.orientation.x = 0.0159365; + pose1.orientation.y = -0.00578704; + pose1.orientation.z = -0.446597; + pose1.orientation.w = 0.894575; + const auto result1 = spline.getSValue(pose1); + EXPECT_TRUE(result1); + EXPECT_DOUBLE_EQ(result1.value(), 0.42440442127906564); } -TEST(CatmullRomSpline, GetTrajectory) +TEST(CatmullRomSpline, getSValueEdge) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - geometry_msgs::msg::Point p2; - p2.x = 2; - geometry_msgs::msg::Point p3; - p3.x = 3; - auto points = {p0, p1, p2, p3}; - auto spline = math::geometry::CatmullRomSpline(points); - auto trajectory = spline.getTrajectory(0, 3, 1.0); - EXPECT_EQ(trajectory.size(), static_cast(4)); - EXPECT_DECIMAL_EQ(trajectory[0].x, 0, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[1].x, 1, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[2].x, 2, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[3].x, 3, 0.00001); - trajectory = spline.getTrajectory(3, 0, 1.0); + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto pose = makePose(-2.0, -2.0); + EXPECT_FALSE(spline.getSValue(pose, 1.0)); +} + +TEST(CatmullRomSpline, getSquaredDistanceIn2D) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + const auto distance = spline.getSquaredDistanceIn2D(makePoint(2.0, 2.0), 1.47895776); + EXPECT_NEAR(distance, 2.0, 1e-2); +} + +TEST(CatmullRomSpline, getSquaredDistanceIn2DSamePoint) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + const auto distance = spline.getSquaredDistanceIn2D(makePoint(1.0, 1.0), 1.47895776); + EXPECT_NEAR(distance, 0.0, 1e-2); +} + +TEST(CatmullRomSpline, getSquaredDistanceVector) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + const auto vector = spline.getSquaredDistanceVector(makePoint(2.0, 2.0), 1.47895776); + EXPECT_POINT_NEAR(vector, makeVector(1.0, 1.0), 1e-2); +} + +TEST(CatmullRomSpline, getSquaredDistanceVectorSamePoint) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + const auto vector = spline.getSquaredDistanceVector(makePoint(1.0, 1.0), 1.47895776); + EXPECT_POINT_NEAR(vector, makeVector(0.0, 0.0), 1e-2); +} + +TEST(CatmullRomSpline, getTrajectoryLine) +{ + std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 0.0), makePoint(2.0, 0.0), makePoint(3.0, 0.0)}; + const auto spline = math::geometry::CatmullRomSpline(points); + + const auto trajectory = spline.getTrajectory(0.0, 3.0, 1.0); EXPECT_EQ(trajectory.size(), static_cast(4)); - EXPECT_DECIMAL_EQ(trajectory[0].x, 3, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[1].x, 2, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[2].x, 1, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[3].x, 0, 0.00001); + EXPECT_POINT_NEAR(trajectory[0], makePoint(0.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory[1], makePoint(1.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory[2], makePoint(2.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory[3], makePoint(3.0, 0.0), EPS); + + const auto trajectory_reverse = spline.getTrajectory(3.0, 0.0, 1.0); + EXPECT_EQ(trajectory_reverse.size(), static_cast(4)); + EXPECT_POINT_NEAR(trajectory_reverse[0], makePoint(3.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory_reverse[1], makePoint(2.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory_reverse[2], makePoint(1.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory_reverse[3], makePoint(0.0, 0.0), EPS); +} + +TEST(CatmullRomSpline, getTrajectoryCurve) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + + const auto trajectory = spline.getTrajectory(0.0, 2.957916, 0.5); + EXPECT_POINT_NEAR(trajectory[0], makePoint(0.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory[1], makePoint(0.559992, 0.336669), EPS); + EXPECT_POINT_NEAR(trajectory[2], makePoint(0.893292, 0.673338), EPS); + EXPECT_POINT_NEAR(trajectory[3], makePoint(0.999898, 1.010091), EPS); + EXPECT_POINT_NEAR(trajectory[4], makePoint(0.87779, 1.349586), EPS); + EXPECT_POINT_NEAR(trajectory[5], makePoint(0.525168, 1.68908), EPS); + EXPECT_POINT_NEAR(trajectory[6], makePoint(0.0, 2.0), EPS); + + const auto trajectory_offset = spline.getTrajectory(0.0, 2.957916, 0.5, -1.0); + EXPECT_POINT_NEAR(trajectory_offset[0], makePoint(0.447214, -0.894427), EPS); + EXPECT_POINT_NEAR(trajectory_offset[1], makePoint(1.161918, -0.461883), EPS); + EXPECT_POINT_NEAR(trajectory_offset[2], makePoint(1.730462, 0.126395), EPS); + EXPECT_POINT_NEAR(trajectory_offset[3], makePoint(1.999695, 1.030269), EPS); + EXPECT_POINT_NEAR(trajectory_offset[4], makePoint(1.697341, 1.922592), EPS); + EXPECT_POINT_NEAR(trajectory_offset[5], makePoint(1.112457, 2.498458), EPS); + EXPECT_POINT_NEAR(trajectory_offset[6], makePoint(0.447213, 2.894428), EPS); +} + +TEST(CatmullRomSpline, getTrajectoryEmpty) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + const auto trajectory = spline.getTrajectory(0.0, 1.47895776, 2.0); + constexpr double eps = 1e-2; + EXPECT_EQ(trajectory.size(), static_cast(2)); + EXPECT_POINT_NEAR(trajectory[0], makePoint(0.0, 0.0), eps); + EXPECT_POINT_NEAR(trajectory[1], makePoint(1.0, 1.0), eps); } -TEST(CatmullRomSpline, CheckThrowingErrorWhenTheControlPointsAreNotEnough) +TEST(CatmullRomSpline, initializationNotEnoughControlPoints) { - EXPECT_THROW( - math::geometry::CatmullRomSpline(std::vector(0)), - common::SemanticError); + const std::vector points; + EXPECT_THROW(math::geometry::CatmullRomSpline{points}, common::SemanticError); } int main(int argc, char ** argv) diff --git a/common/math/geometry/test/test_catmull_rom_subspline.cpp b/common/math/geometry/test/test_catmull_rom_subspline.cpp new file mode 100644 index 00000000000..ab298e78f90 --- /dev/null +++ b/common/math/geometry/test/test_catmull_rom_subspline.cpp @@ -0,0 +1,111 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +constexpr double EPS = 1e-6; + +/// @brief Helper function generating line: p(0,0)-> p(1,3) -> p(2,6) +std::shared_ptr makeLine() +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 3.0), makePoint(2.0, 6.0)}; + return std::make_shared(points); +} + +TEST(CatmullRomSubspline, getLength) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline0( + spline_ptr, std::hypot(0.5, 1.5), std::hypot(1.5, 4.5)); + EXPECT_DOUBLE_EQ(spline0.getLength(), std::hypot(1.0, 3.0)); + + math::geometry::CatmullRomSubspline spline1( + spline_ptr, std::hypot(0.5, 1.5), std::hypot(2.0, 6.0)); + EXPECT_DOUBLE_EQ(spline1.getLength(), std::hypot(1.5, 4.5)); +} + +TEST(CatmullRomSubspline, getLength_zero) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline0(spline_ptr, 0.0, 0.0); + EXPECT_DOUBLE_EQ(spline0.getLength(), 0.0); + + math::geometry::CatmullRomSubspline spline1(spline_ptr, 5.0, 5.0); + EXPECT_DOUBLE_EQ(spline1.getLength(), 0.0); +} + +TEST(CatmullRomSubspline, getCollisionPointIn2D_edge) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline( + spline_ptr, std::hypot(0.5, 1.5), std::hypot(1.5, 4.5)); + + std::vector polygon{ + makePoint(-2.0, -2.0), makePoint(-2.0, -1.0), makePoint(-1.0, -1.0), makePoint(-1.0, -2.0)}; + + const auto ans0 = spline.getCollisionPointIn2D(polygon); + EXPECT_FALSE(ans0); + const auto ans1 = spline.getCollisionPointIn2D(polygon, true); + EXPECT_FALSE(ans1); +} + +TEST(CatmullRomSubspline, getCollisionPointIn2D_base) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline( + spline_ptr, std::hypot(0.5, 1.5), std::hypot(1.5, 4.5)); + + std::vector polygon{ + makePoint(0.0, 0.0), makePoint(0.0, 1.0), makePoint(1.0, 1.0), makePoint(1.0, 0.0)}; + + const auto ans0 = spline.getCollisionPointIn2D(polygon); + EXPECT_FALSE(ans0); + const auto ans1 = spline.getCollisionPointIn2D(polygon, true); + EXPECT_FALSE(ans1); +} + +TEST(CatmullRomSubspline, getCollisionPointIn2D_wrongPolygon) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline( + spline_ptr, std::hypot(0.5, 1.5), std::hypot(1.5, 4.5)); + + std::vector polygon0; + + EXPECT_THROW(spline.getCollisionPointIn2D(polygon0), common::SimulationError); + EXPECT_THROW(spline.getCollisionPointIn2D(polygon0, true), common::SimulationError); + + std::vector polygon1(1); + + EXPECT_THROW(spline.getCollisionPointIn2D(polygon1), common::SimulationError); + EXPECT_THROW(spline.getCollisionPointIn2D(polygon1, true), common::SimulationError); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/math/geometry/test/test_collision.cpp b/common/math/geometry/test/test_collision.cpp index eeb6bba08ea..cf60a70eba2 100644 --- a/common/math/geometry/test/test_collision.cpp +++ b/common/math/geometry/test/test_collision.cpp @@ -17,15 +17,13 @@ #include #include +#include "test_utils.hpp" + TEST(Collision, DifferentHeight) { geometry_msgs::msg::Pose pose0; - geometry_msgs::msg::Pose pose1; - traffic_simulator_msgs::msg::BoundingBox box; - box.dimensions.x = 1.0; - box.dimensions.y = 1.0; - box.dimensions.z = 1.0; - pose1.position.z = 30.0; + geometry_msgs::msg::Pose pose1 = makePose(0.0, 0.0, 30.0); + traffic_simulator_msgs::msg::BoundingBox box = makeBbox(1.0, 1.0, 1.0); EXPECT_FALSE(math::geometry::checkCollision2D(pose0, box, pose1, box)); } @@ -33,25 +31,67 @@ TEST(Collision, SamePosition) { geometry_msgs::msg::Pose pose0; geometry_msgs::msg::Pose pose1; - traffic_simulator_msgs::msg::BoundingBox box; - box.dimensions.x = 1.0; - box.dimensions.y = 1.0; - box.dimensions.z = 1.0; + traffic_simulator_msgs::msg::BoundingBox box = makeBbox(1.0, 1.0, 1.0); EXPECT_TRUE(math::geometry::checkCollision2D(pose0, box, pose1, box)); } TEST(Collision, SameHeightNoCollision) { - geometry_msgs::msg::Pose pose0; + geometry_msgs::msg::Pose pose0 = makePose(0.0, 0.0, 30.0); geometry_msgs::msg::Pose pose1; - traffic_simulator_msgs::msg::BoundingBox box; - box.dimensions.x = 1.0; - box.dimensions.y = 1.0; - box.dimensions.z = 1.0; - pose0.position.x = 30; + traffic_simulator_msgs::msg::BoundingBox box = makeBbox(1.0, 1.0, 1.0); EXPECT_FALSE(math::geometry::checkCollision2D(pose0, box, pose1, box)); } +TEST(Collision, Touching) +{ + geometry_msgs::msg::Pose pose0; + geometry_msgs::msg::Pose pose1 = makePose(1.0, 1.0, 1.0); + traffic_simulator_msgs::msg::BoundingBox box = makeBbox(1.0, 1.0, 1.0); + EXPECT_TRUE(math::geometry::checkCollision2D(pose0, box, pose1, box)); +} + +TEST(Collision, PointInside) +{ + std::vector polygon(4); + polygon[1] = makePoint(1.0, 0.0); + polygon[2] = makePoint(1.0, 1.0); + polygon[3] = makePoint(0.0, 1.0); + geometry_msgs::msg::Point point = makePoint(0.5, 0.5); + EXPECT_TRUE(math::geometry::contains(polygon, point)); +} + +TEST(Collision, PointOutside) +{ + std::vector polygon(4); + polygon[1] = makePoint(1.0, 0.0); + polygon[2] = makePoint(1.0, 1.0); + polygon[3] = makePoint(0.0, 1.0); + geometry_msgs::msg::Point point = makePoint(1.5, 0.5); + EXPECT_FALSE(math::geometry::contains(polygon, point)); +} + +TEST(Collision, Line) +{ + std::vector polygon(2); + polygon[1] = makePoint(1.0, 1.0); + geometry_msgs::msg::Point point = makePoint(0.5, 0.5); + + bool ans = true; + EXPECT_NO_THROW(ans = math::geometry::contains(polygon, point)); + EXPECT_FALSE(ans); +} + +TEST(Collision, EmptyVector) +{ + std::vector polygon; + geometry_msgs::msg::Point point = makePoint(0.5, 0.5); + + bool ans = true; + EXPECT_NO_THROW(ans = math::geometry::contains(polygon, point)); + EXPECT_FALSE(ans); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/common/math/geometry/test/test_distance.cpp b/common/math/geometry/test/test_distance.cpp index 585bbc18703..05e304dbea8 100644 --- a/common/math/geometry/test/test_distance.cpp +++ b/common/math/geometry/test/test_distance.cpp @@ -18,46 +18,101 @@ #include #include +#include "test_utils.hpp" + TEST(Distance, PointToPoint) { - geometry_msgs::msg::Point p0, p1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 0); - p1.x = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 1); - p0.y = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), std::sqrt(2)); + geometry_msgs::msg::Point p00, p01; + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p00, p01), 0.0); + + geometry_msgs::msg::Point p10, p11 = makePoint(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p10, p11), 1.0); + + geometry_msgs::msg::Point p20 = makePoint(0.0, 1.0); + geometry_msgs::msg::Point p21 = makePoint(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p20, p21), std::sqrt(2.0)); } TEST(Distance, PoseToPose) { - geometry_msgs::msg::Pose p0, p1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 0); - p1.position.x = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 1); - p0.position.y = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), std::sqrt(2)); + geometry_msgs::msg::Pose p00, p01; + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p00, p01), 0.0); + + geometry_msgs::msg::Pose p10, p11 = makePose(1, 0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p10, p11), 1.0); + + geometry_msgs::msg::Pose p20 = makePose(0.0, 1.0); + geometry_msgs::msg::Pose p21 = makePose(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p20, p21), std::sqrt(2.0)); } TEST(Distance, PointToPose) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Pose p1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 0); - p1.position.x = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 1); - p0.y = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), std::sqrt(2)); + geometry_msgs::msg::Point p00; + geometry_msgs::msg::Pose p01; + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p00, p01), 0.0); + + geometry_msgs::msg::Point p10; + geometry_msgs::msg::Pose p11 = makePose(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p10, p11), 1.0); + + geometry_msgs::msg::Point p20 = makePoint(0.0, 1.0); + geometry_msgs::msg::Pose p21 = makePose(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p20, p21), std::sqrt(2.0)); } TEST(Distance, PoseToPoint) { - geometry_msgs::msg::Pose p0; - geometry_msgs::msg::Point p1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 0); - p1.x = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 1); - p0.position.y = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), std::sqrt(2)); + geometry_msgs::msg::Pose p00; + geometry_msgs::msg::Point p01; + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p00, p01), 0.0); + + geometry_msgs::msg::Pose p10; + geometry_msgs::msg::Point p11 = makePoint(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p10, p11), 1.0); + + geometry_msgs::msg::Pose p20 = makePose(0.0, 1.0); + geometry_msgs::msg::Point p21 = makePoint(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p20, p21), std::sqrt(2.0)); +} + +TEST(Distance, Distance2DDisjoint) +{ + std::vector polygon0(4), polygon1(4); + polygon0[0] = makePoint(0.0, 0.0); + polygon0[1] = makePoint(0.0, 1.0); + polygon0[2] = makePoint(1.0, 1.0); + polygon0[3] = makePoint(1.0, 0.0); + polygon1[0] = makePoint(2.0, 2.0); + polygon1[1] = makePoint(2.0, 3.0); + polygon1[2] = makePoint(3.0, 3.0); + polygon1[3] = makePoint(3.0, 2.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance2D(polygon0, polygon1), std::sqrt(2.0)); +} + +TEST(Distance, Distance2DIntersect) +{ + std::vector polygon0(4), polygon1(4); + polygon0[0] = makePoint(0.0, 0.0); + polygon0[1] = makePoint(0.0, 2.0); + polygon0[2] = makePoint(2.0, 2.0); + polygon0[3] = makePoint(2.0, 0.0); + polygon1[0] = makePoint(1.0, 1.0); + polygon1[1] = makePoint(1.0, 3.0); + polygon1[2] = makePoint(3.0, 3.0); + polygon1[3] = makePoint(3.0, 1.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance2D(polygon0, polygon1), 0.0); +} + +TEST(Distance, Distance2DZeroElements) +{ + std::vector polygon0(4), polygon1; + polygon0[0] = makePoint(0.0, 0.0); + polygon0[1] = makePoint(0.0, 1.0); + polygon0[2] = makePoint(1.0, 1.0); + polygon0[3] = makePoint(1.0, 0.0); + EXPECT_THROW( + math::geometry::getDistance2D(polygon0, polygon1), boost::geometry::empty_input_exception); } int main(int argc, char ** argv) diff --git a/common/math/geometry/test/test_hermite_curve.cpp b/common/math/geometry/test/test_hermite_curve.cpp index f8a3eccfdaf..c5360a7852b 100644 --- a/common/math/geometry/test/test_hermite_curve.cpp +++ b/common/math/geometry/test/test_hermite_curve.cpp @@ -16,152 +16,575 @@ #include -TEST(HermiteCurveTest, CheckCollisionToLine) +#include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +constexpr double EPS = 1e-3; + +/// @brief Helper function generating straight line: p(0,0) v(1,0)-> p(1,0) v(1,0) +math::geometry::HermiteCurve makeLine1() { - geometry_msgs::msg::Pose start_pose, goal_pose; - geometry_msgs::msg::Vector3 start_vec, goal_vec; - goal_pose.position.x = 1; - start_vec.x = 1; - goal_vec.x = 1; - math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); - EXPECT_DOUBLE_EQ(curve.getLength(), 1); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.1, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.2, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.3, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.4, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.5, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.6, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.7, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.8, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.9, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(1.0, true), 0); - EXPECT_DOUBLE_EQ(curve.getPoint(0.5, false).x, 0.5); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x, 1); - EXPECT_DOUBLE_EQ(curve.getMaximum2DCurvature(), 0); - geometry_msgs::msg::Pose p; - p.position.x = 0.1; - p.position.y = 0; - p.position.z = 0; - EXPECT_TRUE(curve.getSValue(p, 1, true)); - EXPECT_TRUE( - (curve.getSValue(p, 1, true).value() > 0.099) && (curve.getSValue(p, 1, true).value() < 0.101)); - { - geometry_msgs::msg::Point start; - start.x = 0.1; - start.y = 1.0; - geometry_msgs::msg::Point goal; - goal.x = 0.1; - goal.y = -1.0; - auto collision_s = curve.getCollisionPointIn2D(start, goal); - EXPECT_FALSE(curve.getCollisionPointIn2D({})); - EXPECT_FALSE(curve.getCollisionPointIn2D({start})); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.1); - } - } - { - geometry_msgs::msg::Point start; - start.x = 0.1; - start.y = 1.0; - geometry_msgs::msg::Point goal; - goal.x = 0.2; - goal.y = -1.0; - auto collision_s = curve.getCollisionPointIn2D(start, goal); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.15); - } - } + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(1.0, 0.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(1.0, 0.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(1.0, 0.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); } -TEST(HermiteCurveTest, getNewtonMethodStepSize) {} - -TEST(HermiteCurveTest, CheckNormalVector) -{ - { //p(0,0) v(1,0)-> p(1,1) v(0,1) - geometry_msgs::msg::Pose start_pose, goal_pose; - geometry_msgs::msg::Vector3 start_vec, goal_vec; - start_pose.position.x = 0; - start_pose.position.y = 0; - goal_pose.position.x = 1; - goal_pose.position.y = 1; - start_vec.x = 1; - start_vec.y = 0; - goal_vec.x = 0; - goal_vec.y = 1; - math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); - double norm = std::sqrt( - curve.getTangentVector(0.5, false).x * curve.getTangentVector(0.5, false).x + - curve.getTangentVector(0.5, false).y * curve.getTangentVector(0.5, false).y); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, 1 / std::sqrt(2)); - } - { //p(0,0) v(1,0)-> p(1,-1) v(0,-1) - geometry_msgs::msg::Pose start_pose, goal_pose; - geometry_msgs::msg::Vector3 start_vec, goal_vec; - start_pose.position.x = 0; - start_pose.position.y = 0; - goal_pose.position.x = 1; - goal_pose.position.y = -1; - start_vec.x = 1; - start_vec.y = 0; - goal_vec.x = 0; - goal_vec.y = -1; - math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); - double norm = std::sqrt( - curve.getTangentVector(0.5, false).x * curve.getTangentVector(0.5, false).x + - curve.getTangentVector(0.5, false).y * curve.getTangentVector(0.5, false).y); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, 1 / std::sqrt(2)); - } - { //p(1,1) v(0,-1)-> p(0,0) v(-1,0) - geometry_msgs::msg::Pose start_pose, goal_pose; - geometry_msgs::msg::Vector3 start_vec, goal_vec; - start_pose.position.x = 1; - start_pose.position.y = 1; - goal_pose.position.x = 0; - goal_pose.position.y = 0; - start_vec.x = 0; - start_vec.y = -1; - goal_vec.x = -1; - goal_vec.y = 0; - math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); - double norm = std::sqrt( - curve.getTangentVector(0.5, false).x * curve.getTangentVector(0.5, false).x + - curve.getTangentVector(0.5, false).y * curve.getTangentVector(0.5, false).y); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, -1 / std::sqrt(2)); - } - { //p(1,-1) v(0,1)-> p(0,0) v(-1,0) - geometry_msgs::msg::Pose start_pose, goal_pose; - geometry_msgs::msg::Vector3 start_vec, goal_vec; - start_pose.position.x = 1; - start_pose.position.y = -1; - goal_pose.position.x = 0; - goal_pose.position.y = 0; - start_vec.x = 0; - start_vec.y = 1; - goal_vec.x = -1; - goal_vec.y = 0; - math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); - double norm = std::sqrt( - curve.getTangentVector(0.5, false).x * curve.getTangentVector(0.5, false).x + - curve.getTangentVector(0.5, false).y * curve.getTangentVector(0.5, false).y); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, -1 / std::sqrt(2)); +/// @brief Helper function generating straight line: p(0,0) v(1,1)-> p(2,2) v(1,1) +math::geometry::HermiteCurve makeLine2() +{ + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(2.0, 2.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(1.0, 1.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(1.0, 1.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); +} + +/// @brief Helper function generating curve: p(0,0) v(1,0)-> p(1,1) v(0,1) +math::geometry::HermiteCurve makeCurve1(bool concave_upward = true) +{ + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(1.0, 1.0); + geometry_msgs::msg::Vector3 start_vec = + concave_upward ? makeVector(1.0, 0.0) : makeVector(0.0, 1.0); + geometry_msgs::msg::Vector3 goal_vec = + concave_upward ? makeVector(0.0, 1.0) : makeVector(1.0, 0.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); +} + +/// @brief Helper function generating curve: p(0,0) v(1,0)-> p(1,-1) v(0,-1) +math::geometry::HermiteCurve makeCurve2() +{ + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(1.0, -1.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(1.0, 0.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(0.0, -1.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); +} + +/// @brief Helper function generating curve: p(1,1) v(0,-1)-> p(0,0) v(-1,0) +math::geometry::HermiteCurve makeCurve3() +{ + geometry_msgs::msg::Pose start_pose = makePose(1.0, 1.0); + geometry_msgs::msg::Pose goal_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(0.0, -1.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(-1.0, 0.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); +} + +/// @brief Helper function generating curve: p(1,-1) v(0,1)-> p(0,0) v(-1,0) +math::geometry::HermiteCurve makeCurve4() +{ + geometry_msgs::msg::Pose start_pose = makePose(1.0, -1.0); + geometry_msgs::msg::Pose goal_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(0.0, 1.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(-1.0, 0.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); +} + +/** + * @brief Helper function generating a reference trajectory for testing + * @param start_x starting X position + * @param start_y starting Y position + * @param increment_x increment over X axis on every step + * @param increment_y increment over Y axis on every step + * @param vec vector container to generate a reference trajectory in (has to be the size of desired trajectory) + * @param start_idx index from which to start + */ +void generateReferenceTrajectory( + double start_x, double start_y, double increment_x, double increment_y, + std::vector & vec, unsigned int start_idx = 0u) +{ + for (size_t i = 0; i < vec.size(); ++i) { + vec[i].x = start_x + increment_x * (i + start_idx); + vec[i].y = start_y + increment_y * (i + start_idx); } } +TEST(HermiteCurveTest, initializationLine) +{ + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(1.0, 0.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(1.0, 0.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(1.0, 0.0); + EXPECT_NO_THROW(math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec)); +} + +TEST(HermiteCurveTest, initializationCurve) +{ + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(1.0, 1.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(1.0, 0.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(0.0, 1.0); + EXPECT_NO_THROW(math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec)); +} + +TEST(HermiteCurveTest, initializationParams) +{ + EXPECT_NO_THROW(math::geometry::HermiteCurve curve( + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0)); +} + +TEST(HermiteCurveTest, getTrajectoryPast1) +{ + const auto curve = makeLine2(); + EXPECT_NO_THROW(auto trajectory = curve.getTrajectory(0.0, 1.0, 0.1, false)); + auto trajectory = curve.getTrajectory(0.0, 1.0, 0.1, false); + std::vector ans(11); + generateReferenceTrajectory(0.0, 0.0, 0.2, 0.2, ans, 1u); + + EXPECT_EQ(trajectory.size(), ans.size()); + // The uncertainty of the calculations is high because the parameter space is equidistant + // interpolated, which does not translate into equidistant spline interpolation + constexpr double eps = 0.15; + EXPECT_POINT_NEAR(trajectory[0], ans[0], eps); + EXPECT_POINT_NEAR(trajectory[1], ans[1], eps); + EXPECT_POINT_NEAR(trajectory[2], ans[2], eps); + EXPECT_POINT_NEAR(trajectory[3], ans[3], eps); + EXPECT_POINT_NEAR(trajectory[4], ans[4], eps); + EXPECT_POINT_NEAR(trajectory[5], ans[5], eps); + EXPECT_POINT_NEAR(trajectory[6], ans[6], eps); + EXPECT_POINT_NEAR(trajectory[7], ans[7], eps); + EXPECT_POINT_NEAR(trajectory[8], ans[8], eps); + EXPECT_POINT_NEAR(trajectory[9], ans[9], eps); + EXPECT_POINT_NEAR(trajectory[10], ans[10], eps); +} + +TEST(HermiteCurveTest, getTrajectoryPast2) +{ + const auto curve = makeLine2(); + EXPECT_NO_THROW(auto trajectory = curve.getTrajectory(0.0, std::sqrt(2.0) * 2.0, 0.1, true)); + auto trajectory = curve.getTrajectory(0, std::sqrt(2.0) * 2.0, 0.1 * std::sqrt(2.0) * 2.0, true); + std::vector ans(11); + generateReferenceTrajectory(0.0, 0.0, 0.2, 0.2, ans, 1u); + + EXPECT_EQ(trajectory.size(), ans.size()); + // The uncertainty of the calculations is high because the parameter space is equidistant + // interpolated, which does not translate into equidistant spline interpolation + constexpr double eps = 0.15; + EXPECT_POINT_NEAR(trajectory[0], ans[0], eps); + EXPECT_POINT_NEAR(trajectory[1], ans[1], eps); + EXPECT_POINT_NEAR(trajectory[2], ans[2], eps); + EXPECT_POINT_NEAR(trajectory[3], ans[3], eps); + EXPECT_POINT_NEAR(trajectory[4], ans[4], eps); + EXPECT_POINT_NEAR(trajectory[5], ans[5], eps); + EXPECT_POINT_NEAR(trajectory[6], ans[6], eps); + EXPECT_POINT_NEAR(trajectory[7], ans[7], eps); + EXPECT_POINT_NEAR(trajectory[8], ans[8], eps); + EXPECT_POINT_NEAR(trajectory[9], ans[9], eps); + EXPECT_POINT_NEAR(trajectory[10], ans[10], eps); +} + +TEST(HermiteCurveTest, getPointLine) +{ + const auto curve = makeLine2(); + + // The uncertainty of the calculations is high because the parameter space is equidistant + // interpolated, which does not translate into equidistant spline interpolation + constexpr double eps = 0.15; + EXPECT_POINT_NEAR(curve.getPoint(0.0, false), makePoint(0.0, 0.0), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.1, false), makePoint(0.2, 0.2), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.2, false), makePoint(0.4, 0.4), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.3, false), makePoint(0.6, 0.6), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.4, false), makePoint(0.8, 0.8), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.5, false), makePoint(1.0, 1.0), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.6, false), makePoint(1.2, 1.2), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.7, false), makePoint(1.4, 1.4), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.8, false), makePoint(1.6, 1.6), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.9, false), makePoint(1.8, 1.8), eps); + EXPECT_POINT_NEAR(curve.getPoint(1.0, false), makePoint(2.0, 2.0), eps); +} + +TEST(HermiteCurveTest, getPointCurve) +{ + geometry_msgs::msg::Pose start_pose = makePose(0, 0); + geometry_msgs::msg::Pose goal_pose = makePose(1, 1); + geometry_msgs::msg::Vector3 start_vec = makeVector(0, 1); + geometry_msgs::msg::Vector3 goal_vec = makeVector(1, 0); + math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); + + // The uncertainty of the calculations is high because the parameter space is equidistant + // interpolated, which does not translate into equidistant spline interpolation + constexpr double eps = 0.15; + EXPECT_POINT_NEAR(curve.getPoint(0.0, true), makePoint(0.0, 0.0), eps); + EXPECT_POINT_NEAR( + curve.getPoint(0.75, true), makePoint(1.0 - 1.0 / std::sqrt(2.0), 1.0 / std::sqrt(2.0)), eps); + EXPECT_POINT_NEAR(curve.getPoint(1.5, true), makePoint(1.0, 1.0), eps); +} + +TEST(HermiteCurveTest, get2DCurvatureLine) +{ + const auto curve = makeLine1(); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.0, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.1, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.2, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.3, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.4, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.5, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.6, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.7, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.8, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.9, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(1.0, true), 0.0); +} + +TEST(HermiteCurveTest, get2DCurvatureCurve) +{ + constexpr double eps = 0.01; + const auto curve1 = makeCurve1(); + EXPECT_NEAR(curve1.get2DCurvature(0.0, false), 4.0, eps); + EXPECT_NEAR(curve1.get2DCurvature(0.5, false), 0.45, eps); + EXPECT_NEAR(curve1.get2DCurvature(1.0, false), 4.0, eps); + + const auto curve2 = makeCurve1(false); + EXPECT_NEAR(curve2.get2DCurvature(0.0, false), -4.0, eps); + EXPECT_NEAR(curve2.get2DCurvature(0.5, false), -0.45, eps); + EXPECT_NEAR(curve2.get2DCurvature(1.0, false), -4.0, eps); +} + +TEST(HermiteCurveTest, getMaximum2DCurvatureLine) +{ + const auto curve = makeLine1(); + EXPECT_DOUBLE_EQ(curve.getMaximum2DCurvature(), 0.0); +} + +TEST(HermiteCurveTest, getMaximum2DCurvatureCurve) +{ + constexpr double eps = 0.01; + const auto curve1 = makeCurve1(); + EXPECT_NEAR(curve1.getMaximum2DCurvature(), 4.0, eps); + + const auto curve2 = makeCurve1(false); + EXPECT_NEAR(curve2.getMaximum2DCurvature(), -4.0, eps); +} + +TEST(HermiteCurveTest, getLengthNoParameter) +{ + const auto curve = makeLine1(); + EXPECT_NEAR(curve.getLength(), 1.0, EPS); +} + +TEST(HermiteCurveTest, getLengthParameter) +{ + const auto curve = makeLine1(); + EXPECT_NEAR(curve.getLength(1000), 1.0, EPS); +} + +TEST(HermiteCurveTest, getSValue) +{ + const auto curve = makeLine2(); + + const auto s0 = curve.getSValue(makePose(0.0, 0.0), 1.0, false); + EXPECT_TRUE(s0); + EXPECT_NEAR(s0.value(), 0.0, EPS); + const auto s1 = curve.getSValue(makePose(1.0, 1.0), 1.0, false); + EXPECT_TRUE(s1); + EXPECT_NEAR(s1.value(), 0.5, EPS); + const auto s2 = curve.getSValue(makePose(2.0, 2.0), 1.0, false); + EXPECT_TRUE(s2); + EXPECT_NEAR(s2.value(), 1.0, EPS); +} + +TEST(HermiteCurveTest, getSValueAutoscale) +{ + const auto curve = makeLine2(); + + const auto s0 = curve.getSValue(makePose(0.0, 0.0), 1.0, true); + EXPECT_TRUE(s0); + EXPECT_NEAR(s0.value(), 0.0, EPS); + const auto s1 = curve.getSValue(makePose(1.0, 1.0), 1.0, true); + EXPECT_TRUE(s1); + EXPECT_NEAR(s1.value(), 0.5 * 2.0 * std::sqrt(2.0), EPS); + const auto s2 = curve.getSValue(makePose(2.0, 2.0), 1.0, true); + EXPECT_TRUE(s2); + EXPECT_NEAR(s2.value(), 2.0 * std::sqrt(2.0), EPS); +} + +TEST(HermiteCurveTest, getSquaredDistanceIn2D) +{ + const auto curve = makeLine2(); + + EXPECT_NEAR(curve.getSquaredDistanceIn2D(makePoint(2.0, 0.0), 0.5, false), 2.0, EPS); + EXPECT_NEAR(curve.getSquaredDistanceIn2D(makePoint(2.0, 0.0), 1.0, false), 4.0, EPS); + EXPECT_NEAR(curve.getSquaredDistanceIn2D(makePoint(-1.0, -2.0), 0.0, false), 5.0, EPS); +} + +TEST(HermiteCurveTest, getSquaredDistanceIn2DZeroDistance) +{ + const auto curve = makeLine2(); + + EXPECT_NEAR( + curve.getSquaredDistanceIn2D(makePoint(2.0, 2.0), std::sqrt(2.0) * 2.0, true), 0.0, EPS); + EXPECT_NEAR(curve.getSquaredDistanceIn2D(makePoint(1.0, 1.0), std::sqrt(2.0), true), 0.0, EPS); + EXPECT_NEAR(curve.getSquaredDistanceIn2D(makePoint(0.0, 0.0), 0.0, true), 0.0, EPS); +} + +TEST(HermiteCurveTest, getSquaredDistanceVector) +{ + const auto curve = makeLine2(); + + const auto p0 = curve.getSquaredDistanceVector(makePoint(2.0, 0.0), 0.5, false); + EXPECT_POINT_NEAR(p0, makePoint(1.0, -1.0, 0.0), EPS); + const auto p1 = curve.getSquaredDistanceVector(makePoint(2.0, 0.0), 1.0, false); + EXPECT_POINT_NEAR(p1, makePoint(0.0, -2.0, 0.0), EPS); + const auto p2 = curve.getSquaredDistanceVector(makePoint(-1.0, -2.0), 0.0, false); + EXPECT_POINT_NEAR(p2, makePoint(-1.0, -2.0, 0.0), EPS); +} + +TEST(HermiteCurveTest, getSquaredDistanceVectorZeroDistance) +{ + const auto curve = makeLine2(); + + const auto p0 = curve.getSquaredDistanceVector(makePoint(2.0, 2.0), std::sqrt(2.0) * 2.0, true); + EXPECT_POINT_NEAR(p0, makePoint(0.0, 0.0, 0.0), EPS); + const auto p1 = curve.getSquaredDistanceVector(makePoint(1.0, 1.0), std::sqrt(2.0), true); + EXPECT_POINT_NEAR(p1, makePoint(0.0, 0.0, 0.0), EPS); + const auto p2 = curve.getSquaredDistanceVector(makePoint(0.0, 0.0), 0.0, true); + EXPECT_POINT_NEAR(p2, makePoint(0.0, 0.0, 0.0), EPS); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DLine) +{ + const auto curve = makeLine2(); + + constexpr double eps = 0.1; + const auto s = curve.getCollisionPointIn2D(makePoint(1.0, 0.0), makePoint(0.0, 1.0)); + EXPECT_TRUE(s); + EXPECT_NEAR(s.value(), 0.25, eps); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DLineNoCollision) +{ + const auto curve = makeLine2(); + + const auto s = curve.getCollisionPointIn2D(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(s); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DCurve) +{ + const auto curve = makeCurve1(); + + constexpr double eps = 0.1; + const auto s0 = curve.getCollisionPointIn2D(makePoint(1.0, 0.0), makePoint(0.0, 1.0)); + EXPECT_TRUE(s0); + EXPECT_NEAR(s0.value(), 0.5, eps); + + const auto s1 = curve.getCollisionPointIn2D(makePoint(0.1, 0.0), makePoint(1.0, 0.9)); + EXPECT_TRUE(s1); + EXPECT_NEAR(s1.value(), 0.2, eps); + + const auto s2 = curve.getCollisionPointIn2D(makePoint(0.1, 0.0), makePoint(1.0, 0.9), true); + EXPECT_TRUE(s2); + EXPECT_NEAR(s2.value(), 0.8, eps); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DCurveEdge) +{ + const auto curve = makeCurve1(); + + constexpr double eps = 0.1; + const auto s = curve.getCollisionPointIn2D(makePoint(-1.0, 1.0), makePoint(1.0, -1.0)); + EXPECT_TRUE(s); + EXPECT_NEAR(s.value(), 0.0, eps); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DVectorWrongCases) +{ + const auto curve = makeCurve1(); + + std::vector polygon; + EXPECT_FALSE(curve.getCollisionPointIn2D(polygon)); + polygon.emplace_back(makePoint(1.0, 1.0)); + EXPECT_FALSE(curve.getCollisionPointIn2D(polygon)); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DVectorOneCollision) +{ + const auto curve = makeLine1(); + + std::vector polygon{ + makePoint(0.5, 0.5), makePoint(-0.5, 0.5), makePoint(-0.5, -0.5), makePoint(0.5, -0.5)}; + const auto s = curve.getCollisionPointIn2D(polygon); + EXPECT_TRUE(s); + EXPECT_NEAR(s.value(), 0.5, EPS); + EXPECT_FALSE(curve.getCollisionPointIn2D(polygon, false, false)); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DVectorMultipleCollisions) +{ + const auto curve = makeLine2(); + + std::vector polygon{ + makePoint(1, 0), makePoint(2, 1), makePoint(1, 2), makePoint(0, 1)}; + + constexpr double eps = 0.1; + const auto s0 = curve.getCollisionPointIn2D(polygon); + EXPECT_TRUE(s0); + EXPECT_NEAR(s0.value(), 0.25, eps); + + const auto s1 = curve.getCollisionPointIn2D(polygon, true); + EXPECT_TRUE(s1); + EXPECT_NEAR(s1.value(), 0.75, eps); + + const auto s2 = curve.getCollisionPointIn2D(polygon, false, false); + EXPECT_TRUE(s2); + EXPECT_NEAR(s2.value(), 0.75, eps); +} + +TEST(HermiteCurveTest, getTangentVector1) +{ //p(0,0) v(1,0)-> p(1,1) v(0,1) + const auto curve = makeCurve1(); + double norm = + std::hypot(curve.getTangentVector(0.5, false).x, curve.getTangentVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, 1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, 1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getTangentVector2) +{ //p(0,0) v(1,0)-> p(1,-1) v(0,-1) + const auto curve = makeCurve2(); + double norm = + std::hypot(curve.getTangentVector(0.5, false).x, curve.getTangentVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, 1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, -1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getTangentVector3) +{ //p(1,1) v(0,-1)-> p(0,0) v(-1,0) + const auto curve = makeCurve3(); + double norm = + std::hypot(curve.getTangentVector(0.5, false).x, curve.getTangentVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, -1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, -1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getTangentVector4) +{ //p(1,-1) v(0,1)-> p(0,0) v(-1,0) + const auto curve = makeCurve4(); + double norm = + std::hypot(curve.getTangentVector(0.5, false).x, curve.getTangentVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, -1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, 1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getTangentVectorAutoscale1) +{ //p(0,0) v(1,0)-> p(1,1) v(0,1) + const auto curve = makeCurve1(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getTangentVector(0.75, true).x, curve.getTangentVector(0.75, true).y); + EXPECT_NEAR(curve.getTangentVector(0.75, true).x / norm, 1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getTangentVectorAutoscale2) +{ //p(0,0) v(1,0)-> p(1,-1) v(0,-1) + const auto curve = makeCurve2(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getTangentVector(0.75, true).x, curve.getTangentVector(0.75, true).y); + EXPECT_NEAR(curve.getTangentVector(0.75, true).x / norm, 1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getTangentVectorAutoscale3) +{ //p(1,1) v(0,-1)-> p(0,0) v(-1,0) + const auto curve = makeCurve3(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getTangentVector(0.75, true).x, curve.getTangentVector(0.75, true).y); + EXPECT_NEAR(curve.getTangentVector(0.75, true).x / norm, -1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getTangentVectorAutoscale4) +{ //p(1,-1) v(0,1)-> p(0,0) v(-1,0) + const auto curve = makeCurve4(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getTangentVector(0.75, true).x, curve.getTangentVector(0.75, true).y); + EXPECT_NEAR(curve.getTangentVector(0.75, true).x / norm, -1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getNormalVector1) +{ //p(0,0) v(1,0)-> p(1,1) v(0,1) + const auto curve = makeCurve1(); + double norm = + std::hypot(curve.getNormalVector(0.5, false).x, curve.getNormalVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, -1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, 1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getNormalVector2) +{ //p(0,0) v(1,0)-> p(1,-1) v(0,-1) + const auto curve = makeCurve2(); + double norm = + std::hypot(curve.getNormalVector(0.5, false).x, curve.getNormalVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, 1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, 1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getNormalVector3) +{ //p(1,1) v(0,-1)-> p(0,0) v(-1,0) + const auto curve = makeCurve3(); + double norm = + std::hypot(curve.getNormalVector(0.5, false).x, curve.getNormalVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, 1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, -1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getNormalVector4) +{ //p(1,-1) v(0,1)-> p(0,0) v(-1,0) + const auto curve = makeCurve4(); + double norm = + std::hypot(curve.getNormalVector(0.5, false).x, curve.getNormalVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, -1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, -1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getNormalVectorAutoscale1) +{ //p(0,0) v(1,0)-> p(1,1) v(0,1) + const auto curve = makeCurve1(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getNormalVector(0.75, true).x, curve.getNormalVector(0.75, true).y); + EXPECT_NEAR(curve.getNormalVector(0.75, true).x / norm, -1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getNormalVectorAutoscale2) +{ //p(0,0) v(1,0)-> p(1,-1) v(0,-1) + const auto curve = makeCurve2(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getNormalVector(0.75, true).x, curve.getNormalVector(0.75, true).y); + EXPECT_NEAR(curve.getNormalVector(0.75, true).x / norm, 1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getNormalVectorAutoscale3) +{ //p(1,1) v(0,-1)-> p(0,0) v(-1,0) + const auto curve = makeCurve3(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getNormalVector(0.75, true).x, curve.getNormalVector(0.75, true).y); + EXPECT_NEAR(curve.getNormalVector(0.75, true).x / norm, 1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getNormalVectorAutoscale4) +{ //p(1,-1) v(0,1)-> p(0,0) v(-1,0) + const auto curve = makeCurve4(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getNormalVector(0.75, true).x, curve.getNormalVector(0.75, true).y); + EXPECT_NEAR(curve.getNormalVector(0.75, true).x / norm, -1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/common/math/geometry/test/test_intersection.cpp b/common/math/geometry/test/test_intersection.cpp new file mode 100644 index 00000000000..8d726479389 --- /dev/null +++ b/common/math/geometry/test/test_intersection.cpp @@ -0,0 +1,104 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +TEST(Intersection, isIntersect2DDisjoint) +{ + math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(math::geometry::isIntersect2D(line0, line1)); +} + +TEST(Intersection, isIntersect2DIntersect) +{ + math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(0.0, 1.0)); + EXPECT_TRUE(math::geometry::isIntersect2D(line0, line1)); +} + +TEST(Intersection, isIntersect2DIntersectVector) +{ + std::vector lines{ + {makePoint(1.0, 0.0), makePoint(0.0, 1.0)}, {makePoint(0.0, 0.0), makePoint(1.0, 1.0)}}; + EXPECT_TRUE(math::geometry::isIntersect2D(lines)); +} + +TEST(Intersection, isIntersect2DIdentical) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + EXPECT_TRUE(math::geometry::isIntersect2D(line, line)); +} + +TEST(Intersection, isIntersect2DIdenticalVector) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + std::vector lines; + lines.push_back(line); + lines.push_back(line); + lines.push_back(line); + EXPECT_TRUE(math::geometry::isIntersect2D(lines)); +} + +TEST(Intersection, isIntersect2DEmptyVector) +{ + std::vector lines; + bool ans = true; + EXPECT_NO_THROW(ans = math::geometry::isIntersect2D(lines)); + EXPECT_FALSE(ans); +} + +TEST(Intersection, getIntersection2DDisjoint) +{ + math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(math::geometry::getIntersection2D(line0, line1)); +} + +TEST(Intersection, getIntersection2DIntersect) +{ + math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(0.0, 1.0)); + auto ans = math::geometry::getIntersection2D(line0, line1); + EXPECT_TRUE(ans); + EXPECT_POINT_EQ(ans.value(), makePoint(0.5, 0.5)); +} + +TEST(Intersection, getIntersection2DIdentical) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + auto ans = math::geometry::getIntersection2D(line, line); + EXPECT_TRUE(ans); + EXPECT_POINT_NAN(ans.value()); +} + +TEST(Intersection, getIntersection2DEmptyVector) +{ + std::vector lines; + std::vector ans; + EXPECT_NO_THROW(ans = math::geometry::getIntersection2D(lines)); + EXPECT_EQ(ans.size(), size_t(0)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/math/geometry/test/test_line_segment.cpp b/common/math/geometry/test/test_line_segment.cpp index 5802ad3df9e..f7a299b85db 100644 --- a/common/math/geometry/test/test_line_segment.cpp +++ b/common/math/geometry/test/test_line_segment.cpp @@ -13,15 +13,313 @@ // limitations under the License. #include +#include #include #include #include #include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +TEST(LineSegment, initializeDifferentPoints) +{ + geometry_msgs::msg::Point point0 = makePoint(0.0, 0.0); + geometry_msgs::msg::Point point1 = makePoint(1.0, 1.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point0, point1)); + const math::geometry::LineSegment line_segment(point0, point1); + EXPECT_POINT_EQ(line_segment.start_point, point0); + EXPECT_POINT_EQ(line_segment.end_point, point1); +} + +TEST(LineSegment, initializeIdenticalPoints) +{ + geometry_msgs::msg::Point point = makePoint(0.0, 0.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, point)); + const math::geometry::LineSegment line_segment(point, point); + EXPECT_POINT_EQ(line_segment.start_point, point); + EXPECT_POINT_EQ(line_segment.end_point, point); +} + +TEST(LineSegment, initializeVector) +{ + geometry_msgs::msg::Point point = makePoint(0.0, 0.0); + geometry_msgs::msg::Vector3 vec = makeVector(1.0, 1.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, vec, 1.0)); + const math::geometry::LineSegment line_segment(point, vec, 1.0); + EXPECT_POINT_EQ(line_segment.start_point, point); + EXPECT_POINT_EQ(line_segment.end_point, makePoint(std::sqrt(2.0) / 2.0, std::sqrt(2.0) / 2.0)); +} + +TEST(LineSegment, initializeVectorZeroLength) +{ + geometry_msgs::msg::Point point = makePoint(0.0, 0.0); + geometry_msgs::msg::Vector3 vec = makeVector(1.0, 1.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, vec, 0.0)); + const math::geometry::LineSegment line_segment(point, vec, 0.0); + EXPECT_POINT_EQ(line_segment.start_point, point); + EXPECT_POINT_EQ(line_segment.end_point, point); +} + +TEST(LineSegment, isIntersect2DDisjoint) +{ + const math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(line0.isIntersect2D(line1)); +} + +TEST(LineSegment, isIntersect2DIntersect) +{ + const math::geometry::LineSegment line0(makePoint(1.0, 1.0), makePoint(2.0, 1.0)); + const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makeVector(1.0, 1.0), 3.0); + EXPECT_TRUE(line0.isIntersect2D(line1)); +} + +TEST(LineSegment, isIntersect2DIdentical) +{ + const math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + EXPECT_TRUE(line.isIntersect2D(line)); +} + +TEST(LineSegment, getIntersection2DDisjoint) +{ + const math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(line0.getIntersection2D(line1)); +} + +TEST(LineSegment, getVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_VECTOR3_EQ(line.getVector(), makeVector(1.0, 1.0, 1.0)); +} + +TEST(LineSegment, getVectorZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_VECTOR3_EQ(line.getVector(), makeVector(0.0, 0.0, 0.0)); +} + +TEST(LineSegment, getNormalVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_VECTOR3_EQ(line.getNormalVector(), makeVector(-1.0, 1.0, 0.0)); +} + +TEST(LineSegment, getNormalVector_zeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_VECTOR3_EQ(line.getNormalVector(), makeVector(0.0, 0.0, 0.0)); +} + +TEST(LineSegment, get2DVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_VECTOR3_EQ(line.get2DVector(), makeVector(1.0, 1.0, 0.0)); +} + +TEST(LineSegment, get2DVectorZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_VECTOR3_EQ(line.get2DVector(), makeVector(0.0, 0.0, 0.0)); +} + +TEST(LineSegment, getLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_DOUBLE_EQ(line.getLength(), std::sqrt(3.0)); +} + +TEST(LineSegment, getLengthZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_DOUBLE_EQ(line.getLength(), 0.0); +} + +TEST(LineSegment, get2DLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_DOUBLE_EQ(line.get2DLength(), std::sqrt(2.0)); +} + +TEST(LineSegment, get2DLengthZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_DOUBLE_EQ(line.get2DLength(), 0.0); +} + +TEST(LineSegment, getSlope) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 3.0, 4.0)); + EXPECT_DOUBLE_EQ(line.getSlope(), 0.5); +} + +TEST(LineSegment, getSlopeZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_TRUE(std::isnan(line.getSlope())); +} + +TEST(LineSegment, getSquaredDistanceIn2D) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_DOUBLE_EQ(line.getSquaredDistanceIn2D(makePoint(0.0, 1.0, 2.0), 0.5, false), 8.0); +} + +TEST(LineSegment, getSquaredDistanceIn2D_denormalize) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_DOUBLE_EQ( + line.getSquaredDistanceIn2D(makePoint(0.0, 1.0, 2.0), std::sqrt(3.0), true), 8.0); +} + +TEST(LineSegment, getSquaredDistanceVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_VECTOR3_EQ( + line.getSquaredDistanceVector(makePoint(0.0, 1.0, 2.0), 0.5, false), + makeVector(-2.0, -2.0, -2.0)); +} + +TEST(LineSegment, getSquaredDistanceVector_denormalize) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_VECTOR3_EQ( + line.getSquaredDistanceVector(makePoint(0.0, 1.0, 2.0), std::sqrt(3.0), true), + makeVector(-2.0, -2.0, -2.0)); +} + +TEST(LineSegment, getLineSegments) +{ + const std::vector points{ + makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0), makePoint(3.0, 4.0, 5.0), + makePoint(4.0, 5.0, 6.0)}; + const std::vector lines = + math::geometry::getLineSegments(points, false); + EXPECT_EQ(lines.size(), size_t(3)); + EXPECT_POINT_EQ(lines[0].start_point, points[0]); + EXPECT_POINT_EQ(lines[0].end_point, points[1]); + EXPECT_POINT_EQ(lines[1].start_point, points[1]); + EXPECT_POINT_EQ(lines[1].end_point, points[2]); + EXPECT_POINT_EQ(lines[2].start_point, points[2]); + EXPECT_POINT_EQ(lines[2].end_point, points[3]); +} + +TEST(LineSegment, getLineSegments_closeStartEnd) +{ + const std::vector points{ + makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0), makePoint(3.0, 4.0, 5.0), + makePoint(4.0, 5.0, 6.0)}; + const std::vector lines = + math::geometry::getLineSegments(points, true); + EXPECT_EQ(lines.size(), size_t(4)); + EXPECT_POINT_EQ(lines[0].start_point, points[0]); + EXPECT_POINT_EQ(lines[0].end_point, points[1]); + EXPECT_POINT_EQ(lines[1].start_point, points[1]); + EXPECT_POINT_EQ(lines[1].end_point, points[2]); + EXPECT_POINT_EQ(lines[2].start_point, points[2]); + EXPECT_POINT_EQ(lines[2].end_point, points[3]); + EXPECT_POINT_EQ(lines[3].start_point, points[3]); + EXPECT_POINT_EQ(lines[3].end_point, points[0]); +} + +TEST(LineSegment, getLineSegmentsVectorEmpty) +{ + const std::vector points; + const std::vector lines = math::geometry::getLineSegments(points); + EXPECT_EQ(lines.size(), size_t(0)); +} + +TEST(LineSegment, getLineSegmentsVectorIdentical) +{ + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + const std::vector points{point, point, point, point}; + const std::vector lines = + math::geometry::getLineSegments(points, false); + EXPECT_EQ(lines.size(), size_t(3)); + EXPECT_POINT_EQ(lines[0].start_point, point); + EXPECT_POINT_EQ(lines[0].end_point, point); + EXPECT_POINT_EQ(lines[1].start_point, point); + EXPECT_POINT_EQ(lines[1].end_point, point); + EXPECT_POINT_EQ(lines[2].start_point, point); + EXPECT_POINT_EQ(lines[2].end_point, point); +} + +TEST(LineSegment, getLineSegmentsVectorIdentical_closeStartEnd) +{ + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + const std::vector points{point, point, point, point}; + const std::vector lines = + math::geometry::getLineSegments(points, true); + EXPECT_EQ(lines.size(), size_t(4)); + EXPECT_POINT_EQ(lines[0].start_point, point); + EXPECT_POINT_EQ(lines[0].end_point, point); + EXPECT_POINT_EQ(lines[1].start_point, point); + EXPECT_POINT_EQ(lines[1].end_point, point); + EXPECT_POINT_EQ(lines[2].start_point, point); + EXPECT_POINT_EQ(lines[2].end_point, point); + EXPECT_POINT_EQ(lines[3].start_point, point); + EXPECT_POINT_EQ(lines[3].end_point, point); +} + +TEST(LineSegment, getSValue) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(2.0, 2.0, 2.0), 1.0, false); + EXPECT_TRUE(s); + if (s) { + EXPECT_DOUBLE_EQ(s.value(), 2.0 / 3.0); + } +} + +TEST(LineSegment, getSValue_denormalize) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(2.0, 2.0, 2.0), 1.0, true); + EXPECT_TRUE(s); + if (s) { + EXPECT_DOUBLE_EQ(s.value(), std::hypot(2.0, 2.0, 2.0)); + } +} + +TEST(LineSegment, getSValue_outOfRange) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(4.0, 4.0, 4.0), 1.0, false); + EXPECT_FALSE(s); +} + +TEST(LineSegment, getSValue_outOfRangeDenormalize) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(4.0, 4.0, 4.0), 1.0, true); + EXPECT_FALSE(s); +} + +TEST(LineSegment, getSValue_parallel) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 0.0)); + const auto s = line.getSValue( + makePose( + 1.0, 0.0, 0.0, + quaternion_operation::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), + 1000.0, false); + EXPECT_FALSE(s); +} + +TEST(LineSegment, getSValue_parallelDenormalize) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 0.0)); + const auto s = line.getSValue( + makePose( + 1.0, 0.0, 0.0, + quaternion_operation::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), + 1000.0, true); + EXPECT_FALSE(s); +} /// @brief In this test case, testing the `LineSegment::getPoint` function can find the point on the line segment with start point (x,y,z) = (0,0,0) and end point (x,y,z) = (1,1,1) in the cartesian coordinate system. (variable name `line`). -TEST(LineSegmentTest, GetPoint) +TEST(LineSegment, GetPoint) { { math::geometry::LineSegment line( @@ -235,6 +533,45 @@ TEST(LineSegment, getIntersection2DSValue) // [Snippet_getIntersection2DSValue_with_point_1_0_0] /// @snippet test/test_line_segment.cpp Snippet_getIntersection2DSValue_with_point_1_0_0 + { // parallel no denormalize + EXPECT_THROW( + line.getIntersection2DSValue( + math::geometry::LineSegment(makePoint(0.0, -1.0), makePoint(0.0, 1.0)), false), + common::SimulationError); + } + { // parallel denormalize + EXPECT_THROW( + line.getIntersection2DSValue( + math::geometry::LineSegment(makePoint(0.0, -1.0), makePoint(0.0, 1.0)), true), + common::SimulationError); + } + { // intersect no denormalize + const auto collision_s = line.getIntersection2DSValue( + math::geometry::LineSegment(makePoint(-1.0, 0.5), makePoint(1.0, 0.5)), false); + EXPECT_TRUE(collision_s); + if (collision_s) { + EXPECT_DOUBLE_EQ(collision_s.value(), 0.75); + } + } + { // intersect denormalize + const auto collision_s = line.getIntersection2DSValue( + math::geometry::LineSegment(makePoint(-1.0, 0.5), makePoint(1.0, 0.5)), true); + EXPECT_TRUE(collision_s); + if (collision_s) { + EXPECT_DOUBLE_EQ(collision_s.value(), 1.5); + } + } + { // no intersect no denormalize + const auto collision_s = line.getIntersection2DSValue( + math::geometry::LineSegment(makePoint(-1.0, 1.5), makePoint(1.0, 1.5)), false); + EXPECT_FALSE(collision_s); + } + { // no intersect denormalize + const auto collision_s = line.getIntersection2DSValue( + math::geometry::LineSegment(makePoint(-1.0, 1.5), makePoint(1.0, 1.5)), true); + EXPECT_FALSE(collision_s); + } + /** * @note Testing the `LineSegment::getIntersection2D` function can throw error getting intersection with exact same line segment. * In this case, any s value can be a intersection point, so we cannot return single value. @@ -264,9 +601,14 @@ TEST(LineSegment, getIntersection2DSValue) /// @note @note Testing the `LineSegment::getIntersection2D` function can find collision with the line segment with start point (x,y,z) = (1,0,0) and end point (x,y,z) = (-1,0,0) in the cartesian coordinate system and `line`. // [Snippet_getIntersection2D_line_1_0_0_-1_0_0] { - EXPECT_TRUE(line.getIntersection2D(math::geometry::LineSegment( + const auto point = line.getIntersection2D(math::geometry::LineSegment( geometry_msgs::build().x(1).y(0).z(0), - geometry_msgs::build().x(-1).y(0).z(0)))); + geometry_msgs::build().x(-1).y(0).z(0))); + EXPECT_TRUE(point); + if (point) { + EXPECT_POINT_EQ( + point.value(), geometry_msgs::build().x(0).y(0).z(0)); + } } // [Snippet_getIntersection2D_line_1_0_0_-1_0_0] /// @snippet test/test_line_segment.cpp Snippet_getIntersection2D_line_1_0_0_-1_0_0 diff --git a/common/math/geometry/test/test_linear_algebra.cpp b/common/math/geometry/test/test_linear_algebra.cpp index d346e9a9498..567f25d3b42 100644 --- a/common/math/geometry/test/test_linear_algebra.cpp +++ b/common/math/geometry/test/test_linear_algebra.cpp @@ -15,64 +15,142 @@ #include #include +#include #include "expect_eq_macros.hpp" +#include "test_utils.hpp" -TEST(LINEAR_ALGEBRA, GET_SIZE) +constexpr double EPS = 1e-3; + +TEST(LINEAR_ALGEBRA, GET_SIZE_ZERO) { geometry_msgs::msg::Vector3 vec; EXPECT_DOUBLE_EQ(math::geometry::getSize(vec), 0.0); - vec.x = 1.0; - vec.y = 0.0; - vec.z = 3.0; +} + +TEST(LINEAR_ALGEBRA, GET_SIZE) +{ + geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0); EXPECT_DOUBLE_EQ(math::geometry::getSize(vec), std::sqrt(10.0)); } -TEST(LINEAR_ALGEBRA, NORMALIZE) +TEST(LINEAR_ALGEBRA, NORMALIZE_ZERO) { geometry_msgs::msg::Vector3 vec; EXPECT_THROW(math::geometry::normalize(vec), common::SimulationError); - vec.x = 1.0; - vec.y = 0.0; - vec.z = 3.0; +} + +TEST(LINEAR_ALGEBRA, NORMALIZE) +{ + geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0); vec = math::geometry::normalize(vec); - EXPECT_DOUBLE_EQ(vec.x, 0.31622776601683794); - EXPECT_DOUBLE_EQ(vec.y, 0.0); - EXPECT_DOUBLE_EQ(vec.z, 0.94868329805051377); + + geometry_msgs::msg::Vector3 ans = makeVector(0.31622776601683794, 0.0, 0.94868329805051377); + EXPECT_VECTOR3_EQ(vec, ans); EXPECT_DOUBLE_EQ(math::geometry::getSize(vec), 1.0); } -TEST(LINEAR_ALGEBRA, MULTIPLY) +TEST(LINEAR_ALGEBRA, INNER_PRODUCT) { - geometry_msgs::msg::Vector3 vec = math::geometry::vector3(0, 3, 1); - vec * 1.0; - EXPECT_VECTOR3_EQ((vec * 1.0), math::geometry::vector3(0, 3, 1)); - EXPECT_VECTOR3_EQ((vec * 2.0), math::geometry::vector3(0, 6, 2)); - EXPECT_VECTOR3_EQ((vec * 2.0), (2.0 * vec)); + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0), vec1 = makeVector(-1.0, 0.0, -3.0); + EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec1), -10.0); } -TEST(LINEAR_ALGEBRA, ADDITION) +TEST(LINEAR_ALGEBRA, INNER_PRODUCT_IDENTICAL) { - geometry_msgs::msg::Vector3 vec = math::geometry::vector3(0, 3, 1); - EXPECT_VECTOR3_EQ((vec + vec), (2.0 * vec)); - geometry_msgs::msg::Point p; - p.x = 0; - p.y = 3; - p.z = 1; - EXPECT_VECTOR3_EQ((p + vec), (2.0 * vec)); + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0); + EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec0), 10.0); } -TEST(LINEAR_ALGEBRA, SUBTRACTION) +TEST(LINEAR_ALGEBRA, INNER_PRODUCT_ZERO) { - geometry_msgs::msg::Vector3 vec = math::geometry::vector3(0, 3, 1); - EXPECT_VECTOR3_EQ((vec - vec), geometry_msgs::msg::Vector3()); - geometry_msgs::msg::Point p; - p.x = 0; - p.y = 3; - p.z = 1; + geometry_msgs::msg::Vector3 vec0, vec1 = makeVector(1.0, 0.0, 3.0); + EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec1), 0.0); +} + +TEST(LINEAR_ALGEBRA, GET_INTERNAL_ANGLE) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0), vec1 = makeVector(-1.0, 0.0, -3.0); + EXPECT_NEAR(math::geometry::getInternalAngle(vec0, vec1), M_PI, EPS); +} + +TEST(LINEAR_ALGEBRA, GET_INTERNAL_ANGLE_IDENTICAL) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0); + EXPECT_NEAR(math::geometry::getInternalAngle(vec0, vec0), 0.0, EPS); +} + +TEST(LINEAR_ALGEBRA, GET_INTERNAL_ANGLE_ZERO) +{ + geometry_msgs::msg::Vector3 vec0, vec1 = makeVector(1.0, 0.0, 3.0); + EXPECT_THROW(math::geometry::getInternalAngle(vec0, vec1), common::SimulationError); +} + +TEST(LINEAR_ALGEBRA, DIVIDE) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((vec / 2.0), makeVector(0.0, 1.5, 0.5)); +} + +TEST(LINEAR_ALGEBRA, DIVIDE_ZERO) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + vec = vec / 0.0; + EXPECT_TRUE(std::isnan(vec.x)); + EXPECT_TRUE(std::isinf(vec.y)); + EXPECT_TRUE(std::isinf(vec.z)); +} + +TEST(LINEAR_ALGEBRA, MULTIPLY_FIRST) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((vec * 2.0), makeVector(0.0, 6.0, 2.0)); +} + +TEST(LINEAR_ALGEBRA, MULTIPLY_SECOND) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((2.0 * vec), makeVector(0.0, 6.0, 2.0)); +} + +TEST(LINEAR_ALGEBRA, ADDITION_POINT_VECTOR) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + geometry_msgs::msg::Point p = makePoint(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((p + vec), makeVector(0.0, 6.0, 2.0)); +} + +TEST(LINEAR_ALGEBRA, ADDITION_VECTOR_VECTOR) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((vec + vec), makeVector(0.0, 6.0, 2.0)); +} + +TEST(LINEAR_ALGEBRA, ADDITION_POINT_POINT) +{ + geometry_msgs::msg::Point p0 = makePoint(0.0, 3.0, 1.0), p1 = makePoint(2.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((p0 + p1), makePoint(2.0, 6.0, 2.0)); +} + +TEST(LINEAR_ALGEBRA, SUBTRACTION_POINT_VECTOR) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + geometry_msgs::msg::Point p = makePoint(0.0, 3.0, 1.0); EXPECT_VECTOR3_EQ((p - vec), geometry_msgs::msg::Vector3()); } +TEST(LINEAR_ALGEBRA, SUBTRACTION_VECTOR_VECTOR) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((vec - vec), geometry_msgs::msg::Vector3()); +} + +TEST(LINEAR_ALGEBRA, SUBTRACTION_POINT_POINT) +{ + geometry_msgs::msg::Point p0 = makePoint(0.0, 3.0, 1.0), p1 = makePoint(2.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((p0 - p1), makePoint(-2.0, 0.0, 0.0)); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/common/math/geometry/test/test_polygon.cpp b/common/math/geometry/test/test_polygon.cpp index 0cab8a4535f..e0344cd68c8 100644 --- a/common/math/geometry/test/test_polygon.cpp +++ b/common/math/geometry/test/test_polygon.cpp @@ -15,117 +15,94 @@ #include #include +#include #include "expect_eq_macros.hpp" +#include "test_utils.hpp" TEST(Polygon, filterByAxis) { - std::vector points; - geometry_msgs::msg::Point p0; - { - p0.x = 5.0; - p0.y = 2.0; - p0.z = 3.0; - } - points.emplace_back(p0); - geometry_msgs::msg::Point p1; - { - p1.x = 1.0; - p1.y = 4.0; - p1.z = 5.0; - } - points.emplace_back(p1); - geometry_msgs::msg::Point p2; - { - p2.x = -1.0; - p2.y = 2.0; - p2.z = -3.0; - } - points.emplace_back(p2); - std::vector values; - values = math::geometry::filterByAxis(points, math::geometry::Axis::X); - EXPECT_DOUBLE_EQ(values[0], 5); - EXPECT_DOUBLE_EQ(values[1], 1); - EXPECT_DOUBLE_EQ(values[2], -1); - values = math::geometry::filterByAxis(points, math::geometry::Axis::Y); - EXPECT_DOUBLE_EQ(values[0], 2); - EXPECT_DOUBLE_EQ(values[1], 4); - EXPECT_DOUBLE_EQ(values[2], 2); - values = math::geometry::filterByAxis(points, math::geometry::Axis::Z); - EXPECT_DOUBLE_EQ(values[0], 3); - EXPECT_DOUBLE_EQ(values[1], 5); - EXPECT_DOUBLE_EQ(values[2], -3); + std::vector points{ + makePoint(5.0, 2.0, 3.0), makePoint(1.0, 4.0, 5.0), makePoint(-1.0, 2.0, -3.0)}; + + std::vector values_x = math::geometry::filterByAxis(points, math::geometry::Axis::X); + EXPECT_DOUBLE_EQ(values_x[0], 5.0); + EXPECT_DOUBLE_EQ(values_x[1], 1.0); + EXPECT_DOUBLE_EQ(values_x[2], -1.0); + + std::vector values_y = math::geometry::filterByAxis(points, math::geometry::Axis::Y); + EXPECT_DOUBLE_EQ(values_y[0], 2.0); + EXPECT_DOUBLE_EQ(values_y[1], 4.0); + EXPECT_DOUBLE_EQ(values_y[2], 2.0); + + std::vector values_z = math::geometry::filterByAxis(points, math::geometry::Axis::Z); + EXPECT_DOUBLE_EQ(values_z[0], 3.0); + EXPECT_DOUBLE_EQ(values_z[1], 5.0); + EXPECT_DOUBLE_EQ(values_z[2], -3.0); } -TEST(Polygon, GetMinMaxValue) +TEST(Polygon, filterByAxisEmptyVector) { std::vector points; - geometry_msgs::msg::Point p0; - { - p0.x = 5.0; - p0.y = 2.0; - p0.z = 3.0; - } - points.emplace_back(p0); - geometry_msgs::msg::Point p1; - { - p1.x = 1.0; - p1.y = 4.0; - p1.z = 5.0; - } - points.emplace_back(p1); - geometry_msgs::msg::Point p2; - { - p2.x = -1.0; - p2.y = 2.0; - p2.z = -3.0; - } - points.emplace_back(p2); - EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::X), 5); - EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::X), -1); - EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::Y), 4); - EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::Y), 2); - EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::Z), 5); - EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::Z), -3); + + std::vector values_x = math::geometry::filterByAxis(points, math::geometry::Axis::X); + EXPECT_EQ(values_x.size(), size_t(0)); + + std::vector values_y = math::geometry::filterByAxis(points, math::geometry::Axis::Y); + EXPECT_EQ(values_y.size(), size_t(0)); + + std::vector values_z = math::geometry::filterByAxis(points, math::geometry::Axis::Z); + EXPECT_EQ(values_z.size(), size_t(0)); +} + +TEST(Polygon, getMaxValue) +{ + std::vector points{ + makePoint(5.0, 2.0, 3.0), makePoint(1.0, 4.0, 5.0), makePoint(-1.0, 2.0, -3.0)}; + EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::X), 5.0); + EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::Y), 4.0); + EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::Z), 5.0); +} + +TEST(Polygon, getMinValue) +{ + std::vector points{ + makePoint(5.0, 2.0, 3.0), makePoint(1.0, 4.0, 5.0), makePoint(-1.0, 2.0, -3.0)}; + EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::X), -1.0); + EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::Y), 2.0); + EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::Z), -3.0); } TEST(Polygon, get2DConvexHull) { - std::vector points; - geometry_msgs::msg::Point p0; - { - p0.x = 2.0; - p0.y = 2.0; - p0.z = 0.0; - } - points.emplace_back(p0); - geometry_msgs::msg::Point p1; - { - p1.x = -2.0; - p1.y = 2.0; - p1.z = 0.0; - } - points.emplace_back(p1); - geometry_msgs::msg::Point p2; - { - p2.x = -2.0; - p2.y = -2.0; - p2.z = 0.0; - } - points.emplace_back(p2); - geometry_msgs::msg::Point p3; - { - p3.x = -1.0; - p3.y = 0.0; - p3.z = 0.0; - } - points.emplace_back(p3); + std::vector points{ + makePoint(2.0, 2.0), makePoint(-2.0, 2.0), makePoint(-2.0, -2.0), makePoint(-1.0, 0.0)}; const auto hull = math::geometry::get2DConvexHull(points); EXPECT_EQ(hull.size(), static_cast(4)); - EXPECT_POINT_EQ(hull[0], p2); - EXPECT_POINT_EQ(hull[1], p1); - EXPECT_POINT_EQ(hull[2], p0); - EXPECT_POINT_EQ(hull[3], p2); + EXPECT_POINT_EQ(hull[0], points[2]); + EXPECT_POINT_EQ(hull[1], points[1]); + EXPECT_POINT_EQ(hull[2], points[0]); + EXPECT_POINT_EQ(hull[3], points[2]); +} + +TEST(Polygon, get2DConvexHullIdle) +{ + std::vector points{ + makePoint(2.0, 2.0), makePoint(-2.0, 2.0), makePoint(-2.0, -2.0), makePoint(2.0, -2.0)}; + const auto hull = math::geometry::get2DConvexHull(points); + EXPECT_EQ(hull.size(), static_cast(5)); + EXPECT_POINT_EQ(hull[0], points[2]); + EXPECT_POINT_EQ(hull[1], points[1]); + EXPECT_POINT_EQ(hull[2], points[0]); + EXPECT_POINT_EQ(hull[3], points[3]); + EXPECT_POINT_EQ(hull[4], points[2]); +} + +TEST(Polygon, get2DConvexHullEmpty) +{ + std::vector points; + const auto hull = math::geometry::get2DConvexHull(points); + EXPECT_EQ(hull.size(), size_t(0)); } int main(int argc, char ** argv) diff --git a/common/math/geometry/test/test_transform.cpp b/common/math/geometry/test/test_transform.cpp new file mode 100644 index 00000000000..f544d02f4cc --- /dev/null +++ b/common/math/geometry/test/test_transform.cpp @@ -0,0 +1,169 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +#include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +constexpr double EPS = 1e-3; + +geometry_msgs::msg::Pose getFilledPose() +{ + geometry_msgs::msg::Pose pose; + pose.position = makePoint(1.0, 2.0, 3.0); + pose.orientation = + quaternion_operation::convertEulerAngleToQuaternion(makeVector(90.0 * M_PI / 180.0, 0.0)); + return pose; +} + +TEST(Transform, getRelativePoseDifferent) +{ + geometry_msgs::msg::Pose pose0 = getFilledPose(); + geometry_msgs::msg::Pose pose1 = makePose(3.0, 0.0); + + geometry_msgs::msg::Pose ans = makePose( + 2.0, -3.0, 2.0, + quaternion_operation::convertEulerAngleToQuaternion(makeVector(-90.0 * M_PI / 180.0, 0.0))); + EXPECT_POSE_NEAR(math::geometry::getRelativePose(pose0, pose1), ans, EPS); +} + +TEST(Transform, getRelativePoseIdentical) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + EXPECT_POSE_EQ(math::geometry::getRelativePose(pose, pose), geometry_msgs::msg::Pose()); +} + +TEST(Transform, transformPointRealPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + + geometry_msgs::msg::Point ans = makePoint(2.0, -1.0, 5.0); + EXPECT_POINT_EQ(math::geometry::transformPoint(pose, point), ans); +} + +TEST(Transform, transformPointNoPose) +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + EXPECT_POINT_EQ(math::geometry::transformPoint(pose, point), point); +} + +TEST(Transform, transformPointRealSensorPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Pose sensor_pose = getFilledPose(); + sensor_pose.orientation = geometry_msgs::msg::Quaternion(); + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + + geometry_msgs::msg::Point ans = makePoint(1.0, -3.0, 2.0); + EXPECT_POINT_EQ(math::geometry::transformPoint(pose, sensor_pose, point), ans); +} + +TEST(Transform, transformPointNoSensorPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Pose sensor_pose; + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + + geometry_msgs::msg::Point ans = makePoint(2.0, -1.0, 5.0); + EXPECT_POINT_EQ(math::geometry::transformPoint(pose, sensor_pose, point), ans); +} + +TEST(Transform, transformPointsRealPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + std::vector points{makePoint(1.0, 2.0, 3.0), makePoint(1.0, 4.0, 3.0)}; + + geometry_msgs::msg::Point ans0 = makePoint(2.0, -1.0, 5.0); + geometry_msgs::msg::Point ans1 = makePoint(2.0, -1.0, 7.0); + + std::vector ans = math::geometry::transformPoints(pose, points); + EXPECT_EQ(ans.size(), size_t(2)); + EXPECT_POINT_NEAR(ans[0], ans0, EPS); + EXPECT_POINT_NEAR(ans[1], ans1, EPS); +} + +TEST(Transform, transformPointsNoPose) +{ + geometry_msgs::msg::Pose pose; + std::vector points{makePoint(1.0, 2.0, 3.0), makePoint(1.0, 4.0, 3.0)}; + + std::vector ans = math::geometry::transformPoints(pose, points); + EXPECT_EQ(ans.size(), size_t(2)); + EXPECT_POINT_NEAR(ans[0], points[0], EPS); + EXPECT_POINT_NEAR(ans[1], points[1], EPS); +} + +TEST(Transform, transformPointsEmptyVector) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + std::vector points; + + std::vector ans; + EXPECT_NO_THROW(ans = math::geometry::transformPoints(pose, points)); + EXPECT_EQ(ans.size(), size_t(0)); +} + +TEST(Transform, transformPointsRealSensorPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Pose sensor_pose = getFilledPose(); + sensor_pose.orientation = geometry_msgs::msg::Quaternion(); + std::vector points{makePoint(1.0, 2.0, 3.0), makePoint(1.0, 4.0, 3.0)}; + + std::vector ans = + math::geometry::transformPoints(pose, sensor_pose, points); + geometry_msgs::msg::Point ans0 = makePoint(1.0, -3.0, 2.0); + geometry_msgs::msg::Point ans1 = makePoint(1.0, -3.0, 4.0); + EXPECT_EQ(ans.size(), size_t(2)); + EXPECT_POINT_NEAR(ans[0], ans0, EPS); + EXPECT_POINT_NEAR(ans[1], ans1, EPS); +} + +TEST(Transform, transformPointsNoSensorPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Pose sensor_pose; + std::vector points{makePoint(1.0, 2.0, 3.0), makePoint(1.0, 4.0, 3.0)}; + + std::vector ans = + math::geometry::transformPoints(pose, sensor_pose, points); + geometry_msgs::msg::Point ans0 = makePoint(2.0, -1.0, 5.0); + geometry_msgs::msg::Point ans1 = makePoint(2.0, -1.0, 7.0); + EXPECT_EQ(ans.size(), size_t(2)); + EXPECT_POINT_NEAR(ans[0], ans0, EPS); + EXPECT_POINT_NEAR(ans[1], ans1, EPS); +} + +TEST(Transform, transformPointsEmptyVectorWithSensorPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Pose sensor_pose = getFilledPose(); + std::vector points; + + std::vector ans; + EXPECT_NO_THROW(ans = math::geometry::transformPoints(pose, sensor_pose, points)); + EXPECT_EQ(ans.size(), size_t(0)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/math/geometry/test/test_utils.hpp b/common/math/geometry/test/test_utils.hpp new file mode 100644 index 00000000000..31e99e3bdb4 --- /dev/null +++ b/common/math/geometry/test/test_utils.hpp @@ -0,0 +1,107 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOMETRY__TEST__TEST_UTILS_HPP_ +#define GEOMETRY__TEST__TEST_UTILS_HPP_ + +#include +#include +#include +#include + +/** + * Creates a `geometry_msgs::msg::Point` object with the specified coordinates. + * + * @param x The x-coordinate of the point. + * @param y The y-coordinate of the point. + * @param z The z-coordinate of the point. Defaults to 0 if not provided. + * + * @return The created `geometry_msgs::msg::Point` object with the specified coordinates. + */ +inline geometry_msgs::msg::Point makePoint(double x, double y, double z = 0.0) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + p.z = z; + return p; +} + +/** + * Creates a `geometry_msgs::msg::Vector3` object with the specified values for `x`, `y`, and `z` coordinates. + * + * @param x The value for the `x` coordinate. + * @param y The value for the `y` coordinate. + * @param z The value for the `z` coordinate. Default value is 0. + * + * @return A `geometry_msgs::msg::Vector3` object with the specified values for `x`, `y`, and `z` coordinates. + */ +inline geometry_msgs::msg::Vector3 makeVector(double x, double y, double z = 0.0) +{ + geometry_msgs::msg::Vector3 v; + v.x = x; + v.y = y; + v.z = z; + return v; +} + +/** + * Generates a `Pose` message with the given coordinates and orientation. + * + * @param x The x-coordinate of the position. + * @param y The y-coordinate of the position. + * @param z The z-coordinate of the position (default: 0). + * @param q The orientation of the pose (default: identity quaternion). + * + * @return A `Pose` message with the specified coordinates and orientation. + */ +inline geometry_msgs::msg::Pose makePose( + double x, double y, double z = 0.0, + geometry_msgs::msg::Quaternion q = geometry_msgs::msg::Quaternion()) +{ + geometry_msgs::msg::Pose p; + p.position.x = x; + p.position.y = y; + p.position.z = z; + p.orientation = q; + return p; +} + +/** + * Generates a bounding box with the given dimensions and center coordinates. + * + * @param dim_x The dimension of the bounding box along the x-axis. + * @param dim_y The dimension of the bounding box along the y-axis. + * @param dim_z The dimension of the bounding box along the z-axis. (default = 0.0) + * @param center_x The x-coordinate of the center of the bounding box. (default = 0.0) + * @param center_y The y-coordinate of the center of the bounding box. (default = 0.0) + * @param center_z The z-coordinate of the center of the bounding box. (default = 0.0) + * + * @return The generated bounding box. + */ +inline traffic_simulator_msgs::msg::BoundingBox makeBbox( + double dim_x, double dim_y, double dim_z = 0.0, double center_x = 0.0, double center_y = 0.0, + double center_z = 0.0) +{ + traffic_simulator_msgs::msg::BoundingBox bbox; + bbox.dimensions.x = dim_x; + bbox.dimensions.y = dim_y; + bbox.dimensions.z = dim_z; + bbox.center.x = center_x; + bbox.center.y = center_y; + bbox.center.z = center_z; + return bbox; +} + +#endif // GEOMETRY__TEST__TEST_UTILS_HPP_ diff --git a/common/math/geometry/test/vector3/CMakeLists.txt b/common/math/geometry/test/vector3/CMakeLists.txt new file mode 100644 index 00000000000..4c258e2d872 --- /dev/null +++ b/common/math/geometry/test/vector3/CMakeLists.txt @@ -0,0 +1,6 @@ +# These tests have to be separate, because one requires including operator.hpp and other requires not including operator.hpp +ament_add_gtest(test_truncate + test_vector3.cpp + test_truncate_msg.cpp + test_truncate_custom.cpp) +target_link_libraries(test_truncate geometry) diff --git a/common/math/geometry/test/vector3/test_truncate_custom.cpp b/common/math/geometry/test/vector3/test_truncate_custom.cpp new file mode 100644 index 00000000000..0ded3633562 --- /dev/null +++ b/common/math/geometry/test/vector3/test_truncate_custom.cpp @@ -0,0 +1,56 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "../expect_eq_macros.hpp" + +constexpr double EPS = 1e-6; + +/** + * @brief Custom Vector3 struct using T type with multiplication and division operators + */ +template +struct CustomVector3 +{ + T x, y, z; + CustomVector3() = default; + CustomVector3(T x, T y, T z) : x(x), y(y), z(z) {} + template + CustomVector3 operator*(U v) const + { + return CustomVector3(x * v, y * v, z * v); + } + template + CustomVector3 operator/(U v) const + { + return CustomVector3(x / v, y / v, z / v); + } +}; + +TEST(Vector3, truncate_customVectorBelowMax) +{ + CustomVector3 vec0(4.0, 4.0, 4.0); + EXPECT_VECTOR3_EQ(math::geometry::truncate(vec0, 16), vec0) +} + +TEST(Vector3, truncate_customVectorOverMax) +{ + CustomVector3 vec0(4.0, 4.0, 4.0); + EXPECT_VECTOR3_EQ(math::geometry::truncate(vec0, std::sqrt(12.0)), CustomVector3(2.0, 2.0, 2.0)); +} diff --git a/common/math/geometry/test/vector3/test_truncate_msg.cpp b/common/math/geometry/test/vector3/test_truncate_msg.cpp new file mode 100644 index 00000000000..454007e1cf5 --- /dev/null +++ b/common/math/geometry/test/vector3/test_truncate_msg.cpp @@ -0,0 +1,37 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include // this header is neaded for truncate to work with geometry_msgs::msg::Vector3 +#include + +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" + +constexpr double EPS = 1e-6; + +TEST(Vector3, truncate_msgVectorBelowMax) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(4.0, 4.0, 4.0); + EXPECT_VECTOR3_EQ(math::geometry::truncate(vec0, 16), vec0) +} + +TEST(Vector3, truncate_msgVectorOverMax) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(4.0, 4.0, 4.0); + EXPECT_VECTOR3_EQ(math::geometry::truncate(vec0, std::sqrt(12.0)), makeVector(2.0, 2.0, 2.0)); +} diff --git a/common/math/geometry/test/vector3/test_vector3.cpp b/common/math/geometry/test/vector3/test_vector3.cpp new file mode 100644 index 00000000000..6f47513d8a0 --- /dev/null +++ b/common/math/geometry/test/vector3/test_vector3.cpp @@ -0,0 +1,190 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" + +constexpr double EPS = 1e-6; + +/** + * @brief Custom Vector3 struct using T type + */ +template +struct CustomVector3 +{ + T x, y, z; + CustomVector3() = default; + CustomVector3(T x, T y, T z) : x(x), y(y), z(z) {} +}; + +TEST(Vector3, hypot_msgVector) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_DOUBLE_EQ(math::geometry::hypot(vec0, vec1), 6.0); +} + +TEST(Vector3, hypot_customVector) +{ + CustomVector3 vec0(1.0, 2.0, 3.0); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_DOUBLE_EQ(math::geometry::hypot(vec0, vec1), 6.0); +} + +TEST(Vector3, norm_msgVector) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + + EXPECT_DOUBLE_EQ(math::geometry::norm(vec0), std::sqrt(14.0)); +} + +TEST(Vector3, norm_customVector) +{ + CustomVector3 vec0(1u, 2u, 3u); + + EXPECT_DOUBLE_EQ(math::geometry::norm(vec0), std::sqrt(14.0)); +} + +TEST(Vector3, normalize_msgVector) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + + const double norm = std::sqrt(14.0); + EXPECT_VECTOR3_EQ( + math::geometry::normalize(vec0), makeVector(1.0 / norm, 2.0 / norm, 3.0 / norm)); +} + +TEST(Vector3, normalize_customVector) +{ + CustomVector3 vec0(1.0, 2.0, 3.0); + + const double norm = std::sqrt(14.0); + EXPECT_VECTOR3_NEAR( + math::geometry::normalize(vec0), makeVector(1.0 / norm, 2.0 / norm, 3.0 / norm), EPS); +} + +TEST(Vector3, addition_msgVector) +{ + using math::geometry::operator+; + + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_VECTOR3_EQ((vec0 + vec1), makeVector(0.0, 0.0, 2.0)); +} + +TEST(Vector3, addition_customVector) +{ + using math::geometry::operator+; + + CustomVector3 vec0(1u, 2u, 3u); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_VECTOR3_EQ((vec0 + vec1), makeVector(0.0, 0.0, 2.0)); +} + +TEST(Vector3, subtraction_msgVector) +{ + using math::geometry::operator-; + + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_VECTOR3_EQ((vec0 - vec1), makeVector(2.0, 4.0, 4.0)); +} + +TEST(Vector3, subtraction_CustomVector) +{ + using math::geometry::operator-; + + CustomVector3 vec0(1u, 2u, 3u); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_VECTOR3_EQ((vec0 - vec1), makeVector(2.0, 4.0, 4.0)); +} + +TEST(Vector3, multiplication_msgVector) +{ + using math::geometry::operator*; + + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + + EXPECT_VECTOR3_EQ((vec0 * 3.0), makeVector(3.0, 6.0, 9.0)); +} + +TEST(Vector3, multiplication_CustomVector) +{ + using math::geometry::operator*; + + CustomVector3 vec0(1u, 2u, 3u); + + EXPECT_VECTOR3_EQ((vec0 * 3.0), makeVector(3.0, 6.0, 9.0)); +} + +TEST(Vector3, division_msgVector) +{ + using math::geometry::operator/; + + geometry_msgs::msg::Vector3 vec0 = makeVector(2.0, 14.0, 6.0); + + EXPECT_VECTOR3_EQ((vec0 / 2.0), makeVector(1.0, 7.0, 3.0)); +} + +TEST(Vector3, division_CustomVector) +{ + using math::geometry::operator/; + + CustomVector3 vec0(2u, 14u, 6u); + + EXPECT_VECTOR3_EQ((vec0 / 2.0), makeVector(1.0, 7.0, 3.0)); +} + +TEST(Vector3, additionAssignment_msgVector) +{ + using math::geometry::operator+=; + + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + vec0 += vec1; + + EXPECT_VECTOR3_EQ(vec0, makeVector(0.0, 0.0, 2.0)); +} + +TEST(Vector3, additionAssignment_customVector) +{ + using math::geometry::operator+=; + + CustomVector3 vec0(1u, 2u, 3u); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + vec0 += vec1; + + EXPECT_VECTOR3_EQ(vec0, makeVector(0.0, 0.0, 2.0)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}