diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index b99e10e21..9b9fa8440 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2) 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 8e1f017fe..1c79475bc 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 } }