fix bad PyKDL usage

fixes #39

calibration_estimation/src/calibration_estimation/single_transform.py

@@ -114,14 +114,10 @@ def RPY_to_angle_axis(vec):
     return axis
 
 def rpy_to_quat(rpy):
-    q = [0, 0, 0, 0]
-    PyKDL.RPY(rpy[0], rpy[1], rpy[2]).GetQuaternion(q[0], q[1], q[2], q[3])
-    return q
+    return PyKDL.Rotation.RPY(rpy[0], rpy[1], rpy[2]).GetQuaternion()
 
 def quat_to_rpy(q):
-    rpy = [0, 0, 0]
-    PyKDL.Quaternion(q[0], q[1], q[2], q[3]).GetRPY(rpy[0], rpy[1], rpy[2])
-    return rpy
+    return PyKDL.Rotation.Quaternion(q[0], q[1], q[2], q[3]).GetRPY()
 
 #return 1 if value1 and value2 are within eps of each other, 0 otherwise
 def epsEq(value1, value2, eps = 1e-10):

calibration_estimation/test/single_transform_unittest.py

@@ -32,16 +32,14 @@
 # POSSIBILITY OF SUCH DAMAGE.
 
 
-import roslib; roslib.load_manifest('calibration_estimation')
-
 import sys
 import unittest
 import os
 import rospy
 import time
 import numpy
 
-from calibration_estimation.single_transform import SingleTransform
+from calibration_estimation.single_transform import SingleTransform, rpy_to_quat, quat_to_rpy
 from numpy import *
 
 class TestSingleTransform(unittest.TestCase):
@@ -153,6 +151,12 @@ def test_hard(self):
 
             self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected_result), 0.0, 4, "Failed on %s" % params_filename)
 
+    def test_math(self):
+        rpy_init = (1, 0.5, 0.7)
+        rpy = quat_to_rpy(rpy_to_quat(rpy_init))
+        for i in range(3):
+            self.assertAlmostEqual(rpy_init[i], rpy[i])
+
 if __name__ == '__main__':
     import rostest
     rostest.unitrun('pr2_calibration_estimation', 'test_SingleTransform', TestSingleTransform, coverage_packages=['pr2_calibration_estimation.single_transform'])