[ros-geometry] 01/03: Imported Upstream version 1.11.8
Jochen Sprickerhof
jspricke-guest at moszumanska.debian.org
Sun Jul 10 08:46:57 UTC 2016
This is an automated email from the git hooks/post-receive script.
jspricke-guest pushed a commit to annotated tag debian/1.11.8-1
in repository ros-geometry.
commit b4985da77d86a0b14ef6aedde08a73ef0e38d55b
Author: Jochen Sprickerhof <git at jochen.sprickerhof.de>
Date: Sun Jul 10 10:38:41 2016 +0200
Imported Upstream version 1.11.8
---
eigen_conversions/CHANGELOG.rst | 5 +
.../include/eigen_conversions/eigen_kdl.h | 10 +-
.../include/eigen_conversions/eigen_msg.h | 20 ++-
eigen_conversions/package.xml | 2 +-
eigen_conversions/src/eigen_kdl.cpp | 59 ++++++---
eigen_conversions/src/eigen_msg.cpp | 134 ++++++++++++++-------
geometry/CHANGELOG.rst | 3 +
geometry/package.xml | 2 +-
kdl_conversions/CHANGELOG.rst | 3 +
kdl_conversions/package.xml | 2 +-
tf/CHANGELOG.rst | 7 ++
tf/conf.py | 2 -
tf/include/tf/tf.h | 14 +++
tf/package.xml | 2 +-
tf/scripts/python_benchmark.py | 2 -
tf/scripts/tf_remap | 2 -
tf/scripts/view_frames | 4 -
tf/src/tf/listener.py | 4 -
tf/test/method_test.py | 2 -
tf/test/python_debug_test.py | 2 -
tf/test/testPython.py | 14 ++-
tf/test/test_datatype_conversion.py | 1 -
tf/test/tf_unittest.cpp | 34 ++++++
tf_conversions/CHANGELOG.rst | 6 +
tf_conversions/conf.py | 2 -
tf_conversions/include/tf_conversions/tf_eigen.h | 12 ++
tf_conversions/package.xml | 2 +-
tf_conversions/src/tf_eigen.cpp | 64 +++++++---
tf_conversions/test/posemath.py | 2 -
tf_conversions/test/test_eigen_tf.cpp | 45 ++++---
30 files changed, 333 insertions(+), 130 deletions(-)
diff --git a/eigen_conversions/CHANGELOG.rst b/eigen_conversions/CHANGELOG.rst
index 7560e80..ec224ef 100644
--- a/eigen_conversions/CHANGELOG.rst
+++ b/eigen_conversions/CHANGELOG.rst
@@ -2,6 +2,11 @@
Changelog for package eigen_conversions
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.11.8 (2016-03-04)
+-------------------
+* eigen_conversions: Add conversions for Eigen::Isometry3d
+* Contributors: Maarten de Vries
+
1.11.7 (2015-04-21)
-------------------
diff --git a/eigen_conversions/include/eigen_conversions/eigen_kdl.h b/eigen_conversions/include/eigen_conversions/eigen_kdl.h
index 27e0920..1ffffbc 100644
--- a/eigen_conversions/include/eigen_conversions/eigen_kdl.h
+++ b/eigen_conversions/include/eigen_conversions/eigen_kdl.h
@@ -47,12 +47,18 @@ void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e);
/// Converts an Eigen quaternion into a KDL rotation
void quaternionEigenToKDL(const Eigen::Quaterniond &e, KDL::Rotation &k);
-/// Converts a KDL frame into an Eigen transform
+/// Converts a KDL frame into an Eigen Affine3d
void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e);
-/// Converts an Eigen transform into a KDL frame
+/// Converts a KDL frame into an Eigen Isometry3d
+void transformKDLToEigen(const KDL::Frame &k, Eigen::Isometry3d &e);
+
+/// Converts an Eigen Affine3d into a KDL frame
void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k);
+/// Converts an Eigen Isometry3d into a KDL frame
+void transformEigenToKDL(const Eigen::Isometry3d &e, KDL::Frame &k);
+
/// Converts a KDL twist into an Eigen matrix
void twistKDLToEigen(const KDL::Twist &k, Eigen::Matrix<double, 6, 1> &e);
diff --git a/eigen_conversions/include/eigen_conversions/eigen_msg.h b/eigen_conversions/include/eigen_conversions/eigen_msg.h
index 1a7aa85..3d68fd1 100644
--- a/eigen_conversions/include/eigen_conversions/eigen_msg.h
+++ b/eigen_conversions/include/eigen_conversions/eigen_msg.h
@@ -54,24 +54,36 @@ void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e);
/// Converts an Eigen Vector into a Point message
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m);
-/// Converts a Pose message into an Eigen Transform
+/// Converts a Pose message into an Eigen Affine3d
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e);
-/// Converts an Eigen transform into a Pose message
+/// Converts a Pose message into an Eigen Isometry3d
+void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Isometry3d &e);
+
+/// Converts an Eigen Affine3d into a Pose message
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m);
+/// Converts an Eigen Isometry3d into a Pose message
+void poseEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Pose &m);
+
/// Converts a Quaternion message into an Eigen Quaternion
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e);
/// Converts an Eigen Quaternion into a Quaternion message
void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m);
-/// Converts a Transform message into an Eigen Transform
+/// Converts a Transform message into an Eigen Affine3d
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e);
-/// Converts an Eigen transform into a Transform message
+/// Converts a Transform message into an Eigen Isometry3d
+void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Isometry3d &e);
+
+/// Converts an Eigen Affine3d into a Transform message
void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m);
+/// Converts an Eigen Isometry3d into a Transform message
+void transformEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Transform &m);
+
/// Converts a Twist message into an Eigen matrix
void twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix<double,6,1> &e);
diff --git a/eigen_conversions/package.xml b/eigen_conversions/package.xml
index 4dc6f85..3f93f6d 100644
--- a/eigen_conversions/package.xml
+++ b/eigen_conversions/package.xml
@@ -1,6 +1,6 @@
<package>
<name>eigen_conversions</name>
- <version>1.11.7</version>
+ <version>1.11.8</version>
<description>
Conversion functions between:
diff --git a/eigen_conversions/src/eigen_kdl.cpp b/eigen_conversions/src/eigen_kdl.cpp
index 11c1005..afcf76e 100644
--- a/eigen_conversions/src/eigen_kdl.cpp
+++ b/eigen_conversions/src/eigen_kdl.cpp
@@ -47,29 +47,54 @@ void quaternionEigenToKDL(const Eigen::Quaterniond &e, KDL::Rotation &k)
k = KDL::Rotation::Quaternion(e.x(), e.y(), e.z(), e.w());
}
+namespace {
+ template<typename T>
+ void transformKDLToEigenImpl(const KDL::Frame &k, T &e)
+ {
+ // translation
+ for (unsigned int i = 0; i < 3; ++i)
+ e(i, 3) = k.p[i];
+
+ // rotation matrix
+ for (unsigned int i = 0; i < 9; ++i)
+ e(i/3, i%3) = k.M.data[i];
+
+ // "identity" row
+ e(3,0) = 0.0;
+ e(3,1) = 0.0;
+ e(3,2) = 0.0;
+ e(3,3) = 1.0;
+ }
+
+ template<typename T>
+ void transformEigenToKDLImpl(const T &e, KDL::Frame &k)
+ {
+ for (unsigned int i = 0; i < 3; ++i)
+ k.p[i] = e(i, 3);
+ for (unsigned int i = 0; i < 9; ++i)
+ k.M.data[i] = e(i/3, i%3);
+ }
+
+}
+
void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e)
{
- // translation
- for (unsigned int i = 0; i < 3; ++i)
- e(i, 3) = k.p[i];
-
- // rotation matrix
- for (unsigned int i = 0; i < 9; ++i)
- e(i/3, i%3) = k.M.data[i];
-
- // "identity" row
- e(3,0) = 0.0;
- e(3,1) = 0.0;
- e(3,2) = 0.0;
- e(3,3) = 1.0;
+ transformKDLToEigenImpl(k, e);
+}
+
+void transformKDLToEigen(const KDL::Frame &k, Eigen::Isometry3d &e)
+{
+ transformKDLToEigenImpl(k, e);
}
void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k)
{
- for (unsigned int i = 0; i < 3; ++i)
- k.p[i] = e(i, 3);
- for (unsigned int i = 0; i < 9; ++i)
- k.M.data[i] = e(i/3, i%3);
+ transformEigenToKDLImpl(e, k);
+}
+
+void transformEigenToKDL(const Eigen::Isometry3d &e, KDL::Frame &k)
+{
+ transformEigenToKDLImpl(e, k);
}
void twistEigenToKDL(const Eigen::Matrix<double, 6, 1> &e, KDL::Twist &k)
diff --git a/eigen_conversions/src/eigen_msg.cpp b/eigen_conversions/src/eigen_msg.cpp
index 4c24b1c..f025545 100644
--- a/eigen_conversions/src/eigen_msg.cpp
+++ b/eigen_conversions/src/eigen_msg.cpp
@@ -46,33 +46,88 @@ void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
m.z = e(2);
}
+namespace {
+ template<typename T>
+ void poseMsgToEigenImpl(const geometry_msgs::Pose &m, T &e)
+ {
+ e = Eigen::Translation3d(m.position.x,
+ m.position.y,
+ m.position.z) *
+ Eigen::Quaterniond(m.orientation.w,
+ m.orientation.x,
+ m.orientation.y,
+ m.orientation.z);
+ }
+
+ template<typename T>
+ void poseEigenToMsgImpl(const T &e, geometry_msgs::Pose &m)
+ {
+ m.position.x = e.translation()[0];
+ m.position.y = e.translation()[1];
+ m.position.z = e.translation()[2];
+ Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear();
+ m.orientation.x = q.x();
+ m.orientation.y = q.y();
+ m.orientation.z = q.z();
+ m.orientation.w = q.w();
+ if (m.orientation.w < 0) {
+ m.orientation.x *= -1;
+ m.orientation.y *= -1;
+ m.orientation.z *= -1;
+ m.orientation.w *= -1;
+ }
+ }
+
+ template<typename T>
+ void transformMsgToEigenImpl(const geometry_msgs::Transform &m, T &e)
+ {
+ e = Eigen::Translation3d(m.translation.x,
+ m.translation.y,
+ m.translation.z) *
+ Eigen::Quaterniond(m.rotation.w,
+ m.rotation.x,
+ m.rotation.y,
+ m.rotation.z);
+ }
+
+ template<typename T>
+ void transformEigenToMsgImpl(const T &e, geometry_msgs::Transform &m)
+ {
+ m.translation.x = e.translation()[0];
+ m.translation.y = e.translation()[1];
+ m.translation.z = e.translation()[2];
+ Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear();
+ m.rotation.x = q.x();
+ m.rotation.y = q.y();
+ m.rotation.z = q.z();
+ m.rotation.w = q.w();
+ if (m.rotation.w < 0) {
+ m.rotation.x *= -1;
+ m.rotation.y *= -1;
+ m.rotation.z *= -1;
+ m.rotation.w *= -1;
+ }
+ }
+}
+
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
{
- e = Eigen::Translation3d(m.position.x,
- m.position.y,
- m.position.z) *
- Eigen::Quaterniond(m.orientation.w,
- m.orientation.x,
- m.orientation.y,
- m.orientation.z);
+ poseMsgToEigenImpl(m, e);
+}
+
+void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Isometry3d &e)
+{
+ poseMsgToEigenImpl(m, e);
}
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
{
- m.position.x = e.translation()[0];
- m.position.y = e.translation()[1];
- m.position.z = e.translation()[2];
- Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear();
- m.orientation.x = q.x();
- m.orientation.y = q.y();
- m.orientation.z = q.z();
- m.orientation.w = q.w();
- if (m.orientation.w < 0) {
- m.orientation.x *= -1;
- m.orientation.y *= -1;
- m.orientation.z *= -1;
- m.orientation.w *= -1;
- }
+ poseEigenToMsgImpl(e, m);
+}
+
+void poseEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Pose &m)
+{
+ poseEigenToMsgImpl(e, m);
}
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
@@ -89,32 +144,23 @@ void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion
}
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
-{
- e = Eigen::Translation3d(m.translation.x,
- m.translation.y,
- m.translation.z) *
- Eigen::Quaterniond(m.rotation.w,
- m.rotation.x,
- m.rotation.y,
- m.rotation.z);
+{
+ transformMsgToEigenImpl(m, e);
+}
+
+void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Isometry3d &e)
+{
+ transformMsgToEigenImpl(m, e);
}
void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m)
-{
- m.translation.x = e.translation()[0];
- m.translation.y = e.translation()[1];
- m.translation.z = e.translation()[2];
- Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear();
- m.rotation.x = q.x();
- m.rotation.y = q.y();
- m.rotation.z = q.z();
- m.rotation.w = q.w();
- if (m.rotation.w < 0) {
- m.rotation.x *= -1;
- m.rotation.y *= -1;
- m.rotation.z *= -1;
- m.rotation.w *= -1;
- }
+{
+ transformEigenToMsgImpl(e, m);
+}
+
+void transformEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Transform &m)
+{
+ transformEigenToMsgImpl(e, m);
}
void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e)
diff --git a/geometry/CHANGELOG.rst b/geometry/CHANGELOG.rst
index a056266..466bc16 100644
--- a/geometry/CHANGELOG.rst
+++ b/geometry/CHANGELOG.rst
@@ -2,6 +2,9 @@
Changelog for package geometry
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.11.8 (2016-03-04)
+-------------------
+
1.11.7 (2015-04-21)
-------------------
diff --git a/geometry/package.xml b/geometry/package.xml
index 18b90cd..cc76180 100644
--- a/geometry/package.xml
+++ b/geometry/package.xml
@@ -1,6 +1,6 @@
<package>
<name>geometry</name>
- <version>1.11.7</version>
+ <version>1.11.8</version>
<description>Geometry Library</description>
<maintainer email="tfoote at osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license>
diff --git a/kdl_conversions/CHANGELOG.rst b/kdl_conversions/CHANGELOG.rst
index f5adb3d..fc55667 100644
--- a/kdl_conversions/CHANGELOG.rst
+++ b/kdl_conversions/CHANGELOG.rst
@@ -2,6 +2,9 @@
Changelog for package kdl_conversions
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.11.8 (2016-03-04)
+-------------------
+
1.11.7 (2015-04-21)
-------------------
diff --git a/kdl_conversions/package.xml b/kdl_conversions/package.xml
index e718a09..6725f42 100644
--- a/kdl_conversions/package.xml
+++ b/kdl_conversions/package.xml
@@ -1,6 +1,6 @@
<package>
<name>kdl_conversions</name>
- <version>1.11.7</version>
+ <version>1.11.8</version>
<description>
Conversion functions between KDL and geometry_msgs types.
diff --git a/tf/CHANGELOG.rst b/tf/CHANGELOG.rst
index e9b307c..dcaaa89 100644
--- a/tf/CHANGELOG.rst
+++ b/tf/CHANGELOG.rst
@@ -2,6 +2,13 @@
Changelog for package tf
^^^^^^^^^^^^^^^^^^^^^^^^
+1.11.8 (2016-03-04)
+-------------------
+* Update assertQuaternionValid to check for NaNs
+* Remove outdated manifest loading in python files
+* update unit tests to catch https://github.com/ros/geometry_experimental/issues/102
+* Contributors: Chris Mansley, Michael Hwang, Tully Foote
+
1.11.7 (2015-04-21)
-------------------
* add a unit test for pytf wait_for_transform
diff --git a/tf/conf.py b/tf/conf.py
index b348213..d32908d 100644
--- a/tf/conf.py
+++ b/tf/conf.py
@@ -11,8 +11,6 @@
# All configuration values have a default; values that are commented out
# serve to show the default.
-import roslib
-roslib.load_manifest('tf')
import sys, os
# If extensions (or modules to document with autodoc) are in another directory,
diff --git a/tf/include/tf/tf.h b/tf/include/tf/tf.h
index 39cadd1..f628fce 100644
--- a/tf/include/tf/tf.h
+++ b/tf/include/tf/tf.h
@@ -392,6 +392,13 @@ protected:
/** \brief Throw InvalidArgument if quaternion is malformed */
inline void assertQuaternionValid(const tf::Quaternion & q)
{
+ if(std::isnan(q.x()) || std::isnan(q.y()) || std::isnan(q.z()) || std::isnan(q.w()))
+ {
+ std::stringstream ss;
+ ss << "Quaternion contains a NaN" << std::endl;
+ throw tf::InvalidArgument(ss.str());
+ }
+
if(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() - 1) > 0.01)
{
std::stringstream ss;
@@ -403,6 +410,13 @@ inline void assertQuaternionValid(const tf::Quaternion & q)
/** \brief Throw InvalidArgument if quaternion is malformed */
inline void assertQuaternionValid(const geometry_msgs::Quaternion & q)
{
+ if(std::isnan(q.x) || std::isnan(q.y) || std::isnan(q.z) || std::isnan(q.w))
+ {
+ std::stringstream ss;
+ ss << "Quaternion contains a NaN" << std::endl;
+ throw tf::InvalidArgument(ss.str());
+ }
+
if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01)
{
std::stringstream ss;
diff --git a/tf/package.xml b/tf/package.xml
index c453d59..6316950 100644
--- a/tf/package.xml
+++ b/tf/package.xml
@@ -1,6 +1,6 @@
<package>
<name>tf</name>
- <version>1.11.7</version>
+ <version>1.11.8</version>
<description>
tf is a package that lets the user keep track of multiple coordinate
diff --git a/tf/scripts/python_benchmark.py b/tf/scripts/python_benchmark.py
index d2ec919..f3f5144 100644
--- a/tf/scripts/python_benchmark.py
+++ b/tf/scripts/python_benchmark.py
@@ -30,8 +30,6 @@
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
-import roslib
-roslib.load_manifest('tf')
import rostest
import rospy
import numpy
diff --git a/tf/scripts/tf_remap b/tf/scripts/tf_remap
index 37c14db..b1ab087 100755
--- a/tf/scripts/tf_remap
+++ b/tf/scripts/tf_remap
@@ -35,8 +35,6 @@
## remap a tf topic
-import roslib; roslib.load_manifest('tf')
-
import rospy
from tf.msg import tfMessage
diff --git a/tf/scripts/view_frames b/tf/scripts/view_frames
index 8e765ef..1428fdf 100755
--- a/tf/scripts/view_frames
+++ b/tf/scripts/view_frames
@@ -38,10 +38,6 @@
from __future__ import with_statement
-PKG = 'tf' # this package name
-
-import roslib; roslib.load_manifest(PKG)
-
import sys, time
import os
import string
diff --git a/tf/src/tf/listener.py b/tf/src/tf/listener.py
index 2b732e7..8d057da 100644
--- a/tf/src/tf/listener.py
+++ b/tf/src/tf/listener.py
@@ -25,10 +25,6 @@
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
-PKG = 'tf'
-import roslib
-roslib.load_manifest(PKG)
-
import rospy
import tf as TFX
from tf import transformations
diff --git a/tf/test/method_test.py b/tf/test/method_test.py
index 1d552bd..37ca99b 100644
--- a/tf/test/method_test.py
+++ b/tf/test/method_test.py
@@ -1,5 +1,3 @@
-import roslib; roslib.load_manifest("tf")
-
import rospy
import tf
import time
diff --git a/tf/test/python_debug_test.py b/tf/test/python_debug_test.py
index ed633d0..c79f4d9 100644
--- a/tf/test/python_debug_test.py
+++ b/tf/test/python_debug_test.py
@@ -25,8 +25,6 @@
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
-import roslib; roslib.load_manifest("tf")
-
import rospy
import tf
import time
diff --git a/tf/test/testPython.py b/tf/test/testPython.py
index b2c2f52..76c16f4 100644
--- a/tf/test/testPython.py
+++ b/tf/test/testPython.py
@@ -68,14 +68,14 @@ class TestPython(unittest.TestCase):
def test_wait_for_transform(self):
def elapsed_time_within_epsilon(t, delta, epsilon):
- self.assertGreater( t - epsilon, delta)
- self.assertLess( delta, t + epsilon)
+ self.assertLess( t - epsilon, delta)
+ self.assertGreater( delta, t + epsilon)
t = tf.Transformer()
self.common(t)
- timeout = rospy.Duration(0.1)
- epsilon = 0.05
+ timeout = rospy.Duration(1.1)
+ epsilon = 0.1
# Check for dedicated thread exception, existing frames
self.assertRaises(tf.Exception, lambda: t.waitForTransform("PARENT", "THISFRAME", rospy.Time(), timeout))
@@ -85,6 +85,12 @@ class TestPython(unittest.TestCase):
# This will no longer thorw
self.assertEqual(t.waitForTransform("PARENT", "THISFRAME", rospy.Time(), timeout), None)
+ self.assertEqual(t.waitForTransform("PARENT", "THISFRAME", rospy.Time(15), timeout), None)
+
+ # Verify exception still thrown with unavailable time near timeout
+ start = time.clock()
+ self.assertRaises(tf.Exception, lambda: t.waitForTransform("PARENT", "THISFRAME", rospy.Time(25), timeout))
+ elapsed_time_within_epsilon(start, timeout.to_sec(), epsilon)
# Verify exception stil thrown with non-existing frames near timeout
start = time.clock()
diff --git a/tf/test/test_datatype_conversion.py b/tf/test/test_datatype_conversion.py
index 495be3e..67e2114 100755
--- a/tf/test/test_datatype_conversion.py
+++ b/tf/test/test_datatype_conversion.py
@@ -1,5 +1,4 @@
#!/usr/bin/env python
-import roslib; roslib.load_manifest('tf')
import sys
import unittest
diff --git a/tf/test/tf_unittest.cpp b/tf/test/tf_unittest.cpp
index 3a77f2d..0c01cdf 100644
--- a/tf/test/tf_unittest.cpp
+++ b/tf/test/tf_unittest.cpp
@@ -2390,6 +2390,23 @@ TEST(tf, assertQuaternionValid)
q.setZ(sqrt(2.0)/2.0 - 0.01);
EXPECT_TRUE(expectInvalidQuaternion(q));
+ // check NaNs
+ q.setValue(1,0,0,0);
+ q.setX(0.0/0.0);
+ EXPECT_TRUE(expectInvalidQuaternion(q));
+ q.setX(1.0);
+
+ q.setY(NAN);
+ EXPECT_TRUE(expectInvalidQuaternion(q));
+ q.setY(0.0);
+
+ q.setZ(0.0/0.0);
+ EXPECT_TRUE(expectInvalidQuaternion(q));
+ q.setZ(0.0);
+
+ q.setW(0.0/0.0);
+ EXPECT_TRUE(expectInvalidQuaternion(q));
+ q.setW(0.0);
/* Waiting for gtest 1.1 or later
EXPECT_NO_THROW(tf::assertQuaternionValid(q));
@@ -2428,6 +2445,23 @@ TEST(tf, assertQuaternionMsgValid)
q.z = sqrt(2.0)/2.0 - 0.01;
EXPECT_TRUE(expectInvalidQuaternion(q));
+ // check NaNs
+ q.x = 1.0; q.y = 0.0; q.z = 0.0; q.w = 0.0;
+ q.x = 0.0/0.0;
+ EXPECT_TRUE(expectInvalidQuaternion(q));
+ q.x = 1.0;
+
+ q.y = NAN;
+ EXPECT_TRUE(expectInvalidQuaternion(q));
+ q.y = 0.0;
+
+ q.z = 0.0/0.0;
+ EXPECT_TRUE(expectInvalidQuaternion(q));
+ q.z = 0.0;
+
+ q.w = 0.0/0.0;
+ EXPECT_TRUE(expectInvalidQuaternion(q));
+ q.w = 0.0;
/* Waiting for gtest 1.1 or later
EXPECT_NO_THROW(tf::assertQuaternionValid(q));
diff --git a/tf_conversions/CHANGELOG.rst b/tf_conversions/CHANGELOG.rst
index 7e18608..77b63e0 100644
--- a/tf_conversions/CHANGELOG.rst
+++ b/tf_conversions/CHANGELOG.rst
@@ -2,6 +2,12 @@
Changelog for package tf_conversions
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.11.8 (2016-03-04)
+-------------------
+* tf_conversions: Add conversion functions for Eigen::Isometry3d
+* Remove outdated manifest loading in python files
+* Contributors: Maarten de Vries, Michael Hwang
+
1.11.7 (2015-04-21)
-------------------
* Fixed Vector3 documentation
diff --git a/tf_conversions/conf.py b/tf_conversions/conf.py
index 4f6b760..ac4c35e 100644
--- a/tf_conversions/conf.py
+++ b/tf_conversions/conf.py
@@ -11,8 +11,6 @@
# All configuration values have a default; values that are commented out
# serve to show the default.
-import roslib
-roslib.load_manifest('tf_conversions')
import sys, os
# If extensions (or modules to document with autodoc) are in another directory,
diff --git a/tf_conversions/include/tf_conversions/tf_eigen.h b/tf_conversions/include/tf_conversions/tf_eigen.h
index d33cbe1..41d2a7c 100644
--- a/tf_conversions/include/tf_conversions/tf_eigen.h
+++ b/tf_conversions/include/tf_conversions/tf_eigen.h
@@ -47,9 +47,15 @@ void matrixEigenToTF(const Eigen::Matrix3d &e, tf::Matrix3x3 &t);
/// Converts a tf Pose into an Eigen Affine3d
void poseTFToEigen(const tf::Pose &t, Eigen::Affine3d &e);
+/// Converts a tf Pose into an Eigen Isometry3d
+void poseTFToEigen(const tf::Pose &t, Eigen::Isometry3d &e);
+
/// Converts an Eigen Affine3d into a tf Transform
void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t);
+/// Converts an Eigen Isometry3d into a tf Transform
+void poseEigenToTF(const Eigen::Isometry3d &e, tf::Pose &t);
+
/// Converts a tf Quaternion into an Eigen Quaternion
void quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e);
@@ -59,9 +65,15 @@ void quaternionEigenToTF(const Eigen::Quaterniond &e, tf::Quaternion &t);
/// Converts a tf Transform into an Eigen Affine3d
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e);
+/// Converts a tf Transform into an Eigen Isometry3d
+void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e);
+
/// Converts an Eigen Affine3d into a tf Transform
void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t);
+/// Converts an Eigen Isometry3d into a tf Transform
+void transformEigenToTF(const Eigen::Isometry3d &e, tf::Transform &t);
+
/// Converts a tf Vector3 into an Eigen Vector3d
void vectorTFToEigen(const tf::Vector3 &t, Eigen::Vector3d &e);
diff --git a/tf_conversions/package.xml b/tf_conversions/package.xml
index 840a163..726bbfd 100644
--- a/tf_conversions/package.xml
+++ b/tf_conversions/package.xml
@@ -1,6 +1,6 @@
<package>
<name>tf_conversions</name>
- <version>1.11.7</version>
+ <version>1.11.8</version>
<description>
This package contains a set of conversion functions to convert
diff --git a/tf_conversions/src/tf_eigen.cpp b/tf_conversions/src/tf_eigen.cpp
index c71a8e9..8114442 100644
--- a/tf_conversions/src/tf_eigen.cpp
+++ b/tf_conversions/src/tf_eigen.cpp
@@ -52,11 +52,21 @@ namespace tf {
transformTFToEigen(t, e);
}
+ void poseTFToEigen(const tf::Pose &t, Eigen::Isometry3d &e)
+ {
+ transformTFToEigen(t, e);
+ }
+
void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t)
{
transformEigenToTF(e, t);
}
+ void poseEigenToTF(const Eigen::Isometry3d &e, tf::Pose &t)
+ {
+ transformEigenToTF(e, t);
+ }
+
void quaternionTFToEigen(const tf::Quaternion& t, Eigen::Quaterniond& e)
{
e = Eigen::Quaterniond(t[3],t[0],t[1],t[2]);
@@ -70,30 +80,54 @@ namespace tf {
t[3] = e.w();
}
- void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
- {
- for(int i=0; i<3; i++)
+ namespace {
+ template<typename Transform>
+ void transformTFToEigenImpl(const tf::Transform &t, Transform & e)
{
- e.matrix()(i,3) = t.getOrigin()[i];
- for(int j=0; j<3; j++)
+ for(int i=0; i<3; i++)
{
- e.matrix()(i,j) = t.getBasis()[i][j];
+ e.matrix()(i,3) = t.getOrigin()[i];
+ for(int j=0; j<3; j++)
+ {
+ e.matrix()(i,j) = t.getBasis()[i][j];
+ }
}
+ // Fill in identity in last row
+ for (int col = 0 ; col < 3; col ++)
+ e.matrix()(3, col) = 0;
+ e.matrix()(3,3) = 1;
}
- // Fill in identity in last row
- for (int col = 0 ; col < 3; col ++)
- e.matrix()(3, col) = 0;
- e.matrix()(3,3) = 1;
+
+ template<typename T>
+ void transformEigenToTFImpl(const T &e, tf::Transform &t)
+ {
+ t.setOrigin(tf::Vector3( e.matrix()(0,3), e.matrix()(1,3), e.matrix()(2,3)));
+ t.setBasis(tf::Matrix3x3(e.matrix()(0,0), e.matrix()(0,1), e.matrix()(0,2),
+ e.matrix()(1,0), e.matrix()(1,1), e.matrix()(1,2),
+ e.matrix()(2,0), e.matrix()(2,1), e.matrix()(2,2)));
+ }
+ }
+
+ void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
+ {
+ transformTFToEigenImpl(t, e);
+ };
+
+ void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e)
+ {
+ transformTFToEigenImpl(t, e);
};
void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
{
- t.setOrigin(tf::Vector3( e.matrix()(0,3), e.matrix()(1,3), e.matrix()(2,3)));
- t.setBasis(tf::Matrix3x3(e.matrix()(0,0), e.matrix()(0,1),e.matrix()(0,2),
- e.matrix()(1,0), e.matrix()(1,1),e.matrix()(1,2),
- e.matrix()(2,0), e.matrix()(2,1),e.matrix()(2,2)));
+ transformEigenToTFImpl(e, t);
}
-
+
+ void transformEigenToTF(const Eigen::Isometry3d &e, tf::Transform &t)
+ {
+ transformEigenToTFImpl(e, t);
+ }
+
void vectorTFToEigen(const tf::Vector3& t, Eigen::Vector3d& e)
{
e(0) = t[0];
diff --git a/tf_conversions/test/posemath.py b/tf_conversions/test/posemath.py
index f12d718..047a2d8 100644
--- a/tf_conversions/test/posemath.py
+++ b/tf_conversions/test/posemath.py
@@ -1,5 +1,3 @@
-import roslib
-roslib.load_manifest('tf_conversions')
import rostest
import rospy
import numpy
diff --git a/tf_conversions/test/test_eigen_tf.cpp b/tf_conversions/test/test_eigen_tf.cpp
index bb0c6a8..2a68572 100644
--- a/tf_conversions/test/test_eigen_tf.cpp
+++ b/tf_conversions/test/test_eigen_tf.cpp
@@ -85,43 +85,56 @@ TEST(TFEigenConversions, tf_eigen_transform)
t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
t.setRotation(tq);
- Eigen::Affine3d k;
- transformTFToEigen(t,k);
+ Eigen::Affine3d affine;
+ Eigen::Isometry3d isometry;
+ transformTFToEigen(t, affine);
+ transformTFToEigen(t, isometry);
for(int i=0; i < 3; i++)
{
- ASSERT_NEAR(t.getOrigin()[i],k.matrix()(i,3),1e-6);
+ ASSERT_NEAR(t.getOrigin()[i],affine.matrix()(i,3),1e-6);
+ ASSERT_NEAR(t.getOrigin()[i],isometry.matrix()(i,3),1e-6);
for(int j=0; j < 3; j++)
- {
- ASSERT_NEAR(t.getBasis()[i][j],k.matrix()(i,j),1e-6);
+ {
+ ASSERT_NEAR(t.getBasis()[i][j],affine.matrix()(i,j),1e-6);
+ ASSERT_NEAR(t.getBasis()[i][j],isometry.matrix()(i,j),1e-6);
}
}
for (int col = 0 ; col < 3; col ++)
- ASSERT_NEAR(k.matrix()(3, col), 0, 1e-6);
- ASSERT_NEAR(k.matrix()(3,3), 1, 1e-6);
-
+ {
+ ASSERT_NEAR(affine.matrix()(3, col), 0, 1e-6);
+ ASSERT_NEAR(isometry.matrix()(3, col), 0, 1e-6);
+ }
+ ASSERT_NEAR(affine.matrix()(3,3), 1, 1e-6);
+ ASSERT_NEAR(isometry.matrix()(3,3), 1, 1e-6);
}
TEST(TFEigenConversions, eigen_tf_transform)
{
- tf::Transform t;
- Eigen::Affine3d k;
+ tf::Transform t1;
+ tf::Transform t2;
+ Eigen::Affine3d affine;
+ Eigen::Isometry3d isometry;
Eigen::Quaterniond kq;
kq.coeffs()(0) = gen_rand(-1.0,1.0);
kq.coeffs()(1) = gen_rand(-1.0,1.0);
kq.coeffs()(2) = gen_rand(-1.0,1.0);
kq.coeffs()(3) = gen_rand(-1.0,1.0);
kq.normalize();
- k.translate(Eigen::Vector3d(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
- k.rotate(kq);
+ isometry.translate(Eigen::Vector3d(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
+ isometry.rotate(kq);
+ affine = isometry;
- transformEigenToTF(k,t);
+ transformEigenToTF(affine,t1);
+ transformEigenToTF(isometry,t2);
for(int i=0; i < 3; i++)
{
- ASSERT_NEAR(t.getOrigin()[i],k.matrix()(i,3),1e-6);
+ ASSERT_NEAR(t1.getOrigin()[i],affine.matrix()(i,3),1e-6);
+ ASSERT_NEAR(t2.getOrigin()[i],isometry.matrix()(i,3),1e-6);
for(int j=0; j < 3; j++)
- {
- ASSERT_NEAR(t.getBasis()[i][j],k.matrix()(i,j),1e-6);
+ {
+ ASSERT_NEAR(t1.getBasis()[i][j],affine.matrix()(i,j),1e-6);
+ ASSERT_NEAR(t2.getBasis()[i][j],isometry.matrix()(i,j),1e-6);
}
}
}
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/ros/ros-geometry.git
More information about the debian-science-commits
mailing list