Add testcases from tf2_eigen_kdl to tf2_test

Author: gleichdick

test_tf2/test/test_convert.cpp

@@ -182,6 +182,80 @@ TEST(tf2Convert, PointVectorOtherMessagetype)
   }
 }
 
+TEST(TfEigenKdl, TestRotationQuaternion)
+{
+  const auto kdl_v = KDL::Rotation::RPY(1.5, 0.2, 0.3);
+  Eigen::Quaterniond eigen_v = Eigen::Quaterniond::Identity();
+  tf2::convert(kdl_v, eigen_v);
+  KDL::Rotation kdl_v1;
+  tf2::convert(eigen_v, kdl_v1);
+  EXPECT_EQ(kdl_v, kdl_v1);
+}
+
+TEST(TfEigenKdl, TestQuaternionRotation)
+{
+  const Eigen::Quaterniond eigen_v = Eigen::Quaterniond(1, 2, 1.5, 3).normalized();
+  KDL::Rotation kdl_v;
+  tf2::convert(eigen_v, kdl_v);
+  Eigen::Quaterniond eigen_v1;
+  tf2::convert(kdl_v, eigen_v1);
+  EXPECT_TRUE(eigen_v.isApprox(eigen_v1));
+}
+
+TEST(TfEigenKdl, TestFrameIsometry3d)
+{
+  const auto kdl_v = KDL::Frame(KDL::Rotation::RPY(1.2, 0.2, 0), KDL::Vector(1, 2, 3));
+  Eigen::Isometry3d eigen_v = Eigen::Isometry3d::Identity();
+  tf2::convert(kdl_v, eigen_v);
+  KDL::Frame kdl_v1;
+  tf2::convert(eigen_v, kdl_v1);
+  EXPECT_EQ(kdl_v, kdl_v1);
+}
+
+TEST(TfEigenKdl, TestIsometry3dFrame)
+{
+  const Eigen::Isometry3d eigen_v(
+    Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxisd(1, Eigen::Vector3d::UnitX()));
+  KDL::Frame kdl_v;
+  tf2::convert(eigen_v, kdl_v);
+  Eigen::Isometry3d eigen_v1;
+  tf2::convert(kdl_v, eigen_v1);
+  EXPECT_EQ(eigen_v.translation(), eigen_v1.translation());
+  EXPECT_EQ(eigen_v.rotation(), eigen_v1.rotation());
+}
+
+TEST(TfEigenKdl, TestFrameAffine3d)
+{
+  const auto kdl_v = KDL::Frame(KDL::Rotation::RPY(1.2, 0.2, 0), KDL::Vector(1, 2, 3));
+  Eigen::Affine3d eigen_v = Eigen::Affine3d::Identity();
+  tf2::convert(kdl_v, eigen_v);
+  KDL::Frame kdl_v1;
+  tf2::convert(eigen_v, kdl_v1);
+  EXPECT_EQ(kdl_v, kdl_v1);
+}
+
+
+TEST(TfEigenKdl, TestVectorVector3d)
+{
+  const auto kdl_v = KDL::Vector(1, 2, 3);
+  Eigen::Vector3d eigen_v;
+  tf2::convert(kdl_v, eigen_v);
+  KDL::Vector kdl_v1;
+  tf2::convert(eigen_v, kdl_v1);
+  EXPECT_EQ(kdl_v, kdl_v1);
+}
+
+TEST(TfEigenKdl, TestVector3dVector)
+{
+  Eigen::Vector3d eigen_v;
+  eigen_v << 1, 2, 3;
+  KDL::Vector kdl_v;
+  tf2::convert(eigen_v, kdl_v);
+  Eigen::Vector3d eigen_v1;
+  tf2::convert(kdl_v, eigen_v1);
+  EXPECT_EQ(eigen_v, eigen_v1);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);