Reenable python bindings

Author: rhaschke

core/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(moveit_task_constructor_msgs REQUIRED)
+find_package(py_binding_tools REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rviz_marker_tools REQUIRED)
 find_package(tf2_eigen REQUIRED)
@@ -26,8 +27,8 @@ add_compile_options(-fvisibility-inlines-hidden)
 set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
 
 add_subdirectory(src)
-# TODO: enable python wrapper
-#add_subdirectory(python)
+ament_python_install_package(moveit PACKAGE_DIR python/moveit)
+add_subdirectory(python)
 add_subdirectory(test)
 
 install(DIRECTORY include/ DESTINATION include
@@ -42,6 +43,7 @@ ament_export_dependencies(moveit_core)
 ament_export_dependencies(moveit_ros_planning)
 ament_export_dependencies(moveit_ros_planning_interface)
 ament_export_dependencies(moveit_task_constructor_msgs)
+ament_export_dependencies(py_binding_tools)
 ament_export_dependencies(rclcpp)
 ament_export_dependencies(rviz_marker_tools)
 ament_export_dependencies(tf2_eigen)

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

@@ -35,12 +35,12 @@ class PropertyConverter : protected PropertyConverterBase
 	static boost::any fromPython(const pybind11::object& po) { return pybind11::cast<T>(po); }
 
 	template <class Q = T>
-	typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type rosMsgName() {
-		return ros::message_traits::DataType<T>::value();
+	typename std::enable_if<rosidl_generator_traits::is_message<Q>::value, std::string>::type rosMsgName() {
+		return rosidl_generator_traits::name<Q>();
 	}
 
 	template <class Q = T>
-	typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type rosMsgName() {
+	typename std::enable_if<!rosidl_generator_traits::is_message<Q>::value, std::string>::type rosMsgName() {
 		return std::string();
 	}
 };

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

@@ -20,6 +20,7 @@
 	<depend>moveit_ros_planning</depend>
 	<depend>moveit_ros_planning_interface</depend>
 	<depend>moveit_task_constructor_msgs</depend>
+	<depend>py_binding_tools</depend>
 	<depend>rclcpp</depend>
 	<depend>rviz_marker_tools</depend>
 	<depend>tf2_eigen</depend>

core/package.xml

@@ -24,7 +24,6 @@ add_subdirectory(pybind11)
 # C++ wrapper code
 add_subdirectory(bindings)
 
-ament_python_install_package("src")
 
 if(BUILD_TESTING)
 	find_package(ament_cmake_pytest REQUIRED)

core/python/CMakeLists.txt

@@ -1,27 +1,3 @@
-# python tools support lib
-set(TOOLS_LIB_NAME moveit_python_tools)
-set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/python_tools)
-add_library(${TOOLS_LIB_NAME} SHARED
-	${INCLUDES}/ros_init.h
-	${INCLUDES}/ros_types.h
-	src/ros_init.cpp
-	src/ros_types.cpp
-)
-target_include_directories(${TOOLS_LIB_NAME}
-	PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
-	PUBLIC $<INSTALL_INTERFACE:include>
-)
-target_link_libraries(${TOOLS_LIB_NAME} PUBLIC pybind11::pybind11 ${Boost_LIBRARIES} rclcpp::rclcpp)
-
-install(TARGETS ${TOOLS_LIB_NAME}
-	ARCHIVE DESTINATION lib
-	LIBRARY DESTINATION lib
-)
-
-# moveit.python_tools
-pybind11_add_module(pymoveit_python_tools src/python_tools.cpp)
-target_link_libraries(pymoveit_python_tools PRIVATE ${TOOLS_LIB_NAME})
-
 # moveit.task_constructor
 set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor)
 pybind11_add_module(pymoveit_mtc
@@ -34,9 +10,9 @@ pybind11_add_module(pymoveit_mtc
 	src/module.cpp
 )
 target_include_directories(pymoveit_mtc PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>)
-target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages ${TOOLS_LIB_NAME})
+target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages py_binding_tools::py_binding_tools)
 
 # install python libs
-install(TARGETS pymoveit_python_tools pymoveit_mtc
-	LIBRARY DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
+install(TARGETS pymoveit_mtc
+	LIBRARY DESTINATION "${PYTHON_INSTALL_DIR}"
 )

core/python/bindings/CMakeLists.txt

@@ -412,7 +412,7 @@ void export_core(pybind11::module& m) {
 	    .def_property_readonly("failures", &Task::failures, "Solutions: Failed Solutions of the stage (read-only)")
 	    .def_property("name", &Task::name, &Task::setName, "str: name of the task displayed e.g. in rviz")
 
-	    .def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description",
+	    .def("loadRobotModel", &Task::loadRobotModel, "node"_a, "robot_description"_a = "robot_description",
 	         "Load robot model from given ROS parameter")
 	    .def("getRobotModel", &Task::getRobotModel)
 	    .def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true,
@@ -471,30 +471,7 @@ void export_core(pybind11::module& m) {
 	        "publish",
 	        [](Task& self, const SolutionBasePtr& solution) { self.introspection().publishSolution(*solution); },
 	        "solution"_a, "Publish the given solution to the ROS topic ``solution``")
-	    .def_static(
-	        "execute",
-	        [](const SolutionBasePtr& solution) {
-		        using namespace moveit::planning_interface;
-		        PlanningSceneInterface psi;
-		        MoveGroupInterface mgi(solution->start()->scene()->getRobotModel()->getJointModelGroupNames()[0]);
-
-		        MoveGroupInterface::Plan plan;
-		        moveit_task_constructor_msgs::msg::Solution serialized;
-		        solution->toMsg(serialized);
-
-		        for (const moveit_task_constructor_msgs::msg::SubTrajectory& traj : serialized.sub_trajectory) {
-			        if (!traj.trajectory.joint_trajectory.points.empty()) {
-				        plan.trajectory_ = traj.trajectory;
-				        if (!mgi.execute(plan)) {
-					        ROS_ERROR("Execution failed! Aborting!");
-					        return;
-				        }
-			        }
-			        psi.applyPlanningScene(traj.scene_diff);
-		        }
-		        ROS_INFO("Executed successfully");
-	        },
-	        "solution"_a, "Send given solution to ``move_group`` node for execution");
+	    .def("execute", &Task::execute, "solution"_a, "Send given solution to ``move_group`` node for execution");
 }
 }  // namespace python
 }  // namespace moveit

core/python/bindings/src/core.cpp

@@ -119,12 +119,15 @@ py::object PropertyConverterRegistry::toPython(const boost::any& value) {
 
 std::string rosMsgName(PyObject* object) {
 	py::object o = py::reinterpret_borrow<py::object>(object);
-	try {
-		return o.attr("_type").cast<std::string>();
-	} catch (const py::error_already_set& e) {
+	auto cls = o.attr("__class__");
+	auto name = cls.attr("__name__").cast<std::string>();
+	auto module = cls.attr("__module__").cast<std::string>();
+	auto pos = module.find(".msg");
+	if (pos == std::string::npos)
 		// object is not a ROS message type, return it's class name instead
-		return o.attr("__class__").attr("__name__").cast<std::string>();
-	}
+		return module + "." + name;
+	else
+		return module.substr(0, pos) + "/msg/" + name;
 }
 
 boost::any PropertyConverterRegistry::fromPython(const py::object& po) {

core/python/bindings/src/properties.cpp

@@ -37,7 +37,7 @@
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
 #include <moveit/task_constructor/solvers/multi_planner.h>
-#include <moveit_msgs/WorkspaceParameters.h>
+#include <moveit_msgs/msg/workspace_parameters.hpp>
 #include "utils.h"
 
 namespace py = pybind11;
@@ -69,19 +69,18 @@ void export_solvers(py::module& m) {
 				from moveit.task_constructor import core
 
 				# Create and configure a planner instance
-				pipelinePlanner = core.PipelinePlanner()
-				pipelinePlanner.planner = 'PRMkConfigDefault'
+				pipelinePlanner = core.PipelinePlanner(node, 'ompl', 'PRMkConfigDefault')
 				pipelinePlanner.num_planning_attempts = 10
 			)")
-	    .property<std::string>("planner", "str: Planner ID")
 	    .property<uint>("num_planning_attempts", "int: Number of planning attempts")
 	    .property<moveit_msgs::msg::WorkspaceParameters>(
 	        "workspace_parameters",
 	        ":moveit_msgs:`WorkspaceParameters`: Specifies workspace box to be used for Cartesian sampling")
 	    .property<double>("goal_joint_tolerance", "float: Tolerance for reaching joint goals")
 	    .property<double>("goal_position_tolerance", "float: Tolerance for reaching position goals")
 	    .property<double>("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals")
-	    .def(py::init<const std::string&>(), "pipeline"_a = std::string("ompl"));
+	    .def(py::init<const rclcpp::Node::SharedPtr&, const std::string&, const std::string&>(), "node"_a,
+	         "pipeline"_a = std::string("ompl"), "planner_id"_a = std::string(""));
 
 	properties::class_<JointInterpolationPlanner, PlannerInterface>(
 	    m, "JointInterpolationPlanner",

core/python/bindings/src/solvers.cpp

@@ -38,7 +38,7 @@
 #include <moveit/task_constructor/stages/pick.h>
 #include <moveit/task_constructor/stages/simple_grasp.h>
 #include <moveit/planning_scene/planning_scene.h>
-#include <moveit_msgs/PlanningScene.h>
+#include <moveit_msgs/msg/planning_scene.hpp>
 #include <pybind11/stl.h>
 #include <py_binding_tools/ros_msg_typecasters.h>
 
@@ -357,7 +357,7 @@ void export_stages(pybind11::module& m) {
 			str: Name of the object in the planning scene, attached to the robot which should be placed
 		)")
 		.property<std::string>("eef", "str: Name of the end effector that should be used for grasping")
-		.property<geometry_msgs::msg::msg::PoseStamped>("pose", R"(
+		.property<geometry_msgs::msg::PoseStamped>("pose", R"(
 			PoseStamped_: The pose where the object should be placed, i.e. states should be sampled
 
 			.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
@@ -393,7 +393,7 @@ void export_stages(pybind11::module& m) {
 			for an implementation of a task hierarchy that makes use of the
 			``GeneratePose`` stage.
 		)")
-	    .property<geometry_msgs::msg::msg::msg::PoseStamped>("pose", R"(
+	    .property<geometry_msgs::msg::PoseStamped>("pose", R"(
 			PoseStamped_: Set the pose, which should be spawned on each new solution of the monitored stage.
 
 			.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html