ROS2 port

.gitignore

@@ -1 +1,3 @@
 build
+__pycache__
+*.pyc

.travis.yml

@@ -15,14 +15,14 @@ env:
   global:
     # generate "exotic" locale for parser test
     - BEFORE_SCRIPT="sudo apt-get -qq install -y locales; sudo locale-gen nl_NL.UTF-8"
-    - ROS_DISTRO=melodic
+    - ROS_DISTRO=crystal
     - ROS_REPO=ros
   matrix:
-    - TEST="clang-format, catkin_lint"
-    - ROS_DISTRO=melodic
+    - TEST="clang-format"
+    - ROS_DISTRO=crystal
 
 before_script:
-  - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci
+  - git clone -q -b ros2 --depth=1 -b ros2 https://github.com/ros-planning/moveit_ci.git .moveit_ci
 
 script:
   - .moveit_ci/travis.sh

CMakeLists.txt

@@ -1,66 +1,91 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(srdfdom)
 
-find_package(Boost REQUIRED)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
 
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(console_bridge_vendor REQUIRED) # Provides console_bridge 0.4.0 on platforms without it.
 find_package(console_bridge REQUIRED)
 find_package(urdfdom_headers REQUIRED)
+find_package(urdf REQUIRED)
+find_package(tinyxml_vendor REQUIRED)
+find_package(TinyXML REQUIRED)  # provided by tinyxml_vendor
+find_package(ament_index_cpp REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS cmake_modules urdf urdfdom_py)
-
-find_package(TinyXML REQUIRED)
+find_package(Boost REQUIRED)
 
 include_directories(
   include
+  ${rclcpp_INCLUDE_DIRS}
+  ${rmw_implementation_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIR}
   ${TinyXML_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
   ${console_bridge_INCLUDE_DIRS}
   ${urdfdom_headers_INCLUDE_DIRS}
-  )
-
-add_compile_options(-std=c++11)
-
-catkin_python_setup()
-
-catkin_package(
-  LIBRARIES ${PROJECT_NAME}
-  INCLUDE_DIRS include
-  DEPENDS TinyXML console_bridge urdfdom_headers
+  ${std_msgs_INCLUDE_DIRS}
 )
 
-add_library(${PROJECT_NAME} 
+ament_python_install_package(${PROJECT_NAME}
+  PACKAGE_DIR src/${PROJECT_NAME})
+
+add_library(${PROJECT_NAME}
   src/model.cpp
   src/srdf_writer.cpp
 )
-target_link_libraries(${PROJECT_NAME} ${TinyXML_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES} ${urdfdom_headers_LIBRARIES})
 
+ament_target_dependencies(${PROJECT_NAME}
+  ${TinyXML_LIBRARIES}
+  ${console_bridge_LIBRARIES}
+  ${urdfdom_headers_LIBRARIES}
+  ${urdf_LIBRARIES}
+)
 
-install(TARGETS ${PROJECT_NAME}
-  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+install(
+  TARGETS
+    ${PROJECT_NAME}
+  DESTINATION lib
 )
+
 install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-  FILES_MATCHING PATTERN "*.h"
+  DESTINATION include/${PROJECT_NAME}
 )
 
-install(PROGRAMS 
+# Mark resources for installation
+install(DIRECTORY test/resources
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(PROGRAMS
   scripts/display_srdf
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-if(CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-  add_rostest(test/srdf_parser.test)
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
 
-  add_definitions(-DTEST_RESOURCE_LOCATION="${CMAKE_SOURCE_DIR}/test/resources")
-  execute_process(COMMAND bash -c "locale -a | grep -q ^nl_NL"
-    RESULT_VARIABLE TEST_LOCALE
-    OUTPUT_QUIET ERROR_QUIET)
-  if (TEST_LOCALE)
-    message(WARNING "Locale nl_NL not available. Locale test will not be meaningful.")
-  endif()
+  ament_add_gtest(test_parser test/test_parser.cpp)
+  target_link_libraries(test_parser
+    ${PROJECT_NAME}
+    ${rclcpp_LIBRARIES}
+    ${TinyXML_LIBRARIES}
+    ${console_bridge_LIBRARIES}
+    ${urdfdom_headers_LIBRARIES}
+    ${urdf_LIBRARIES}
+    ament_index_cpp::ament_index_cpp
+  )
 
-  add_rostest_gtest(test_cpp test/srdf_parser_cpp.test test/test_parser.cpp)
-  target_link_libraries(test_cpp ${catkin_LIBRARIES} ${PROJECT_NAME})
+  # python tests with python interfaces of message filters
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_pytest_test(test.py "test/test.py"
+    APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+    TIMEOUT 90)
 endif()
+
+ament_package()

package.xml

@@ -1,4 +1,6 @@
-<package>
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>srdfdom</name>
   <version>0.5.1</version>
   <description>Parser for Semantic Robot Description Format (SRDF).</description>
@@ -10,23 +12,32 @@
   <url type="website">http://ros.org/wiki/srdfdom</url>
   <url type="bugtracker">https://github.com/ros-planning/srdfdom/issues</url>
   <url type="repository">https://github.com/ros-planning/srdfdom</url>
-
-  <buildtool_depend>catkin</buildtool_depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>python_cmake_module</buildtool_depend>
+
+  <depend>console_bridge_vendor</depend>
 
   <build_depend>boost</build_depend>
-  <build_depend>cmake_modules</build_depend>
+  <build_depend>python_cmake_module</build_depend>
   <build_depend>libconsole-bridge-dev</build_depend>
   <build_depend>urdf</build_depend>
   <build_depend>liburdfdom-headers-dev</build_depend>
   <build_depend>urdfdom_py</build_depend>
   <build_depend>tinyxml</build_depend>
 
-  <run_depend>boost</run_depend>
-  <run_depend>libconsole-bridge-dev</run_depend>
-  <run_depend>liburdfdom-headers-dev</run_depend>
-  <run_depend>tinyxml</run_depend>
-  <run_depend>urdfdom_py</run_depend>
+  <exec_depend>boost</exec_depend>
+  <exec_depend>libconsole-bridge-dev</exec_depend>
+  <exec_depend>liburdfdom-headers-dev</exec_depend>
+  <exec_depend>tinyxml</exec_depend>
+  <exec_depend>urdfdom_py</exec_depend>
 
-  <test_depend>rostest</test_depend>
+  <test_depend>ament_index_cpp</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>python3-pytest</test_depend>
 
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>

src/srdfdom/srdf.py

@@ -49,16 +49,16 @@ def __init__(self, center = None, radius = 0.0):
 
 class VirtualJoint(xmlr.Object):
   TYPES = ['unknown', 'fixed', 'floating', 'planar']
-  
+
   def __init__(self, name = None, child_link = None, parent_frame = None, joint_type = None):
     self.name = name
     self.child_link = child_link
     self.parent_frame = parent_frame
     self.type = joint_type
-    
+
   def check_valid(self):
     assert self.type in self.TYPES, "Invalid joint type: {}".format(self.type)
-  
+
   # Aliases
   @property
   def joint_type(self): return self.type
@@ -94,7 +94,7 @@ def __init__(self, name = None, group = None, parent_link = None, parent_group =
   name_attribute,
   xmlr.Attribute('group', str),
   xmlr.Attribute('parent_link', str),
-  xmlr.Attribute('parent_group', str, False)  
+  xmlr.Attribute('parent_group', str, False)
   ])
 
 class PassiveJoint(xmlr.Object):
@@ -148,8 +148,8 @@ def __init__(self, name = None, group = None):
   xmlr.AggregateElement('joint', JointVal),
   xmlr.Attribute('group', str)
   ])
-  
-  
+
+
 class LinkSphereApproximation(xmlr.Object):
   def __init__(self, link = None):
     self.aggregate_init()
@@ -160,11 +160,11 @@ def __init__(self, link = None):
   xmlr.Attribute('link', str),
   xmlr.AggregateElement('sphere', Sphere)
   ])
-  
+
 class Robot(xmlr.Object):
   def __init__(self, name = None):
     self.aggregate_init()
-    
+
     self.name = name
     self.groups = []
     self.group_states = []
@@ -175,10 +175,10 @@ def __init__(self, name = None):
     self.link_sphere_approximations = []
     self.group_map = {}
     self.group_state_map = {}
-    
+
   def add_aggregate(self, typeName, elem):
     xmlr.Object.add_aggregate(self, typeName, elem)
-    
+
     if typeName == 'group':
       group = elem
       self.group_map[group.name] = group
@@ -191,22 +191,22 @@ def add_link(self, link):
 
   def add_joint(self, joint):
     self.add_aggregate('joint', joint)
-  
+
   def add_chain(self, chain):
     self.add_aggregate('chain', chain)
-  
+
   def add_group(self, group):
     self.add_aggregate('group', group)
-    
+
   def add_passive_joint(self, joint):
     self.add_aggregate('passive_joint', joint)
-  
+
   def add_disable_collisions(self, col):
     self.add_aggregate('disable_collisions', col)
-  
+
   def add_link_sphere_approximation(self, link):
     self.add_aggregate('link_sphere_approximation', link)
-      
+
 
   @classmethod
   def from_parameter_server(cls, key = 'robot_description_semantic'):
@@ -219,7 +219,7 @@ def from_parameter_server(cls, key = 'robot_description_semantic'):
     # Could move this into xml_reflection
     import rospy
     return cls.from_xml_string(rospy.get_param(key))
-  
+
 xmlr.reflect(Robot, tag = 'robot', params = [
 #   name_attribute,
   xmlr.Attribute('name', str, False), # Is 'name' a required attribute?

test/resources/pr2_desc.urdf

@@ -73,10 +73,10 @@
   <!-- Now we can start using the macros included above to define the actual PR2 -->
   <!-- The first use of a macro.  This one was defined in base.urdf.xacro above.
        A macro like this will expand to a set of link and joint definitions, and to additional
-       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name, 
-       that equals "base", and uses it to generate names for its component links and joints 
-       (e.g., base_link).  The included origin block is also an argument to the macro.  By convention, 
-       the origin block defines where the component is w.r.t its parent (in this case the parent 
+       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name,
+       that equals "base", and uses it to generate names for its component links and joints
+       (e.g., base_link).  The included origin block is also an argument to the macro.  By convention,
+       the origin block defines where the component is w.r.t its parent (in this case the parent
        is the world frame). For more, see http://www.ros.org/wiki/xacro -->
   <link name="base_link">
     <inertial>