Port kdl_parser_py to ROS 2

kdl_parser_py/kdl_parser_py/__init__.py

@@ -0,0 +1,32 @@
+# Copyright 2019 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.
+from kdl_parser_py import *

kdl_parser_py/kdl_parser_py/urdf.py

@@ -1,3 +1,34 @@
+# Copyright 2019 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.
 from __future__ import print_function
 
 import urdf_parser_py.urdf as urdf
@@ -7,26 +38,26 @@
 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):
@@ -49,8 +80,7 @@ def _toKdlInertia(i):
             kdl.RotationalInertia(inertia.ixx, inertia.iyy, inertia.izz, inertia.ixy, inertia.ixz, inertia.iyz));
 
 def _toKdlJoint(jnt):
-
-    fixed = lambda j,F: kdl.Joint(j.name, kdl.Joint.None)
+    fixed = lambda j,F: kdl.Joint(j.name, kdl.Joint.JointType(8))
     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)
 
@@ -67,8 +97,6 @@ def _toKdlJoint(jnt):
     return type_map[jnt.type](jnt, _toKdlPose(jnt.origin))
 
 def _add_children_to_tree(robot_model, root, tree):
-
-
     # constructs the optional inertia
     inert = kdl.RigidBodyInertia(0)
     if root.inertial:
@@ -108,7 +136,6 @@ 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:

kdl_parser_py/package.xml

@@ -1,4 +1,4 @@
-<package>
+<package format="3">
   <name>kdl_parser_py</name>
   <version>2.3.0</version>
   <description>
@@ -19,16 +19,19 @@
   <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>
+  <depend>version_gte="1.4.0">orocos_kdl</depend>
+  <depend>urdf</depend>
+  <depend>rostest</depend>
+  <depend>urdfdom_py</depend>
+  <depend>python_orocos_kdl</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>
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_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,29 @@
-#!/usr/bin/env python
+from setuptools import setup
 
-from distutils.core import setup
-from catkin_pkg.python_setup import generate_distutils_setup
+package_name = 'kdl_parser_py'
 
-d = generate_distutils_setup(
-   packages=['kdl_parser_py'],
-   package_dir={'': ''}
+setup(
+    name=package_name,
+    version='2.3.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name, ['test/test.urdf'])
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Chris Lalancette',
+    maintainer_email='[email protected]"',
+    description='The Kinematics and Dynamics Library (KDL)'
+    'defines a tree structure to represent the kinematic and'
+    'dynamic parameters of a robot mechanism. <tt>kdl_parser_py</tt>'
+    'provides Python tools to construct a KDL tree from an XML robot representation in URDF.',
+    license='BSD',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
 )
-
-setup(**d)

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)

kdl_parser_py/test/test_kdl_parser.py

@@ -1,22 +1,39 @@
 #!/usr/bin/env python
 
+# Copyright 2021 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 sys
+import os
 
 import kdl_parser_py.urdf
 import unittest
-
-import rostest
+from ament_index_python.packages import get_package_share_directory
 
 PKG = "kdl_parser_py"
 NAME = "test_kdl_parser"
 
 class TestKdlParser(unittest.TestCase):
     def runTest(self):
-        filename = None
-        if (sys.argv > 1):
-            filename = sys.argv[1]
-        else:
-            self.fail("Expected filename!")
+        # filename = None
+        # if (sys.argv > 1):
+        #     filename = sys.argv[1]
+        # else:
+        #     self.fail("Expected filename!")
+        package_dir = get_package_share_directory(PKG)
+
+        filename  = os.path.join(package_dir, "test.urdf")
         (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
         self.assertTrue(ok)
         # KDL doesn't count fixed joints (since they aren't kinematic)
@@ -35,4 +52,4 @@ def runTest(self):
         self.assertAlmostEqual(inertia.getCOG().z(), 3.0)
 
 if __name__ == '__main__':
-    rostest.run(PKG, NAME, TestKdlParser, sys.argv)
+    unittest.main()

kdl_parser_py/test/test_pep257.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_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'