ROS2 tf2-eigen Reloaded (#4)

* Make tf2_eigen work for ROS2. Signed-off-by: Chris Lalancette <[email protected]>
ros2 · Apr 24, 2017 · 735f7a4 · 735f7a4
1 parent e2392e0
commit 735f7a4
Show file tree

Hide file tree

Showing 10 changed files with 128 additions and 110 deletions.
diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h
@@ -118,6 +118,7 @@ template <class A, class B>
     //printf("In double type convert\n");
     impl::Converter<ros::message_traits::IsMessage<A>::value, ros::message_traits::IsMessage<B>::value>::convert(a, b);
   }
+  */
 
 template <class A>
   void convert(const A& a1, A& a2)
@@ -126,7 +127,6 @@ template <class A>
     if(&a1 != &a2)
       a2 = a1;
   }
-  */
 
 
 }

diff --git a/tf2/include/tf2/transform_datatypes.h b/tf2/include/tf2/transform_datatypes.h
@@ -34,6 +34,7 @@
 
 #include <chrono>
 #include <string>
+#include <tf2/time.h>
 
 namespace tf2
 {
@@ -43,7 +44,6 @@ namespace tf2
 template <typename T>
 class Stamped : public T{
  public:
-  typedef std::chrono::system_clock::time_point TimePoint;
   TimePoint stamp_; ///< The timestamp associated with this data
   std::string frame_id_; ///< The frame_id associated this data
 

diff --git a/tf2_eigen/AMENT_IGNORE b/tf2_eigen/AMENT_IGNORE
diff --git a/tf2_eigen/CMakeLists.txt b/tf2_eigen/CMakeLists.txt
@@ -1,31 +1,35 @@
 cmake_minimum_required(VERSION 3.5)
 project(tf2_eigen)
 
-find_package(catkin REQUIRED COMPONENTS
-  cmake_modules
-  geometry_msgs
-  tf2
-)
-
-find_package(Eigen REQUIRED)
-include_directories(${EIGEN_INCLUDE_DIRS})
-add_definitions(${EIGEN_DEFINITIONS})
-
-include_directories(${catkin_INCLUDE_DIRS})
+if(NOT WIN32)
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -Wextra")
+endif()
 
-include_directories(include)
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
 
-catkin_package(
-  INCLUDE_DIRS include
-  DEPENDS eigen)
+find_package(Eigen3 REQUIRED)
+set(Eigen_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}")
+message("Using Eigen3 include dirs: ${Eigen_INCLUDE_DIR}")
 
 install(DIRECTORY include/${PROJECT_NAME}/
-        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
-
-
-if(CATKIN_ENABLE_TESTING)
-
-  catkin_add_gtest(tf2_eigen-test test/tf2_eigen-test.cpp)
-  target_link_libraries(tf2_eigen-test ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
-
+        DESTINATION include/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(tf2_eigen-test test/tf2_eigen-test.cpp)
+  if(TARGET tf2_eigen-test)
+    target_include_directories(tf2_eigen-test PUBLIC
+      include
+      ${tf2_INCLUDE_DIRS}
+      ${tf2_ros_INCLUDE_DIRS}
+      ${Eigen_INCLUDE_DIR}
+    )
+  endif()
 endif()
+
+ament_export_include_directories(include)
+ament_export_dependencies(tf2_ros)
+ament_package()
diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h
@@ -30,20 +30,20 @@
 #define TF2_EIGEN_H
 
 #include <tf2/convert.h>
+#include <tf2_ros/buffer_interface.h>
 #include <Eigen/Geometry>
-#include <geometry_msgs/PointStamped.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 
 
 namespace tf2
 {
-
 /** \brief Convert a timestamped transform to the equivalent Eigen data type.
  * \param t The transform to convert, as a geometry_msgs TransformedStamped message.
  * \return The transform message converted to an Eigen Affine3d transform.
  */
 inline
-Eigen::Affine3d transformToEigen(const geometry_msgs::TransformStamped& t) {
+Eigen::Affine3d transformToEigen(const geometry_msgs::msg::TransformStamped& t) {
   return Eigen::Affine3d(Eigen::Translation3d(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)
 			 * Eigen::Quaterniond(t.transform.rotation.w,
 					      t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z));
@@ -54,9 +54,9 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::TransformStamped& t) {
  * \return The transform converted to a TransformStamped message.
  */
 inline
-geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d& T)
+geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Affine3d& T)
 {
-  geometry_msgs::TransformStamped t;
+  geometry_msgs::msg::TransformStamped t;
   t.transform.translation.x = T.translation().x();
   t.transform.translation.y = T.translation().y();
   t.transform.translation.z = T.translation().z();
@@ -81,7 +81,7 @@ geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d& T)
  */
 template <>
 inline
-void doTransform(const Eigen::Vector3d& t_in, Eigen::Vector3d& t_out, const geometry_msgs::TransformStamped& transform)
+void doTransform(const Eigen::Vector3d& t_in, Eigen::Vector3d& t_out, const geometry_msgs::msg::TransformStamped& transform)
 {
   t_out = Eigen::Vector3d(transformToEigen(transform) * t_in);
 }
@@ -92,9 +92,9 @@ void doTransform(const Eigen::Vector3d& t_in, Eigen::Vector3d& t_out, const geom
  * \return The vector converted to a Point message.
  */
 inline
-geometry_msgs::Point toMsg(const Eigen::Vector3d& in)
+geometry_msgs::msg::Point toMsg(const Eigen::Vector3d& in)
 {
-  geometry_msgs::Point msg;
+  geometry_msgs::msg::Point msg;
   msg.x = in.x();
   msg.y = in.y();
   msg.z = in.z();
@@ -107,7 +107,7 @@ geometry_msgs::Point toMsg(const Eigen::Vector3d& in)
  * \param out The point converted to a Eigen Vector3d.
  */
 inline
-void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out)
+void fromMsg(const geometry_msgs::msg::Point& msg, Eigen::Vector3d& out)
 {
   out.x() = msg.x;
   out.y() = msg.y;
@@ -124,9 +124,9 @@ template <>
 inline
 void doTransform(const tf2::Stamped<Eigen::Vector3d>& t_in,
 		 tf2::Stamped<Eigen::Vector3d>& t_out,
-		 const geometry_msgs::TransformStamped& transform) {
+		 const geometry_msgs::msg::TransformStamped& transform) {
   t_out = tf2::Stamped<Eigen::Vector3d>(transformToEigen(transform) * t_in,
-					transform.header.stamp,
+                                        tf2_ros::fromMsg(transform.header.stamp),
 					transform.header.frame_id);
 }
 
@@ -136,10 +136,10 @@ void doTransform(const tf2::Stamped<Eigen::Vector3d>& t_in,
  * \return The vector converted to a PointStamped message.
  */
 inline
-geometry_msgs::PointStamped toMsg(const tf2::Stamped<Eigen::Vector3d>& in)
+geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped<Eigen::Vector3d>& in)
 {
-  geometry_msgs::PointStamped msg;
-  msg.header.stamp = in.stamp_;
+  geometry_msgs::msg::PointStamped msg;
+  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
   msg.header.frame_id = in.frame_id_;
   msg.point = toMsg(static_cast<const Eigen::Vector3d&>(in));
   return msg;
@@ -151,8 +151,8 @@ geometry_msgs::PointStamped toMsg(const tf2::Stamped<Eigen::Vector3d>& in)
  * \param out The point converted to a timestamped Eigen Vector3d.
  */
 inline
-void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped<Eigen::Vector3d>& out) {
-  out.stamp_ = msg.header.stamp;
+void fromMsg(const geometry_msgs::msg::PointStamped& msg, tf2::Stamped<Eigen::Vector3d>& out) {
+  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
   out.frame_id_ = msg.header.frame_id;
   fromMsg(msg.point, static_cast<Eigen::Vector3d&>(out));
 }
@@ -170,7 +170,7 @@ template <>
 inline
 void doTransform(const Eigen::Affine3d& t_in,
                  Eigen::Affine3d& t_out,
-                 const geometry_msgs::TransformStamped& transform) {
+                 const geometry_msgs::msg::TransformStamped& transform) {
   t_out = Eigen::Affine3d(transformToEigen(transform) * t_in);
 }
 
@@ -181,8 +181,8 @@ void doTransform(const Eigen::Affine3d& t_in,
  * \return The Eigen transform converted to a Pose message.
  */
 inline
-geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) {
-  geometry_msgs::Pose msg;
+geometry_msgs::msg::Pose toMsg(const Eigen::Affine3d& in) {
+  geometry_msgs::msg::Pose msg;
   msg.position.x = in.translation().x();
   msg.position.y = in.translation().y();
   msg.position.z = in.translation().z();
@@ -199,7 +199,7 @@ geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) {
  * \param out The pose converted to a Eigen Affine3d.
  */
 inline
-void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) {
+void fromMsg(const geometry_msgs::msg::Pose& msg, Eigen::Affine3d& out) {
   out = Eigen::Affine3d(
       Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
       Eigen::Quaterniond(msg.orientation.w,
@@ -221,8 +221,8 @@ template <>
 inline
 void doTransform(const tf2::Stamped<Eigen::Affine3d>& t_in,
 		 tf2::Stamped<Eigen::Affine3d>& t_out,
-		 const geometry_msgs::TransformStamped& transform) {
-  t_out = tf2::Stamped<Eigen::Affine3d>(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id);
+		 const geometry_msgs::msg::TransformStamped& transform) {
+  t_out = tf2::Stamped<Eigen::Affine3d>(transformToEigen(transform) * t_in, tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
 }
 
 /** \brief Convert a stamped Eigen Affine3d transform type to a Pose message.
@@ -231,10 +231,10 @@ void doTransform(const tf2::Stamped<Eigen::Affine3d>& t_in,
  * \return The Eigen transform converted to a PoseStamped message.
  */
 inline
-geometry_msgs::PoseStamped toMsg(const tf2::Stamped<Eigen::Affine3d>& in)
+geometry_msgs::msg::PoseStamped toMsg(const tf2::Stamped<Eigen::Affine3d>& in)
 {
-  geometry_msgs::PoseStamped msg;
-  msg.header.stamp = in.stamp_;
+  geometry_msgs::msg::PoseStamped msg;
+  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
   msg.header.frame_id = in.frame_id_;
   msg.pose = toMsg(static_cast<const Eigen::Affine3d&>(in));
   return msg;
@@ -246,9 +246,9 @@ geometry_msgs::PoseStamped toMsg(const tf2::Stamped<Eigen::Affine3d>& in)
  * \param out The pose converted to a timestamped Eigen Affine3d.
  */
 inline
-void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<Eigen::Affine3d>& out)
+void fromMsg(const geometry_msgs::msg::PoseStamped& msg, tf2::Stamped<Eigen::Affine3d>& out)
 {
-  out.stamp_ = msg.header.stamp;
+  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
   out.frame_id_ = msg.header.frame_id;
   fromMsg(msg.pose, static_cast<Eigen::Affine3d&>(out));
 }
@@ -266,22 +266,22 @@ namespace Eigen {
 // tf2::convert().
 
 inline
-geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) {
+geometry_msgs::msg::Pose toMsg(const Eigen::Affine3d& in) {
   return tf2::toMsg(in);
 }
 
 inline
-void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) {
+void fromMsg(const geometry_msgs::msg::Point& msg, Eigen::Vector3d& out) {
   tf2::fromMsg(msg, out);
 }
 
 inline
-geometry_msgs::Point toMsg(const Eigen::Vector3d& in) {
+geometry_msgs::msg::Point toMsg(const Eigen::Vector3d& in) {
   return tf2::toMsg(in);
 }
 
 inline
-void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) {
+void fromMsg(const geometry_msgs::msg::Pose& msg, Eigen::Affine3d& out) {
   tf2::fromMsg(msg, out);
 }
 

diff --git a/tf2_eigen/package.xml b/tf2_eigen/package.xml
@@ -7,14 +7,19 @@
   <maintainer email="[email protected]">Koji Terada</maintainer>
   <license>BSD</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>eigen</build_depend>
 
   <depend>geometry_msgs</depend>
   <depend>tf2</depend>
+  <depend>tf2_ros</depend>
 
-  <build_depend>cmake_modules</build_depend>
-  <build_depend>eigen</build_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
 
   <build_export_depend>eigen</build_export_depend>
 
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>