diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt
index 3329f1a08..a2543ca9e 100644
--- a/test_tf2/CMakeLists.txt
+++ b/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)
diff --git a/test_tf2/package.xml b/test_tf2/package.xml
index 48e372719..83bbc009c 100644
--- a/test_tf2/package.xml
+++ b/test_tf2/package.xml
@@ -23,6 +23,7 @@
tf2_geometry_msgs
tf2_kdl
tf2_msgs
+ tf2_eigen
rosconsole
roscpp
@@ -34,6 +35,7 @@
tf2_geometry_msgs
tf2_kdl
tf2_msgs
+ tf2_eigen
rosunit
rosbash
diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp
index d5fb2629e..a8b0dfe71 100644
--- a/test_tf2/test/test_convert.cpp
+++ b/test_tf2/test/test_convert.cpp
@@ -39,6 +39,7 @@
#include
#include
#include
+#include
#include
TEST(tf2Convert, kdlToBullet)
@@ -97,6 +98,18 @@ TEST(tf2Convert, kdlBulletROSConversions)
EXPECT_NEAR(b1.z(), b4.z(), epsilon);
}
+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());
+}
+
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h
index 6ccca3128..6b68ccdd4 100644
--- a/tf2/include/tf2/impl/convert.h
+++ b/tf2/include/tf2/impl/convert.h
@@ -55,21 +55,33 @@ template <>
template
inline void Converter::convert(const A& a, B& b)
{
+#ifdef _MSC_VER
+ tf2::fromMsg(a, b);
+#else
fromMsg(a, b);
+#endif
}
template <>
template
inline void Converter::convert(const A& a, B& b)
{
+#ifdef _MSC_VER
+ b = tf2::toMsg(a);
+#else
b = toMsg(a);
+#endif
}
template <>
template
inline void Converter::convert(const A& a, B& b)
{
+#ifdef _MSC_VER
+ tf2::fromMsg(tf2::toMsg(a), b);
+#else
fromMsg(toMsg(a), b);
+#endif
}
}