From 306bef4f447924082e0616ae9af8c5c4d4b3c051 Mon Sep 17 00:00:00 2001
From: Bjar Ne <gleichdick@users.noreply.github.com>
Date: Sat, 29 May 2021 14:24:22 +0000
Subject: [PATCH 1/3] Reenable stamped eigen tests

---
 tf2_eigen/CMakeLists.txt          |  1 +
 tf2_eigen/test/tf2_eigen-test.cpp | 68 ++++++++++++++++++-------------
 2 files changed, 41 insertions(+), 28 deletions(-)

diff --git a/tf2_eigen/CMakeLists.txt b/tf2_eigen/CMakeLists.txt
index 2bd4852e9..5dbd59ad5 100644
--- a/tf2_eigen/CMakeLists.txt
+++ b/tf2_eigen/CMakeLists.txt
@@ -49,5 +49,6 @@ ament_export_include_directories(include)
 ament_export_dependencies(
   eigen3_cmake_module
   Eigen3
+  tf2
   tf2_ros)
 ament_package()
diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp
index 561793c46..438be7c52 100644
--- a/tf2_eigen/test/tf2_eigen-test.cpp
+++ b/tf2_eigen/test/tf2_eigen-test.cpp
@@ -35,29 +35,38 @@
 #endif
 
 #include <gtest/gtest.h>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <rclcpp/clock.hpp>
 #include <tf2/convert.h>
+#include <tf2/transform_datatypes.h>
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <Eigen/Geometry>
+
 #include <cmath>
 #include <memory>
 
-// TODO(clalancette) Re-enable these tests once we have tf2/convert.h:convert(A, B) implemented
-// TEST(TfEigen, ConvertVector3dStamped)
-// {
-//   const tf2::Stamped<Eigen::Vector3d> v(Eigen::Vector3d(1,2,3),
-//     tf2::TimePoint(std::chrono::seconds(5)), "test");
+TEST(TfEigen, ConvertVector3dStamped)
+{
+  const tf2::Stamped<Eigen::Vector3d> v(Eigen::Vector3d(1, 2, 3), tf2::TimePoint(
+      std::chrono::seconds(
+        5)), "test");
 
-//   tf2::Stamped<Eigen::Vector3d> v1;
-//   geometry_msgs::msg::PointStamped p1;
-//   tf2::convert(v, p1);
-//   tf2::convert(p1, v1);
+  tf2::Stamped<Eigen::Vector3d> v1;
+  geometry_msgs::msg::PointStamped p1;
+  tf2::convert(v, p1);
+  tf2::convert(p1, v1);
 
-//   EXPECT_EQ(v, v1);
-// }
+  EXPECT_EQ(v, v1);
+}
 
+// TODO(clalancette) Re-enable these tests once we have tf2/convert.h:convert(A, B) implemented
 // TEST(TfEigen, ConvertVector3d)
 // {
 //   const Eigen::Vector3d v(1,2,3);
@@ -70,24 +79,27 @@
 //   EXPECT_EQ(v, v1);
 // }
 
-// TEST(TfEigen, ConvertAffine3dStamped)
-// {
-//   const Eigen::Affine3d v_nonstamped(
-//     Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
-//   const tf2::Stamped<Eigen::Affine3d> v(
-//     v_nonstamped, tf2::TimePoint(std::chrono::seconds(42)), "test_frame");
-
-//   tf2::Stamped<Eigen::Affine3d> v1;
-//   geometry_msgs::msg::PoseStamped p1;
-//   tf2::convert(v, p1);
-//   tf2::convert(p1, v1);
-
-//   EXPECT_EQ(v.translation(), v1.translation());
-//   EXPECT_EQ(v.rotation(), v1.rotation());
-//   EXPECT_EQ(v.frame_id_, v1.frame_id_);
-//   EXPECT_EQ(v.stamp_, v1.stamp_);
-// }
+TEST(TfEigen, ConvertAffine3dStamped)
+{
+  const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxis<double>(
+      1,
+      Eigen::Vector3d::UnitX()));
+  const tf2::Stamped<Eigen::Affine3d> v(v_nonstamped, tf2::TimePoint(
+      std::chrono::seconds(
+        42)), "test_frame");
+
+  tf2::Stamped<Eigen::Affine3d> v1;
+  geometry_msgs::msg::PoseStamped p1;
+  tf2::convert(v, p1);
+  tf2::convert(p1, v1);
+
+  EXPECT_EQ(v.translation(), v1.translation());
+  EXPECT_EQ(v.rotation(), v1.rotation());
+  EXPECT_EQ(v.frame_id_, v1.frame_id_);
+  EXPECT_EQ(v.stamp_, v1.stamp_);
+}
 
+// TODO(clalancette) Re-enable these tests once we have tf2/convert.h:convert(A, B) implemented
 // TEST(TfEigen, ConvertAffine3d)
 // {
 //   const Eigen::Affine3d v(

From cbd5491d28b14737366b3e341f97c01887ec207b Mon Sep 17 00:00:00 2001
From: Bjar Ne <43565432+gleichdick@users.noreply.github.com>
Date: Mon, 31 May 2021 10:49:33 +0200
Subject: [PATCH 2/3] Update tf2_eigen/test/tf2_eigen-test.cpp
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
---
 tf2_eigen/test/tf2_eigen-test.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp
index 438be7c52..f578af523 100644
--- a/tf2_eigen/test/tf2_eigen-test.cpp
+++ b/tf2_eigen/test/tf2_eigen-test.cpp
@@ -55,8 +55,7 @@
 TEST(TfEigen, ConvertVector3dStamped)
 {
   const tf2::Stamped<Eigen::Vector3d> v(Eigen::Vector3d(1, 2, 3), tf2::TimePoint(
-      std::chrono::seconds(
-        5)), "test");
+      std::chrono::seconds(5)), "test");
 
   tf2::Stamped<Eigen::Vector3d> v1;
   geometry_msgs::msg::PointStamped p1;

From 2c709fc41ff5966efa011c2e820cab6a9510a9dc Mon Sep 17 00:00:00 2001
From: Bjar Ne <43565432+gleichdick@users.noreply.github.com>
Date: Mon, 31 May 2021 10:49:39 +0200
Subject: [PATCH 3/3] Update tf2_eigen/test/tf2_eigen-test.cpp
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
---
 tf2_eigen/test/tf2_eigen-test.cpp | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp
index f578af523..8a8a1112e 100644
--- a/tf2_eigen/test/tf2_eigen-test.cpp
+++ b/tf2_eigen/test/tf2_eigen-test.cpp
@@ -81,11 +81,9 @@ TEST(TfEigen, ConvertVector3dStamped)
 TEST(TfEigen, ConvertAffine3dStamped)
 {
   const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxis<double>(
-      1,
-      Eigen::Vector3d::UnitX()));
+      1, Eigen::Vector3d::UnitX()));
   const tf2::Stamped<Eigen::Affine3d> v(v_nonstamped, tf2::TimePoint(
-      std::chrono::seconds(
-        42)), "test_frame");
+      std::chrono::seconds(42)), "test_frame");
 
   tf2::Stamped<Eigen::Affine3d> v1;
   geometry_msgs::msg::PoseStamped p1;