Remove transforms3d as dependency because of packaging problems (#746)

swri_transform_util/package.xml

@@ -48,7 +48,6 @@
   <test_depend>launch_ros</test_depend>
   <test_depend>launch_testing</test_depend>
   <test_depend>launch_testing_ament_cmake</test_depend>
-  <test_depend>python-transforms3d-pip</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

swri_transform_util/setup.py

@@ -13,6 +13,8 @@
     ],
     install_requires=['setuptools'],
     zip_safe=True,
+    maintainer='Southwest Research Institute',
+    maintainer_email='[email protected]'
     author='P. J. Reed',
     author_email='[email protected]',
     keywords=['ROS'],

swri_transform_util/test/test_initialize_origin.py

@@ -25,7 +25,8 @@
 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-#
+
+import math
 import sys
 import time
 import unittest
@@ -38,7 +39,6 @@
 from rclpy.clock import ClockType
 from rclpy.qos import QoSProfile, QoSDurabilityPolicy
 from ros2topic.api import get_msg_class
-import transforms3d as t3d
 
 PKG = 'swri_transform_util'
 NAME = 'test_initialize_origin'
@@ -59,6 +59,12 @@ def __init__(self):
         super().__init__()
         self.got_origin = False
 
+    def _yaw_from_quaternion(self, w, x, y, z):
+        sc = 2 * ((w * z) + (x * y))
+        cc = 1 - 2 * ((y * y) + (z * z))
+        yaw = math.atan2(sc, cc)
+        return yaw
+
     def subscribeToOrigin(self):
         self.assertIsNotNone(self.node)
         self.origin_class = get_msg_class(self.node, ORIGIN_TOPIC, blocking=True)
@@ -86,12 +92,11 @@ def originCallback(self, msg):
             latitude = msg.pose.position.y
             longitude = msg.pose.position.x
             altitude = msg.pose.position.z
-            quaternion = (msg.pose.orientation.w,
-                          msg.pose.orientation.x,
-                          msg.pose.orientation.y,
-                          msg.pose.orientation.z)
-            euler = t3d.euler.quat2euler(quaternion)
-            yaw = euler[2]
+            yaw = self._yaw_from_quaternion(
+                msg.pose.orientation.w,
+                msg.pose.orientation.x,
+                msg.pose.orientation.y,
+                msg.pose.orientation.z)
             self.assertAlmostEqual(yaw, 0)
         elif self.origin_class == GPSFix:
             self.node.get_logger().info("Status: %d" % msg.status.status)
@@ -110,12 +115,11 @@ def originCallback(self, msg):
             latitude = msg.position.latitude
             longitude = msg.position.longitude
             altitude = msg.position.altitude
-            quaternion = [msg.pose.orientation.w,
-                        msg.pose.orientation.x,
-                        msg.pose.orientation.y,
-                        msg.pose.orientation.z]
-            euler = t3d.euler.quat2euler(quaternion)
-            yaw = euler[2]
+            yaw = self._yaw_from_quaternion(
+                msg.pose.orientation.w,
+                msg.pose.orientation.x,
+                msg.pose.orientation.y,
+                msg.pose.orientation.z)
             self.assertAlmostEqual(yaw, 0)
         self.assertEqual(msg.header.frame_id, '/far_field')
         if self.test_stamp: