Skip to content

Commit 0df0d83

Browse files
esteveesteve
authored
Backport #311 to Foxy (#356)
* Backport #311 to Foxy. Original author: Jafar Abdi (@JafarAbdi)
1 parent 599c064 commit 0df0d83

File tree

6 files changed

+636
-0
lines changed

6 files changed

+636
-0
lines changed

‎tf2_eigen_kdl/CMakeLists.txt‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(tf2_eigen_kdl)
3+
4+
# Default to C++14
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 14)
7+
endif()
8+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
9+
add_compile_options(-Wall -Wextra -Wpedantic)
10+
endif()
11+
12+
find_package(ament_cmake REQUIRED)
13+
find_package(eigen3_cmake_module REQUIRED)
14+
find_package(Eigen3 REQUIRED)
15+
find_package(orocos_kdl REQUIRED)
16+
find_package(tf2 REQUIRED)
17+
18+
add_library(${PROJECT_NAME} SHARED
19+
src/tf2_eigen_kdl.cpp
20+
)
21+
target_include_directories(${PROJECT_NAME} PUBLIC
22+
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
23+
"$<INSTALL_INTERFACE:include>")
24+
ament_target_dependencies(${PROJECT_NAME}
25+
Eigen3
26+
eigen3_cmake_module
27+
orocos_kdl
28+
tf2
29+
)
30+
# Causes the visibility macros to use dllexport rather than dllimport,
31+
# which is appropriate when building the dll but not consuming it.
32+
target_compile_definitions(${PROJECT_NAME} PRIVATE "TF2_EIGEN_KDL_BUILDING_DLL")
33+
34+
install(DIRECTORY include/${PROJECT_NAME}/
35+
DESTINATION include/${PROJECT_NAME})
36+
37+
install(TARGETS ${PROJECT_NAME}
38+
EXPORT ${PROJECT_NAME}
39+
ARCHIVE DESTINATION lib
40+
LIBRARY DESTINATION lib
41+
RUNTIME DESTINATION lib)
42+
43+
ament_export_include_directories(include)
44+
ament_export_dependencies(
45+
eigen3_cmake_module
46+
Eigen3
47+
orocos_kdl
48+
tf2
49+
)
50+
ament_export_libraries(${PROJECT_NAME})
51+
ament_export_targets(${PROJECT_NAME})
52+
53+
if(BUILD_TESTING)
54+
find_package(ament_lint_auto REQUIRED)
55+
ament_lint_auto_find_test_dependencies()
56+
57+
find_package(ament_cmake_gtest REQUIRED)
58+
59+
ament_add_gtest(tf2_eigen_kdl_test test/tf2_eigen_kdl_test.cpp)
60+
target_link_libraries(tf2_eigen_kdl_test ${PROJECT_NAME})
61+
ament_target_dependencies(tf2_eigen_kdl_test tf2)
62+
endif()
63+
64+
ament_package()

‎tf2_eigen_kdl/include/tf2_eigen_kdl/tf2_eigen_kdl.hpp‎

