diff --git a/swri_transform_util/package.xml b/swri_transform_util/package.xml
index ae2cdd1a..72f4831c 100644
--- a/swri_transform_util/package.xml
+++ b/swri_transform_util/package.xml
@@ -48,7 +48,6 @@
launch_ros
launch_testing
launch_testing_ament_cmake
- python-transforms3d-pip
ament_cmake
diff --git a/swri_transform_util/setup.py b/swri_transform_util/setup.py
index fc88e066..e4b799de 100644
--- a/swri_transform_util/setup.py
+++ b/swri_transform_util/setup.py
@@ -13,6 +13,8 @@
],
install_requires=['setuptools'],
zip_safe=True,
+ maintainer='Southwest Research Institute',
+ maintainer_email='swri-robotics@swri.org'
author='P. J. Reed',
author_email='preed@swri.org',
keywords=['ROS'],
diff --git a/swri_transform_util/test/test_initialize_origin.py b/swri_transform_util/test/test_initialize_origin.py
index 48a1db4e..9642a9bf 100644
--- a/swri_transform_util/test/test_initialize_origin.py
+++ b/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: