mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
cleanup folder structure
This commit is contained in:
parent
135c9c2148
commit
f7ddd43d53
@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8.12)
|
|||||||
project(moveit_task_constructor_core)
|
project(moveit_task_constructor_core)
|
||||||
|
|
||||||
find_package(catkin REQUIRED COMPONENTS
|
find_package(catkin REQUIRED COMPONENTS
|
||||||
|
roslint
|
||||||
eigen_conversions
|
eigen_conversions
|
||||||
geometry_msgs
|
geometry_msgs
|
||||||
moveit_core
|
moveit_core
|
||||||
@ -13,6 +14,8 @@ find_package(catkin REQUIRED COMPONENTS
|
|||||||
rviz_marker_tools
|
rviz_marker_tools
|
||||||
)
|
)
|
||||||
|
|
||||||
|
catkin_python_setup()
|
||||||
|
|
||||||
catkin_package(
|
catkin_package(
|
||||||
LIBRARIES
|
LIBRARIES
|
||||||
${PROJECT_NAME}
|
${PROJECT_NAME}
|
||||||
|
|||||||
@ -9,6 +9,7 @@
|
|||||||
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
|
||||||
|
<build_depend>roslint</build_depend>
|
||||||
<build_depend>eigen_conversions</build_depend>
|
<build_depend>eigen_conversions</build_depend>
|
||||||
<build_depend>geometry_msgs</build_depend>
|
<build_depend>geometry_msgs</build_depend>
|
||||||
<build_depend>roscpp</build_depend>
|
<build_depend>roscpp</build_depend>
|
||||||
|
|||||||
@ -1,25 +1,10 @@
|
|||||||
find_package(PythonLibs REQUIRED)
|
# C++ wrapper code
|
||||||
find_package(Boost REQUIRED COMPONENTS python)
|
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
|
roslint_python()
|
||||||
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}
|
|
||||||
)
|
|
||||||
|
|||||||
0
core/python/src/moveit/__init__.py
Normal file
0
core/python/src/moveit/__init__.py
Normal file
1
core/python/src/moveit/python_tools/__init__.py
Normal file
1
core/python/src/moveit/python_tools/__init__.py
Normal file
@ -0,0 +1 @@
|
|||||||
|
from _moveit_python_tools import *
|
||||||
3
core/python/src/moveit/task_constructor/__init__.py
Normal file
3
core/python/src/moveit/task_constructor/__init__.py
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
import moveit.python_tools
|
||||||
|
from _moveit_task_constructor_core_core import *
|
||||||
|
from _moveit_task_constructor_core_stages import *
|
||||||
0
core/python/test/CMakeLists.txt
Normal file
0
core/python/test/CMakeLists.txt
Normal file
27
core/python/test/test_mtc.py
Normal file
27
core/python/test/test_mtc.py
Normal file
@ -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()
|
||||||
@ -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()
|
|
||||||
68
core/python/wrapper/CMakeLists.txt
Normal file
68
core/python/wrapper/CMakeLists.txt
Normal file
@ -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 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
)
|
||||||
|
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 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
)
|
||||||
|
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}
|
||||||
|
)
|
||||||
@ -0,0 +1,29 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <memory>
|
||||||
|
#include <boost/python.hpp>
|
||||||
|
#include <boost/thread/mutex.hpp>
|
||||||
|
#include <ros/init.h>
|
||||||
|
|
||||||
|
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<InitProxy> singleton_instance;
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::unique_ptr<ros::AsyncSpinner> spinner;
|
||||||
|
};
|
||||||
|
|
||||||
|
} }
|
||||||
@ -1,4 +1,4 @@
|
|||||||
#include "conversions.h"
|
#include <moveit/python/python_tools/conversions.h>
|
||||||
#include <ros/serialization.h>
|
#include <ros/serialization.h>
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
@ -9,7 +9,6 @@ using namespace moveit::task_constructor;
|
|||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace python {
|
namespace python {
|
||||||
|
|
||||||
void export_ros_init();
|
|
||||||
void export_properties();
|
void export_properties();
|
||||||
void export_solvers();
|
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_properties();
|
||||||
moveit::python::export_solvers();
|
moveit::python::export_solvers();
|
||||||
moveit::python::export_core();
|
moveit::python::export_core();
|
||||||
@ -1,5 +1,5 @@
|
|||||||
#include "properties.h"
|
#include <moveit/python/python_tools/conversions.h>
|
||||||
#include "conversions.h"
|
#include <moveit/python/task_constructor/properties.h>
|
||||||
|
|
||||||
#include <geometry_msgs/PoseStamped.h>
|
#include <geometry_msgs/PoseStamped.h>
|
||||||
|
|
||||||
15
core/python/wrapper/src/python_tools.cpp.in
Normal file
15
core/python/wrapper/src/python_tools.cpp.in
Normal file
@ -0,0 +1,15 @@
|
|||||||
|
#include <boost/python.hpp>
|
||||||
|
|
||||||
|
namespace bp = boost::python;
|
||||||
|
|
||||||
|
namespace moveit {
|
||||||
|
namespace python {
|
||||||
|
|
||||||
|
void export_ros_init();
|
||||||
|
|
||||||
|
} }
|
||||||
|
|
||||||
|
BOOST_PYTHON_MODULE(@TOOLS_LIB_NAME@)
|
||||||
|
{
|
||||||
|
moveit::python::export_ros_init();
|
||||||
|
}
|
||||||
@ -3,28 +3,12 @@
|
|||||||
#include <boost/thread/mutex.hpp>
|
#include <boost/thread/mutex.hpp>
|
||||||
#include <ros/init.h>
|
#include <ros/init.h>
|
||||||
|
|
||||||
#include "conversions.h"
|
#include <moveit/python/python_tools/ros_init.h>
|
||||||
|
#include <moveit/python/python_tools/conversions.h>
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace python {
|
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<InitProxy> singleton_instance;
|
|
||||||
|
|
||||||
private:
|
|
||||||
std::unique_ptr<ros::AsyncSpinner> spinner;
|
|
||||||
};
|
|
||||||
boost::mutex InitProxy::lock;
|
boost::mutex InitProxy::lock;
|
||||||
std::unique_ptr<InitProxy> InitProxy::singleton_instance;
|
std::unique_ptr<InitProxy> InitProxy::singleton_instance;
|
||||||
|
|
||||||
@ -1,6 +1,6 @@
|
|||||||
#include <boost/python.hpp>
|
#include <boost/python.hpp>
|
||||||
|
|
||||||
#include "properties.h"
|
#include <moveit/python/task_constructor/properties.h>
|
||||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||||
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||||
|
|
||||||
@ -23,7 +23,7 @@ Connect* initConnect(const std::string& name, const bp::list& l) {
|
|||||||
|
|
||||||
} // anonymous namespace
|
} // anonymous namespace
|
||||||
|
|
||||||
BOOST_PYTHON_MODULE(_stages)
|
BOOST_PYTHON_MODULE(@STAGES_LIB_NAME@)
|
||||||
{
|
{
|
||||||
bp::class_<CurrentState, bp::bases<Stage>, boost::noncopyable>
|
bp::class_<CurrentState, bp::bases<Stage>, boost::noncopyable>
|
||||||
("CurrentState", bp::init<bp::optional<const std::string&>>())
|
("CurrentState", bp::init<bp::optional<const std::string&>>())
|
||||||
12
core/setup.py
Normal file
12
core/setup.py
Normal file
@ -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)
|
||||||
@ -32,7 +32,7 @@ add_library(${PROJECT_NAME}
|
|||||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||||
target_include_directories(${PROJECT_NAME}
|
target_include_directories(${PROJECT_NAME}
|
||||||
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
|
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
|
||||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
||||||
)
|
)
|
||||||
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
||||||
|
|
||||||
|
|||||||
@ -31,7 +31,7 @@ add_library(${PROJECT_NAME}_stages
|
|||||||
simple_grasp.cpp
|
simple_grasp.cpp
|
||||||
pick.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
|
install(TARGETS ${PROJECT_NAME}_stages
|
||||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user