diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index 561793c46..7d09b6009 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -35,72 +35,83 @@ #endif #include +#include +#include +#include +#include +#include #include #include +#include #include #include #include +#include + #include #include -// TODO(clalancette) Re-enable these tests once we have tf2/convert.h:convert(A, B) implemented -// TEST(TfEigen, ConvertVector3dStamped) -// { -// const tf2::Stamped v(Eigen::Vector3d(1,2,3), -// tf2::TimePoint(std::chrono::seconds(5)), "test"); - -// tf2::Stamped v1; -// geometry_msgs::msg::PointStamped p1; -// tf2::convert(v, p1); -// tf2::convert(p1, v1); - -// EXPECT_EQ(v, v1); -// } - -// TEST(TfEigen, ConvertVector3d) -// { -// const Eigen::Vector3d v(1,2,3); - -// Eigen::Vector3d v1; -// geometry_msgs::msg::Point p1; -// tf2::convert(v, p1); -// tf2::convert(p1, v1); - -// EXPECT_EQ(v, v1); -// } - -// TEST(TfEigen, ConvertAffine3dStamped) -// { -// const Eigen::Affine3d v_nonstamped( -// Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); -// const tf2::Stamped v( -// v_nonstamped, tf2::TimePoint(std::chrono::seconds(42)), "test_frame"); - -// tf2::Stamped v1; -// geometry_msgs::msg::PoseStamped p1; -// tf2::convert(v, p1); -// tf2::convert(p1, v1); - -// EXPECT_EQ(v.translation(), v1.translation()); -// EXPECT_EQ(v.rotation(), v1.rotation()); -// EXPECT_EQ(v.frame_id_, v1.frame_id_); -// EXPECT_EQ(v.stamp_, v1.stamp_); -// } - -// TEST(TfEigen, ConvertAffine3d) -// { -// const Eigen::Affine3d v( -// Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); - -// Eigen::Affine3d v1; -// geometry_msgs::msg::Pose p1; -// tf2::convert(v, p1); -// tf2::convert(p1, v1); - -// EXPECT_EQ(v.translation(), v1.translation()); -// EXPECT_EQ(v.rotation(), v1.rotation()); -// } +TEST(TfEigen, ConvertVector3dStamped) +{ + const tf2::Stamped v(Eigen::Vector3d(1, 2, 3), tf2::TimePoint( + std::chrono::seconds( + 5)), "test"); + + tf2::Stamped v1; + geometry_msgs::msg::PointStamped p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v, v1); +} + +TEST(TfEigen, ConvertVector3d) +{ + const Eigen::Vector3d v(1, 2, 3); + + Eigen::Vector3d v1; + geometry_msgs::msg::Point p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v, v1); +} + +TEST(TfEigen, ConvertAffine3dStamped) +{ + const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxis( + 1, + Eigen::Vector3d::UnitX())); + const tf2::Stamped v(v_nonstamped, tf2::TimePoint( + std::chrono::seconds( + 42)), "test_frame"); + + tf2::Stamped v1; + geometry_msgs::msg::PoseStamped p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v.translation(), v1.translation()); + EXPECT_EQ(v.rotation(), v1.rotation()); + EXPECT_EQ(v.frame_id_, v1.frame_id_); + EXPECT_EQ(v.stamp_, v1.stamp_); +} + +TEST(TfEigen, ConvertAffine3d) +{ + const Eigen::Affine3d v(Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxis( + 1, + Eigen::Vector3d::UnitX())); + + Eigen::Affine3d v1; + geometry_msgs::msg::Pose p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v.translation(), v1.translation()); + EXPECT_EQ(v.rotation(), v1.rotation()); +} TEST(TfEigen, ConvertTransform) {