From d584f141985424b3beff15b5e94a3ce4dd1cc90c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 5 Sep 2023 16:54:39 +0200 Subject: [PATCH] (tf2_pykdl_ros) add Twist/WrenchStamped to integration test --- .../test/test_tf2_ros_transform_node.py | 61 +++++++++++++------ 1 file changed, 44 insertions(+), 17 deletions(-) diff --git a/tf2_pykdl_ros/test/test_tf2_ros_transform_node.py b/tf2_pykdl_ros/test/test_tf2_ros_transform_node.py index 7e0e9ac..cad394d 100755 --- a/tf2_pykdl_ros/test/test_tf2_ros_transform_node.py +++ b/tf2_pykdl_ros/test/test_tf2_ros_transform_node.py @@ -5,7 +5,7 @@ from builtin_interfaces.msg import Time from geometry_msgs.msg import TransformStamped import PyKDL as kdl -from pykdl_ros import FrameStamped +from pykdl_ros import VectorStamped, FrameStamped, TwistStamped, WrenchStamped import rclpy from rclpy.duration import Duration import tf2_ros @@ -19,34 +19,41 @@ def setUpClass(cls) -> None: cls.context = rclpy.context.Context() rclpy.init(context=cls.context) - @classmethod - def tearDownClass(cls) -> None: - rclpy.shutdown(context=cls.context) - - def setUp(self) -> None: - self.node = rclpy.create_node(node_name="test_tf2_ros_transform", context=self.context) - - def tearDown(self) -> None: - self.node.destroy_node() - - def test_transform(self): - buffer = tf2_ros.Buffer() + cls.node = rclpy.create_node(node_name="test_tf2_ros_transform", context=cls.context) + cls.buffer = tf2_ros.Buffer() t = TransformStamped() t.transform.translation.x = 1.0 t.transform.rotation.x = 1.0 + t.transform.rotation.y = 0.0 + t.transform.rotation.z = 0.0 t.transform.rotation.w = 0.0 t.header.stamp = Time(sec=2) t.header.frame_id = "a" t.child_frame_id = "b" - buffer.set_transform(t, "test_tf2_ros_convert") - out = buffer.lookup_transform("a", "b", Time(sec=2), Duration(seconds=2)) + cls.buffer.set_transform(t, "test_tf2_ros_convert") + + @classmethod + def tearDownClass(cls) -> None: + cls.node.destroy_node() + rclpy.shutdown(context=cls.context) + + def test_lookup_transform(self): + out = self.buffer.lookup_transform("a", "b", Time(sec=2), Duration(seconds=2)) self.assertEqual(out.transform.translation.x, 1) self.assertEqual(out.transform.rotation.x, 1) self.assertEqual(out.header.frame_id, "a") self.assertEqual(out.child_frame_id, "b") - f = FrameStamped(kdl.Frame(kdl.Rotation.RPY(1, 2, 3), kdl.Vector(1, 2, 3)), Time(sec=2), "a") - out = buffer.transform(f, "b") + def test_transform_vector(self): + v = kdl.Vector(1, 2, 3) + out: VectorStamped = self.buffer.transform(VectorStamped(v, Time(sec=2), "a"), "b") + self.assertEqual(out.vector.x(), 0) + self.assertEqual(out.vector.y(), -2) + self.assertEqual(out.vector.z(), -3) + + def test_transform_frame(self): + f = kdl.Frame(kdl.Rotation.RPY(1, 2, 3), kdl.Vector(1, 2, 3)) + out: FrameStamped = self.buffer.transform(FrameStamped(f, Time(sec=2), "a"), "b") self.assertEqual(out.frame.p.x(), 0) self.assertEqual(out.frame.p.y(), -2) self.assertEqual(out.frame.p.z(), -3) @@ -54,3 +61,23 @@ def test_transform(self): out.frame.M.GetQuaternion(), (0.43595284407356577, -0.44443511344300074, 0.310622451065704, 0.7182870182434113), ) + + def test_transform_twist(self): + t = kdl.Twist(kdl.Vector(1, 2, 3), kdl.Vector(4, 5, 6)) + out: TwistStamped = self.buffer.transform(TwistStamped(t, Time(sec=2), "a"), "b") + self.assertEqual(out.twist.vel.x(), 1) + self.assertEqual(out.twist.vel.y(), -8) + self.assertEqual(out.twist.vel.z(), 2) + self.assertEqual(out.twist.rot.x(), 4) + self.assertEqual(out.twist.rot.y(), -5) + self.assertEqual(out.twist.rot.z(), -6) + + def test_transform_wrench(self): + w = kdl.Wrench(kdl.Vector(1, 2, 3), kdl.Vector(4, 5, 6)) + out: WrenchStamped = self.buffer.transform(WrenchStamped(w, Time(sec=2), "a"), "b") + self.assertEqual(out.wrench.force.x(), 1) + self.assertEqual(out.wrench.force.y(), -2) + self.assertEqual(out.wrench.force.z(), -3) + self.assertEqual(out.wrench.torque.x(), 4) + self.assertEqual(out.wrench.torque.y(), -8) + self.assertEqual(out.wrench.torque.z(), -4)