diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt
index e9ea79c3..c0e3f679 100644
--- a/core/CMakeLists.txt
+++ b/core/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8.12)
project(moveit_task_constructor_core)
find_package(catkin REQUIRED COMPONENTS
+ roslint
eigen_conversions
geometry_msgs
moveit_core
@@ -13,6 +14,8 @@ find_package(catkin REQUIRED COMPONENTS
rviz_marker_tools
)
+catkin_python_setup()
+
catkin_package(
LIBRARIES
${PROJECT_NAME}
diff --git a/core/package.xml b/core/package.xml
index ca283bfd..81e0affa 100644
--- a/core/package.xml
+++ b/core/package.xml
@@ -9,6 +9,7 @@
catkin
+ roslint
eigen_conversions
geometry_msgs
roscpp
diff --git a/core/python/CMakeLists.txt b/core/python/CMakeLists.txt
index e681a585..c1955965 100644
--- a/core/python/CMakeLists.txt
+++ b/core/python/CMakeLists.txt
@@ -1,25 +1,10 @@
-find_package(PythonLibs REQUIRED)
-find_package(Boost REQUIRED COMPONENTS python)
+# C++ wrapper code
+add_subdirectory(wrapper)
-include_directories(${PYTHON_INCLUDE_PATH})
+if(CATKIN_ENABLE_TESTING)
+ add_subdirectory(test)
+ # Add folders to be run by python nosetests
+ catkin_add_nosetests(test)
+endif()
-add_library(_core SHARED
- conversions.cpp
- ros_init.cpp
- properties.cpp
- solvers.cpp
- core.cpp
-)
-target_link_libraries(_core ${PROJECT_NAME} ${PYTHON_LIBRARIES} ${Boost_LIBRARIES})
-set_target_properties(_core PROPERTIES PREFIX "")
-
-add_library(_stages SHARED
- stages.cpp
-)
-target_link_libraries(_stages _core ${PROJECT_NAME}_stages)
-set_target_properties(_stages PROPERTIES PREFIX "")
-
-install(TARGETS _core _stages
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-)
+roslint_python()
diff --git a/core/python/src/moveit/__init__.py b/core/python/src/moveit/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/core/python/src/moveit/python_tools/__init__.py b/core/python/src/moveit/python_tools/__init__.py
new file mode 100644
index 00000000..f12b0b0e
--- /dev/null
+++ b/core/python/src/moveit/python_tools/__init__.py
@@ -0,0 +1 @@
+from _moveit_python_tools import *
diff --git a/core/python/src/moveit/task_constructor/__init__.py b/core/python/src/moveit/task_constructor/__init__.py
new file mode 100644
index 00000000..10c0b712
--- /dev/null
+++ b/core/python/src/moveit/task_constructor/__init__.py
@@ -0,0 +1,3 @@
+import moveit.python_tools
+from _moveit_task_constructor_core_core import *
+from _moveit_task_constructor_core_stages import *
diff --git a/core/python/test/CMakeLists.txt b/core/python/test/CMakeLists.txt
new file mode 100644
index 00000000..e69de29b
diff --git a/core/python/test/test_mtc.py b/core/python/test/test_mtc.py
new file mode 100644
index 00000000..9ea0d1ad
--- /dev/null
+++ b/core/python/test/test_mtc.py
@@ -0,0 +1,27 @@
+#! /usr/bin/env python
+# -*- coding: utf-8 -*-
+
+from moveit.task_constructor import PropertyMap
+from geometry_msgs.msg import Pose
+import unittest
+
+class TestPropertyMap(unittest.TestCase):
+ def __init__(self, *args, **kwargs):
+ super(TestPropertyMap, self).__init__(*args, **kwargs)
+ self.props = PropertyMap()
+
+ def _check(self, name, value):
+ self.props[name] = value
+ self.assertEqual(self.props[name], value)
+
+ def test_assign(self):
+ self._check("double", 3.14)
+ self._check("long", 42)
+ self._check("long", 13)
+ self._check("bool", True)
+ self._check("bool", False)
+ self._check("pose", Pose())
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/core/python/test_script.py b/core/python/test_script.py
deleted file mode 100644
index a20a043a..00000000
--- a/core/python/test_script.py
+++ /dev/null
@@ -1,19 +0,0 @@
-from _core import *
-from geometry_msgs.msg import Pose
-
-roscpp_init()
-task = Task()
-print task.name
-
-props = task.properties
-props["float"] = 3.14
-props["int"] = 42
-props["bool"] = True
-props["pose"] = Pose()
-
-print props["float"]
-print props["int"]
-print props["bool"]
-print props["pose"], type(props["pose"])
-
-roscpp_shutdown()
diff --git a/core/python/wrapper/CMakeLists.txt b/core/python/wrapper/CMakeLists.txt
new file mode 100644
index 00000000..9a38364f
--- /dev/null
+++ b/core/python/wrapper/CMakeLists.txt
@@ -0,0 +1,68 @@
+find_package(PythonLibs REQUIRED)
+find_package(Boost REQUIRED COMPONENTS python)
+include_directories(${PYTHON_INCLUDE_PATH})
+
+# python tools support lib
+set(TOOLS_LIB_NAME moveit_python_tools)
+set(INCLUDES include/moveit/python/python_tools)
+add_library(${TOOLS_LIB_NAME} SHARED
+ ${INCLUDES}/conversions.h
+ ${INCLUDES}/ros_init.h
+
+ src/conversions.cpp
+ src/ros_init.cpp
+)
+target_include_directories(${TOOLS_LIB_NAME}
+ PUBLIC $
+)
+target_link_libraries(${TOOLS_LIB_NAME} PUBLIC ${PYTHON_LIBRARIES} ${Boost_LIBRARIES} ${roscpp_LIBRARIES})
+
+install(TARGETS ${TOOLS_LIB_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+# python_tools
+set(TOOLS_LIB_NAME _moveit_python_tools)
+configure_file(src/python_tools.cpp.in python_tools.cpp @ONLY)
+add_library(${TOOLS_LIB_NAME} SHARED ${CMAKE_CURRENT_BINARY_DIR}/python_tools.cpp)
+target_link_libraries(${TOOLS_LIB_NAME} PRIVATE moveit_python_tools)
+
+
+# core
+set(CORE_LIB_NAME _${PROJECT_NAME}_core)
+set(INCLUDES include/moveit/python/task_constructor)
+configure_file(src/core.cpp.in core.cpp @ONLY)
+add_library(${CORE_LIB_NAME} SHARED
+ ${INCLUDES}/properties.h
+
+ src/properties.cpp
+ src/solvers.cpp
+ ${CMAKE_CURRENT_BINARY_DIR}/core.cpp
+)
+target_include_directories(${CORE_LIB_NAME}
+ PUBLIC $
+)
+target_link_libraries(${CORE_LIB_NAME} PUBLIC ${PROJECT_NAME} moveit_python_tools)
+
+# stages
+set(STAGES_LIB_NAME _${PROJECT_NAME}_stages)
+configure_file(src/stages.cpp.in stages.cpp @ONLY)
+add_library(${STAGES_LIB_NAME} SHARED
+ ${CMAKE_CURRENT_BINARY_DIR}/stages.cpp
+)
+target_link_libraries(${STAGES_LIB_NAME} ${CORE_LIB_NAME} ${PROJECT_NAME}_stages)
+
+
+# common properties of all python wrapper libs
+set_target_properties(${TOOLS_LIB_NAME} ${CORE_LIB_NAME} ${STAGES_LIB_NAME}
+ PROPERTIES
+ # create all libs in python's main folder to enable loading also from devel space
+ LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}
+ # no "lib" prefix
+ PREFIX "")
+
+install(TARGETS ${TOOLS_LIB_NAME} ${CORE_LIB_NAME} ${STAGES_LIB_NAME}
+ ARCHIVE DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION}
+)
diff --git a/core/python/conversions.h b/core/python/wrapper/include/moveit/python/python_tools/conversions.h
similarity index 100%
rename from core/python/conversions.h
rename to core/python/wrapper/include/moveit/python/python_tools/conversions.h
diff --git a/core/python/wrapper/include/moveit/python/python_tools/ros_init.h b/core/python/wrapper/include/moveit/python/python_tools/ros_init.h
new file mode 100644
index 00000000..d55b97ee
--- /dev/null
+++ b/core/python/wrapper/include/moveit/python/python_tools/ros_init.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include
+#include
+#include
+#include
+
+namespace moveit {
+namespace python {
+
+class InitProxy {
+public:
+ static void init(const std::string& node_name="moveit_python_wrapper",
+ const boost::python::dict& remappings = boost::python::dict());
+ static void shutdown();
+
+ ~InitProxy();
+
+private:
+ InitProxy(const std::string& node_name, const boost::python::dict& remappings);
+
+ static boost::mutex lock;
+ static std::unique_ptr singleton_instance;
+
+private:
+ std::unique_ptr spinner;
+};
+
+} }
diff --git a/core/python/properties.h b/core/python/wrapper/include/moveit/python/task_constructor/properties.h
similarity index 100%
rename from core/python/properties.h
rename to core/python/wrapper/include/moveit/python/task_constructor/properties.h
diff --git a/core/python/conversions.cpp b/core/python/wrapper/src/conversions.cpp
similarity index 95%
rename from core/python/conversions.cpp
rename to core/python/wrapper/src/conversions.cpp
index 5d52aff0..a5e59d36 100644
--- a/core/python/conversions.cpp
+++ b/core/python/wrapper/src/conversions.cpp
@@ -1,4 +1,4 @@
-#include "conversions.h"
+#include
#include
namespace moveit {
diff --git a/core/python/core.cpp b/core/python/wrapper/src/core.cpp.in
similarity index 95%
rename from core/python/core.cpp
rename to core/python/wrapper/src/core.cpp.in
index 841abe74..9d921275 100644
--- a/core/python/core.cpp
+++ b/core/python/wrapper/src/core.cpp.in
@@ -9,7 +9,6 @@ using namespace moveit::task_constructor;
namespace moveit {
namespace python {
-void export_ros_init();
void export_properties();
void export_solvers();
@@ -51,9 +50,8 @@ void export_core()
} }
-BOOST_PYTHON_MODULE(_core)
+BOOST_PYTHON_MODULE(@CORE_LIB_NAME@)
{
- moveit::python::export_ros_init();
moveit::python::export_properties();
moveit::python::export_solvers();
moveit::python::export_core();
diff --git a/core/python/properties.cpp b/core/python/wrapper/src/properties.cpp
similarity index 95%
rename from core/python/properties.cpp
rename to core/python/wrapper/src/properties.cpp
index 4b726f2a..cd06ab56 100644
--- a/core/python/properties.cpp
+++ b/core/python/wrapper/src/properties.cpp
@@ -1,5 +1,5 @@
-#include "properties.h"
-#include "conversions.h"
+#include
+#include
#include
diff --git a/core/python/wrapper/src/python_tools.cpp.in b/core/python/wrapper/src/python_tools.cpp.in
new file mode 100644
index 00000000..982d4c3a
--- /dev/null
+++ b/core/python/wrapper/src/python_tools.cpp.in
@@ -0,0 +1,15 @@
+#include
+
+namespace bp = boost::python;
+
+namespace moveit {
+namespace python {
+
+void export_ros_init();
+
+} }
+
+BOOST_PYTHON_MODULE(@TOOLS_LIB_NAME@)
+{
+ moveit::python::export_ros_init();
+}
diff --git a/core/python/ros_init.cpp b/core/python/wrapper/src/ros_init.cpp
similarity index 71%
rename from core/python/ros_init.cpp
rename to core/python/wrapper/src/ros_init.cpp
index 2eb5584f..50254d2a 100644
--- a/core/python/ros_init.cpp
+++ b/core/python/wrapper/src/ros_init.cpp
@@ -3,28 +3,12 @@
#include
#include
-#include "conversions.h"
+#include
+#include
namespace moveit {
namespace python {
-class InitProxy {
-public:
- static void init(const std::string& node_name="moveit_python_wrapper",
- const boost::python::dict& remappings = boost::python::dict());
- static void shutdown();
-
- ~InitProxy();
-
-private:
- InitProxy(const std::string& node_name, const boost::python::dict& remappings);
-
- static boost::mutex lock;
- static std::unique_ptr singleton_instance;
-
-private:
- std::unique_ptr spinner;
-};
boost::mutex InitProxy::lock;
std::unique_ptr InitProxy::singleton_instance;
diff --git a/core/python/solvers.cpp b/core/python/wrapper/src/solvers.cpp
similarity index 94%
rename from core/python/solvers.cpp
rename to core/python/wrapper/src/solvers.cpp
index b0c324c4..ce6a8c7f 100644
--- a/core/python/solvers.cpp
+++ b/core/python/wrapper/src/solvers.cpp
@@ -1,6 +1,6 @@
#include
-#include "properties.h"
+#include
#include
#include
diff --git a/core/python/stages.cpp b/core/python/wrapper/src/stages.cpp.in
similarity index 97%
rename from core/python/stages.cpp
rename to core/python/wrapper/src/stages.cpp.in
index fa15b7db..93ac0fad 100644
--- a/core/python/stages.cpp
+++ b/core/python/wrapper/src/stages.cpp.in
@@ -23,7 +23,7 @@ Connect* initConnect(const std::string& name, const bp::list& l) {
} // anonymous namespace
-BOOST_PYTHON_MODULE(_stages)
+BOOST_PYTHON_MODULE(@STAGES_LIB_NAME@)
{
bp::class_, boost::noncopyable>
("CurrentState", bp::init>())
diff --git a/core/setup.py b/core/setup.py
new file mode 100644
index 00000000..e49f4dfa
--- /dev/null
+++ b/core/setup.py
@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+ # list of packages to setup
+ packages = ['moveit', 'moveit.python_tools', 'moveit.task_constructor'],
+ # specify location of root ('') package dir
+ package_dir = {'' : 'python/src'}
+)
+setup(**d)
diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt
index fb1e9bef..fde1a84c 100644
--- a/core/src/CMakeLists.txt
+++ b/core/src/CMakeLists.txt
@@ -32,7 +32,7 @@ add_library(${PROJECT_NAME}
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_include_directories(${PROJECT_NAME}
PUBLIC $
- PUBLIC $
+ PRIVATE $
)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt
index ff3e06be..f935bec8 100644
--- a/core/src/stages/CMakeLists.txt
+++ b/core/src/stages/CMakeLists.txt
@@ -31,7 +31,7 @@ add_library(${PROJECT_NAME}_stages
simple_grasp.cpp
pick.cpp
)
-target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_stages PUBLIC ${PROJECT_NAME} ${catkin_LIBRARIES})
install(TARGETS ${PROJECT_NAME}_stages
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}