From 5b5529a96d60d69948990160918d35cd9f8e77cd Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 5 Sep 2023 16:53:09 +0200 Subject: [PATCH] (tf2_pykdl_ros) add Twist/WrenchStamped to unittest --- .../test/test_tf2_ros_integration.py | 141 +++++++++++++++++- 1 file changed, 139 insertions(+), 2 deletions(-) diff --git a/tf2_pykdl_ros/test/test_tf2_ros_integration.py b/tf2_pykdl_ros/test/test_tf2_ros_integration.py index a644d22..8490509 100644 --- a/tf2_pykdl_ros/test/test_tf2_ros_integration.py +++ b/tf2_pykdl_ros/test/test_tf2_ros_integration.py @@ -1,9 +1,15 @@ import unittest from builtin_interfaces.msg import Time -from geometry_msgs.msg import PointStamped, PoseStamped, TransformStamped +from geometry_msgs.msg import ( + PointStamped, + PoseStamped, + TransformStamped, + TwistStamped as TwistStampedMsg, + WrenchStamped as WrenchStampedMsg, +) import PyKDL as kdl -from pykdl_ros import VectorStamped, FrameStamped +from pykdl_ros import VectorStamped, FrameStamped, TwistStamped, WrenchStamped import tf2_ros import tf2_geometry_msgs # noqa: F401 import tf2_pykdl_ros # noqa: F401 @@ -44,6 +50,34 @@ def test_frame_stamped_convert(self): def test_frame_stamped_transform(self): self.transform_reg.get(FrameStamped) + def test_twist_stamped_from_msg(self): + self.convert_reg.get_from_msg(TwistStamped) + + def test_twist_stamp_to_msg(self): + self.convert_reg.get_to_msg(TwistStamped) + + def test_twist_stamp_convert(self): + self.convert_reg.get_convert((TwistStampedMsg, TwistStamped)) + self.convert_reg.get_convert((TwistStamped, TwistStampedMsg)) + self.convert_reg.get_convert((TwistStamped, TwistStamped)) + + def test_twist_stamped_transform(self): + self.transform_reg.get(TwistStamped) + + def test_wrench_stamped_from_msg(self): + self.convert_reg.get_from_msg(WrenchStamped) + + def test_wrench_stamped_to_msg(self): + self.convert_reg.get_to_msg(WrenchStamped) + + def test_wrench_stamped_convert(self): + self.convert_reg.get_convert((WrenchStampedMsg, WrenchStamped)) + self.convert_reg.get_convert((WrenchStamped, WrenchStampedMsg)) + self.convert_reg.get_convert((WrenchStamped, WrenchStamped)) + + def test_wrench_stamped_transform(self): + self.transform_reg.get(WrenchStamped) + class TestConvert(unittest.TestCase): def test_point_stamped_vector_stamped(self): @@ -71,6 +105,109 @@ def test_vector_stamped_point_stamped(self): self.assertEqual(p.header.stamp, v.header.stamp) self.assertEqual(p.header.frame_id, v.header.frame_id) + def test_pose_stamp_frame_stamped(self): + p = PoseStamped() + p.header.frame_id = "map" + p.header.stamp = Time(sec=4) + p.pose.position.x = 1.0 + p.pose.position.y = 2.0 + p.pose.position.z = 3.0 + p.pose.orientation.x = 0.0 + p.pose.orientation.y = 0.0 + p.pose.orientation.z = 0.0 + p.pose.orientation.w = 1.0 + f = tf2_ros.convert(p, FrameStamped) + self.assertIsInstance(f, FrameStamped) + self.assertEqual(f.frame.p.x(), p.pose.position.x) + self.assertEqual(f.frame.p.y(), p.pose.position.y) + self.assertEqual(f.frame.p.z(), p.pose.position.z) + self.assertEqual(f.frame.M.GetQuaternion(), (0.0, 0.0, 0.0, 1.0)) + self.assertEqual(f.header.stamp, p.header.stamp) + self.assertEqual(f.header.frame_id, p.header.frame_id) + + def test_frame_stamped_pose_stamped(self): + f = FrameStamped.from_xyz_rpy(1, 2, 3, 4, 5, 6, Time(sec=4), "map") + p = tf2_ros.convert(f, PoseStamped) + self.assertIsInstance(p, PoseStamped) + self.assertEqual(p.pose.position.x, f.frame.p.x()) + self.assertEqual(p.pose.position.y, f.frame.p.y()) + self.assertEqual(p.pose.position.z, f.frame.p.z()) + q = f.frame.M.GetQuaternion() + self.assertEqual(p.pose.orientation.x, q[0]) + self.assertEqual(p.pose.orientation.y, q[1]) + self.assertEqual(p.pose.orientation.z, q[2]) + self.assertEqual(p.pose.orientation.w, q[3]) + self.assertEqual(p.header.stamp, f.header.stamp) + self.assertEqual(p.header.frame_id, f.header.frame_id) + + def test_twist_stamped_msg_twist_stamped(self): + msg = TwistStampedMsg() + msg.header.frame_id = "map" + msg.header.stamp = Time(sec=4) + msg.twist.linear.x = 1.0 + msg.twist.linear.y = 2.0 + msg.twist.linear.z = 3.0 + msg.twist.angular.x = 4.0 + msg.twist.angular.y = 5.0 + msg.twist.angular.z = 6.0 + t = tf2_ros.convert(msg, TwistStamped) + self.assertIsInstance(t, TwistStamped) + self.assertEqual(t.twist.vel.x(), msg.twist.linear.x) + self.assertEqual(t.twist.vel.y(), msg.twist.linear.y) + self.assertEqual(t.twist.vel.z(), msg.twist.linear.z) + self.assertEqual(t.twist.rot.x(), msg.twist.angular.x) + self.assertEqual(t.twist.rot.y(), msg.twist.angular.y) + self.assertEqual(t.twist.rot.z(), msg.twist.angular.z) + self.assertEqual(t.header.stamp, msg.header.stamp) + self.assertEqual(t.header.frame_id, msg.header.frame_id) + + def test_twist_stamped_twist_stamped_msg(self): + t = TwistStamped.from_xyz_rpy(1, 2, 3, 4, 5, 6, Time(sec=4), "map") + msg = tf2_ros.convert(t, TwistStampedMsg) + self.assertIsInstance(msg, TwistStampedMsg) + self.assertEqual(msg.twist.linear.x, t.twist.vel.x()) + self.assertEqual(msg.twist.linear.y, t.twist.vel.y()) + self.assertEqual(msg.twist.linear.z, t.twist.vel.z()) + self.assertEqual(msg.twist.angular.x, t.twist.rot.x()) + self.assertEqual(msg.twist.angular.y, t.twist.rot.y()) + self.assertEqual(msg.twist.angular.z, t.twist.rot.z()) + self.assertEqual(msg.header.stamp, t.header.stamp) + self.assertEqual(msg.header.frame_id, t.header.frame_id) + + def test_wrench_stamped_msg_wrench_stamped(self): + msg = WrenchStampedMsg() + msg.header.frame_id = "map" + msg.header.stamp = Time(sec=4) + msg.wrench.force.x = 1.0 + msg.wrench.force.y = 2.0 + msg.wrench.force.z = 3.0 + msg.wrench.torque.x = 4.0 + msg.wrench.torque.y = 5.0 + msg.wrench.torque.z = 6.0 + w = tf2_ros.convert(msg, WrenchStamped) + self.assertIsInstance(w, WrenchStamped) + self.assertEqual(w.wrench.force.x(), msg.wrench.force.x) + self.assertEqual(w.wrench.force.y(), msg.wrench.force.y) + self.assertEqual(w.wrench.force.z(), msg.wrench.force.z) + self.assertEqual(w.wrench.torque.x(), msg.wrench.torque.x) + self.assertEqual(w.wrench.torque.y(), msg.wrench.torque.y) + self.assertEqual(w.wrench.torque.z(), msg.wrench.torque.z) + self.assertEqual(w.header.stamp, msg.header.stamp) + self.assertEqual(w.header.frame_id, msg.header.frame_id) + + def test_wrench_stamped_wrench_stamped_msg(self): + w = WrenchStamped.from_fxfyfz_txtytz(1, 2, 3, 4, 5, 6, Time(sec=4), "map") + msg = tf2_ros.convert(w, WrenchStampedMsg) + self.assertIsInstance(msg, WrenchStampedMsg) + self.assertEqual(msg.wrench.force.x, w.wrench.force.x()) + self.assertEqual(msg.wrench.force.y, w.wrench.force.y()) + self.assertEqual(msg.wrench.force.z, w.wrench.force.z()) + self.assertEqual(msg.wrench.torque.x, w.wrench.torque.x()) + self.assertEqual(msg.wrench.torque.y, w.wrench.torque.y()) + self.assertEqual(msg.wrench.torque.z, w.wrench.torque.z()) + self.assertEqual(msg.header.stamp, w.header.stamp) + self.assertEqual(msg.header.frame_id, w.header.frame_id) + class TestTransform(unittest.TestCase): @classmethod