Conversion tests for toMsg()

test_tf2/CMakeLists.txt

@@ -20,23 +20,20 @@ endif()
 
 find_package(ament_cmake_gtest REQUIRED)
 find_package(builtin_interfaces REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
 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})
-
 ament_add_gtest(buffer_core_test test/buffer_core_test.cpp)
 if(TARGET buffer_core_test)
   ament_target_dependencies(buffer_core_test
@@ -60,14 +57,18 @@ 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
+    Eigen3
+    eigen3_cmake_module
+    tf2
+    tf2_bullet
+    tf2_eigen
+    tf2_geometry_msgs
+    tf2_kdl
+  )
+endif()
 
 ament_add_gtest(test_utils test/test_utils.cpp)
 if(TARGET test_utils)
@@ -79,28 +80,27 @@ if(TARGET test_utils)
   )
 endif()
 
-# TODO (ahcorde): activate when tf2_bullet is merged
-# add_executable(test_buffer_server test/test_buffer_server.cpp)
-# if(TARGET test_buffer_server)
-#   ament_target_dependencies(test_buffer_server
-#     rclcpp
-#     tf2_bullet
-#     tf2_ros
-#   )
-# endif()
-
-# TODO (ahcorde): activate when tf2_bullet is merged
-# add_executable(test_buffer_client test/test_buffer_client.cpp)
-# if(TARGET test_buffer_client)
-#   ament_target_dependencies(test_buffer_client
-#     rclcpp
-#     tf2_bullet
-#     tf2_kdl
-#     tf2_ros
-#   )
-#   target_link_libraries(test_buffer_client ${GTEST_LIBRARIES})
-#   add_launch_test(test/buffer_client_tester.launch.py)
-# endif()
+add_executable(test_buffer_server test/test_buffer_server.cpp)
+if(TARGET test_buffer_server)
+  ament_target_dependencies(test_buffer_server
+    rclcpp
+    tf2_bullet
+    tf2_ros
+  )
+endif()
+
+add_executable(test_buffer_client test/test_buffer_client.cpp)
+if(TARGET test_buffer_client)
+  ament_target_dependencies(test_buffer_client
+    rclcpp
+    tf2_bullet
+    tf2_geometry_msgs
+    tf2_kdl
+    tf2_ros
+  )
+  target_link_libraries(test_buffer_client ${GTEST_LIBRARIES})
+  add_launch_test(test/buffer_client_tester.launch.py)
+endif()
 
 add_executable(test_static_publisher test/test_static_publisher.cpp)
 if(TARGET test_static_publisher)
@@ -115,23 +115,22 @@ if(TARGET test_static_publisher)
   add_launch_test(test/static_publisher.launch.py)
 endif()
 
-# TODO (ahcorde): activate when tf2_bullet is merged
-# ament_add_gtest(test_tf2_bullet test/test_tf2_bullet.cpp)
-# if(TARGET test_tf2_bullet)
-#   ament_target_dependencies(test_tf2_bullet
-#       rclcpp
-#       tf2_bullet
-#       tf2_ros
-#   )
-# endif()
-#
+ament_add_gtest(test_tf2_bullet test/test_tf2_bullet.cpp)
+if(TARGET test_tf2_bullet)
+  ament_target_dependencies(test_tf2_bullet
+      rclcpp
+      tf2_bullet
+      tf2_ros
+  )
+endif()
+
+# TODO(ahcorde): enable once python part of tf2_geometry_msgs is working
 # add_launch_test(test/test_buffer_client.launch.py)
-# add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_bullet.launch)
 
 # install executables
 install(TARGETS
-  # test_buffer_client
-  # test_buffer_server
+  test_buffer_client
+  test_buffer_server
   test_static_publisher
   DESTINATION lib/${PROJECT_NAME}
 )

test_tf2/package.xml

@@ -12,11 +12,16 @@
   <author>Eitan Marder-Eppstein</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <build_depend>eigen</build_depend>
 
   <depend>builtin_interfaces</depend>
   <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

@@ -35,16 +35,23 @@
 * Author: Eitan Marder-Eppstein
 *********************************************************************/
 #include <gtest/gtest.h>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
 #include <tf2/convert.h>
+#include <tf2/LinearMath/Quaternion.h>
 #include <tf2_kdl/tf2_kdl.hpp>
 #include <tf2_bullet/tf2_bullet.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
+#include <Eigen/Geometry>
+
 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,11 +78,12 @@ 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;
 
-  // Do bullet -> self -> bullet -> KDL -> self -> KDL -> ROS -> self -> ROS -> KDL -> bullet -> ROS -> bullet
+  // Do bullet -> self -> bullet -> KDL -> self -> KDL -> ROS -> self
+  //   -> ROS -> KDL -> bullet -> ROS -> bullet
   tf2::convert(b1, b1);
   tf2::convert(b1, b2);
   tf2::convert(b2, k1);
@@ -96,7 +104,91 @@ TEST(tf2Convert, kdlBulletROSConversions)
   EXPECT_NEAR(b1.z(), b4.z(), epsilon);
 }
 
-int main(int argc, char** argv)
+TEST(tf2Convert, ConvertTf2Quaternion)
+{
+  const tf2::Quaternion tq(1, 2, 3, 4);
+  Eigen::Quaterniond eq;
+  // TODO(gleichdick): switch to tf2::convert() when it's working
+  tf2::fromMsg(tf2::toMsg(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::TimePoint(), "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_);
+  }
+  {
+    // 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::TimePoint(), "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_);
+  }
+}
+
+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);
+  }
+}
+
+int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();

tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp

@@ -35,12 +35,15 @@
 #include <tf2/convert.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2/LinearMath/Transform.h>
+#include <tf2/LinearMath/Vector3.h>
 #include <tf2_ros/buffer_interface.h>
 #include <geometry_msgs/msg/point_stamped.hpp>
 #include <geometry_msgs/msg/quaternion_stamped.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
 #include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <kdl/frames.hpp>
@@ -60,6 +63,35 @@ KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped& t)
 		      KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
   }
 
+/*************/
+/** Vector3 **/
+/*************/
+
+/** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * \param in A tf2 Vector3 object.
+ * \return The Vector3 converted to a geometry_msgs message type.
+ */
+inline
+geometry_msgs::msg::Vector3 toMsg(const tf2::Vector3 & in)
+{
+  geometry_msgs::msg::Vector3 out;
+  out.x = in.getX();
+  out.y = in.getY();
+  out.z = in.getZ();
+  return out;
+}
+
+/** \brief Convert a Vector3 message to its equivalent tf2 representation.
+ * This function is a specialization of the fromMsg template defined in tf2/convert.h.
+ * \param in A Vector3 message type.
+ * \param out The Vector3 converted to a tf2 type.
+ */
+inline
+void fromMsg(const geometry_msgs::msg::Vector3 & in, tf2::Vector3 & out)
+{
+  out = tf2::Vector3(in.x, in.y, in.z);
+}
 
 /********************/
 /** Vector3Stamped **/
@@ -124,6 +156,35 @@ void fromMsg(const geometry_msgs::msg::Vector3Stamped& msg, geometry_msgs::msg::
 }
 
 
+/***********/
+/** Point **/
+/***********/
+
+/** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * \param in A tf2 Vector3 object.
+ * \return The Vector3 converted to a geometry_msgs message type.
+ */
+inline
+geometry_msgs::msg::Point & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point & out)
+{
+  out.x = in.getX();
+  out.y = in.getY();
+  out.z = in.getZ();
+  return out;
+}
+
+/** \brief Convert a Vector3 message to its equivalent tf2 representation.
+ * This function is a specialization of the fromMsg template defined in tf2/convert.h.
+ * \param in A Vector3 message type.
+ * \param out The Vector3 converted to a tf2 type.
+ */
+inline
+void fromMsg(const geometry_msgs::msg::Point & in, tf2::Vector3 & out)
+{
+  out = tf2::Vector3(in.x, in.y, in.z);
+}
+
 
 /******************/
 /** PointStamped **/