[ROS2] transformstamped

tf2_pykdl_ros/test/test_tf2_ros_integration.py

@@ -35,6 +35,8 @@ def test_vector_stamped_to_msg(self) -> None:
     def test_vector_stamped_convert(self) -> None:
         self.convert_reg.get_convert((PointStamped, VectorStamped))
         self.convert_reg.get_convert((PoseStamped, VectorStamped))
+        self.convert_reg.get_convert((TransformStamped, VectorStamped))
+
         self.convert_reg.get_convert((VectorStamped, PointStamped))
         self.convert_reg.get_convert((VectorStamped, VectorStamped))
 
@@ -48,8 +50,13 @@ def test_frame_stamped_to_msg(self) -> None:
         self.convert_reg.get_to_msg(FrameStamped)
 
     def test_frame_stamped_convert(self) -> None:
+        self.convert_reg.get_convert((PointStamped, FrameStamped))
         self.convert_reg.get_convert((PoseStamped, FrameStamped))
+        self.convert_reg.get_convert((TransformStamped, FrameStamped))
+
+        self.convert_reg.get_convert((FrameStamped, PointStamped))
         self.convert_reg.get_convert((FrameStamped, PoseStamped))
+        self.convert_reg.get_convert((FrameStamped, TransformStamped))
         self.convert_reg.get_convert((FrameStamped, FrameStamped))
 
     def test_frame_stamped_transform(self) -> None:
@@ -85,13 +92,36 @@ def test_wrench_stamped_transform(self) -> None:
 
 
 class TestConvert(unittest.TestCase):
+    point_stamped: ClassVar[PointStamped]
+    pose_stamped: ClassVar[PoseStamped]
+    frame_stamped: ClassVar[FrameStamped]
+
+    @classmethod
+    def setUpClass(cls) -> None:
+        point = PointStamped()
+        point.header.frame_id = "map"
+        point.header.stamp = Time(sec=4)
+        point.point.x = 1.0
+        point.point.y = 2.0
+        point.point.z = 3.0
+        cls.point_stamped = point
+
+        pose = PoseStamped()
+        pose.header.frame_id = "map"
+        pose.header.stamp = Time(sec=4)
+        pose.pose.position.x = 1.0
+        pose.pose.position.y = 2.0
+        pose.pose.position.z = 3.0
+        pose.pose.orientation.x = 0.0
+        pose.pose.orientation.y = 0.0
+        pose.pose.orientation.z = 0.0
+        pose.pose.orientation.w = 1.0
+        cls.pose_stamped = pose
+
+        cls.frame_stamped = FrameStamped.from_xyz_rpy(1, 2, 3, 4, 5, 6, Time(sec=4), "map")
+
     def test_point_stamped_vector_stamped(self) -> None:
-        p = PointStamped()
-        p.header.frame_id = "map"
-        p.header.stamp = Time(sec=4)
-        p.point.x = 1.0
-        p.point.y = 2.0
-        p.point.z = 3.0
+        p = self.point_stamped
         v = tf2_ros.convert(p, VectorStamped)
         self.assertIsInstance(v, VectorStamped)
         self.assertEqual(v.vector.x(), p.point.x)
@@ -100,6 +130,16 @@ def test_point_stamped_vector_stamped(self) -> None:
         self.assertEqual(v.header.stamp, p.header.stamp)
         self.assertEqual(v.header.frame_id, p.header.frame_id)
 
+    def test_pose_stamped_vector_stamped(self) -> None:
+        p = self.pose_stamped
+        v = tf2_ros.convert(p, VectorStamped)
+        self.assertIsInstance(v, VectorStamped)
+        self.assertEqual(v.vector.x(), p.pose.position.x)
+        self.assertEqual(v.vector.y(), p.pose.position.y)
+        self.assertEqual(v.vector.z(), p.pose.position.z)
+        self.assertEqual(v.header.stamp, p.header.stamp)
+        self.assertEqual(v.header.frame_id, p.header.frame_id)
+
     def test_vector_stamped_point_stamped(self) -> None:
         v = VectorStamped.from_xyz(1, 2, 3, Time(sec=4), "map")
         p = tf2_ros.convert(v, PointStamped)
@@ -110,28 +150,62 @@ def test_vector_stamped_point_stamped(self) -> None:
         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) -> None:
-        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
+    def test_point_stamped_frane_stamped(self) -> None:
+        p = self.point_stamped
+        f = tf2_ros.convert(p, FrameStamped)
+        self.assertIsInstance(f, FrameStamped)
+        self.assertEqual(f.frame.p.x(), p.point.x)
+        self.assertEqual(f.frame.p.y(), p.point.y)
+        self.assertEqual(f.frame.p.z(), p.point.z)
+        self.assertEqual(f.frame.M.GetQuaternion(), kdl.Rotation.Identity().GetQuaternion())
+        self.assertEqual(f.header.stamp, p.header.stamp)
+        self.assertEqual(f.header.frame_id, p.header.frame_id)
+
+    def test_pose_stamped_frame_stamped(self) -> None:
+        p = self.pose_stamped
         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))
+        q = p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w
+        self.assertEqual(f.frame.M.GetQuaternion(), q)
         self.assertEqual(f.header.stamp, p.header.stamp)
         self.assertEqual(f.header.frame_id, p.header.frame_id)
 
+    def test_transform_stamped_frame_stamped(self) -> None:
+        t = TransformStamped()
+        t.transform.translation.x = self.pose_stamped.pose.position.x
+        t.transform.translation.y = self.pose_stamped.pose.position.y
+        t.transform.translation.z = self.pose_stamped.pose.position.z
+        t.transform.rotation.x = self.pose_stamped.pose.orientation.x
+        t.transform.rotation.y = self.pose_stamped.pose.orientation.y
+        t.transform.rotation.z = self.pose_stamped.pose.orientation.z
+        t.transform.rotation.w = self.pose_stamped.pose.orientation.w
+        t.header = self.pose_stamped.header
+        t.child_frame_id = "child"  # Not used
+        f = tf2_ros.convert(t, FrameStamped)
+        self.assertIsInstance(f, FrameStamped)
+        self.assertEqual(f.frame.p.x(), t.transform.translation.x)
+        self.assertEqual(f.frame.p.y(), t.transform.translation.y)
+        self.assertEqual(f.frame.p.z(), t.transform.translation.z)
+        q = t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w
+        self.assertEqual(f.frame.M.GetQuaternion(), q)
+        self.assertEqual(f.header.stamp, t.header.stamp)
+        self.assertEqual(f.header.frame_id, t.header.frame_id)
+
+    def test_frame_stamped_point_stamped(self) -> None:
+        f = self.frame_stamped
+        p = tf2_ros.convert(f, PointStamped)
+        self.assertIsInstance(p, PointStamped)
+        self.assertEqual(p.point.x, f.frame.p.x())
+        self.assertEqual(p.point.y, f.frame.p.y())
+        self.assertEqual(p.point.z, f.frame.p.z())
+        self.assertEqual(p.header.stamp, f.header.stamp)
+        self.assertEqual(p.header.frame_id, f.header.frame_id)
+
     def test_frame_stamped_pose_stamped(self) -> None:
-        f = FrameStamped.from_xyz_rpy(1, 2, 3, 4, 5, 6, Time(sec=4), "map")
+        f = self.frame_stamped
         p = tf2_ros.convert(f, PoseStamped)
         self.assertIsInstance(p, PoseStamped)
         self.assertEqual(p.pose.position.x, f.frame.p.x())
@@ -145,6 +219,22 @@ def test_frame_stamped_pose_stamped(self) -> None:
         self.assertEqual(p.header.stamp, f.header.stamp)
         self.assertEqual(p.header.frame_id, f.header.frame_id)
 