Lines changed: 190 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,190 @@
1+
// Copyright 2020 Open Source Robotics Foundation, Inc.
2+
// All rights reserved.
3+
//
4+
// Software License Agreement (BSD License 2.0)
5+
//
6+
// Redistribution and use in source and binary forms, with or without
7+
// modification, are permitted provided that the following conditions
8+
// are met:
9+
//
10+
// * Redistributions of source code must retain the above copyright
11+
// notice, this list of conditions and the following disclaimer.
12+
// * Redistributions in binary form must reproduce the above
13+
// copyright notice, this list of conditions and the following
14+
// disclaimer in the documentation and/or other materials provided
15+
// with the distribution.
16+
// * Neither the name of Open Source Robotics Foundation, Inc. nor the names of its
17+
// contributors may be used to endorse or promote products derived
18+
// from this software without specific prior written permission.
19+
//
20+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21+
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22+
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23+
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24+
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25+
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26+
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27+
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28+
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29+
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30+
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31+
// POSSIBILITY OF SUCH DAMAGE.
32+
33+
/*
34+
* Author: Adam Leeper, Stuart Glaser
35+
*/
36+
37+
#ifndef TF2_EIGEN_KDL__TF2_EIGEN_KDL_HPP_
38+
#define TF2_EIGEN_KDL__TF2_EIGEN_KDL_HPP_
39+
40+
#include <Eigen/Core>
41+
#include <Eigen/Geometry>
42+
43+
#include <kdl/frames.hpp>
44+
45+
#include <tf2/impl/convert.h>
46+
47+
#include "tf2_eigen_kdl/visibility_control.h"
48+
49+
namespace tf2
50+
{
51+
52+
/// Converts a KDL rotation into an Eigen quaternion
53+
TF2_EIGEN_KDL_PUBLIC
54+
void quaternionKDLToEigen(const KDL::Rotation & k, Eigen::Quaterniond & e);
55+
56+
/// Converts an Eigen quaternion into a KDL rotation
57+
TF2_EIGEN_KDL_PUBLIC
58+
void quaternionEigenToKDL(const Eigen::Quaterniond & e, KDL::Rotation & k);
59+
60+
/// Converts a KDL frame into an Eigen Affine3d
61+
TF2_EIGEN_KDL_PUBLIC
62+
void transformKDLToEigen(const KDL::Frame & k, Eigen::Affine3d & e);
63+
64+
/// Converts a KDL frame into an Eigen Isometry3d
65+
TF2_EIGEN_KDL_PUBLIC
66+
void transformKDLToEigen(const KDL::Frame & k, Eigen::Isometry3d & e);
67+
68+
/// Converts an Eigen Affine3d into a KDL frame
69+
TF2_EIGEN_KDL_PUBLIC
70+
void transformEigenToKDL(const Eigen::Affine3d & e, KDL::Frame & k);
71+
72+
/// Converts an Eigen Isometry3d into a KDL frame
73+
TF2_EIGEN_KDL_PUBLIC
74+
void transformEigenToKDL(const Eigen::Isometry3d & e, KDL::Frame & k);
75+
76+
/// Converts a KDL twist into an Eigen matrix
77+
TF2_EIGEN_KDL_PUBLIC
78+
void twistKDLToEigen(const KDL::Twist & k, Eigen::Matrix<double, 6, 1> & e);
79+
80+
/// Converts an Eigen matrix into a KDL Twist
81+
TF2_EIGEN_KDL_PUBLIC
82+
void twistEigenToKDL(const Eigen::Matrix<double, 6, 1> & e, KDL::Twist & k);
83+
84+
/// Converts a KDL vector into an Eigen matrix
85+
TF2_EIGEN_KDL_PUBLIC
86+
void vectorKDLToEigen(const KDL::Vector & k, Eigen::Matrix<double, 3, 1> & e);
87+
88+
/// Converts an Eigen matrix into a KDL vector
89+
TF2_EIGEN_KDL_PUBLIC
90+
void vectorEigenToKDL(const Eigen::Matrix<double, 3, 1> & e, KDL::Vector & k);
91+
92+
/// Converts a KDL wrench into an Eigen matrix
93+
TF2_EIGEN_KDL_PUBLIC
94+
void wrenchKDLToEigen(const KDL::Wrench & k, Eigen::Matrix<double, 6, 1> & e);
95+
96+
/// Converts an Eigen matrix into a KDL wrench
97+
TF2_EIGEN_KDL_PUBLIC
98+
void wrenchEigenToKDL(const Eigen::Matrix<double, 6, 1> & e, KDL::Wrench & k);
99+
100+
namespace impl
101+
{
102+
103+
template<>
104+
template<>
105+
inline void Converter<false, false>::convert(const KDL::Rotation & a, Eigen::Quaterniond & b)
106+
{
107+
quaternionKDLToEigen(a, b);
108+
}
109+
110+
template<>
111+
template<>
112+
inline void Converter<false, false>::convert(const Eigen::Quaterniond & a, KDL::Rotation & b)
113+
{
114+
quaternionEigenToKDL(a, b);
115+
}
116+
117+
template<>
118+
template<>
119+
inline void Converter<false, false>::convert(const KDL::Frame & a, Eigen::Affine3d & b)
120+
{
121+
transformKDLToEigen(a, b);
122+
}
123+
124+
template<>
125+
template<>
126+
inline void Converter<false, false>::convert(const KDL::Frame & a, Eigen::Isometry3d & b)
127+
{
128+
transformKDLToEigen(a, b);
129+
}
130+
131+
template<>
132+
template<>
133+
inline void Converter<false, false>::convert(const Eigen::Affine3d & a, KDL::Frame & b)
134+
{
135+
transformEigenToKDL(a, b);
136+
}
137+
138+
template<>
139+
template<>
140+
inline void Converter<false, false>::convert(const Eigen::Isometry3d & a, KDL::Frame & b)
141+
{
142+
transformEigenToKDL(a, b);
143+
}
144+
145+
template<>
146+
template<>
147+
inline void Converter<false, false>::convert(const KDL::Twist & a, Eigen::Matrix<double, 6, 1> & b)
148+
{
149+
twistKDLToEigen(a, b);
150+
}
151+
152+
template<>
153+
template<>
154+
inline void Converter<false, false>::convert(const Eigen::Matrix<double, 6, 1> & a, KDL::Twist & b)
155+
{
156+
twistEigenToKDL(a, b);
157+
}
158+
159+
template<>
160+
template<>
161+
inline void Converter<false, false>::convert(const KDL::Vector & a, Eigen::Matrix<double, 3, 1> & b)
162+
{
163+
vectorKDLToEigen(a, b);
164+
}
165+
166+
template<>
167+
template<>
168+
inline void Converter<false, false>::convert(const Eigen::Matrix<double, 3, 1> & a, KDL::Vector & b)
169+
{
170+
vectorEigenToKDL(a, b);
171+
}
172+
173+
template<>
174+
template<>
175+
inline void Converter<false, false>::convert(const KDL::Wrench & a, Eigen::Matrix<double, 6, 1> & b)
176+
{
177+
wrenchKDLToEigen(a, b);
178+
}
179+
180+
template<>
181+
template<>
182+
inline void Converter<false, false>::convert(const Eigen::Matrix<double, 6, 1> & a, KDL::Wrench & b)
183+
{
184+
wrenchEigenToKDL(a, b);
185+
}
186+
} // namespace impl
187+
188+
} // namespace tf2
189+
190+
#endif // TF2_EIGEN_KDL__TF2_EIGEN_KDL_HPP_

‎tf2_eigen_kdl/include/tf2_eigen_kdl/visibility_control.h‎

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
// Copyright 2020, Open Source Robotics Foundation, Inc. All rights reserved.
2+
//
3+
// Redistribution and use in source and binary forms, with or without
4+
// modification, are permitted provided that the following conditions are met:
5+
//
6+
// * Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
//
9+
// * Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
12+
//
13+
// * Neither the name of the Open Source Robotics Foundation nor the names of its
14+
// contributors may be used to endorse or promote products derived from
15+
// this software without specific prior written permission.
16+
//
17+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
// POSSIBILITY OF SUCH DAMAGE.
28+
29+
#ifndef TF2_EIGEN_KDL__VISIBILITY_CONTROL_H_
30+
#define TF2_EIGEN_KDL__VISIBILITY_CONTROL_H_
31+
32+
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
33+
// https://gcc.gnu.org/wiki/Visibility
34+
35+
#if defined _WIN32 || defined __CYGWIN__
36+
#ifdef __GNUC__
37+
#define TF2_EIGEN_KDL_EXPORT __attribute__ ((dllexport))
38+
#define TF2_EIGEN_KDL_IMPORT __attribute__ ((dllimport))
39+
#else
40+
#define TF2_EIGEN_KDL_EXPORT __declspec(dllexport)
41+
#define TF2_EIGEN_KDL_IMPORT __declspec(dllimport)
42+
#endif
43+
#ifdef TF2_EIGEN_KDL_BUILDING_DLL
44+
#define TF2_EIGEN_KDL_PUBLIC TF2_EIGEN_KDL_EXPORT
45+
#else
46+
#define TF2_EIGEN_KDL_PUBLIC TF2_EIGEN_KDL_IMPORT
47+
#endif
48+
#define TF2_EIGEN_KDL_PUBLIC_TYPE TF2_EIGEN_KDL_PUBLIC
49+
#define TF2_EIGEN_KDL_LOCAL
50+
#else
51+
#define TF2_EIGEN_KDL_EXPORT __attribute__ ((visibility("default")))
52+
#define TF2_EIGEN_KDL_IMPORT
53+
#if __GNUC__ >= 4
54+
#define TF2_EIGEN_KDL_PUBLIC __attribute__ ((visibility("default")))
55+
#define TF2_EIGEN_KDL_LOCAL __attribute__ ((visibility("hidden")))
56+
#else
57+
#define TF2_EIGEN_KDL_PUBLIC
58+
#define TF2_EIGEN_KDL_LOCAL
59+
#endif
60+
#define TF2_EIGEN_KDL_PUBLIC_TYPE
61+
#endif
62+
63+
#endif // TF2_EIGEN_KDL__VISIBILITY_CONTROL_H_

‎tf2_eigen_kdl/package.xml‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
<?xml version="1.0"?>
2+
<package format="2">
3+
<name>tf2_eigen_kdl</name>
4+
<version>0.13.8</version>
5+
<description>
6+
7+
Conversion functions between:
8+
- Eigen and KDL
9+
10+
</description>
11+
<maintainer email="[email protected]">Chris Lalancette</maintainer>
12+
<maintainer email="[email protected]">Alejandro Hernandez Cordero</maintainer>
13+
<license>BSD</license>
14+
<author>Stuart Glaser</author>
15+
<author>Adam Leeper</author>
16+
17+
<buildtool_depend>ament_cmake</buildtool_depend>
18+
19+
<buildtool_depend>eigen3_cmake_module</buildtool_depend>
20+
<buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
21+
22+
<build_depend>eigen</build_depend>
23+
<build_export_depend>eigen</build_export_depend>
24+
25+
<depend>orocos_kdl</depend>
26+
<depend>tf2</depend>
27+
28+
<test_depend>ament_cmake_gtest</test_depend>
29+
<test_depend>ament_lint_auto</test_depend>
30+
<test_depend>ament_lint_common</test_depend>
31+
32+
<export>
33+
<build_type>ament_cmake</build_type>
34+
</export>
35+
</package>

0 commit comments

Comments
 (0)