Drop PyKDL dependency in tf2_geometry_msgs (ros#509)

* Drop PyKDL dependency

Signed-off-by: Florian Vahl <[email protected]>
Co-authored-by: Chris Lalancette <[email protected]>

tf2_geometry_msgs/CMakeLists.txt

@@ -14,14 +14,8 @@ find_package(orocos_kdl REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 
-# TODO(ros2/geometry2#110) Port python once PyKDL becomes usable in ROS 2
-# ament_python_install_package(${PROJECT_NAME}
-#      PACKAGE_DIR src/${PROJECT_NAME})
-
-# TODO(ros2/geometry2#110) Port python once PyKDL becomes usable in ROS 2
-# install(PROGRAMS scripts/test.py
-#    DESTINATION lib/${PROJECT_NAME}
-# )
+ament_python_install_package(${PROJECT_NAME}
+  PACKAGE_DIR src/${PROJECT_NAME})
 
 add_library(${PROJECT_NAME} INTERFACE)
 target_include_directories(${PROJECT_NAME} INTERFACE
@@ -46,6 +40,9 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(rclcpp REQUIRED)
 
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_pytest_test(test_tf2_geometry_msgs_py test/test_tf2_geometry_msgs.py)
+
   ament_add_gtest(test_tf2_geometry_msgs test/test_tf2_geometry_msgs.cpp)
   if(TARGET test_tf2_geometry_msgs)
     target_link_libraries(test_tf2_geometry_msgs

tf2_geometry_msgs/package.xml

@@ -18,15 +18,11 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
 
-  <!-- python support not yet ported
-  <build_depend>python_orocos_kdl</build_depend>
-
-  <exec_depend>python_orocos_kdl</exec_depend>
-  -->
-
+  <exec_depend>python3-numpy</exec_depend>
   <exec_depend>tf2_ros_py</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>rclcpp</test_depend>

tf2_geometry_msgs/pytest.ini

@@ -0,0 +1,2 @@
+[pytest]
+junit_family=xunit2

tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py

@@ -29,10 +29,12 @@
 
 # author: Wim Meeussen
 
-from geometry_msgs.msg import (PointStamped, PoseStamped,
-                               PoseWithCovarianceStamped, Vector3Stamped)
+from typing import Iterable, Optional, Tuple
+
+from geometry_msgs.msg import (PointStamped, Pose, PoseStamped,
+                               PoseWithCovarianceStamped, TransformStamped,
+                               Vector3Stamped)
 import numpy as np
-import PyKDL
 import tf2_ros
 
 
@@ -54,17 +56,14 @@ def from_msg_msg(msg):
 tf2_ros.ConvertRegistration().add_from_msg(PointStamped, from_msg_msg)
 
 
-def transform_to_kdl(t):
-    return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x,
-                                                 t.transform.rotation.y,
-                                                 t.transform.rotation.z,
-                                                 t.transform.rotation.w),
-                       PyKDL.Vector(t.transform.translation.x,
-                                    t.transform.translation.y,
-                                    t.transform.translation.z))
-
-
 def transform_covariance(cov_in, transform):
+    """
+    Apply a given transform to a covariance matrix.
+
+    :param cov_in: Covariance matrix
+    :param transform: The transform that will be applies
+    :returns: The transformed covariance matrix
+    """
     # Converting the Quaternion to a Rotation Matrix first
     # Taken from: https://automaticaddison.com/how-to-convert-a-quaternion-to-a-rotation-matrix/
     q0 = transform.transform.rotation.w
@@ -155,15 +154,142 @@ def transform_covariance(cov_in, transform):
     return cov_out.pose.covariance
 
 
+def _build_affine(
+        rotation: Optional[Iterable] = None,
+        translation: Optional[Iterable] = None) -> np.ndarray:
+    """
+    Build an affine matrix from a quaternion and a translation.
+
+    :param rotation: The quaternion as [w, x, y, z]
+    :param translation: The translation as [x, y, z]
+    :returns: The quaternion and the translation array
+    """
+    affine = np.eye(4)
+    if rotation is not None:
+        affine[:3, :3] = _get_mat_from_quat(np.asarray(rotation))
+    if translation is not None:
+        affine[:3, 3] = np.asarray(translation)
+    return affine
+
+
+def _transform_to_affine(transform: TransformStamped) -> np.ndarray:
+    """
+    Convert a `TransformStamped` to a affine matrix.
+
+    :param transform: The transform that should be converted
+    :returns: The affine transform
+    """
+    transform = transform.transform
+    transform_rotation_matrix = [
+        transform.rotation.w,
+        transform.rotation.x,
+        transform.rotation.y,
+        transform.rotation.z
+    ]
+    transform_translation = [
+        transform.translation.x,
+        transform.translation.y,
+        transform.translation.z
+    ]
+    return _build_affine(transform_rotation_matrix, transform_translation)
+
+
+def _get_mat_from_quat(quaternion: np.ndarray) -> np.ndarray:
+    """
+    Convert a quaternion to a rotation matrix.
+
+    This method is based on quat2mat from https://github.com
+    f185e866ecccb66c545559bc9f2e19cb5025e0ab/transforms3d/quaternions.py#L101 ,
+    since that library is not available via rosdep.
+
+    :param quaternion: A numpy array containing the w, x, y, and z components of the quaternion
+    :returns: The rotation matrix
+    """
+    Nq = np.sum(np.square(quaternion))
+    if Nq < np.finfo(np.float64).eps:
+        return np.eye(3)
+
+    XYZ = quaternion[1:] * 2.0 / Nq
+    wXYZ = XYZ * quaternion[0]
+    xXYZ = XYZ * quaternion[1]
+    yYZ = XYZ[1:] * quaternion[2]
+    zZ = XYZ[2] * quaternion[3]
+
+    return np.array(
+        [[1.0-(yYZ[0]+zZ), xXYZ[1]-wXYZ[2], xXYZ[2]+wXYZ[1]],
+         [xXYZ[1]+wXYZ[2], 1.0-(xXYZ[0]+zZ), yYZ[1]-wXYZ[0]],
+         [xXYZ[2]-wXYZ[1], yYZ[1]+wXYZ[0], 1.0-(xXYZ[0]+yYZ[0])]])
+
+
+def _get_quat_from_mat(rot_mat: np.ndarray) -> np.ndarray:
+    """
+    Convert a rotation matrix to a quaternion.
+
+    This method is a copy of mat2quat from https://github.com
+    f185e866ecccb66c545559bc9f2e19cb5025e0ab/transforms3d/quaternions.py#L150 ,
+    since that library is not available via rosdep.
+
+    Method from
+    Bar-Itzhack, Itzhack Y. (2000), "New method for extracting the
+    quaternion from a rotation matrix", AIAA Journal of Guidance,
+    Control and Dynamics 23(6):1085-1087 (Engineering Note), ISSN
+    0731-5090
+
+    :param rot_mat: A roatation matrix
+    :returns: An quaternion
+    """
+    # Decompose rotation matrix
+    Qxx, Qyx, Qzx, Qxy, Qyy, Qzy, Qxz, Qyz, Qzz = rot_mat.flat
+    # Create matrix
+    K = np.array([
+        [Qxx - Qyy - Qzz, 0,               0,               0],
+        [Qyx + Qxy,       Qyy - Qxx - Qzz, 0,               0],
+        [Qzx + Qxz,       Qzy + Qyz,       Qzz - Qxx - Qyy, 0],
+        [Qyz - Qzy,       Qzx - Qxz,       Qxy - Qyx,       Qxx + Qyy + Qzz]]
+    ) / 3.0
+    vals, vecs = np.linalg.eigh(K)
+    # Select largest eigenvector and reorder to w,x,y,z
+    q = vecs[[3, 0, 1, 2], np.argmax(vals)]
+    # Invert quaternion if w is negative (results in positive w)
+    if q[0] < 0:
+        q *= -1
+    return q
+
+
+def _decompose_affine(affine: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
+    """
+    Decompose an affine transformation into a quaternion and the translation.
+
+    :param affine: The affine transformation matrix
+    :returns: The quaternion and the translation array
+    """
+    return _get_quat_from_mat(affine[:3, :3]), affine[:3, 3]
+
+
 # PointStamped
-def do_transform_point(point, transform):
-    p = transform_to_kdl(transform) * PyKDL.Vector(point.point.x,
-                                                   point.point.y,
-                                                   point.point.z)
+def do_transform_point(
+        point: PointStamped,
+        transform: TransformStamped) -> PointStamped:
+    """
+    Transform a `PointStamped` using a given `TransformStamped`.
+
+    :param point: The point
+    :param transform: The transform
+    :returns: The transformed point
+    """
+    _, point = _decompose_affine(
+        np.matmul(
+            _transform_to_affine(transform),
+            _build_affine(translation=[
+                point.point.x,
+                point.point.y,
+                point.point.z
+            ])))
+
     res = PointStamped()
-    res.point.x = p[0]
-    res.point.y = p[1]
-    res.point.z = p[2]
+    res.point.x = point[0]
+    res.point.y = point[1]
+    res.point.z = point[2]
     res.header = transform.header
     return res
 
@@ -172,59 +298,110 @@ def do_transform_point(point, transform):
 
 
 # Vector3Stamped
-def do_transform_vector3(vector3, transform):
+def do_transform_vector3(
+        vector3: Vector3Stamped,
+        transform: TransformStamped) -> Vector3Stamped:
+    """
+    Transform a `Vector3Stamped` using a given `TransformStamped`.
+
+    :param vector3: The vector3
+    :param transform: The transform
+    :returns: The transformed vector3
+    """
     transform.transform.translation.x = 0.0
     transform.transform.translation.y = 0.0
     transform.transform.translation.z = 0.0
-    p = transform_to_kdl(transform) * PyKDL.Vector(vector3.vector.x,
-                                                   vector3.vector.y,
-                                                   vector3.vector.z)
+    _, point = _decompose_affine(
+        np.matmul(
+            _transform_to_affine(transform),
+            _build_affine(translation=[
+                vector3.vector.x,
+                vector3.vector.y,
+                vector3.vector.z
+            ])))
     res = Vector3Stamped()
-    res.vector.x = p[0]
-    res.vector.y = p[1]
-    res.vector.z = p[2]
+    res.vector.x = point[0]
+    res.vector.y = point[1]
+    res.vector.z = point[2]
     res.header = transform.header
     return res
 
 
 tf2_ros.TransformRegistration().add(Vector3Stamped, do_transform_vector3)
 
 
+# Pose
+def do_transform_pose(
+        pose: Pose,
+        transform: TransformStamped) -> Pose:
+    """
+    Transform a `Pose` using a given `TransformStamped`.
+
+    This method is used to share the tranformation done in
+    `do_transform_pose_stamped()` and `do_transform_pose_with_covariance_stamped()`
+
+    :param pose: The pose
+    :param transform: The transform
+    :returns: The transformed pose
+    """
+    quaternion, point = _decompose_affine(
+        np.matmul(
+            _transform_to_affine(transform),
+            _build_affine(
+                translation=[
+                    pose.position.x,
+                    pose.position.y,
+                    pose.position.z
+                ],
+                rotation=[
+                    pose.orientation.w,
+                    pose.orientation.x,
+                    pose.orientation.y,
+                    pose.orientation.z])))
+    res = Pose()
+    res.position.x = point[0]
+    res.position.y = point[1]
+    res.position.z = point[2]
+    res.orientation.w = quaternion[0]
+    res.orientation.x = quaternion[1]
+    res.orientation.y = quaternion[2]
+    res.orientation.z = quaternion[3]
+    return res
+
+
 # PoseStamped
-def do_transform_pose(pose, transform):
-    q = PyKDL.Rotation.Quaternion(pose.pose.orientation.x, pose.pose.orientation.y,
-                                  pose.pose.orientation.z, pose.pose.orientation.w)
-    vector = PyKDL.Vector(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)
-    f = transform_to_kdl(transform) * PyKDL.Frame(q, vector)
+def do_transform_pose_stamped(
+        pose: PoseStamped,
+        transform: TransformStamped) -> PoseStamped:
+    """
+    Transform a `PoseStamped` using a given `TransformStamped`.
+
+    :param pose: The stamped pose
+    :param transform: The transform
+    :returns: The transformed pose stamped
+    """
     res = PoseStamped()
-    res.pose.position.x = f.p[0]
-    res.pose.position.y = f.p[1]
-    res.pose.position.z = f.p[2]
-    (res.pose.orientation.x, res.pose.orientation.y,
-     res.pose.orientation.z, res.pose.orientation.w) = f.M.GetQuaternion()
+    res.pose = do_transform_pose(pose.pose, transform)
     res.header = transform.header
     return res
 
 
-tf2_ros.TransformRegistration().add(PoseStamped, do_transform_pose)
+tf2_ros.TransformRegistration().add(PoseStamped, do_transform_pose_stamped)
 
 
 # PoseWithCovarianceStamped
-def do_transform_pose_with_covariance_stamped(pose, transform):
-    q = PyKDL.Rotation.Quaternion(pose.pose.pose.orientation.x, pose.pose.pose.orientation.y,
-                                  pose.pose.pose.orientation.z, pose.pose.pose.orientation.w)
-    vector = PyKDL.Vector(pose.pose.pose.position.x,
-                          pose.pose.pose.position.y,
-                          pose.pose.pose.position.z)
-    f = transform_to_kdl(transform) * PyKDL.Frame(q, vector)
+def do_transform_pose_with_covariance_stamped(
+        pose: PoseWithCovarianceStamped,
+        transform: TransformStamped) -> PoseWithCovarianceStamped:
+    """
+    Transform a `PoseWithCovarianceStamped` using a given `TransformStamped`.
+
+    :param pose: The pose with covariance stamped
+    :param transform: The transform
+    :returns: The transformed pose with covariance stamped
+    """
     res = PoseWithCovarianceStamped()
-    res.pose.pose.position.x = f.p[0]
-    res.pose.pose.position.y = f.p[1]
-    res.pose.pose.position.z = f.p[2]
-    (res.pose.pose.orientation.x,
-     res.pose.pose.orientation.y,
-     res.pose.pose.orientation.z,
-     res.pose.pose.orientation.w) = f.M.GetQuaternion()
+    res.pose.pose = do_transform_pose(pose.pose.pose, transform)
     res.pose.covariance = transform_covariance(pose.pose.covariance, transform)
     res.header = transform.header
     return res