From f7ddd43d53e67d89c41a79510260d7697a304634 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 12 May 2018 17:16:05 +0200 Subject: [PATCH] cleanup folder structure --- core/CMakeLists.txt | 3 + core/package.xml | 1 + core/python/CMakeLists.txt | 31 +++------ core/python/src/moveit/__init__.py | 0 .../src/moveit/python_tools/__init__.py | 1 + .../src/moveit/task_constructor/__init__.py | 3 + core/python/test/CMakeLists.txt | 0 core/python/test/test_mtc.py | 27 ++++++++ core/python/test_script.py | 19 ------ core/python/wrapper/CMakeLists.txt | 68 +++++++++++++++++++ .../moveit/python/python_tools}/conversions.h | 0 .../moveit/python/python_tools/ros_init.h | 29 ++++++++ .../python/task_constructor}/properties.h | 0 core/python/{ => wrapper/src}/conversions.cpp | 2 +- .../{core.cpp => wrapper/src/core.cpp.in} | 4 +- core/python/{ => wrapper/src}/properties.cpp | 4 +- core/python/wrapper/src/python_tools.cpp.in | 15 ++++ core/python/{ => wrapper/src}/ros_init.cpp | 20 +----- core/python/{ => wrapper/src}/solvers.cpp | 2 +- .../{stages.cpp => wrapper/src/stages.cpp.in} | 2 +- core/setup.py | 12 ++++ core/src/CMakeLists.txt | 2 +- core/src/stages/CMakeLists.txt | 2 +- 23 files changed, 177 insertions(+), 70 deletions(-) create mode 100644 core/python/src/moveit/__init__.py create mode 100644 core/python/src/moveit/python_tools/__init__.py create mode 100644 core/python/src/moveit/task_constructor/__init__.py create mode 100644 core/python/test/CMakeLists.txt create mode 100644 core/python/test/test_mtc.py delete mode 100644 core/python/test_script.py create mode 100644 core/python/wrapper/CMakeLists.txt rename core/python/{ => wrapper/include/moveit/python/python_tools}/conversions.h (100%) create mode 100644 core/python/wrapper/include/moveit/python/python_tools/ros_init.h rename core/python/{ => wrapper/include/moveit/python/task_constructor}/properties.h (100%) rename core/python/{ => wrapper/src}/conversions.cpp (95%) rename core/python/{core.cpp => wrapper/src/core.cpp.in} (95%) rename core/python/{ => wrapper/src}/properties.cpp (95%) create mode 100644 core/python/wrapper/src/python_tools.cpp.in rename core/python/{ => wrapper/src}/ros_init.cpp (71%) rename core/python/{ => wrapper/src}/solvers.cpp (94%) rename core/python/{stages.cpp => wrapper/src/stages.cpp.in} (97%) create mode 100644 core/setup.py 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}