New approach for toMsg()/fromMsg()

test_tf2/CMakeLists.txt

@@ -24,18 +24,19 @@ find_package(geometry_msgs REQUIRED)
 find_package(launch_testing_ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(tf2 REQUIRED)
-# TODO (ahcorde): activate when tf2_bullet is merged
-# find_package(tf2_bullet REQUIRED)
+find_package(tf2_bullet REQUIRED)
+find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_kdl REQUIRED)
 find_package(tf2_ros REQUIRED)
 
 ament_find_gtest()
 
-# TODO (ahcorde): activate when tf2_bullet is merged
-# find_package(PkgConfig REQUIRED)
-# pkg_check_modules(bullet REQUIRED bullet)
-# include_directories(include ${bullet_INCLUDE_DIRS})
+if(WIN32)
+  set(BULLET_ROOT $ENV{ChocolateyInstall}/lib/bullet)
+endif()
+find_package(Bullet REQUIRED)
+include_directories(include ${BULLET_INCLUDE_DIRS})
 
 ament_add_gtest(buffer_core_test test/buffer_core_test.cpp)
 if(TARGET buffer_core_test)
@@ -60,14 +61,16 @@ if(TARGET test_message_filter)
   )
 endif()
 
-# TODO (ahcorde): activate when tf2_bullet is merged
-# ament_add_gtest(test_convert test/test_convert.cpp)
-# if(TARGET test_convert)
-#   ament_target_dependencies(test_convert
-#     tf2
-#     tf2_bullet
-#   )
-# endif()
+ament_add_gtest(test_convert test/test_convert.cpp)
+if(TARGET test_convert)
+  ament_target_dependencies(test_convert
+    tf2
+    tf2_bullet
+    tf2_eigen
+    tf2_geometry_msgs
+    tf2_kdl
+  )
+endif()
 
 ament_add_gtest(test_utils test/test_utils.cpp)
 if(TARGET test_utils)

test_tf2/package.xml

@@ -17,6 +17,8 @@
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
   <depend>tf2</depend>
+  <depend>tf2_bullet</depend>
+  <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_kdl</depend>
   <depend>tf2_ros</depend>

test_tf2/test/test_convert.cpp

@@ -38,13 +38,68 @@
 #include <tf2/convert.h>
 #include <tf2_kdl/tf2_kdl.h>
 #include <tf2_bullet/tf2_bullet.h>
