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: