Make kdl_parser_py build successfully under ros2

.gitignore

@@ -0,0 +1 @@
+__pycache__

kdl_parser_py/kdl_parser_py/urdf.py

@@ -1,42 +1,73 @@
-from __future__ import print_function
+# Copyright (c) 2021 Open Source Robotics Foundation, Inc.
+# All rights reserved.
+#
+# Software License Agreement (BSD License 2.0)
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the copyright holder nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
 
+import PyKDL as kdl
 import urdf_parser_py.urdf as urdf
 
-import PyKDL as kdl
 
 def treeFromFile(filename):
     """
     Construct a PyKDL.Tree from an URDF file.
+
     :param filename: URDF file path
     """
-
     with open(filename) as urdf_file:
         return treeFromUrdfModel(urdf.URDF.from_xml_string(urdf_file.read()))
 
+
 def treeFromParam(param):
     """
     Construct a PyKDL.Tree from an URDF in a ROS parameter.
+
     :param param: Parameter name, ``str``
     """
-
     return treeFromUrdfModel(urdf.URDF.from_parameter_server())
 
+
 def treeFromString(xml):
     """
     Construct a PyKDL.Tree from an URDF xml string.
+
     :param xml: URDF xml string, ``str``
     """
-
     return treeFromUrdfModel(urdf.URDF.from_xml_string(xml))
 
+
 def _toKdlPose(pose):
     # URDF might have RPY OR XYZ unspecified. Both default to zeros
     rpy = pose.rpy if pose and pose.rpy and len(pose.rpy) == 3 else [0, 0, 0]
     xyz = pose.xyz if pose and pose.xyz and len(pose.xyz) == 3 else [0, 0, 0]
 
-    return kdl.Frame(
-          kdl.Rotation.RPY(*rpy),
-          kdl.Vector(*xyz))
+    return kdl.Frame(kdl.Rotation.RPY(*rpy), kdl.Vector(*xyz))
 
 
 def _toKdlInertia(i):
@@ -45,29 +76,43 @@ def _toKdlInertia(i):
     origin = _toKdlPose(i.origin)
     inertia = i.inertia
     return origin.M * kdl.RigidBodyInertia(
-            i.mass, origin.p,
-            kdl.RotationalInertia(inertia.ixx, inertia.iyy, inertia.izz, inertia.ixy, inertia.ixz, inertia.iyz));
+        i.mass,
+        origin.p,
+        kdl.RotationalInertia(
+            inertia.ixx, inertia.iyy, inertia.izz, inertia.ixy, inertia.ixz, inertia.iyz
+        ),
+    )
+
 
 def _toKdlJoint(jnt):
+    def fixed(j, F):
+        return kdl.Joint(
+            j.name,
+            getattr(kdl.Joint, "Fixed")
+            if hasattr(kdl.Joint, "Fixed")
+            else getattr(kdl.Joint, "None"),
+        )
 
-    fixed = lambda j,F: kdl.Joint(j.name, kdl.Joint.None)
-    rotational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.RotAxis)
-    translational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.TransAxis)
+    def rotational(j, F):
+        return kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.RotAxis)
+
+    def translational(j, F):
+        return kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.TransAxis)
 
     type_map = {
-            'fixed': fixed,
-            'revolute': rotational,
-            'continuous': rotational,
-            'prismatic': translational,
-            'floating': fixed,
-            'planar': fixed,
-            'unknown': fixed,
-            }
+        "fixed": fixed,
+        "revolute": rotational,
+        "continuous": rotational,
+        "prismatic": translational,
+        "floating": fixed,
+        "planar": fixed,
+        "unknown": fixed,
+    }
 
     return type_map[jnt.type](jnt, _toKdlPose(jnt.origin))
 
-def _add_children_to_tree(robot_model, root, tree):
 
+def _add_children_to_tree(robot_model, root, tree):
 
     # constructs the optional inertia
     inert = kdl.RigidBodyInertia(0)
@@ -80,10 +125,8 @@ def _add_children_to_tree(robot_model, root, tree):
 
     # construct the kdl segment
     sgm = kdl.Segment(
-        root.name,
-        _toKdlJoint(parent_joint),
-        _toKdlPose(parent_joint.origin),
-        inert)
+        root.name, _toKdlJoint(parent_joint), _toKdlPose(parent_joint.origin), inert
+    )
 
     # add segment to tree
     if not tree.addSegment(sgm, parent_link_name):
