[WIP] Python API

.gitignore

@@ -1,2 +1,3 @@
 *.swp
-
+*.pyc
+*.so

core/CMakeLists.txt

@@ -3,6 +3,7 @@ project(moveit_task_constructor_core)
 
 find_package(Boost REQUIRED)
 find_package(catkin REQUIRED COMPONENTS
+	roslint
 	eigen_conversions
 	geometry_msgs
 	moveit_core
@@ -14,6 +15,8 @@ find_package(catkin REQUIRED COMPONENTS
 	rviz_marker_tools
 )
 
+catkin_python_setup()
+
 catkin_package(
 	LIBRARIES
 		${PROJECT_NAME}
@@ -33,9 +36,10 @@ else ()
 	set(CMAKE_CXX_STANDARD 14)
 endif ()
 
-set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
+set(PROJECT_INCLUDE ${PROJECT_SOURCE_DIR}/include/moveit/task_constructor)
 
 add_subdirectory(src)
+add_subdirectory(python)
 add_subdirectory(test)
 
 install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})

core/include/moveit/python/python_tools/conversions.h

@@ -0,0 +1,128 @@
+#pragma once
+
+#include <boost/python.hpp>
+#include <boost/python/stl_iterator.hpp>
+#include <string>
+#include <vector>
+#include <map>
+#include <memory>
+
+#include <ros/serialization.h>
+
+namespace moveit {
+namespace python {
+
+template <typename T>
+std::vector<T> fromList(const boost::python::list& values) {
+	boost::python::stl_input_iterator<T> begin(values), end;
+	return std::vector<T>(begin, end);
+}
+
+template <typename T>
+boost::python::list toList(const std::vector<T>& v) {
+	boost::python::list l;
+	for (const T& value : v)
+		l.append(value);
+	return l;
+}
+
+template <typename T>
+std::map<std::string, T> fromDict(const boost::python::dict& values) {
+	std::map<std::string, T> m;
+	for (boost::python::stl_input_iterator<boost::python::tuple> it(values.iteritems()), end; it != end; ++it) {
+		const std::string& key = boost::python::extract<std::string>((*it)[0]);
+		const T& value = boost::python::extract<T>((*it)[1]);
+		m.insert(std::make_pair(key, value));
+	}
+	return m;
+}
+
+template <typename T>
+boost::python::dict toDict(const std::map<std::string, T>& v) {
+	boost::python::dict d;
+	for (const std::pair<std::string, T>& p : v)
+		d[p.first] = p.second;
+	return d;
+}
+
+/// convert a ROS msg to a string
+template <typename T>
+std::string serializeMsg(const T& msg) {
+	static_assert(sizeof(uint8_t) == sizeof(char), "Assuming char has same size as uint8_t");
+	std::size_t size = ros::serialization::serializationLength(msg);
+	std::string result(size, '\0');
+	if (size) {
+		ros::serialization::OStream stream(reinterpret_cast<uint8_t*>(&result[0]), size);
+		ros::serialization::serialize(stream, msg);
+	}
+	return result;
+}
+
+/// convert a string to a ROS message
+template <typename T>
+void deserializeMsg(const std::string& data, T& msg) {
+	ros::serialization::IStream stream(const_cast<uint8_t*>(reinterpret_cast<const uint8_t*>(&data[0])), data.size());
+	ros::serialization::deserialize(stream, msg);
+}
+
+/// retrieve name of ROS msg from object instance
+std::string rosMsgName(PyObject* object);
+
+/// non-templated base class for RosMsgConverter<T> providing common methods
+class RosMsgConverterBase
+{
+protected:
+	/// Register type internally and return true if registered first time
+	static bool insert(const boost::python::type_info& type_info, const std::string& ros_msg_name);
+
+	/// Determine if python object can be converted into C++ msg type
+	static void* convertible(PyObject* object);
+
+	/// Convert a ROS message (from python) to a string
+	static std::string fromPython(const boost::python::object& msg);
+
+	/// Convert a string to a python ROS message of given type
+	static PyObject* toPython(const std::string& data, const boost::python::type_info& type_info);
+};
+
+/// converter type to be registered with boost::python type conversion
+/// https://sixty-north.com/blog/how-to-write-boost-python-type-converters.html
+template <typename T>
+struct RosMsgConverter : RosMsgConverterBase
+{
+	/// constructor registers the type converter
+	RosMsgConverter() {
+		auto type_info = boost::python::type_id<T>();
+		// register type internally
+		if (insert(type_info, ros::message_traits::DataType<T>::value())) {
+			// https://stackoverflow.com/questions/9888289/checking-whether-a-converter-has-already-been-registered
+			const boost::python::converter::registration* reg = boost::python::converter::registry::query(type_info);
+			if (!reg || !reg->m_to_python) {
+				/// register type with boost::python converter system
+				boost::python::converter::registry::push_back(&convertible, &construct, type_info);
+				boost::python::to_python_converter<T, RosMsgConverter<T>>();
+			}
+		}
+	}
+
+	/// Conversion from Python object to C++ object, using pre-allocated memory block
+	static void construct(PyObject* object, boost::python::converter::rvalue_from_python_stage1_data* data) {
+		// serialize python msgs into string
+		std::string serialized = fromPython(boost::python::object(boost::python::borrowed(object)));
+
+		// Obtain a pointer to the memory block that the converter has allocated for the C++ type.
+		void* storage = reinterpret_cast<boost::python::converter::rvalue_from_python_storage<T>*>(data)->storage.bytes;
+		// Allocate the C++ type into the pre-allocated memory block, and assign its pointer to the converter's
+		// convertible variable.
+		T* result = new (storage) T();
+		data->convertible = result;
+
+		// deserialize string into C++ msg
+		deserializeMsg(serialized, *result);
+	}
+
+	/// Conversion from C++ object to Python object
+	static PyObject* convert(const T& msg) { return toPython(serializeMsg(msg), typeid(T)); }
+};
+}
+}

core/include/moveit/python/python_tools/ros_init.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <memory>
+#include <boost/python.hpp>
+#include <boost/thread/mutex.hpp>
+#include <ros/init.h>
+
+namespace moveit {
+namespace python {
+
+/// singleton class to initialize ROS C++ (once) from Python
+class InitProxy
+{
+public:
+	static void init(const std::string& node_name = "moveit_python_wrapper",
+	                 const boost::python::dict& remappings = boost::python::dict(), uint32_t options = 0);
+	static void shutdown();
+
+	~InitProxy();
+
+private:
+	InitProxy(const std::string& node_name, const boost::python::dict& remappings, uint32_t options);
+
+	static boost::mutex lock;
+	static std::unique_ptr<InitProxy> singleton_instance;
+
+private:
+	std::unique_ptr<ros::AsyncSpinner> spinner;
+};
+}
+}

core/include/moveit/python/task_constructor/properties.h

@@ -0,0 +1,91 @@
+#include <boost/python/class.hpp>
+#include <boost/mpl/vector.hpp>
+
+#include <moveit/task_constructor/properties.h>
+#include <moveit/python/python_tools/conversions.h>
+
+namespace moveit {
+namespace python {
+
+void export_properties();
+
+class PropertyConverterBase
+{
+public:
+	typedef boost::python::object (*to_python_converter_function)(const boost::any&);
+	typedef boost::any (*from_python_converter_function)(const boost::python::object&);
+
+protected:
+	static bool insert(const boost::python::type_info& type_info, const std::string& ros_msg_name,
+	                   to_python_converter_function to, from_python_converter_function from);
+};
+
+/// utility class to register C++ / Python converters for a property of type T
+template <typename T>
+class PropertyConverter : protected PropertyConverterBase
+{
+public:
+	PropertyConverter() {
+		auto type_info = boost::python::type_id<T>();
+		insert(type_info, rosMsgName<T>(), &toPython, &fromPython);
+	}
+
+private:
+	static boost::python::object toPython(const boost::any& value) {
+		return boost::python::object(boost::any_cast<T>(value));
+	}
+	static boost::any fromPython(const boost::python::object& bpo) { return T(boost::python::extract<T>(bpo)); }
+
+	template <class Q = T>
+	typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type rosMsgName() {
+		RosMsgConverter<T>();  // register ROS msg converter
+		return ros::message_traits::DataType<T>::value();
+	}
+
+	template <class Q = T>
+	typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type rosMsgName() {
+		return std::string();
+	}  // empty string if T isn't a ROS msg
+};
+
+namespace properties {
+
+/** Extension for boost::python::class_ to allow convienient definition of properties
+ *
+ * New method property<PropertyType>(const char* name) adds a property getter/setter.
+ */
+
+template <class W  // class being wrapped
+          ,
+          class X1 = ::boost::python::detail::not_specified, class X2 = ::boost::python::detail::not_specified,
+          class X3 = ::boost::python::detail::not_specified>
+class class_ : public boost::python::class_<W, X1, X2, X3>
+{
+public:
+	typedef class_<W, X1, X2, X3> self;
+	// forward all constructors
+	using boost::python::class_<W, X1, X2, X3>::class_;
+
+	template <typename PropertyType>
+	self& property(const char* name, const char* docstr = 0) {
+		auto getter = [name](const W& me) {
+			const moveit::task_constructor::PropertyMap& props = me.properties();
+			return props.get<PropertyType>(name);
+		};
+		auto setter = [name](W& me, const PropertyType& value) {
+			moveit::task_constructor::PropertyMap& props = me.properties();
+			props.set(name, boost::any(value));
+		};
+
+		boost::python::class_<W, X1, X2, X3>::add_property(
+		    name, boost::python::make_function(getter, boost::python::default_call_policies(),
+		                                       boost::mpl::vector<PropertyType, const W&>()),
+		    boost::python::make_function(setter, boost::python::default_call_policies(),
+		                                 boost::mpl::vector<void, W&, const PropertyType&>()),
+		    docstr);
+		return *this;
+	}
+};
+}
+}
+}