+#include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+
+
+// test of tf2 type traits
+static_assert(
+  !tf2::impl::MessageHasStdHeader<geometry_msgs::msg::Vector3>::value,
+  "MessageHasStdHeader traits error");
+static_assert(
+  tf2::impl::MessageHasStdHeader<geometry_msgs::msg::Vector3Stamped>::value,
+  "MessageHasStdHeader traits error");
+
+namespace
+{
+struct MyPODMessage
+{
+  int header;
+};
+
+template<typename Alloc>
+struct MyAllocMessage
+{
+  int header;
+};
+
+static_assert(
+  !tf2::impl::MessageHasStdHeader<MyPODMessage>::value,
+  "MessageHasStdHeader traits error");
+
+template<typename T>
+struct MyAllocator
+{
+  using value_type = T;
+  using size_type = unsigned int;
+  T * allocate(size_type);
+  void deallocate(T *, size_type);
+  template<class U>
+  struct rebind { typedef MyAllocator<U> other; };
+};
+using MyVec = geometry_msgs::msg::Vector3_<MyAllocator<void>>;
+using MyVecStamped = geometry_msgs::msg::Vector3Stamped_<MyAllocator<void>>;
+using MyMessage = MyAllocMessage<MyAllocator<void>>;
+
+static_assert(!tf2::impl::MessageHasStdHeader<MyVec>::value, "MessageHasStdHeader traits error");
+static_assert(
+  tf2::impl::MessageHasStdHeader<MyVecStamped>::value,
+  "MessageHasStdHeader traits error");
+
+static_assert(
+  !tf2::impl::MessageHasStdHeader<MyMessage>::value,
+  "MessageHasStdHeader traits error");
+}
+
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+
 TEST(tf2Convert, kdlToBullet)
 {
   double epsilon = 1e-9;
 
-  tf2::Stamped<btVector3> b(btVector3(1,2,3), tf2::timeFromSec(0), "my_frame");
+  tf2::Stamped<btVector3> b(btVector3(1, 2, 3), tf2::timeFromSec(0), "my_frame");
 
   tf2::Stamped<btVector3> b1 = b;
   tf2::Stamped<KDL::Vector> k1;
@@ -71,7 +126,7 @@ TEST(tf2Convert, kdlBulletROSConversions)
 {
   double epsilon = 1e-9;
 
-  tf2::Stamped<btVector3> b1(btVector3(1,2,3), tf2::timeFromSec(0), "my_frame"), b2, b3, b4;
+  tf2::Stamped<btVector3> b1(btVector3(1, 2, 3), tf2::timeFromSec(0), "my_frame"), b2, b3, b4;
   geometry_msgs::msg::PointStamped r1, r2, r3;
   tf2::Stamped<KDL::Vector> k1, k2, k3;
 
@@ -96,7 +151,192 @@ TEST(tf2Convert, kdlBulletROSConversions)
   EXPECT_NEAR(b1.z(), b4.z(), epsilon);
 }
 
-int main(int argc, char** argv)
+TEST(tf2Convert, ConvertTf2Quaternion)
+{
+  tf2::Quaternion tq(1, 2, 3, 4);
+  Eigen::Quaterniond eq;
+  tf2::convert(tq, eq);
+
+  EXPECT_EQ(tq.w(), eq.w());
+  EXPECT_EQ(tq.x(), eq.x());
+  EXPECT_EQ(tq.y(), eq.y());
+  EXPECT_EQ(tq.z(), eq.z());
+}
+
+TEST(tf2Convert, PointVectorDefaultMessagetype)
+{
+  // Verify the return type of `toMsg()`
+  // as it can return a Vector3 or a Point for certain datatypes
+  {
+    // Bullet
+    const tf2::Stamped<btVector3> b1{btVector3{1.0, 3.0, 4.0}, tf2::get_now(), "my_frame"};
+    const geometry_msgs::msg::PointStamped msg = tf2::toMsg(b1);
+
+    EXPECT_EQ(msg.point.x, 1.0);
+    EXPECT_EQ(msg.point.y, 3.0);
+    EXPECT_EQ(msg.point.z, 4.0);
+    EXPECT_EQ(msg.header.frame_id, b1.frame_id_);
+    EXPECT_EQ(msg.header.stamp, tf2::toMsg<>(b1.stamp_));
+  }
+  {
+    // Eigen
+    const Eigen::Vector3d e1{2.0, 4.0, 5.0};
+    const geometry_msgs::msg::Point msg = tf2::toMsg(e1);
+
+    EXPECT_EQ(msg.x, 2.0);
+    EXPECT_EQ(msg.y, 4.0);
+    EXPECT_EQ(msg.z, 5.0);
+  }
+  {
+    // tf2
+    const tf2::Vector3 t1{2.0, 4.0, 5.0};
+    const geometry_msgs::msg::Vector3 msg = tf2::toMsg(t1);
+
+    EXPECT_EQ(msg.x, 2.0);
+    EXPECT_EQ(msg.y, 4.0);
+    EXPECT_EQ(msg.z, 5.0);
+  }
+  {
+    // KDL
+    const tf2::Stamped<KDL::Vector> k1{KDL::Vector{1.0, 3.0, 4.0}, tf2::get_now(), "my_frame"};
+    const geometry_msgs::msg::PointStamped msg = tf2::toMsg(k1);
+
+    EXPECT_EQ(msg.point.x, 1.0);
+    EXPECT_EQ(msg.point.y, 3.0);
+    EXPECT_EQ(msg.point.z, 4.0);
+    EXPECT_EQ(msg.header.frame_id, k1.frame_id_);
+    EXPECT_EQ(msg.header.stamp, tf2::toMsg<>(k1.stamp_));
+  }
+}
+
+TEST(tf2Convert, PointVectorOtherMessagetype)
+{
+  {
+    const tf2::Vector3 t1{2.0, 4.0, 5.0};
+    geometry_msgs::msg::Point msg;
+    const geometry_msgs::msg::Point & msg2 = tf2::toMsg(t1, msg);
+
+    // returned reference is second argument
+    EXPECT_EQ(&msg2, &msg);
+    EXPECT_EQ(msg.x, 2.0);
+    EXPECT_EQ(msg.y, 4.0);
+    EXPECT_EQ(msg.z, 5.0);
+  }
+  {
+    // Eigen
+    const Eigen::Vector3d e1{2.0, 4.0, 5.0};
+    geometry_msgs::msg::Vector3 msg;
+    const geometry_msgs::msg::Vector3 & msg2 = tf2::toMsg(e1, msg);
+
+    // returned reference is second argument
+    EXPECT_EQ(&msg2, &msg);
+    EXPECT_EQ(msg.x, 2.0);
+    EXPECT_EQ(msg.y, 4.0);
+    EXPECT_EQ(msg.z, 5.0);
+  }
+}
+
+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, TestTwistMatrix)
+{
+  const auto kdl_v = KDL::Twist(KDL::Vector(1, 2, 3), KDL::Vector(4, 5, 6));
+  Vector6d eigen_v;
+  tf2::convert(kdl_v, eigen_v);
+  KDL::Twist kdl_v1;
+  tf2::convert(eigen_v, kdl_v1);
+  EXPECT_EQ(kdl_v, kdl_v1);
+}
+
+TEST(TfEigenKdl, TestMatrixWrench)
+{
+  Vector6d eigen_v;
+  eigen_v << 1, 2, 3, 3, 2, 1;
+  KDL::Wrench kdl_v;
+  tf2::convert(eigen_v, kdl_v);
+  std::array<tf2::Vector3, 2> tf2_v;
+  tf2::convert(kdl_v, tf2_v);
+  Vector6d eigen_v1;
+  tf2::convert(tf2_v, eigen_v1);
+  std::array<tf2::Vector3, 2> tf2_v1;
+  tf2::convert(eigen_v1, tf2_v1);
+  Vector6d eigen_v2;
+  tf2::convert(tf2_v1, eigen_v2);
+  EXPECT_EQ(eigen_v, eigen_v2);
+}
+
+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);
   return RUN_ALL_TESTS();

tf2/CMakeLists.txt

@@ -11,6 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake)
+find_package(builtin_interfaces REQUIRED)
 find_package(console_bridge_vendor REQUIRED) # Provides console_bridge 0.4.0 on platforms without it.
 find_package(console_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -25,6 +26,7 @@ target_include_directories(tf2 PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/bt>"
   "$<INSTALL_INTERFACE:include>")
 ament_target_dependencies(tf2
+  "builtin_interfaces"
   "console_bridge"
   "geometry_msgs"
   "rcutils"
@@ -89,7 +91,7 @@ if(BUILD_TESTING)
 
 endif()
 
-ament_export_dependencies(console_bridge geometry_msgs rcutils)
+ament_export_dependencies(builtin_interfaces console_bridge geometry_msgs rcutils)
 ament_export_include_directories(include)
 ament_export_libraries(tf2)
 ament_export_targets(tf2)