@@ -92,14 +135,15 @@ def _add_children_to_tree(robot_model, root, tree):
     if root.name not in robot_model.child_map:
         return True
 
-    children = [robot_model.link_map[l] for (j,l) in robot_model.child_map[root.name]]
+    children = [robot_model.link_map[l] for (j, l) in robot_model.child_map[root.name]]
 
     # recurslively add all children
     for child in children:
         if not _add_children_to_tree(robot_model, child, tree):
             return False
 
-    return True;
+    return True
+
 
 def treeFromUrdfModel(robot_model, quiet=False):
     """
@@ -108,19 +152,23 @@ def treeFromUrdfModel(robot_model, quiet=False):
     :param robot_model: URDF xml string, ``str``
     :param quiet: If true suppress messages to stdout, ``bool``
     """
-
     root = robot_model.link_map[robot_model.get_root()]
 
     if root.inertial and not quiet:
-        print("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF." % root.name);
+        print(
+            f"The root link {root.name} has an inertia specified in "
+            f"the URDF, but KDL does not support a root link with an "
+            f"inertia.  As a workaround, you can add an extra dummy "
+            f"link to your URDF."
+        )
 
     ok = True
     tree = kdl.Tree(root.name)
 
     #  add all children
-    for (joint,child) in robot_model.child_map[root.name]:
+    for (joint, child) in robot_model.child_map[root.name]:
         if not _add_children_to_tree(robot_model, robot_model.link_map[child], tree):
             ok = False
             break
-  
+
     return (ok, tree)

kdl_parser_py/package.xml

@@ -1,4 +1,5 @@
-<package>
+<?xml version="1.0"?>
+<package format="2">
   <name>kdl_parser_py</name>
   <version>2.3.0</version>
   <description>
@@ -19,16 +20,16 @@
   <url type="repository">https://github.com/ros2/kdl_parser</url>
   <url type="bugtracker">https://github.com/ros2/kdl_parser/issues</url>
 
-  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
-  <buildtool_depend>python-catkin-pkg</buildtool_depend>
-
   <build_depend version_gte="1.3.0">orocos_kdl</build_depend>
   <build_depend>urdf</build_depend>
-  <build_depend>rostest</build_depend>
 
-  <run_depend version_gte="1.3.0">orocos_kdl</run_depend>
-  <run_depend>urdf</run_depend>
-  <run_depend>urdfdom_py</run_depend>
-  <run_depend>python_orocos_kdl</run_depend>
+  <exec_depend version_gte="1.3.0">orocos_kdl</exec_depend>
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>urdfdom_py</exec_depend>
+  <exec_depend>python_orocos_kdl</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
 
 </package>

kdl_parser_py/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/kdl_parser_py
+[install]
+install-scripts=$base/lib/kdl_parser_py

kdl_parser_py/setup.py

@@ -1,11 +1,27 @@
-#!/usr/bin/env python
+import os
 
-from distutils.core import setup
-from catkin_pkg.python_setup import generate_distutils_setup
+from setuptools import setup
 
-d = generate_distutils_setup(
-   packages=['kdl_parser_py'],
-   package_dir={'': ''}
-)
 
-setup(**d)
+package_name = "kdl_parser_py"
+share_path = os.path.join("share", package_name)
+
+setup(
+    name=package_name,
+    version="2.3.0",
+    author="Jonathan Bohren, Jackie Kay",
+    packages=[package_name],
+    data_files=[
+        # Include package file
+        (share_path, ["package.xml"]),
+        # Install marker file in package index
+        (
+            os.path.join("share", "ament_index", "resource_index", "packages"),
+            [os.path.join("resource", package_name)],
+        ),
+        # Install test resources
+        (os.path.join(share_path, "assets"), [os.path.join("test", "test.urdf")]),
+    ],
+    tests_require=["pytest"],
+    test_suite="test",
+)

kdl_parser_py/test/test_copyright.py

@@ -0,0 +1,23 @@
+# Copyright 2015 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.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=[".", "test"])
+    assert rc == 0, "Found errors"

kdl_parser_py/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)