core/python/CMakeLists.txt

@@ -0,0 +1,15 @@
+# C++ wrapper code
+add_subdirectory(wrapper)
+
+if(CATKIN_ENABLE_TESTING)
+	find_package(moveit_resources REQUIRED)
+	find_package(rostest REQUIRED)
+
+	# Add folders to be run by python nosetests
+	catkin_add_nosetests(test)
+
+	# Add rostests
+	add_rostest(test/rostest_mtc.test)
+endif()
+
+roslint_python()

core/python/src/moveit/python_tools/__init__.py

@@ -0,0 +1 @@
+from _python_tools import *

core/python/src/moveit/task_constructor/__init__.py

@@ -0,0 +1,2 @@
+from core import *
+import stages

core/python/src/moveit/task_constructor/core.py

@@ -0,0 +1 @@
+from _core import *

core/python/src/moveit/task_constructor/stages.py

@@ -0,0 +1 @@
+from _stages import *

core/python/test/rostest_mtc.py

@@ -0,0 +1,45 @@
+#! /usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import unittest
+import rostest
+from moveit.python_tools import roscpp_init
+from moveit.task_constructor import core, stages
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import PoseStamped
+import rospy
+
+
+class Test(unittest.TestCase):
+    PLANNING_GROUP = "manipulator"
+
+    @classmethod
+    def setUpClass(self):
+        pass
+
+    @classmethod
+    def tearDown(self):
+        pass
+
+    def test_MoveRelative(self):
+        task = core.Task()
+        task.add(stages.CurrentState("current"))
+        move = stages.MoveRelative("move", core.PipelinePlanner())
+        move.group = self.PLANNING_GROUP
+        move.setDirection({"joint_1" : 0.2, "joint_2" : 0.4})
+        task.add(move)
+
+        task.enableIntrospection()
+        task.init()
+        task.plan()
+
+        self.assertEqual(len(task.solutions), 1)
+        for s in task.solutions:
+            print s
+        s = task.solutions[0]
+        task.execute(s)
+
+
+if __name__ == '__main__':
+    roscpp_init("test_mtc")
+    rostest.rosrun("", "", Test)

core/python/test/rostest_mtc.test

@@ -0,0 +1,4 @@
+<launch>
+  <include file="$(find moveit_resources)/fanuc_moveit_config/launch/test_environment.launch"/>
+  <test pkg="moveit_task_constructor_core" type="rostest_mtc.py" test-name="rostest_mtc" time-limit="300" args=""/>
+</launch>