+    def test_frame_stamped_transform_stamped(self) -> None:
+        f = self.frame_stamped
+        t = tf2_ros.convert(f, TransformStamped)
+        self.assertIsInstance(t, TransformStamped)
+        self.assertEqual(t.transform.translation.x, f.frame.p.x())
+        self.assertEqual(t.transform.translation.y, f.frame.p.y())
+        self.assertEqual(t.transform.translation.z, f.frame.p.z())
+        q = f.frame.M.GetQuaternion()
+        self.assertEqual(t.transform.rotation.x, q[0])
+        self.assertEqual(t.transform.rotation.y, q[1])
+        self.assertEqual(t.transform.rotation.z, q[2])
+        self.assertEqual(t.transform.rotation.w, q[3])
+        self.assertEqual(t.header.stamp, f.header.stamp)
+        self.assertEqual(t.header.frame_id, f.header.frame_id)
+        self.assertEqual(t.child_frame_id, "")  # Not set
+
     def test_twist_stamped_msg_twist_stamped(self) -> None:
         msg = TwistStampedMsg()
         msg.header.frame_id = "map"

tf2_pykdl_ros/tf2_pykdl_ros/__init__.py

@@ -46,24 +46,27 @@ def to_msg_vector(vector: VectorStamped) -> PointStamped:
 tf2_ros.ConvertRegistration().add_to_msg(VectorStamped, to_msg_vector)
 
 
-def from_msg_vector(msg: Union[PointStamped, PoseStamped]) -> VectorStamped:
+def from_msg_vector(msg: Union[PointStamped, PoseStamped, TransformStamped]) -> VectorStamped:
     """
-    Convert a PointStamped/PoseStamped message to a stamped VectorStamped.
+    Convert a PointStamped/PoseStamped/TransformStamped message to a stamped VectorStamped.
 
-    :param msg: The PointStamped/PoseStamped message to convert.
+    :param msg: The PointStamped/PoseStamped/TransformStamped message to convert.
     :return: The timestamped converted PyKDL vector.
     """
     if isinstance(msg, PointStamped):
         v = msg.point
     elif isinstance(msg, PoseStamped):
         v = msg.pose.position
+    elif isinstance(msg, TransformStamped):
+        v = msg.transform.translation
     else:
-        raise TypeError(f"msg should be PointStamped or PoseStamped, not '{type(msg)}'")
+        raise TypeError(f"msg should be PointStamped, PoseStamped or TransformStamped, not '{type(msg)}'")
     return VectorStamped.from_xyz(v.x, v.y, v.z, msg.header.stamp, msg.header.frame_id)
 
 
 tf2_ros.ConvertRegistration().add_convert((PointStamped, VectorStamped), from_msg_vector)
 tf2_ros.ConvertRegistration().add_convert((PoseStamped, VectorStamped), from_msg_vector)
+tf2_ros.ConvertRegistration().add_convert((TransformStamped, VectorStamped), from_msg_vector)
 tf2_ros.ConvertRegistration().add_from_msg(VectorStamped, from_msg_vector)
 
 
@@ -96,7 +99,26 @@ def do_transform_vector(vector: VectorStamped, transform: TransformStamped) -> V
 tf2_ros.TransformRegistration().add(VectorStamped, do_transform_vector)
 
 
