Merge pull request #19 from marip8/update/python-interface

Python interface updates

.gitignore

@@ -0,0 +1,2 @@
+*.pyc
+*.idea

CMakeLists.txt

@@ -12,12 +12,9 @@ if(BUILD_PYTHON)
   find_package(Python REQUIRED COMPONENTS Interpreter Development)
   find_package(PythonLibs 3 REQUIRED)
   find_package(Boost REQUIRED COMPONENTS python numpy)
+  find_package(ament_cmake_python REQUIRED)
 endif()
 
-add_compile_options(-std=c++17)
-set(CMAKE_CXX_FLAGS -rdynamic)
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)
-
 find_package(reach REQUIRED)
 find_package(boost_plugin_loader REQUIRED)
 find_package(yaml-cpp REQUIRED)
@@ -37,6 +34,10 @@ foreach(dep ${ROS2_DEPS})
   find_package(${dep} REQUIRED)
 endforeach()
 
+# Create a variable list for storing exportable library target names Note: this list should not include plugin
+# libraries, which are not for linking
+set(TARGETS "")
+
 # Plugin Implementations
 add_library(
   ${PROJECT_NAME} SHARED
@@ -54,10 +55,8 @@ add_library(
 target_include_directories(${PROJECT_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
                                                   $<INSTALL_INTERFACE:include>)
 target_link_libraries(${PROJECT_NAME} PUBLIC reach::reach)
-if($ENV{ROS_DISTRO} STRGREATER_EQUAL "humble")
-  target_compile_definitions(${PROJECT_NAME} PRIVATE ROS2_AT_LEAST_HUMBLE)
-endif()
 ament_target_dependencies(${PROJECT_NAME} PUBLIC ${ROS2_DEPS})
+list(APPEND TARGETS ${PROJECT_NAME})
 
 # Plugin Library
 add_library(${PROJECT_NAME}_plugins SHARED src/plugins.cpp)
@@ -71,11 +70,15 @@ ament_target_dependencies(${PROJECT_NAME}_node PUBLIC ${ROS2_DEPS})
 if(BUILD_PYTHON)
   message("Building Python bindings")
   add_subdirectory(src/python)
-  install(PROGRAMS demo/python/demo.py DESTINATION lib/${PROJECT_NAME})
+
+  ament_python_install_package(${PROJECT_NAME})
+
+  # Install Python executables
+  install(PROGRAMS scripts/reach_ros_node.py scripts/demo.py DESTINATION lib/${PROJECT_NAME})
 endif()
 
 # Install the libraries
-install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-targets DESTINATION lib)
+install(TARGETS ${TARGETS} EXPORT ${PROJECT_NAME}-targets DESTINATION lib)
 install(TARGETS ${PROJECT_NAME}_plugins DESTINATION lib)
 
 # Install the executable(s)
@@ -88,4 +91,5 @@ install(DIRECTORY include/${PROJECT_NAME} DESTINATION include)
 install(DIRECTORY launch demo DESTINATION share/${PROJECT_NAME})
 
 ament_export_targets(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${ROS2_DEPS})
 ament_package()

dependencies.repos

@@ -9,4 +9,4 @@
 - git:
    local-name: reach
    uri: https://github.com/ros-industrial/reach.git
-   version: 1.3.0
+   version: 1.4.0

package.xml

@@ -10,6 +10,7 @@
   <license>Apache 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <depend>tf2_eigen</depend>
   <depend>interactive_markers</depend>

reach_ros/__init__.py

@@ -0,0 +1,2 @@
+from reach_ros.utils import *
+from _reach_ros import *

reach_ros/utils.py

@@ -0,0 +1,37 @@
+from reach import SEARCH_LIBRARIES_ENV
+
+from rclpy.parameter import Parameter, PARAMETER_SEPARATOR_STRING
+import yaml
+import os
+
+
+def parse_parameter_dict(*, namespace, parameter_dict):
+    parameters = []
+    for param_name, param_value in parameter_dict.items():
+        full_param_name = namespace + param_name
+        # Unroll nested parameters
+        if type(param_value) == dict:
+            parameters += parse_parameter_dict(
+                namespace=full_param_name + PARAMETER_SEPARATOR_STRING,
+                parameter_dict=param_value)
+        else:
+            parameter = Parameter(name=full_param_name, value=param_value)
+            parameters.append(parameter)
+    return parameters
+
+
+def parse_yaml(parameter_file, namespace=''):
+    with open(parameter_file, 'r') as f:
+        param_dict = yaml.safe_load(f)
+        return parse_parameter_dict(namespace=namespace, parameter_dict=param_dict)
+
+
+def update_search_libraries_env():
+    """ Updates the search libraries environment variable to include the plugin library provided by this package
+
+    :return:
+    """
+    if os.environ.get(SEARCH_LIBRARIES_ENV) is None:
+        os.environ[SEARCH_LIBRARIES_ENV] = 'reach_ros_plugins'
+    else:
+        os.environ[SEARCH_LIBRARIES_ENV] += os.pathsep.join('reach_ros_plugins')

scripts/README.md

@@ -0,0 +1,36 @@
+# REACH ROS Python
+
+This package also provides a Python interface that allows executing the ROS 2 based plugins from Python.
+
+REACH uses a ROS node singleton to allow all plugins to share the same node.
+For this to work, it is necessary to initialize ROS and to provide the parameters to the node.
+Since the node is written in C++, the `rclpy` methods can not be used.
+Instead, the Python interface provides methods to initialize ROS with any given command line arguments and to manually handle the parameters of the node.
+
+## Python reach study node
+
+The [`reach_ros_node.py`](./reach_ros_node.py) script is a Python equivalent of the [c++ reach study node](../src/reach_study_node.cpp).
+To test it out, first modify the name of the executable launched in [`start.launch.py`](../launch/start.launch.py):
+
+```diff
+Node(
+    package='reach_ros',
+-   executable='reach_ros_node',
++   executable='reach_ros_node.py',
+    output='screen',
+```
+
+ Then run the demo following the instructions [here](../demo/README.md).
+
+## Python reach study demo
+
+The [`demo.py` script](demo.py) will launch the same demo (described [here](../README.md)) but from Python.
+It will run the reach study two times, once with the original MoveIt IK solver parameters and once with a shorter timeout.
+The results are then compared.
+Please, also have a look into the Python code to see how this works.
+
+You can start the demo like this:
+```
+ros2 run reach_ros demo.py
+```
+

scripts/demo.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+from reach import load, runReachStudy
+from reach_ros import init_ros, set_parameter, parse_yaml, set_logger_level, update_search_libraries_env
+
+from ament_index_python.packages import get_package_share_directory
+import os
+from rclpy.parameter import Parameter
+from rclpy.logging import LoggingSeverity
+import sys
+import xacro
+import yaml
+
+
+def main():
+    # initialize ROS with any parameters provided as arguments
+    init_ros(sys.argv)
+
+    # Set logger level to reduce MoveIt message spam
+    moveit_loggers = ["moveit_ros",
+                      "moveit_kinematics_base",
+                      "moveit_rdf_loader",
+                      "moveit_robot_model"]
+    for logger_name in moveit_loggers:
+        try:
+            set_logger_level(logger_name, LoggingSeverity.WARN)
+        except:
+            pass
+
+    # Manually load the parameters necessary for running MoveIt.
+    reach_ros_dir = get_package_share_directory('reach_ros')
+    model_dir = os.path.join(reach_ros_dir, 'demo', 'model')
+
+    # Generate the URDF string from the xacro file
+    urdf = xacro.process(os.path.join(model_dir, 'reach_study.xacro'))
+
+    # Load the SRDF
+    with open(os.path.join(model_dir, 'reach_study.srdf'), 'r') as f:
+        srdf = f.read()
+
+    moveit_parameters = [
+        *parse_yaml(os.path.join(model_dir, 'kinematics.yaml'), 'robot_description_kinematics.'),
+        *parse_yaml(os.path.join(model_dir, 'joint_limits.yaml'), 'robot_description_joint_limits.'),
+        Parameter(name="robot_description", value=urdf),
+        Parameter(name="robot_description_semantic", value=srdf),
+    ]
+
+    for parameter in moveit_parameters:
+        # We don't need to declare the parameters as the node allows undeclared parameters
+        set_parameter(parameter.name, parameter.value)
+
+    with open(os.path.join(reach_ros_dir, 'demo', 'config', 'reach_study.yaml'), 'r') as f:
+        config = yaml.safe_load(f)
+
+    # Update the search libraries environment variable to include the plugin library provided by this package
+    update_search_libraries_env()
+
+    # Disable the optimization steps to make this demo faster and highlight the performance difference
+    config['optimization']['max_steps'] = 0
+
+    print("Starting first study with default kinematic parameters")
+    save_dir = "/tmp"
+    study_name = "study1"
+    runReachStudy(config, study_name, save_dir, False)
+
+    # Loading the study results
+    results_1 = load(os.path.join(save_dir, study_name, 'reach.db.xml')).calculateResults()
+
+    # Decrease the IK solver time and see if we still find equally good solutions
+    set_parameter(
+        "robot_description_kinematics.manipulator.kinematics_solver_timeout", 0.00005)
+
+    print("Starting second study with decreased solver resolution.")
+    study_name = 'study2'
+    runReachStudy(config, study_name, save_dir, False)
+
+    # Loading the study results
+    results_2 = load(os.path.join(save_dir, study_name, 'reach.db.xml')).calculateResults()
+
+    print('Both REACH studies finished')
+    print(f'\tOriginal parameters: Score {results_1.total_pose_score:.2f}, Reached {results_1.reach_percentage:.2f}%')
+    print(f'\tModified parameters: Score {results_2.total_pose_score:.2f}, Reached {results_2.reach_percentage:.2f}%')
+
+
+if __name__ == '__main__':
+    main()

scripts/reach_ros_node.py

@@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+from reach import runReachStudy
+from reach_ros import init_ros, get_parameter
+import sys
+from yaml import safe_load
+
+
+def main():
+    init_ros(sys.argv)
+
+    with open(get_parameter('config_file'), 'r') as f:
+        config = safe_load(f)
+
+    config_name = get_parameter('config_name')
+    results_dir = get_parameter('results_dir')
+
+    print('Starting Python reach study node...')
+    runReachStudy(config, config_name, results_dir, True)
+
+
+if __name__ == '__main__':
+    main()

src/python/CMakeLists.txt

@@ -7,10 +7,14 @@ target_link_libraries(
           Boost::python
           Boost::numpy
           ${PYTHON_LIBRARIES})
-target_compile_definitions(${PROJECT_NAME}_python PRIVATE MODULE_NAME=${PROJECT_NAME})
+target_compile_definitions(${PROJECT_NAME}_python PRIVATE MODULE_NAME=_${PROJECT_NAME})
+if($ENV{ROS_DISTRO} STRGREATER_EQUAL "humble")
+  target_compile_definitions(${PROJECT_NAME}_python PRIVATE ROS2_AT_LEAST_HUMBLE)
+endif()
 set_target_properties(
-  ${PROJECT_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_INSTALL_PREFIX}/lib/python3/dist-packages
-                                    OUTPUT_NAME ${PROJECT_NAME})
+  ${PROJECT_NAME}_python
+  PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_INSTALL_PREFIX}/lib/python3.${Python_VERSION_MINOR}/site-packages
+             OUTPUT_NAME _${PROJECT_NAME})
 target_cxx_version(${PROJECT_NAME}_python PUBLIC VERSION 17)
 
 list(