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/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..6ce62ff9 100644 --- a/kdl_parser_py/kdl_parser_py/urdf.py +++ b/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) diff --git a/kdl_parser_py/package.xml b/kdl_parser_py/package.xml index 5b60c778..45eac10d 100644 --- a/kdl_parser_py/package.xml +++ b/kdl_parser_py/package.xml @@ -1,4 +1,5 @@ - + + kdl_parser_py 2.3.0 @@ -19,16 +20,16 @@ https://github.com/ros2/kdl_parser https://github.com/ros2/kdl_parser/issues - catkin - python-catkin-pkg - orocos_kdl urdf - rostest - orocos_kdl - urdf - urdfdom_py - python_orocos_kdl + orocos_kdl + urdf + urdfdom_py + python_orocos_kdl + + + ament_python + 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..c1f55b4b 100644 --- a/kdl_parser_py/setup.py +++ b/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", +) diff --git a/kdl_parser_py/test/test_copyright.py b/kdl_parser_py/test/test_copyright.py new file mode 100644 index 00000000..f46f861d --- /dev/null +++ b/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" diff --git a/kdl_parser_py/test/test_flake8.py b/kdl_parser_py/test/test_flake8.py new file mode 100644 index 00000000..ee79f31a --- /dev/null +++ b/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) 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 @@ - - - diff --git a/kdl_parser_py/test/test_kdl_parser.py b/kdl_parser_py/test/test_kdl_parser.py index f3e3066a..c9776f43 100755 --- a/kdl_parser_py/test/test_kdl_parser.py +++ b/kdl_parser_py/test/test_kdl_parser.py @@ -1,22 +1,59 @@ -#!/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 os import sys +import unittest + +import ament_index_python as ament_index import kdl_parser_py.urdf -import unittest -import rostest -PKG = "kdl_parser_py" -NAME = "test_kdl_parser" +PACKAGE_NAME = "kdl_parser_py" + 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 +66,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() diff --git a/kdl_parser_py/test/test_pep257.py b/kdl_parser_py/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/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" diff --git a/kdl_parser_py/test/test_xmllint.py b/kdl_parser_py/test/test_xmllint.py new file mode 100644 index 00000000..48b7886d --- /dev/null +++ b/kdl_parser_py/test/test_xmllint.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_xmllint.main import main +import pytest + + +@pytest.mark.xmllint +@pytest.mark.linter +def test_xmllint(): + rc = main(argv=[]) + assert rc == 0, "Found errors"