Add Python interface for ROS features

CMakeLists.txt

@@ -5,6 +5,15 @@ find_package(ros_industrial_cmake_boilerplate REQUIRED)
 extract_package_metadata(pkg)
 project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX)
 
+option(BUILD_PYTHON "Build Python bindings" ON)
+
+# Python dependencies need to be found first
+if(BUILD_PYTHON)
+  find_package(Python REQUIRED COMPONENTS Interpreter Development)
+  find_package(PythonLibs 3 REQUIRED)
+  find_package(Boost REQUIRED COMPONENTS python numpy)
+endif()
+
 add_compile_options(-std=c++17)
 set(CMAKE_CXX_FLAGS -rdynamic)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
@@ -45,6 +54,9 @@ 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})
 
 # Plugin Library
@@ -56,6 +68,12 @@ add_executable(${PROJECT_NAME}_node src/reach_study_node.cpp)
 target_link_libraries(${PROJECT_NAME}_node PUBLIC reach::reach yaml-cpp ${PROJECT_NAME})
 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})
+endif()
+
 # Install the libraries
 install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-targets DESTINATION lib)
 install(TARGETS ${PROJECT_NAME}_plugins DESTINATION lib)

README.md

@@ -27,7 +27,7 @@ colcon build --symlink-install
 
 ## Demo
 A simple demonstration of the capability of this repository is provided in the `demo` sub-directory.
-See the [instructions](demo/README.md) for details on how to run the demo.
+See the [instructions](demo/README.md) for details on how to launch the demo from ROS 2 and see the [Python instructions](demo/python/README.md) to see how you can use the Python interface to launch studies.
 
 ## Usage
 Use the following steps to run a reach study with a robot using the ROS1 infrastructure and plugins.

demo/python/README.md

@@ -0,0 +1,21 @@
+# REACH Python Demo
+
+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 commandline arguments and to manually handle the parameters of the node.
+
+## Launching the Demo from Python
+
+This will launch the same demo (described [here](../README.md)) from Python.
+It will run it two times. Once with the original MoveIt IK solver parameters and once with a lower 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
+```
+

demo/python/demo.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python3
+import subprocess
+import sys
+import yaml
+
+from ament_index_python.packages import get_package_share_directory
+from rclpy.parameter import Parameter, PARAMETER_SEPARATOR_STRING
+from rclpy.logging import LoggingSeverity
+
+import reach
+import reach_ros
+
+
+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 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
+
+
+# initialize ROS with any parameters provided as arguments
+reach_ros.init_ros(sys.argv)
+
+# Set logger level to reduce MoveIt message spam
+moveit_loggers = ["moveit_ros.robot_model_loader",
+                  "moveit_kinematics_base.kinematics_base",
+                  "moveit_rdf_loader.rdf_loader",
+                  "moveit_robot_model.robot_model"]
+for logger_name in moveit_loggers:
+    reach_ros.set_logger_level(
+        logger_name, LoggingSeverity.WARN)
+
+# Manually load the paramters necessary for running MoveIt.
+reach_ros_dir = get_package_share_directory('reach_ros')
+urdf = subprocess.run(
+    ["xacro", f"{reach_ros_dir}/demo/model/reach_study.xacro"], stdout=subprocess.PIPE).stdout.decode('utf-8')
+with open(f"{reach_ros_dir}/demo/model/reach_study.srdf", 'r') as srdf_file:
+    srdf = srdf_file.read()
+moveit_parameters = []
+kinematics_parameters = parse_yaml(
+    f"{reach_ros_dir}/demo/model/kinematics.yaml",
+    "robot_description_kinematics.")
+moveit_parameters = moveit_parameters + kinematics_parameters
+joint_limit_parameters = parse_yaml(
+    f"{reach_ros_dir}/demo/model/joint_limits.yaml",
+    "robot_description_joint_limits.")
+moveit_parameters = moveit_parameters + joint_limit_parameters
+moveit_parameters.append(Parameter(name="robot_description", value=urdf))
+moveit_parameters.append(
+    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
+    reach_ros.set_parameter(parameter.name, parameter.value)
+
+with open(f"{reach_ros_dir}/demo/config/reach_study.yaml", 'r') as f:
+    config = yaml.safe_load(f)
+
+# Disable the optimization steps to make this demo faster and highlight the performace difference
+config['optimization']['max_steps'] = 0
+
+print("Starting first study with default kinematic parameters")
+reach.runReachStudy(config, "study1", "/tmp", False)
+
+# Loading the study results
+results_1 = reach.load("/tmp/study1/reach.db.xml").calculateResults()
+
+# Decrease the IK solver time and see if we still find equally good solutions
+reach_ros.set_parameter(
+    "robot_description_kinematics.manipulator.kinematics_solver_timeout", 0.00005)
+
+print("Starting second study with decreased solver resolution.")
+reach.runReachStudy(config, "study2", "/tmp", False)
+
+# Loading the study results
+results_2 = reach.load("/tmp/study2/reach.db.xml").calculateResults()
+
+print(
+    f"Both REACH studies are finished. Here are the results\n  Original parameters: Score {results_1.total_pose_score:.2f} Reached {results_1.reach_percentage:.2f} %\n  Modified parameters: Score {results_2.total_pose_score:.2f} Reached {results_2.reach_percentage:.2f} %")

src/python/CMakeLists.txt

@@ -0,0 +1,20 @@
+# Add Python libraries
+python_add_library(${PROJECT_NAME}_python MODULE python_bindings.cpp)
+target_include_directories(${PROJECT_NAME}_python PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_link_libraries(
+  ${PROJECT_NAME}_python
+  PRIVATE ${PROJECT_NAME}
+          Boost::python
+          Boost::numpy
+          ${PYTHON_LIBRARIES})
+target_compile_definitions(${PROJECT_NAME}_python PRIVATE MODULE_NAME=${PROJECT_NAME})
+set_target_properties(
+  ${PROJECT_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_INSTALL_PREFIX}/lib/python3/dist-packages
+                                    OUTPUT_NAME ${PROJECT_NAME})
+target_cxx_version(${PROJECT_NAME}_python PUBLIC VERSION 17)
+
+list(
+  APPEND
+  TARGETS
+  ${PROJECT_NAME}_python
+  PARENT_SCOPE)

src/python/python_bindings.cpp

@@ -0,0 +1,146 @@
+/*
+ * Copyright 2019 Southwest Research Institute
+ *
+ * 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.
+ */
+#include <reach/utils.h>
+#include <reach_ros/utils.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include <boost_plugin_loader/plugin_loader.hpp>
+#include <boost/python.hpp>
+#include <boost/python/converter/builtin_converters.hpp>
+#include <cstdarg>
+
+namespace bp = boost::python;
+
+namespace reach_ros
+{
+
+void init_ros(const bp::list& argv)
+{
+  int argc = bp::len(argv);
+  if (argc == 0)
+  {
+    // init() does not accept argv with length 0
+    rclcpp::init(0, nullptr);
+  }
+  else
+  {
+    char* argv_c[argc];
+    for (bp::ssize_t i = 0; i < argc; ++i)
+      argv_c[i] = bp::extract<char*>{ argv[i] }();
+    rclcpp::init(argc, argv_c);
+  }
+}
+
+bp::object get_parameter(std::string name)
+{
+  rclcpp::Parameter parameter = reach_ros::utils::getNodeInstance()->get_parameter(name);
+  rclcpp::ParameterType type = parameter.get_type();
+  switch (type)
+  {
+    case rclcpp::ParameterType::PARAMETER_BOOL:
+      return bp::object(parameter.as_bool());
+    case rclcpp::ParameterType::PARAMETER_INTEGER:
+      return bp::object(parameter.as_int());
+    case rclcpp::ParameterType::PARAMETER_DOUBLE:
+      return bp::object(parameter.as_double());
+    case rclcpp::ParameterType::PARAMETER_STRING:
+      return bp::object(parameter.as_string());
+    case rclcpp::ParameterType::PARAMETER_BYTE_ARRAY:
+      return bp::object(parameter.as_byte_array());
+    case rclcpp::ParameterType::PARAMETER_BOOL_ARRAY:
+      return bp::object(parameter.as_bool_array());
+    case rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY:
+      return bp::object(parameter.as_integer_array());
+    case rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY:
+      return bp::object(parameter.as_double_array());
+    case rclcpp::ParameterType::PARAMETER_STRING_ARRAY:
+      return bp::object(parameter.as_string_array());
+    default:
+      return bp::object();
+  }
+}
+
+void set_parameter(std::string name, const bp::object& obj)
+{
+  // We use the direct checks for object type since bp::extract().check() does not distinguish between int and bool
+  if (PyBool_Check(obj.ptr()))
+    reach_ros::utils::getNodeInstance()->set_parameter(rclcpp::Parameter(name, bp::extract<bool>(obj)));
+  else if (PyLong_Check(obj.ptr()))
+    reach_ros::utils::getNodeInstance()->set_parameter(rclcpp::Parameter(name, bp::extract<int>(obj)));
+  else if (PyFloat_Check(obj.ptr()))
+    reach_ros::utils::getNodeInstance()->set_parameter(rclcpp::Parameter(name, bp::extract<float>(obj)));
+  else if (PyUnicode_Check(obj.ptr()))
+    reach_ros::utils::getNodeInstance()->set_parameter(rclcpp::Parameter(name, bp::extract<std::string>(obj)));
+  else if (PyList_Check(obj.ptr()))
+  {
+    bp::list list_obj = bp::extract<bp::list>(obj);
+    if (PyBool_Check(bp::object(list_obj[0]).ptr()))
+      reach_ros::utils::getNodeInstance()->set_parameter(rclcpp::Parameter(name, bp::extract<bool*>(list_obj)));
+    else if (PyLong_Check(bp::object(list_obj[0]).ptr()))
+      reach_ros::utils::getNodeInstance()->set_parameter(rclcpp::Parameter(name, bp::extract<int*>(list_obj)));
+    else if (PyFloat_Check(bp::object(list_obj[0]).ptr()))
+      reach_ros::utils::getNodeInstance()->set_parameter(rclcpp::Parameter(name, bp::extract<double*>(list_obj)));
+    else if (PyUnicode_Check(bp::object(list_obj[0]).ptr()))
+      reach_ros::utils::getNodeInstance()->set_parameter(rclcpp::Parameter(name, bp::extract<std::string*>(list_obj)));
+  }
+  else
+    throw std::runtime_error("Unsupported Python value type '" +
+                             bp::extract<std::string>{ obj.attr("__class__").attr("__name__") }() + "'");
+}
+
+#ifdef ROS2_AT_LEAST_HUMBLE
+void set_logger_level(std::string logger_name, int level_int)
+{
+  rclcpp::Logger::Level level;
+  switch (level_int)
+  {
+    case 10:
+      level = rclcpp::Logger::Level::Debug;
+      break;
+    case 20:
+      level = rclcpp::Logger::Level::Info;
+      break;
+    case 30:
+      level = rclcpp::Logger::Level::Warn;
+      break;
+    case 40:
+      level = rclcpp::Logger::Level::Error;
+      break;
+    case 50:
+      level = rclcpp::Logger::Level::Fatal;
+      break;
+    default:
+      std::cerr << "Invalid log level: " << level_int << std::endl;
+      level = rclcpp::Logger::Level::Unset;
+  }
+  rclcpp::get_logger(logger_name).set_level(level);
+}
+#endif
+
+BOOST_PYTHON_MODULE(MODULE_NAME)
+{
+  Py_Initialize();
+  {
+    bp::def("init_ros", &init_ros);
+    bp::def("get_parameter", &get_parameter);
+    bp::def("set_parameter", &set_parameter);
+#ifdef ROS2_AT_LEAST_HUMBLE
+    bp::def("set_logger_level", &set_logger_level);
+#endif
+  }
+}
+
+}  // namespace reach_ros