From dbb21bcd3b57a8a501107a9414c365433b34c158 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Julian=20F=C3=B6rster?= <julian.foerster@mavt.ethz.ch>
Date: Fri, 8 Jan 2021 13:42:29 +0100
Subject: [PATCH 1/9] Made compatible with ros2 way of building python packages

---
 kdl_parser_py/CMakeLists.txt                  | 22 -------------------
 kdl_parser_py/kdl_parser_py/urdf.py           |  2 +-
 kdl_parser_py/package.xml                     | 11 +++++-----
 .../{AMENT_IGNORE => resource/kdl_parser_py}  |  0
 kdl_parser_py/setup.cfg                       |  4 ++++
 kdl_parser_py/setup.py                        | 22 ++++++++++++-------
 6 files changed, 25 insertions(+), 36 deletions(-)
 delete mode 100644 kdl_parser_py/CMakeLists.txt
 rename kdl_parser_py/{AMENT_IGNORE => resource/kdl_parser_py} (100%)
 create mode 100644 kdl_parser_py/setup.cfg

diff --git a/kdl_parser_py/CMakeLists.txt b/kdl_parser_py/CMakeLists.txt
deleted file mode 100644
index 6bbffafb..00000000
--- a/kdl_parser_py/CMakeLists.txt
+++ /dev/null
@@ -1,22 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-
-project(kdl_parser_py)
-
-find_package(catkin REQUIRED
-  COMPONENTS urdf
-)
-
-catkin_package(
-  LIBRARIES ${PROJECT_NAME}
-  CATKIN_DEPENDS urdf
-)
-
-catkin_python_setup()
-
-catkin_install_python(PROGRAMS ${PROJECT_NAME}/urdf.py
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-
-if(CATKIN_ENABLE_TESTING)
-  find_package(catkin REQUIRED COMPONENTS rostest)
-  add_rostest(test/test_kdl_parser.launch)
-endif()
diff --git a/kdl_parser_py/kdl_parser_py/urdf.py b/kdl_parser_py/kdl_parser_py/urdf.py
index 0198856d..d7050860 100644
--- a/kdl_parser_py/kdl_parser_py/urdf.py
+++ b/kdl_parser_py/kdl_parser_py/urdf.py
@@ -50,7 +50,7 @@ def _toKdlInertia(i):
 
 def _toKdlJoint(jnt):
 
-    fixed = lambda j,F: kdl.Joint(j.name, kdl.Joint.None)
+    fixed = lambda j,F: kdl.Joint(j.name, getattr(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)
 
diff --git a/kdl_parser_py/package.xml b/kdl_parser_py/package.xml
index 5b60c778..a9a17f30 100644
--- a/kdl_parser_py/package.xml
+++ b/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>
 
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+
 </package>
diff --git a/kdl_parser_py/AMENT_IGNORE b/kdl_parser_py/resource/kdl_parser_py
similarity index 100%
rename from kdl_parser_py/AMENT_IGNORE
rename to kdl_parser_py/resource/kdl_parser_py
diff --git a/kdl_parser_py/setup.cfg b/kdl_parser_py/setup.cfg
new file mode 100644
index 00000000..2619e99c
--- /dev/null
+++ b/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
diff --git a/kdl_parser_py/setup.py b/kdl_parser_py/setup.py
index 0ff6afe5..46488a4e 100644
--- a/kdl_parser_py/setup.py
+++ b/kdl_parser_py/setup.py
@@ -1,11 +1,17 @@
-#!/usr/bin/env python
+from setuptools import setup
+from glob import glob
 
-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",
+    author="Jonathan Bohren, Jackie Kay",
+    packages=[package_name],
+    data_files=[
+        # Include package file
+        ("share/" + package_name, ["package.xml"]),
+        # Install marker file in package index
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+    ],
 )
-
-setup(**d)

From 94006ceba8cc335025b960ae8deb59ed1a231c59 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Julian=20F=C3=B6rster?= <julian.foerster@mavt.ethz.ch>
Date: Fri, 8 Jan 2021 13:47:52 +0100
Subject: [PATCH 2/9] Compatibility with Orocos KDL after Joint.None was
 removed

See https://github.com/ros/kdl_parser/pull/45
---
 kdl_parser_py/kdl_parser_py/urdf.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/kdl_parser_py/kdl_parser_py/urdf.py b/kdl_parser_py/kdl_parser_py/urdf.py
index d7050860..1b95a86a 100644
--- a/kdl_parser_py/kdl_parser_py/urdf.py
+++ b/kdl_parser_py/kdl_parser_py/urdf.py
@@ -50,7 +50,10 @@ def _toKdlInertia(i):
 
 def _toKdlJoint(jnt):
 
-    fixed = lambda j,F: kdl.Joint(j.name, getattr(kdl.Joint, "None"))
+    fixed = lambda j,F: kdl.Joint(
+        j.name,
+        getattr(kdl.Joint, "Fixed") if hasattr(kdl.Joint, 'Fixed') else getattr(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)
 

From 26e5e55c5b6ddb38516f047a2004c0a46a3f2a2c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Julian=20F=C3=B6rster?= <julian.foerster@mavt.ethz.ch>
Date: Fri, 8 Jan 2021 14:17:36 +0100
Subject: [PATCH 3/9] Correct dependency tags for format 2 of package.xml

---
 kdl_parser_py/package.xml | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/kdl_parser_py/package.xml b/kdl_parser_py/package.xml
index a9a17f30..80fe92dc 100644
--- a/kdl_parser_py/package.xml
+++ b/kdl_parser_py/package.xml
@@ -23,10 +23,10 @@
   <build_depend version_gte="1.3.0">orocos_kdl</build_depend>
   <build_depend>urdf</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</run_depend>
+  <exec_depend>urdf</run_depend>
+  <exec_depend>urdfdom_py</run_depend>
+  <exec_depend>python_orocos_kdl</run_depend>
 
   <export>
     <build_type>ament_python</build_type>

From 94443a8881e554a98321f5bee37b2bbd5e755af0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Julian=20F=C3=B6rster?= <julian.foerster@mavt.ethz.ch>
Date: Fri, 8 Jan 2021 14:19:33 +0100
Subject: [PATCH 4/9] Match tags

---
 kdl_parser_py/package.xml | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/kdl_parser_py/package.xml b/kdl_parser_py/package.xml
index 80fe92dc..45eac10d 100644
--- a/kdl_parser_py/package.xml
+++ b/kdl_parser_py/package.xml
@@ -23,10 +23,10 @@
   <build_depend version_gte="1.3.0">orocos_kdl</build_depend>
   <build_depend>urdf</build_depend>
 
-  <exec_depend version_gte="1.3.0">orocos_kdl</run_depend>
-  <exec_depend>urdf</run_depend>
-  <exec_depend>urdfdom_py</run_depend>
-  <exec_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>

From f5ac5b574bb05248623b857464db0aaa3ba5316d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Julian=20F=C3=B6rster?= <julian.foerster@mavt.ethz.ch>
Date: Fri, 8 Jan 2021 18:53:32 +0100
Subject: [PATCH 5/9] Adapted test file to ros 2

Test passes
---
 kdl_parser_py/setup.py                | 13 ++++++++++---
 kdl_parser_py/test/test_kdl_parser.py | 24 ++++++++++++++++--------
 2 files changed, 26 insertions(+), 11 deletions(-)

diff --git a/kdl_parser_py/setup.py b/kdl_parser_py/setup.py
index 46488a4e..fec92b24 100644
--- a/kdl_parser_py/setup.py
+++ b/kdl_parser_py/setup.py
@@ -1,7 +1,8 @@
 from setuptools import setup
-from glob import glob
+import os
 
 package_name = "kdl_parser_py"
+share_path = os.path.join("share", package_name)
 
 setup(
     name=package_name,
@@ -10,8 +11,14 @@
     packages=[package_name],
     data_files=[
         # Include package file
-        ("share/" + package_name, ["package.xml"]),
+        (share_path, ["package.xml"]),
         # Install marker file in package index
-        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        (
+            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"],
 )
diff --git a/kdl_parser_py/test/test_kdl_parser.py b/kdl_parser_py/test/test_kdl_parser.py
index f3e3066a..e1d1bfc9 100755
--- a/kdl_parser_py/test/test_kdl_parser.py
+++ b/kdl_parser_py/test/test_kdl_parser.py
@@ -1,22 +1,27 @@
 #!/usr/bin/env python
 
 import sys
+import os
+import ament_index_python as ament_index
 
 import kdl_parser_py.urdf
 import unittest
 
-import rostest
+PACKAGE_NAME = "kdl_parser_py"
 
-PKG = "kdl_parser_py"
-NAME = "test_kdl_parser"
 
 class TestKdlParser(unittest.TestCase):
     def runTest(self):
         filename = None
-        if (sys.argv > 1):
+        if len(sys.argv) > 1:
             filename = sys.argv[1]
         else:
-            self.fail("Expected filename!")
+            urdf_dir = ament_index.get_package_prefix(PACKAGE_NAME)
+            filename = os.path.join(
+                urdf_dir, "share", PACKAGE_NAME, "assets", "test.urdf"
+            )
+            if not os.path.isfile(filename):
+                self.fail("Expected filename!")
         (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
         self.assertTrue(ok)
         # KDL doesn't count fixed joints (since they aren't kinematic)
@@ -29,10 +34,13 @@ def runTest(self):
         self.assertEqual(chain.getSegment(0).getName(), "gripper_pole")
         self.assertEqual(chain.getSegment(0).getJoint().getName(), "gripper_extension")
         self.assertEqual(chain.getSegment(1).getName(), "right_gripper")
-        self.assertEqual(chain.getSegment(1).getJoint().getName(), "right_gripper_joint")
+        self.assertEqual(
+            chain.getSegment(1).getJoint().getName(), "right_gripper_joint"
+        )
 
         inertia = chain.getSegment(1).getInertia()
         self.assertAlmostEqual(inertia.getCOG().z(), 3.0)
 
-if __name__ == '__main__':
-    rostest.run(PKG, NAME, TestKdlParser, sys.argv)
+
+if __name__ == "__main__":
+    unittest.main()

From 3488b42a8709bdb08f5a26ec5b3e483096ad3ecd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Julian=20F=C3=B6rster?= <julian.foerster@mavt.ethz.ch>
Date: Fri, 8 Jan 2021 21:03:09 +0100
Subject: [PATCH 6/9] Tests for pep257, flake8, copyright and xmllint

---
 .gitignore                                | 1 +
 kdl_parser_py/setup.py                    | 1 +
 kdl_parser_py/test/test_copyright.py      | 9 +++++++++
 kdl_parser_py/test/test_flake8.py         | 9 +++++++++
 kdl_parser_py/test/test_kdl_parser.launch | 3 ---
 kdl_parser_py/test/test_pep257.py         | 9 +++++++++
 kdl_parser_py/test/test_xmllint.py        | 9 +++++++++
 7 files changed, 38 insertions(+), 3 deletions(-)
 create mode 100644 .gitignore
 create mode 100644 kdl_parser_py/test/test_copyright.py
 create mode 100644 kdl_parser_py/test/test_flake8.py
 delete mode 100644 kdl_parser_py/test/test_kdl_parser.launch
 create mode 100644 kdl_parser_py/test/test_pep257.py
 create mode 100644 kdl_parser_py/test/test_xmllint.py

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 00000000..bee8a64b
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+__pycache__
diff --git a/kdl_parser_py/setup.py b/kdl_parser_py/setup.py
index fec92b24..cb297dcb 100644
--- a/kdl_parser_py/setup.py
+++ b/kdl_parser_py/setup.py
@@ -21,4 +21,5 @@
         (os.path.join(share_path, "assets"), [os.path.join("test", "test.urdf")]),
     ],
     tests_require=["pytest"],
+    test_suite="test",
 )
diff --git a/kdl_parser_py/test/test_copyright.py b/kdl_parser_py/test/test_copyright.py
new file mode 100644
index 00000000..b821e73f
--- /dev/null
+++ b/kdl_parser_py/test/test_copyright.py
@@ -0,0 +1,9 @@
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=["."])
+    assert rc == 0, "Found errors"
diff --git a/kdl_parser_py/test/test_flake8.py b/kdl_parser_py/test/test_flake8.py
new file mode 100644
index 00000000..53cccd48
--- /dev/null
+++ b/kdl_parser_py/test/test_flake8.py
@@ -0,0 +1,9 @@
+from ament_flake8.main import main
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc = main(argv=[])
+    assert rc == 0, "Found errors"
diff --git a/kdl_parser_py/test/test_kdl_parser.launch b/kdl_parser_py/test/test_kdl_parser.launch
deleted file mode 100644
index f6e41a09..00000000
--- a/kdl_parser_py/test/test_kdl_parser.launch
+++ /dev/null
@@ -1,3 +0,0 @@
-<launch>
-  <test test-name="test_kdl_parser" pkg="kdl_parser_py" type="test_kdl_parser.py" args="$(find kdl_parser_py)/test/test.urdf"/>
-</launch>
diff --git a/kdl_parser_py/test/test_pep257.py b/kdl_parser_py/test/test_pep257.py
new file mode 100644
index 00000000..2b67ce0a
--- /dev/null
+++ b/kdl_parser_py/test/test_pep257.py
@@ -0,0 +1,9 @@
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=["."])
+    assert rc == 0, "Found code style errors / warnings"
diff --git a/kdl_parser_py/test/test_xmllint.py b/kdl_parser_py/test/test_xmllint.py
new file mode 100644
index 00000000..4e3bc690
--- /dev/null
+++ b/kdl_parser_py/test/test_xmllint.py
@@ -0,0 +1,9 @@
+from ament_xmllint.main import main
+import pytest
+
+
+@pytest.mark.xmllint
+@pytest.mark.linter
+def test_xmllint():
+    rc = main(argv=[])
+    assert rc == 0, "Found errors"

From 1424d7af3ce61f9cc222f41a2e4697bd51825438 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Julian=20F=C3=B6rster?= <julian.foerster@mavt.ethz.ch>
Date: Mon, 11 Jan 2021 19:15:07 +0100
Subject: [PATCH 7/9] Make formatting more compatible to flake8

Reformat
---
 kdl_parser_py/kdl_parser_py/urdf.py | 84 +++++++++++++++++------------
 1 file changed, 50 insertions(+), 34 deletions(-)

diff --git a/kdl_parser_py/kdl_parser_py/urdf.py b/kdl_parser_py/kdl_parser_py/urdf.py
index 1b95a86a..028e3843 100644
--- a/kdl_parser_py/kdl_parser_py/urdf.py
+++ b/kdl_parser_py/kdl_parser_py/urdf.py
@@ -1,42 +1,44 @@
 from __future__ import print_function
 
+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,32 +47,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,
-        getattr(kdl.Joint, "Fixed") if hasattr(kdl.Joint, 'Fixed') else getattr(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)
@@ -83,10 +96,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):
@@ -95,14 +106,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):
     """
@@ -111,19 +123,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)

From 8e24beb42649bde3b0186657c61a3381b324be27 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Julian=20F=C3=B6rster?= <julian.foerster@mavt.ethz.ch>
Date: Mon, 11 Jan 2021 20:46:15 +0100
Subject: [PATCH 8/9] updated to official test files

---
 kdl_parser_py/test/test_copyright.py | 16 +++++++++++++++-
 kdl_parser_py/test/test_flake8.py    | 22 +++++++++++++++++++---
 kdl_parser_py/test/test_pep257.py    | 16 +++++++++++++++-
 3 files changed, 49 insertions(+), 5 deletions(-)

diff --git a/kdl_parser_py/test/test_copyright.py b/kdl_parser_py/test/test_copyright.py
index b821e73f..f46f861d 100644
--- a/kdl_parser_py/test/test_copyright.py
+++ b/kdl_parser_py/test/test_copyright.py
@@ -1,3 +1,17 @@
+# 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
 
@@ -5,5 +19,5 @@
 @pytest.mark.copyright
 @pytest.mark.linter
 def test_copyright():
-    rc = main(argv=["."])
+    rc = main(argv=[".", "test"])
     assert rc == 0, "Found errors"
diff --git a/kdl_parser_py/test/test_flake8.py b/kdl_parser_py/test/test_flake8.py
index 53cccd48..ee79f31a 100644
--- a/kdl_parser_py/test/test_flake8.py
+++ b/kdl_parser_py/test/test_flake8.py
@@ -1,9 +1,25 @@
-from ament_flake8.main import main
+# 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 = main(argv=[])
-    assert rc == 0, "Found errors"
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/kdl_parser_py/test/test_pep257.py b/kdl_parser_py/test/test_pep257.py
index 2b67ce0a..a2c3deb8 100644
--- a/kdl_parser_py/test/test_pep257.py
+++ b/kdl_parser_py/test/test_pep257.py
@@ -1,3 +1,17 @@
+# 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
 
@@ -5,5 +19,5 @@
 @pytest.mark.linter
 @pytest.mark.pep257
 def test_pep257():
-    rc = main(argv=["."])
+    rc = main(argv=[".", "test"])
     assert rc == 0, "Found code style errors / warnings"

From 2f24ffa12b9462c6ffe5da6baf5e70415e5cf81a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Julian=20F=C3=B6rster?= <julian.foerster@mavt.ethz.ch>
Date: Tue, 12 Jan 2021 15:32:48 +0100
Subject: [PATCH 9/9] Add licenses, make flake8 test pass

---
 kdl_parser_py/kdl_parser_py/urdf.py   | 33 +++++++++++++++++++++--
 kdl_parser_py/setup.py                |  4 ++-
 kdl_parser_py/test/test_kdl_parser.py | 38 ++++++++++++++++++++++++---
 kdl_parser_py/test/test_xmllint.py    | 14 ++++++++++
 4 files changed, 83 insertions(+), 6 deletions(-)

diff --git a/kdl_parser_py/kdl_parser_py/urdf.py b/kdl_parser_py/kdl_parser_py/urdf.py
index 028e3843..6ce62ff9 100644
--- a/kdl_parser_py/kdl_parser_py/urdf.py
+++ b/kdl_parser_py/kdl_parser_py/urdf.py
@@ -1,7 +1,36 @@
-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
 
 
diff --git a/kdl_parser_py/setup.py b/kdl_parser_py/setup.py
index cb297dcb..c1f55b4b 100644
--- a/kdl_parser_py/setup.py
+++ b/kdl_parser_py/setup.py
@@ -1,6 +1,8 @@
-from setuptools import setup
 import os
 
+from setuptools import setup
+
+
 package_name = "kdl_parser_py"
 share_path = os.path.join("share", package_name)
 
diff --git a/kdl_parser_py/test/test_kdl_parser.py b/kdl_parser_py/test/test_kdl_parser.py
index e1d1bfc9..c9776f43 100755
--- a/kdl_parser_py/test/test_kdl_parser.py
+++ b/kdl_parser_py/test/test_kdl_parser.py
@@ -1,11 +1,43 @@
-#!/usr/bin/env python
+# 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 sys
 import os
+import sys
+import unittest
+
 import ament_index_python as ament_index
 
 import kdl_parser_py.urdf
-import unittest
+
 
 PACKAGE_NAME = "kdl_parser_py"
 
diff --git a/kdl_parser_py/test/test_xmllint.py b/kdl_parser_py/test/test_xmllint.py
index 4e3bc690..48b7886d 100644
--- a/kdl_parser_py/test/test_xmllint.py
+++ b/kdl_parser_py/test/test_xmllint.py
@@ -1,3 +1,17 @@
+# 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_xmllint.main import main
 import pytest