-def to_msg_frame(frame: FrameStamped) -> PoseStamped:
+def to_point_msg_frame(frame: FrameStamped) -> PointStamped:
+    """
+    Convert a FrameStamped to a geometry_msgs PointStamped message.
+
+    :param frame: The frame to convert.
+    :return: The converted point.
+    """
+    msg = PointStamped()
+    msg.header = frame.header
+    v = frame.frame.p
+    msg.point.x = v[0]
+    msg.point.y = v[1]
+    msg.point.z = v[2]
+    return msg
+
+
+tf2_ros.ConvertRegistration().add_convert((FrameStamped, PointStamped), to_point_msg_frame)
+
+
+def to_pose_msg_frame(frame: FrameStamped) -> PoseStamped:
     """
     Convert a FrameStamped to a geometry_msgs PoseStamped message.
 
@@ -117,28 +139,84 @@ def to_msg_frame(frame: FrameStamped) -> PoseStamped:
     return msg
 
 
-tf2_ros.ConvertRegistration().add_convert((FrameStamped, PoseStamped), to_msg_frame)
-tf2_ros.ConvertRegistration().add_to_msg(FrameStamped, to_msg_frame)
+tf2_ros.ConvertRegistration().add_convert((FrameStamped, PoseStamped), to_pose_msg_frame)
+tf2_ros.ConvertRegistration().add_to_msg(FrameStamped, to_pose_msg_frame)
+
+
+def to_transform_msg_frame(frame: FrameStamped) -> TransformStamped:
+    """
+    Convert a FrameStamped to a geometry_msgs TransformStamped message.
+
+    The child_frame_id is not set.
+
+    :param frame: The frame to convert.
+    :return: The converted Pose.
+    """
+    msg = TransformStamped()
+    msg.header = frame.header
+    v = frame.frame.p
+    msg.transform.translation.x = v[0]
+    msg.transform.translation.y = v[1]
+    msg.transform.translation.z = v[2]
+    q = frame.frame.M.GetQuaternion()
+    msg.transform.rotation.x = q[0]
+    msg.transform.rotation.y = q[1]
+    msg.transform.rotation.z = q[2]
+    msg.transform.rotation.w = q[3]
+    return msg
+
+
+tf2_ros.ConvertRegistration().add_convert((FrameStamped, TransformStamped), to_transform_msg_frame)
 
 
-def from_msg_frame(msg: PoseStamped) -> FrameStamped:
+def from_point_msg_frame(msg: PointStamped) -> FrameStamped:
     """
-    Convert a PoseStamped message to a stamped FrameStamped.
+    Convert a PointStamped message to a stamped FrameStamped.
 
-    :param msg: The PoseStamped message to convert.
+    :param msg: The PointStamped message to convert.
     :return: The timestamped converted PyKDL frame.
     """
-    if not isinstance(msg, PoseStamped):
-        raise TypeError(f"msg should be PoseStamped, not '{type(msg)}'")
-    v = msg.pose.position
+    if not isinstance(msg, PointStamped):
+        raise TypeError(f"msg should be a PointStamped, not '{type(msg)}'")
+    vector = kdl.Vector(msg.point.x, msg.point.y, msg.point.z)
+    frame = kdl.Frame(vector)
+    return FrameStamped(frame, msg.header.stamp, msg.header.frame_id)
+
+
+tf2_ros.ConvertRegistration().add_convert((PointStamped, FrameStamped), from_point_msg_frame)
+
+
+def from_msg_frame(msg: Union[PoseStamped, TransformStamped]) -> FrameStamped:
+    """
+    Convert a PoseStamped or TransformStamped message to a stamped FrameStamped.
+
+    :param msg: The message to convert.
+    :return: The timestamped converted PyKDL frame.
+    """
+    if not isinstance(msg, PoseStamped) and not isinstance(msg, TransformStamped):
+        raise TypeError(f"msg should be a PoseStamped or TransformStamped, not '{type(msg)}'")
+
+    if isinstance(msg, PoseStamped):
+        pose_attr = "pose"
+        position_attr = "position"
+        orientation_attr = "orientation"
+    elif isinstance(msg, TransformStamped):
+        pose_attr = "transform"
+        position_attr = "translation"
+        orientation_attr = "rotation"
+
+    pose = getattr(msg, pose_attr)
+    v = getattr(pose, position_attr)
     vector = kdl.Vector(v.x, v.y, v.z)
-    q = msg.pose.orientation
+    q = getattr(pose, orientation_attr)
     rotation = kdl.Rotation.Quaternion(q.x, q.y, q.z, q.w)
+
     frame = kdl.Frame(rotation, vector)
     return FrameStamped(frame, msg.header.stamp, msg.header.frame_id)
 
 
 tf2_ros.ConvertRegistration().add_convert((PoseStamped, FrameStamped), from_msg_frame)
+tf2_ros.ConvertRegistration().add_convert((TransformStamped, FrameStamped), from_msg_frame)
 tf2_ros.ConvertRegistration().add_from_msg(FrameStamped, from_msg_frame)