From f2dadaa258c44e43563e1360caa98b0d71fbc794 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 23 Nov 2023 10:27:16 +0100 Subject: [PATCH 1/2] (tf2_pykdl_ros) extend tests --- .../test/test_tf2_ros_integration.py | 128 +++++++++++++++--- 1 file changed, 109 insertions(+), 19 deletions(-) diff --git a/tf2_pykdl_ros/test/test_tf2_ros_integration.py b/tf2_pykdl_ros/test/test_tf2_ros_integration.py index 24934ee..5109b95 100644 --- a/tf2_pykdl_ros/test/test_tf2_ros_integration.py +++ b/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" From 38ed0999fa3507f01b7e7c4d276d3c2d28f57333 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 23 Nov 2023 10:27:45 +0100 Subject: [PATCH 2/2] (tf2_pykdl_ros) add more convert functions --- tf2_pykdl_ros/tf2_pykdl_ros/__init__.py | 106 ++++++++++++++++++++---- 1 file changed, 92 insertions(+), 14 deletions(-) diff --git a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py b/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py index 37e290c..e34ec2f 100644 --- a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py +++ b/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)