From d18d14e434c840eab2e74028a65cd06ab7a8f4f7 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 5 Sep 2023 15:35:26 +0200 Subject: [PATCH 01/26] (pykdl_ros) change repr of FrameStamped --- pykdl_ros/pykdl_ros/__init__.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pykdl_ros/pykdl_ros/__init__.py b/pykdl_ros/pykdl_ros/__init__.py index adc2d27..8c7f0ee 100644 --- a/pykdl_ros/pykdl_ros/__init__.py +++ b/pykdl_ros/pykdl_ros/__init__.py @@ -25,10 +25,10 @@ def __init__(self, frame: kdl.Frame, stamp: Time, frame_id: str): self.header = Header(frame_id=frame_id, stamp=stamp) def __repr__(self): - xyz = f"(x={self.frame.p.x()}, y={self.frame.p.y()}, z={self.frame.p.z()})" + pos = f"(x={self.frame.p.x()}, y={self.frame.p.y()}, z={self.frame.p.z()})" r, p, y = self.frame.M.GetRPY() - rpy = f"(r={r}, p={p}, y={y})" - return f"FrameStamped(pos:{xyz}, rot:{rpy} @ {self.header.frame_id})" + rot = f"(r={r}, p={p}, y={y})" + return f"FrameStamped({pos=}, {rot=} @ {self.header.frame_id})" def __eq__(self, other): if isinstance(other, FrameStamped): From f3df6480af90a6ecc71fae47e3eb1f5b9d9e9821 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 5 Sep 2023 15:35:45 +0200 Subject: [PATCH 02/26] (pykdl_ros) add TwistStamped --- pykdl_ros/pykdl_ros/__init__.py | 57 +++++++++++++++++++++++++++++++++ 1 file changed, 57 insertions(+) diff --git a/pykdl_ros/pykdl_ros/__init__.py b/pykdl_ros/pykdl_ros/__init__.py index 8c7f0ee..8420e3b 100644 --- a/pykdl_ros/pykdl_ros/__init__.py +++ b/pykdl_ros/pykdl_ros/__init__.py @@ -65,6 +65,63 @@ def from_xyz_rpy( return cls(frame, stamp, frame_id) +class TwistStamped: + """Stamped version of PyKDL.Twist.""" + + __slots__ = "twist", "header" + + def __init__(self, twist: kdl.Twist, stamp: Time, frame_id: str): + """ + Construct a TwistStamped object. + + :param twist: twist + :param stamp: TimeStamp + :param frame_id: Frame ID + """ + assert isinstance(twist, kdl.Twist) + assert isinstance(stamp, Time) + assert isinstance(frame_id, str) + self.twist = twist + self.header = Header(frame_id=frame_id, stamp=stamp) + + def __repr__(self): + vel = f"(x={self.twist.vel.x()}, y={self.twist.vel.y()}, z={self.twist.vel.z()})" + rot = f"(r={self.twist.rot.x()}, p={self.twist.rot.y()}, y={self.twist.rot.z()})" + return f"TwistStamped({vel=}, {rot=} @ {self.header.frame_id})" + + def __eq__(self, other): + if isinstance(other, TwistStamped): + return self.twist == other.twist and self.header.frame_id == other.header.frame_id + else: + return False + + def __ne__(self, other): + return not self.__eq__(other) + + def __hash__(self): + return hash((self.twist, self.header.frame_id)) + + @classmethod + def from_xyz_rpy(cls, vx: float, vy: float, vz: float, wx: float, wy: float, wz: float, stamp: Time, frame_id: str): + """ + Construct a TwistStamped from velocity and XYZ and RPY. + + :param vx: vx + :param vy: vy + :param vz: vz + :param wx: wx + :param wy: wy + :param wz: wz + :param stamp: TimeStamp + :param frame_id: Frame ID + :return: Filled object + """ + linear = kdl.Vector(vx, vy, vz) + angular = kdl.Vector(wx, wy, wz) + twist = kdl.Twist(linear, angular) + return cls(twist, stamp, frame_id) + + class VectorStamped: """Stamped version of PyKDL.Vector.""" From 4566714f3919c52ad128d42f9cb95536d1b046b9 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 5 Sep 2023 15:35:58 +0200 Subject: [PATCH 03/26] (pykdl_ros) add WrenchStamped --- pykdl_ros/pykdl_ros/__init__.py | 59 +++++++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/pykdl_ros/pykdl_ros/__init__.py b/pykdl_ros/pykdl_ros/__init__.py index 8420e3b..8169ff5 100644 --- a/pykdl_ros/pykdl_ros/__init__.py +++ b/pykdl_ros/pykdl_ros/__init__.py @@ -181,3 +181,62 @@ def from_framestamped(cls, frame: FrameStamped) -> VectorStamped: :return: Filled object """ return cls(frame.frame.p, frame.header.stamp, frame.header.frame_id) + + +class WrenchStamped: + """Stamped version of PyKDL.Wrench.""" + + __slots__ = "wrench", "header" + + def __init__(self, wrench: kdl.Wrench, stamp: Time, frame_id: str): + """ + Construct a WrenchStamped object. + + :param wrench: wrench + :param stamp: TimeStamp + :param frame_id: Frame ID + """ + assert isinstance(wrench, kdl.Wrench) + assert isinstance(stamp, Time) + assert isinstance(frame_id, str) + self.wrench = wrench + self.header = Header(frame_id=frame_id, stamp=stamp) + + def __repr__(self): + force = f"(x={self.wrench.force.x()}, y={self.wrench.force.y()}, z={self.wrench.force.z()})" + torque = f"(x={self.wrench.torque.x()}, y={self.wrench.torque.y()}, z={self.wrench.torque.z()})" + return f"WrenchStamped({force=}, {torque=} @ {self.header.frame_id})" + + def __eq__(self, other): + if isinstance(other, WrenchStamped): + return self.wrench == other.wrench and self.header.frame_id == other.header.frame_id + else: + return False + + def __ne__(self, other): + return not self.__eq__(other) + + def __hash__(self): + return hash((self.wrench, self.header.frame_id)) + + @classmethod + def from_fxfyfz_txtytz( + cls, fx: float, fy: float, fz: float, tx: float, ty: float, tz: float, stamp: Time, frame_id: str + ): + """ + Construct a WrenchStamped from force and torque in XYZ. + + :param fx: fx + :param fy: fy + :param fz: fz + :param tx: tx + :param ty: ty + :param tz: tz + :param stamp: TimeStamp + :param frame_id: Frame ID + :return: Filled object + """ + force = kdl.Vector(fx, fy, fz) + torque = kdl.Vector(tx, ty, tz) + wrench = kdl.Wrench(force, torque) + return cls(wrench, stamp, frame_id) From c850cfd75470f8eb99be1eb2573e95442c90d37f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 5 Sep 2023 16:06:21 +0200 Subject: [PATCH 04/26] (tf2_pykdl_ros) add TwistStamped --- tf2_pykdl_ros/tf2_pykdl_ros/__init__.py | 84 ++++++++++++++++++++++++- 1 file changed, 82 insertions(+), 2 deletions(-) diff --git a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py b/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py index 9cd4b09..8f5931b 100644 --- a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py +++ b/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py @@ -1,7 +1,13 @@ from typing import Union -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 @@ -162,3 +168,77 @@ def do_transform_frame(frame: FrameStamped, transform: TransformStamped) -> Tran tf2_ros.TransformRegistration().add(FrameStamped, do_transform_frame) + + +def to_msg_twist(twist: TwistStamped) -> TwistStampedMsg: + """ + Convert a FrameStamped to a geometry_msgs TwistStamped message. + + :param twist: The frame to convert. + :return: The converted Twist. + """ + msg = TwistStampedMsg() + msg.header = twist.header + vel = twist.twist.vel + msg.twist.linear.x = vel[0] + msg.twist.linear.y = vel[1] + msg.twist.linear.z = vel[2] + rot = twist.twist.rot + msg.twist.angular.x = rot[0] + msg.twist.angular.y = rot[1] + msg.twist.angular.z = rot[2] + return msg + + +tf2_ros.ConvertRegistration().add_convert((TwistStamped, TwistStampedMsg), to_msg_twist) +tf2_ros.ConvertRegistration().add_to_msg(TwistStamped, to_msg_twist) + + +def from_msg_twist(msg: TwistStampedMsg) -> TwistStamped: + """ + Convert a TwistStamped message to a stamped TwistStamped. + + :param msg: The TwistStamped message to convert. + :return: The timestamped converted PyKDL vector. + """ + if not isinstance(msg, TwistStampedMsg): + raise TypeError(f"msg should be TwistStamped, not '{type(msg)}'") + lin = msg.twist.linear + vel = kdl.Vector(lin.x, lin.y, lin.z) + ang = msg.twist.angular + rot = kdl.Vector(ang.x, ang.y, ang.z) + twist = kdl.Twist(vel, rot) + return TwistStamped(twist, msg.header.stamp, msg.header.frame_id) + + +tf2_ros.ConvertRegistration().add_convert((TwistStampedMsg, TwistStamped), from_msg_twist) +tf2_ros.ConvertRegistration().add_from_msg(TwistStamped, from_msg_twist) + + +def convert_twist(twist: TwistStamped) -> TwistStamped: + """ + Convert a stamped PyKDL Twist to a stamped PyKDL Twist. + + :param twist: The twist to convert. + :return: The timestamped converted PyKDL twist. + """ + return TwistStamped(kdl.Frame(twist.twist), twist.header.stamp, twist.header.frame_id) + + +tf2_ros.ConvertRegistration().add_convert((TwistStamped, TwistStamped), convert_twist) + + +def do_transform_twist(twist: TwistStamped, transform: TransformStamped) -> TwistStamped: + """ + Apply a transform in the form of a geometry_msgs message to a PyKDL Frame. + + :param twist: The PyKDL twist to transform. + :param transform: The transform to apply. + :return: The transformed PyKDL twist. + """ + assert transform.child_frame_id == twist.header.frame_id + res_twist = transform_to_kdl(transform) * twist.twist + return TwistStamped(res_twist, transform.header.stamp, transform.header.frame_id) + + +tf2_ros.TransformRegistration().add(TwistStamped, do_transform_twist) From a5d14ebe67805870a2acabd2049fe406d3808e67 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 5 Sep 2023 16:11:03 +0200 Subject: [PATCH 05/26] (tf2_pykdl_ros) add WrenchStamped --- tf2_pykdl_ros/tf2_pykdl_ros/__init__.py | 74 +++++++++++++++++++++++++ 1 file changed, 74 insertions(+) diff --git a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py b/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py index 8f5931b..1cc4c05 100644 --- a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py +++ b/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py @@ -242,3 +242,77 @@ def do_transform_twist(twist: TwistStamped, transform: TransformStamped) -> Twis tf2_ros.TransformRegistration().add(TwistStamped, do_transform_twist) + + +def to_msg_wrench(wrench: WrenchStamped) -> WrenchStampedMsg: + """ + Convert a FrameStamped to a geometry_msgs WrenchStamped message. + + :param wrench: The wrench to convert. + :return: The converted Wrench. + """ + msg = WrenchStampedMsg() + msg.header = wrench.header + force = wrench.wrench.force + msg.wrench.force.x = force[0] + msg.wrench.force.y = force[1] + msg.wrench.force.z = force[2] + torque = wrench.wrench.torque + msg.wrench.torque.x = torque[0] + msg.wrench.torque.y = torque[1] + msg.wrench.torque.z = torque[2] + return msg + + +tf2_ros.ConvertRegistration().add_convert((WrenchStamped, WrenchStampedMsg), to_msg_wrench) +tf2_ros.ConvertRegistration().add_to_msg(WrenchStamped, to_msg_wrench) + + +def from_msg_wrench(msg: WrenchStampedMsg) -> WrenchStamped: + """ + Convert a WrenchStamped message to a stamped WrenchStamped. + + :param msg: The WrenchStamped message to convert. + :return: The timestamped converted PyKDL wrench. + """ + if not isinstance(msg, WrenchStampedMsg): + raise TypeError(f"msg should be WrenchStamped, not '{type(msg)}'") + f = msg.wrench.force + t = msg.wrench.torque + force = kdl.Vector(f.x, f.y, f.z) + torque = kdl.Vector(t.x, t.y, t.z) + wrench = kdl.Wrench(force, torque) + return WrenchStamped(wrench, msg.header.stamp, msg.header.frame_id) + + +tf2_ros.ConvertRegistration().add_convert((WrenchStampedMsg, WrenchStamped), from_msg_wrench) +tf2_ros.ConvertRegistration().add_from_msg(WrenchStamped, from_msg_wrench) + + +def convert_wrench(wrench: WrenchStamped) -> WrenchStamped: + """ + Convert a stamped PyKDL Wrench to a stamped PyKDL Wrench. + + :param wrench: The wrench to convert. + :return: The timestamped converted PyKDL wrench. + """ + return WrenchStamped(kdl.Frame(wrench.wrench), wrench.header.stamp, wrench.header.frame_id) + + +tf2_ros.ConvertRegistration().add_convert((WrenchStamped, WrenchStamped), convert_wrench) + + +def do_transform_wrench(wrench: WrenchStamped, transform: TransformStamped) -> WrenchStamped: + """ + Apply a transform in the form of a geometry_msgs message to a PyKDL Frame. + + :param wrench: The PyKDL wrench to transform. + :param transform: The transform to apply. + :return: The transformed PyKDL wrench. + """ + assert transform.child_frame_id == wrench.header.frame_id + res_wrench = transform_to_kdl(transform) * wrench.wrench + return WrenchStamped(res_wrench, transform.header.stamp, transform.header.frame_id) + + +tf2_ros.TransformRegistration().add(WrenchStamped, do_transform_wrench) From 8b522699d20524437e8fbae0f93d14894fcef1d6 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Tue, 5 Sep 2023 16:13:13 +0200 Subject: [PATCH 06/26] [tf2_pykdl_ros] small fixes --- tf2_pykdl_ros/tf2_pykdl_ros/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py b/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py index 1cc4c05..a37eeaf 100644 --- a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py +++ b/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py @@ -125,7 +125,7 @@ def from_msg_frame(msg: PoseStamped) -> FrameStamped: Convert a PoseStamped message to a stamped FrameStamped. :param msg: The PoseStamped message to convert. - :return: The timestamped converted PyKDL vector. + :return: The timestamped converted PyKDL frame. """ if not isinstance(msg, PoseStamped): raise TypeError(f"msg should be PoseStamped, not '{type(msg)}'") @@ -154,7 +154,7 @@ def convert_frame(frame: FrameStamped) -> FrameStamped: tf2_ros.ConvertRegistration().add_convert((FrameStamped, FrameStamped), convert_frame) -def do_transform_frame(frame: FrameStamped, transform: TransformStamped) -> TransformStamped: +def do_transform_frame(frame: FrameStamped, transform: TransformStamped) -> FrameStamped: """ Apply a transform in the form of a geometry_msgs message to a PyKDL Frame. 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 07/26] (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 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 08/26] (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) From 361cfe0e58b86eb68d983e7a55224d5ffed5f3f0 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 7 Sep 2023 15:01:32 +0200 Subject: [PATCH 09/26] (pykdl_ros)(test) don't use --arg=value --- pykdl_ros/test/test_flake8.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pykdl_ros/test/test_flake8.py b/pykdl_ros/test/test_flake8.py index fc016fd..54d0279 100644 --- a/pykdl_ros/test/test_flake8.py +++ b/pykdl_ros/test/test_flake8.py @@ -19,5 +19,5 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8(): - rc, errors = main_with_errors(argv=["--linelength=120"]) + rc, errors = main_with_errors(argv=["--linelength", "120"]) assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) From d085e07e36a25617194506fe651d5d82cbee593f Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 7 Sep 2023 15:01:39 +0200 Subject: [PATCH 10/26] (tf2_pykdl_ros)(test) don't use --arg=value --- tf2_pykdl_ros/test/test_flake8.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_pykdl_ros/test/test_flake8.py b/tf2_pykdl_ros/test/test_flake8.py index fc016fd..54d0279 100644 --- a/tf2_pykdl_ros/test/test_flake8.py +++ b/tf2_pykdl_ros/test/test_flake8.py @@ -19,5 +19,5 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8(): - rc, errors = main_with_errors(argv=["--linelength=120"]) + rc, errors = main_with_errors(argv=["--linelength", "120"]) assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) From 9ec9b88a1bc4bf9330e001676bd1a35a49dfabff Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 7 Sep 2023 15:08:01 +0200 Subject: [PATCH 11/26] (pykdl_ros) also run isort --- pykdl_ros/package.xml | 1 + pykdl_ros/pykdl_ros/__init__.py | 2 +- pykdl_ros/pyproject.toml | 3 +++ pykdl_ros/setup.py | 1 + pykdl_ros/test/test_copyright.py | 2 +- pykdl_ros/test/test_flake8.py | 2 +- pykdl_ros/test/test_pep257.py | 2 +- 7 files changed, 9 insertions(+), 4 deletions(-) diff --git a/pykdl_ros/package.xml b/pykdl_ros/package.xml index e6f4c5a..8d92f4d 100644 --- a/pykdl_ros/package.xml +++ b/pykdl_ros/package.xml @@ -21,6 +21,7 @@ ament_copyright ament_flake8 ament_pep257 + python3-flake8-isort python3-pytest diff --git a/pykdl_ros/pykdl_ros/__init__.py b/pykdl_ros/pykdl_ros/__init__.py index 8169ff5..558ddff 100644 --- a/pykdl_ros/pykdl_ros/__init__.py +++ b/pykdl_ros/pykdl_ros/__init__.py @@ -1,7 +1,7 @@ from __future__ import annotations -from builtin_interfaces.msg import Time import PyKDL as kdl +from builtin_interfaces.msg import Time from std_msgs.msg import Header diff --git a/pykdl_ros/pyproject.toml b/pykdl_ros/pyproject.toml index 6d7785e..bbb24c4 100644 --- a/pykdl_ros/pyproject.toml +++ b/pykdl_ros/pyproject.toml @@ -1,5 +1,8 @@ [tool.black] line-length = 120 +[tool.isort] +line_length = 120 + [tool.pytest.ini_options] filterwarnings = ["ignore:SelectableGroups dict interface is deprecated. Use select.:DeprecationWarning"] diff --git a/pykdl_ros/setup.py b/pykdl_ros/setup.py index f955b01..9a13364 100644 --- a/pykdl_ros/setup.py +++ b/pykdl_ros/setup.py @@ -1,4 +1,5 @@ from os import path + from setuptools import find_packages, setup package_name = "pykdl_ros" diff --git a/pykdl_ros/test/test_copyright.py b/pykdl_ros/test/test_copyright.py index 95f0381..60c2d1e 100644 --- a/pykdl_ros/test/test_copyright.py +++ b/pykdl_ros/test/test_copyright.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_copyright.main import main import pytest +from ament_copyright.main import main # Remove the `skip` decorator once the source file(s) have a copyright header diff --git a/pykdl_ros/test/test_flake8.py b/pykdl_ros/test/test_flake8.py index 54d0279..a7e1231 100644 --- a/pykdl_ros/test/test_flake8.py +++ b/pykdl_ros/test/test_flake8.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_flake8.main import main_with_errors import pytest +from ament_flake8.main import main_with_errors @pytest.mark.flake8 diff --git a/pykdl_ros/test/test_pep257.py b/pykdl_ros/test/test_pep257.py index a2c3deb..4eddb46 100644 --- a/pykdl_ros/test/test_pep257.py +++ b/pykdl_ros/test/test_pep257.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_pep257.main import main import pytest +from ament_pep257.main import main @pytest.mark.linter From 9743829093af3fdf49ba96dfd7f0bbeaf82afcf5 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Thu, 7 Sep 2023 15:08:12 +0200 Subject: [PATCH 12/26] (pykdl_ros) also run isort --- tf2_pykdl_ros/package.xml | 1 + tf2_pykdl_ros/pyproject.toml | 3 +++ tf2_pykdl_ros/setup.py | 1 + tf2_pykdl_ros/test/test_copyright.py | 2 +- tf2_pykdl_ros/test/test_flake8.py | 2 +- tf2_pykdl_ros/test/test_pep257.py | 2 +- tf2_pykdl_ros/test/test_tf2_ros_integration.py | 17 +++++++---------- .../test/test_tf2_ros_transform_node.py | 11 ++++++----- tf2_pykdl_ros/tf2_pykdl_ros/__init__.py | 13 +++++-------- 9 files changed, 26 insertions(+), 26 deletions(-) diff --git a/tf2_pykdl_ros/package.xml b/tf2_pykdl_ros/package.xml index 474ac27..8a481b7 100644 --- a/tf2_pykdl_ros/package.xml +++ b/tf2_pykdl_ros/package.xml @@ -24,6 +24,7 @@ builtin_interfaces rclpy tf2_geometry_msgs + python3-flake8-isort python3-pytest diff --git a/tf2_pykdl_ros/pyproject.toml b/tf2_pykdl_ros/pyproject.toml index 6d7785e..bbb24c4 100644 --- a/tf2_pykdl_ros/pyproject.toml +++ b/tf2_pykdl_ros/pyproject.toml @@ -1,5 +1,8 @@ [tool.black] line-length = 120 +[tool.isort] +line_length = 120 + [tool.pytest.ini_options] filterwarnings = ["ignore:SelectableGroups dict interface is deprecated. Use select.:DeprecationWarning"] diff --git a/tf2_pykdl_ros/setup.py b/tf2_pykdl_ros/setup.py index ffea419..91171bd 100644 --- a/tf2_pykdl_ros/setup.py +++ b/tf2_pykdl_ros/setup.py @@ -1,4 +1,5 @@ from os import path + from setuptools import find_packages, setup package_name = "tf2_pykdl_ros" diff --git a/tf2_pykdl_ros/test/test_copyright.py b/tf2_pykdl_ros/test/test_copyright.py index 95f0381..60c2d1e 100644 --- a/tf2_pykdl_ros/test/test_copyright.py +++ b/tf2_pykdl_ros/test/test_copyright.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_copyright.main import main import pytest +from ament_copyright.main import main # Remove the `skip` decorator once the source file(s) have a copyright header diff --git a/tf2_pykdl_ros/test/test_flake8.py b/tf2_pykdl_ros/test/test_flake8.py index 54d0279..a7e1231 100644 --- a/tf2_pykdl_ros/test/test_flake8.py +++ b/tf2_pykdl_ros/test/test_flake8.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_flake8.main import main_with_errors import pytest +from ament_flake8.main import main_with_errors @pytest.mark.flake8 diff --git a/tf2_pykdl_ros/test/test_pep257.py b/tf2_pykdl_ros/test/test_pep257.py index a2c3deb..4eddb46 100644 --- a/tf2_pykdl_ros/test/test_pep257.py +++ b/tf2_pykdl_ros/test/test_pep257.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_pep257.main import main import pytest +from ament_pep257.main import main @pytest.mark.linter diff --git a/tf2_pykdl_ros/test/test_tf2_ros_integration.py b/tf2_pykdl_ros/test/test_tf2_ros_integration.py index 8490509..d5aec82 100644 --- a/tf2_pykdl_ros/test/test_tf2_ros_integration.py +++ b/tf2_pykdl_ros/test/test_tf2_ros_integration.py @@ -1,17 +1,14 @@ import unittest -from builtin_interfaces.msg import Time -from geometry_msgs.msg import ( - PointStamped, - PoseStamped, - TransformStamped, - TwistStamped as TwistStampedMsg, - WrenchStamped as WrenchStampedMsg, -) import PyKDL as kdl -from pykdl_ros import VectorStamped, FrameStamped, TwistStamped, WrenchStamped -import tf2_ros import tf2_geometry_msgs # noqa: F401 +import tf2_ros +from builtin_interfaces.msg import Time +from geometry_msgs.msg import PointStamped, PoseStamped, TransformStamped +from geometry_msgs.msg import TwistStamped as TwistStampedMsg +from geometry_msgs.msg import WrenchStamped as WrenchStampedMsg +from pykdl_ros import FrameStamped, TwistStamped, VectorStamped, WrenchStamped + import tf2_pykdl_ros # noqa: F401 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 cad394d..6b03445 100755 --- a/tf2_pykdl_ros/test/test_tf2_ros_transform_node.py +++ b/tf2_pykdl_ros/test/test_tf2_ros_transform_node.py @@ -2,14 +2,15 @@ import unittest -from builtin_interfaces.msg import Time -from geometry_msgs.msg import TransformStamped import PyKDL as kdl -from pykdl_ros import VectorStamped, FrameStamped, TwistStamped, WrenchStamped import rclpy -from rclpy.duration import Duration -import tf2_ros import tf2_geometry_msgs # noqa: F401 +import tf2_ros +from builtin_interfaces.msg import Time +from geometry_msgs.msg import TransformStamped +from pykdl_ros import FrameStamped, TwistStamped, VectorStamped, WrenchStamped +from rclpy.duration import Duration + import tf2_pykdl_ros # noqa: F401 diff --git a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py b/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py index a37eeaf..d97bcbf 100644 --- a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py +++ b/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py @@ -1,14 +1,11 @@ from typing import Union -from geometry_msgs.msg import ( - PointStamped, - PoseStamped, - TransformStamped, - TwistStamped as TwistStampedMsg, - WrenchStamped as WrenchStampedMsg, -) + import PyKDL as kdl -from pykdl_ros import VectorStamped, FrameStamped, TwistStamped, WrenchStamped import tf2_ros +from geometry_msgs.msg import PointStamped, PoseStamped, TransformStamped +from geometry_msgs.msg import TwistStamped as TwistStampedMsg +from geometry_msgs.msg import WrenchStamped as WrenchStampedMsg +from pykdl_ros import FrameStamped, TwistStamped, VectorStamped, WrenchStamped def transform_to_kdl(t: TransformStamped) -> kdl.Frame: From 01b030cf3577088fec7831ae301122ff7201463b Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 8 Sep 2023 10:01:05 +0200 Subject: [PATCH 13/26] (isort) use profile black --- pykdl_ros/pyproject.toml | 1 + tf2_pykdl_ros/pyproject.toml | 1 + 2 files changed, 2 insertions(+) diff --git a/pykdl_ros/pyproject.toml b/pykdl_ros/pyproject.toml index bbb24c4..4ae6123 100644 --- a/pykdl_ros/pyproject.toml +++ b/pykdl_ros/pyproject.toml @@ -3,6 +3,7 @@ line-length = 120 [tool.isort] line_length = 120 +profile = "black" [tool.pytest.ini_options] filterwarnings = ["ignore:SelectableGroups dict interface is deprecated. Use select.:DeprecationWarning"] diff --git a/tf2_pykdl_ros/pyproject.toml b/tf2_pykdl_ros/pyproject.toml index bbb24c4..4ae6123 100644 --- a/tf2_pykdl_ros/pyproject.toml +++ b/tf2_pykdl_ros/pyproject.toml @@ -3,6 +3,7 @@ line-length = 120 [tool.isort] line_length = 120 +profile = "black" [tool.pytest.ini_options] filterwarnings = ["ignore:SelectableGroups dict interface is deprecated. Use select.:DeprecationWarning"] From 983ac8eba2d30c2c4f002a2ae20eefe13d629cbd Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 8 Sep 2023 10:01:37 +0200 Subject: [PATCH 14/26] (pykdl_ros) add Zero/Identity classmethods --- pykdl_ros/pykdl_ros/__init__.py | 48 +++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/pykdl_ros/pykdl_ros/__init__.py b/pykdl_ros/pykdl_ros/__init__.py index 558ddff..9a1f5b6 100644 --- a/pykdl_ros/pykdl_ros/__init__.py +++ b/pykdl_ros/pykdl_ros/__init__.py @@ -42,6 +42,18 @@ def __ne__(self, other): def __hash__(self): return hash((self.frame, self.header.frame_id)) + @classmethod + def identity(cls, stamp: Time, frame_id: str) -> FrameStamped: + """ + Construct a FrameStamped object with identity frame. + + :param stamp: TimeStamp + :param frame_id: Frame ID + :return: Filled object + """ + frame = kdl.Frame.Identity() + return cls(frame, stamp, frame_id) + @classmethod def from_xyz_rpy( cls, x: float, y: float, z: float, roll: float, pitch: float, yaw: float, stamp: Time, frame_id: str @@ -101,6 +113,18 @@ def __ne__(self, other): def __hash__(self): return hash((self.twist, self.header.frame_id)) + @classmethod + def zero(cls, stamp: Time, frame_id: str) -> TwistStamped: + """ + Construct a TwistStamped object with zero velocity and angular velocity. + + :param stamp: TimeStamp + :param frame_id: Frame ID + :return: Filled object + """ + twist = kdl.Twist.Zero() + return cls(twist, stamp, frame_id) + @classmethod def from_xyz_rpy(cls, vx: float, vy: float, vz: float, wx: float, wy: float, wz: float, stamp: Time, frame_id: str): """ @@ -157,6 +181,18 @@ def __ne__(self, other): def __hash__(self): return hash((self.vector, self.header.frame_id)) + @classmethod + def zero(cls, stamp: Time, frame_id: str) -> VectorStamped: + """ + Construct a VectorStamped object with zero vector. + + :param stamp: TimeStamp + :param frame_id: Frame ID + :return: Filled object + """ + vector = kdl.Vector.Zero() + return cls(vector, stamp, frame_id) + @classmethod def from_xyz(cls, x: float, y: float, z: float, stamp: Time, frame_id: str) -> VectorStamped: """ @@ -219,6 +255,18 @@ def __ne__(self, other): def __hash__(self): return hash((self.wrench, self.header.frame_id)) + @classmethod + def zero(cls, stamp: Time, frame_id: str) -> WrenchStamped: + """ + Construct a WrenchStamped object with zero force and torque. + + :param stamp: TimeStamp + :param frame_id: Frame ID + :return: Filled object + """ + wrench = kdl.Wrench.Zero() + return cls(wrench, stamp, frame_id) + @classmethod def from_fxfyfz_txtytz( cls, fx: float, fy: float, fz: float, tx: float, ty: float, tz: float, stamp: Time, frame_id: str From 0080bdb9c82ba81b9723db999501b330e52bd132 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 10:47:44 +0200 Subject: [PATCH 15/26] (pykdl_ros) add .isort.cfg --- pykdl_ros/.isort.cfg | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 pykdl_ros/.isort.cfg diff --git a/pykdl_ros/.isort.cfg b/pykdl_ros/.isort.cfg new file mode 100644 index 0000000..524a77c --- /dev/null +++ b/pykdl_ros/.isort.cfg @@ -0,0 +1,5 @@ +[settings] +combine_as_imports = true +line_length = 120 +profile = black +skip_gitignore = true From 4df94284f6545131dae122627a7e92f381bb2669 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 10:49:20 +0200 Subject: [PATCH 16/26] (pykdl_ros) add mypy test --- pykdl_ros/package.xml | 1 + pykdl_ros/test/test_mypy.py | 31 +++++++++++++++++++++++++++++++ 2 files changed, 32 insertions(+) create mode 100644 pykdl_ros/test/test_mypy.py diff --git a/pykdl_ros/package.xml b/pykdl_ros/package.xml index 8d92f4d..2fbdeb6 100644 --- a/pykdl_ros/package.xml +++ b/pykdl_ros/package.xml @@ -20,6 +20,7 @@ ament_copyright ament_flake8 + ament_mypy ament_pep257 python3-flake8-isort python3-pytest diff --git a/pykdl_ros/test/test_mypy.py b/pykdl_ros/test/test_mypy.py new file mode 100644 index 0000000..e09937c --- /dev/null +++ b/pykdl_ros/test/test_mypy.py @@ -0,0 +1,31 @@ +# Copyright 2019 Canonical Ltd +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_mypy.main import main + + +@pytest.mark.mypy +@pytest.mark.linter +def test_mypy(): + try: + import importlib.resources as _ # noqa: F401 + except ModuleNotFoundError: + # The importlib_resources package is a backport of the importlib.resources module + # from Python 3.9. The 'policy' module of this project first tries to import from + # importlib.resources, then falls back to the backport package. + # There is a bug in mypy that manifests when this try/except import pattern is + # used: https://github.com/python/mypy/issues/1153 + pytest.skip("This platform does not support mypy checking of importlib properly") + assert main(argv=[]) == 0, "Found errors" From b3e060afa31f39c419d79dd4c6079b223897ce2e Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 10:49:48 +0200 Subject: [PATCH 17/26] (pykdl_ros) add xmllint test --- pykdl_ros/package.xml | 1 + pykdl_ros/test/test_xmllint.py | 23 +++++++++++++++++++++++ 2 files changed, 24 insertions(+) create mode 100644 pykdl_ros/test/test_xmllint.py diff --git a/pykdl_ros/package.xml b/pykdl_ros/package.xml index 2fbdeb6..b6be778 100644 --- a/pykdl_ros/package.xml +++ b/pykdl_ros/package.xml @@ -22,6 +22,7 @@ ament_flake8 ament_mypy ament_pep257 + ament_xmllint python3-flake8-isort python3-pytest diff --git a/pykdl_ros/test/test_xmllint.py b/pykdl_ros/test/test_xmllint.py new file mode 100644 index 0000000..bd9d09c --- /dev/null +++ b/pykdl_ros/test/test_xmllint.py @@ -0,0 +1,23 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_xmllint.main import main + + +@pytest.mark.linter +@pytest.mark.xmllint +def test_xmllint(): + rc = main(argv=[]) + assert rc == 0, "Found errors" From 98aada12a84c460062e260bca539ce4f8fba65f9 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 10:51:57 +0200 Subject: [PATCH 18/26] (pykdl_ros) reorder package.xml to satisfy test --- pykdl_ros/package.xml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/pykdl_ros/package.xml b/pykdl_ros/package.xml index b6be778..e861cf4 100644 --- a/pykdl_ros/package.xml +++ b/pykdl_ros/package.xml @@ -4,16 +4,14 @@ pykdl_ros 0.0.0 Stamped PyKDL classes - - Matthijs van der Burgh - Matthijs van der Burgh - BSD https://github.com/tue-robotics/pykdl_ros/issues https://github.com/tue-robotics/pykdl_ros + Matthijs van der Burgh + builtin_interfaces python_orocos_kdl_vendor std_msgs From 79d0eedafa94168ef66305e220e9e35e643a0c48 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 10:53:12 +0200 Subject: [PATCH 19/26] (pykdl_ros) update pyproject.toml --- pykdl_ros/pyproject.toml | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/pykdl_ros/pyproject.toml b/pykdl_ros/pyproject.toml index 4ae6123..38819bc 100644 --- a/pykdl_ros/pyproject.toml +++ b/pykdl_ros/pyproject.toml @@ -1,9 +1,22 @@ [tool.black] line-length = 120 +[tool.flake8] +max-line-length = 120 + [tool.isort] +combine_as_imports = true line_length = 120 profile = "black" +skip_gitignore = true + +[tool.mypy] +disallow_untyped_defs = true +no_implicit_optional = true +pretty = true +show_error_codes = true +warn_return_any = true +warn_unused_ignores = true [tool.pytest.ini_options] filterwarnings = ["ignore:SelectableGroups dict interface is deprecated. Use select.:DeprecationWarning"] From c6b4a8a502dd08bcad9b7142fea3e0ba476182c6 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 10:58:24 +0200 Subject: [PATCH 20/26] (tf2_pykdl_ros) add .isort.cfg --- tf2_pykdl_ros/.isort.cfg | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 tf2_pykdl_ros/.isort.cfg diff --git a/tf2_pykdl_ros/.isort.cfg b/tf2_pykdl_ros/.isort.cfg new file mode 100644 index 0000000..524a77c --- /dev/null +++ b/tf2_pykdl_ros/.isort.cfg @@ -0,0 +1,5 @@ +[settings] +combine_as_imports = true +line_length = 120 +profile = black +skip_gitignore = true From 59c405cfd5f7ae1f167bfb764930ec16e9967ec5 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 11:12:53 +0200 Subject: [PATCH 21/26] (tf2_pykdl_ros) add xmllint test --- tf2_pykdl_ros/package.xml | 1 + tf2_pykdl_ros/test/test_xmllint.py | 23 +++++++++++++++++++++++ 2 files changed, 24 insertions(+) create mode 100644 tf2_pykdl_ros/test/test_xmllint.py diff --git a/tf2_pykdl_ros/package.xml b/tf2_pykdl_ros/package.xml index 8a481b7..8e10898 100644 --- a/tf2_pykdl_ros/package.xml +++ b/tf2_pykdl_ros/package.xml @@ -21,6 +21,7 @@ ament_copyright ament_flake8 ament_pep257 + ament_xmllint builtin_interfaces rclpy tf2_geometry_msgs diff --git a/tf2_pykdl_ros/test/test_xmllint.py b/tf2_pykdl_ros/test/test_xmllint.py new file mode 100644 index 0000000..bd9d09c --- /dev/null +++ b/tf2_pykdl_ros/test/test_xmllint.py @@ -0,0 +1,23 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_xmllint.main import main + + +@pytest.mark.linter +@pytest.mark.xmllint +def test_xmllint(): + rc = main(argv=[]) + assert rc == 0, "Found errors" From 90b310177e4a7884451b34a64fd5d660dcf1d90c Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 11:13:22 +0200 Subject: [PATCH 22/26] (tf2_pykdl_ros) reorder package.xml to satisfy test --- tf2_pykdl_ros/package.xml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/tf2_pykdl_ros/package.xml b/tf2_pykdl_ros/package.xml index 8e10898..d847119 100644 --- a/tf2_pykdl_ros/package.xml +++ b/tf2_pykdl_ros/package.xml @@ -4,15 +4,14 @@ tf2_pykdl_ros 0.0.0 tf2 conversions for pykdl_ros - Matthijs van der Burgh - Matthijs van der Burgh - BSD https://github.com/tue-robotics/pykdl_ros/issues https://github.com/tue-robotics/pykdl_ros + Matthijs van der Burgh + geometry_msgs pykdl_ros python_orocos_kdl_vendor From fa72a1c7ee4838f6d3333195c4eb61bd77f85c62 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 11:14:20 +0200 Subject: [PATCH 23/26] (tf2_pykdl_ros) add classvar typing --- tf2_pykdl_ros/test/test_tf2_ros_integration.py | 17 ++++++++++++++--- .../test/test_tf2_ros_transform_node.py | 5 +++++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/tf2_pykdl_ros/test/test_tf2_ros_integration.py b/tf2_pykdl_ros/test/test_tf2_ros_integration.py index d5aec82..c3aacfc 100644 --- a/tf2_pykdl_ros/test/test_tf2_ros_integration.py +++ b/tf2_pykdl_ros/test/test_tf2_ros_integration.py @@ -1,18 +1,26 @@ import unittest +from typing import ClassVar import PyKDL as kdl import tf2_geometry_msgs # noqa: F401 import tf2_ros from builtin_interfaces.msg import Time -from geometry_msgs.msg import PointStamped, PoseStamped, TransformStamped -from geometry_msgs.msg import TwistStamped as TwistStampedMsg -from geometry_msgs.msg import WrenchStamped as WrenchStampedMsg +from geometry_msgs.msg import ( + PointStamped, + PoseStamped, + TransformStamped, + TwistStamped as TwistStampedMsg, + WrenchStamped as WrenchStampedMsg, +) from pykdl_ros import FrameStamped, TwistStamped, VectorStamped, WrenchStamped import tf2_pykdl_ros # noqa: F401 class TestRegistration(unittest.TestCase): + convert_reg: ClassVar[tf2_ros.ConvertRegistration] + transform_reg: ClassVar[tf2_ros.TransformRegistration] + @classmethod def setUpClass(cls) -> None: cls.convert_reg = tf2_ros.ConvertRegistration() @@ -207,6 +215,9 @@ def test_wrench_stamped_wrench_stamped_msg(self): class TestTransform(unittest.TestCase): + registration: ClassVar[tf2_ros.TransformRegistration] + t: ClassVar[TransformStamped] + @classmethod def setUpClass(cls) -> None: cls.registration = tf2_ros.TransformRegistration() 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 6b03445..387af3e 100755 --- a/tf2_pykdl_ros/test/test_tf2_ros_transform_node.py +++ b/tf2_pykdl_ros/test/test_tf2_ros_transform_node.py @@ -1,6 +1,7 @@ #! /usr/bin/env python import unittest +from typing import ClassVar import PyKDL as kdl import rclpy @@ -15,6 +16,10 @@ class TestTransform(unittest.TestCase): + buffer: ClassVar[tf2_ros.Buffer] + context: ClassVar[rclpy.context.Context] + node: ClassVar[rclpy.node.Node] + @classmethod def setUpClass(cls) -> None: cls.context = rclpy.context.Context() From 5eac0c1b591074180b4e3f2f66120ab760ec0b73 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 11:14:50 +0200 Subject: [PATCH 24/26] (tf2_pykdl_ros) reorder imports --- tf2_pykdl_ros/tf2_pykdl_ros/__init__.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py b/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py index d97bcbf..fe3a0f7 100644 --- a/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py +++ b/tf2_pykdl_ros/tf2_pykdl_ros/__init__.py @@ -2,9 +2,13 @@ import PyKDL as kdl import tf2_ros -from geometry_msgs.msg import PointStamped, PoseStamped, TransformStamped -from geometry_msgs.msg import TwistStamped as TwistStampedMsg -from geometry_msgs.msg import WrenchStamped as WrenchStampedMsg +from geometry_msgs.msg import ( + PointStamped, + PoseStamped, + TransformStamped, + TwistStamped as TwistStampedMsg, + WrenchStamped as WrenchStampedMsg, +) from pykdl_ros import FrameStamped, TwistStamped, VectorStamped, WrenchStamped From cb039d10eef9b6e4eeb51b69e2e569a5735b4461 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 11:15:08 +0200 Subject: [PATCH 25/26] (tf2_pykdl_ros) add mypy test --- tf2_pykdl_ros/package.xml | 1 + tf2_pykdl_ros/test/test_mypy.py | 31 +++++++++++++++++++++++++++++++ 2 files changed, 32 insertions(+) create mode 100644 tf2_pykdl_ros/test/test_mypy.py diff --git a/tf2_pykdl_ros/package.xml b/tf2_pykdl_ros/package.xml index d847119..dd62a16 100644 --- a/tf2_pykdl_ros/package.xml +++ b/tf2_pykdl_ros/package.xml @@ -19,6 +19,7 @@ ament_copyright ament_flake8 + ament_mypy ament_pep257 ament_xmllint builtin_interfaces diff --git a/tf2_pykdl_ros/test/test_mypy.py b/tf2_pykdl_ros/test/test_mypy.py new file mode 100644 index 0000000..e09937c --- /dev/null +++ b/tf2_pykdl_ros/test/test_mypy.py @@ -0,0 +1,31 @@ +# Copyright 2019 Canonical Ltd +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_mypy.main import main + + +@pytest.mark.mypy +@pytest.mark.linter +def test_mypy(): + try: + import importlib.resources as _ # noqa: F401 + except ModuleNotFoundError: + # The importlib_resources package is a backport of the importlib.resources module + # from Python 3.9. The 'policy' module of this project first tries to import from + # importlib.resources, then falls back to the backport package. + # There is a bug in mypy that manifests when this try/except import pattern is + # used: https://github.com/python/mypy/issues/1153 + pytest.skip("This platform does not support mypy checking of importlib properly") + assert main(argv=[]) == 0, "Found errors" From 89f77851a33e5bccc952c18e1bb265770e14c89e Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Wed, 13 Sep 2023 11:15:22 +0200 Subject: [PATCH 26/26] (tf2_pykdl_ros) update pyproject.toml --- tf2_pykdl_ros/pyproject.toml | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/tf2_pykdl_ros/pyproject.toml b/tf2_pykdl_ros/pyproject.toml index 4ae6123..38819bc 100644 --- a/tf2_pykdl_ros/pyproject.toml +++ b/tf2_pykdl_ros/pyproject.toml @@ -1,9 +1,22 @@ [tool.black] line-length = 120 +[tool.flake8] +max-line-length = 120 + [tool.isort] +combine_as_imports = true line_length = 120 profile = "black" +skip_gitignore = true + +[tool.mypy] +disallow_untyped_defs = true +no_implicit_optional = true +pretty = true +show_error_codes = true +warn_return_any = true +warn_unused_ignores = true [tool.pytest.ini_options] filterwarnings = ["ignore:SelectableGroups dict interface is deprecated. Use select.:DeprecationWarning"]