(tf2_pykdl_ros) add Twist/WrenchStamped to integration test

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,38 +19,65 @@ 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)
         self.assertEqual(
             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)