[test_tf2] PoseStamped and Quaternion convert tests

Author: Bjar Ne

test_tf2/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 
 project(test_tf2)
 
-find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rostest tf tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs)
+find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rostest tf tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs tf2_eigen)
 find_package(Boost REQUIRED COMPONENTS thread)
 find_package(orocos_kdl REQUIRED)
 

test_tf2/package.xml

@@ -23,6 +23,7 @@
   <build_depend>tf2_geometry_msgs</build_depend>
   <build_depend>tf2_kdl</build_depend>
   <build_depend>tf2_msgs</build_depend>
+  <build_depend>tf2_eigen</build_depend>
 
   <run_depend>rosconsole</run_depend>
   <run_depend>roscpp</run_depend>
@@ -34,6 +35,7 @@
   <run_depend>tf2_geometry_msgs</run_depend>
   <run_depend>tf2_kdl</run_depend>
   <run_depend>tf2_msgs</run_depend>
+  <run_depend>tf2_eigen</run_depend>
 
   <test_depend>rosunit</test_depend>
   <test_depend>rosbash</test_depend>

test_tf2/test/test_convert.cpp

@@ -39,6 +39,7 @@
 #include <tf2_kdl/tf2_kdl.h>
 #include <tf2_bullet/tf2_bullet.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_eigen/tf2_eigen.h>
 #include <ros/time.h>
 
 TEST(tf2Convert, kdlToBullet)
@@ -95,7 +96,88 @@ TEST(tf2Convert, kdlBulletROSConversions)
   EXPECT_NEAR(b1.x(), b4.x(), epsilon);
   EXPECT_NEAR(b1.y(), b4.y(), epsilon);
   EXPECT_NEAR(b1.z(), b4.z(), epsilon);
-} 
+}
+
+TEST(tf2Convert, PoseStampedConversions) {
+  double epsilon = 1e-9;
+
+  const tf2::Stamped<tf2::Transform> p_tf2_1(tf2::Transform(tf2::Quaternion(1.0,0.0,0.0,2.0), tf2::Vector3(1,2,3)), ros::Time(), "my_frame");
+  tf2::Stamped<tf2::Transform> p_tf2_2;
+  geometry_msgs::PoseStamped msg;
+  tf2::toMsg(p_tf2_1, msg);
+
+  tf2::Stamped<Eigen::Isometry3d> p_e_iso;
+  tf2::Stamped<Eigen::Affine3d> p_e_aff;
+  tf2::Stamped<KDL::Frame> p_f;
+
+  tf2::convert(p_tf2_1, p_e_iso);
+  tf2::convert(p_e_iso, p_f);
+  tf2::convert(p_f, p_e_aff);
+  tf2::convert(p_e_aff, p_tf2_2);
+
+  tf2::toMsg(p_e_aff, msg);
+
+  EXPECT_EQ(p_tf2_1.frame_id_, p_tf2_2.frame_id_);
+  EXPECT_NEAR(p_tf2_1.stamp_.toSec(), p_tf2_2.stamp_.toSec(), epsilon);
+
+  const auto& q1(p_tf2_1.getRotation()), q2(p_tf2_2.getRotation());
+  EXPECT_NEAR(q1.x(), q2.x(), epsilon);
+  EXPECT_NEAR(q1.y(), q2.y(), epsilon);
+  EXPECT_NEAR(q1.z(), q2.z(), epsilon);
+  EXPECT_NEAR(q1.w(), q2.w(), epsilon);
+
+  const auto& o1(p_tf2_1.getOrigin()), o2(p_tf2_2.getOrigin());
+  EXPECT_NEAR(o1.x(), o2.x(), epsilon);
+  EXPECT_NEAR(o1.y(), o2.y(), epsilon);
+  EXPECT_NEAR(o1.z(), o2.z(), epsilon);
+}
+
+TEST(tf2Convert, QuaternionStampedConversations)
+{
+  const double epsilon = 1e-9;
+  const tf2::Stamped<Eigen::Quaterniond> q_e_1(Eigen::Quaterniond(2.0, 4.0, 0.25, -1),
+                                               ros::Time(), "my_frame");
+  tf2::Stamped<tf2::Quaternion> q_tf_1;
+  tf2::convert(q_e_1, q_tf_1);
+
+  EXPECT_EQ(q_e_1.frame_id_, q_tf_1.frame_id_);
+  EXPECT_NEAR(q_e_1.stamp_.toSec(), q_tf_1.stamp_.toSec(), epsilon);
+  EXPECT_NEAR(q_e_1.x(), q_tf_1.x(), epsilon);
+  EXPECT_NEAR(q_e_1.y(), q_tf_1.y(), epsilon);
+  EXPECT_NEAR(q_e_1.z(), q_tf_1.z(), epsilon);
+  EXPECT_NEAR(q_e_1.w(), q_tf_1.w(), epsilon);
+
+  tf2::Stamped<Eigen::Quaterniond> q_e_2;
+  tf2::convert(q_tf_1, q_e_2);
+
+  EXPECT_EQ(q_e_2.frame_id_, q_tf_1.frame_id_);
+  EXPECT_NEAR(q_e_2.stamp_.toSec(), q_tf_1.stamp_.toSec(), epsilon);
+  EXPECT_NEAR(q_e_2.x(), q_tf_1.x(), epsilon);
+  EXPECT_NEAR(q_e_2.y(), q_tf_1.y(), epsilon);
+  EXPECT_NEAR(q_e_2.z(), q_tf_1.z(), epsilon);
+  EXPECT_NEAR(q_e_2.w(), q_tf_1.w(), epsilon);
+}
+
+TEST(tf2Convert, QuaternionConversations)
+{
+  const double epsilon = 1e-9;
+  const Eigen::Quaterniond q_e_1(2.0, 4.0, 0.25, -1);
+  tf2::Quaternion q_tf_1;
+  tf2::convert(q_e_1, q_tf_1);
+
+  EXPECT_NEAR(q_e_1.x(), q_tf_1.x(), epsilon);
+  EXPECT_NEAR(q_e_1.y(), q_tf_1.y(), epsilon);
+  EXPECT_NEAR(q_e_1.z(), q_tf_1.z(), epsilon);
+  EXPECT_NEAR(q_e_1.w(), q_tf_1.w(), epsilon);
+
+  Eigen::Quaterniond q_e_2;
+  tf2::convert(q_tf_1, q_e_2);
+
+  EXPECT_NEAR(q_e_2.x(), q_tf_1.x(), epsilon);
+  EXPECT_NEAR(q_e_2.y(), q_tf_1.y(), epsilon);
+  EXPECT_NEAR(q_e_2.z(), q_tf_1.z(), epsilon);
+  EXPECT_NEAR(q_e_2.w(), q_tf_1.w(), epsilon);
+}
 
 int main(int argc, char** argv)
 {