ROS2 twist wrench (#21)

Add more testing/linting

pykdl_ros/.isort.cfg

@@ -0,0 +1,5 @@
+[settings]
+combine_as_imports = true
+line_length = 120
+profile = black
+skip_gitignore = true

pykdl_ros/package.xml

@@ -4,23 +4,24 @@
   <name>pykdl_ros</name>
   <version>0.0.0</version>
   <description>Stamped PyKDL classes</description>
-
-  <author email="[email protected]">Matthijs van der Burgh</author>
-
   <maintainer email="[email protected]">Matthijs van der Burgh</maintainer>
-
   <license>BSD</license>
 
   <url type="bugtracker">https://github.com/tue-robotics/pykdl_ros/issues</url>
   <url type="repository">https://github.com/tue-robotics/pykdl_ros</url>
 
+  <author email="[email protected]">Matthijs van der Burgh</author>
+
   <exec_depend>builtin_interfaces</exec_depend>
   <exec_depend>python_orocos_kdl_vendor</exec_depend>
   <exec_depend>std_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_mypy</test_depend>
   <test_depend>ament_pep257</test_depend>
+  <test_depend>ament_xmllint</test_depend>
+  <test_depend>python3-flake8-isort</test_depend>
   <test_depend>python3-pytest</test_depend>
 
   <export>

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
 
 
@@ -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):
@@ -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
@@ -65,6 +77,75 @@ 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 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):
+        """
+        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."""
 
@@ -100,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:
         """
@@ -124,3 +217,74 @@ 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 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
+    ):
+        """
+        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)

pykdl_ros/pyproject.toml

@@ -1,5 +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"]

pykdl_ros/setup.py

@@ -1,4 +1,5 @@
 from os import path
+
 from setuptools import find_packages, setup
 
 package_name = "pykdl_ros"

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

pykdl_ros/test/test_flake8.py

@@ -12,12 +12,12 @@
 # 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
 @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)

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"

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

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"

tf2_pykdl_ros/.isort.cfg

@@ -0,0 +1,5 @@
+[settings]
+combine_as_imports = true
+line_length = 120
+profile = black
+skip_gitignore = true