From b2adcf02475bb28201d3fa6f2624666868077fd7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 Jul 2020 15:09:09 +0200 Subject: [PATCH] Python3 compatibility --- core/include/moveit/python/python_tools/ros_types.h | 2 +- core/package.xml | 4 +++- core/python/src/moveit/python_tools/__init__.py | 2 +- core/python/src/moveit/task_constructor/__init__.py | 2 -- core/python/src/moveit/task_constructor/core.py | 2 +- core/python/src/moveit/task_constructor/stages.py | 3 ++- core/python/test/rostest_mtc.py | 6 ++---- core/python/wrapper/src/core.cpp | 2 +- core/python/wrapper/src/properties.cpp | 11 ++++++++++- core/python/wrapper/src/ros_types.cpp | 2 +- core/setup.py | 2 +- 11 files changed, 23 insertions(+), 15 deletions(-) diff --git a/core/include/moveit/python/python_tools/ros_types.h b/core/include/moveit/python/python_tools/ros_types.h index 0ebcd85b..96ffb7a5 100644 --- a/core/include/moveit/python/python_tools/ros_types.h +++ b/core/include/moveit/python/python_tools/ros_types.h @@ -69,7 +69,7 @@ struct type_caster::value>> if (!moveit::python::convertible(src, ros::message_traits::DataType::value())) return false; // serialize src into (python) buffer - object pstream = module::import("StringIO").attr("StringIO")(); + object pstream = module::import("io").attr("BytesIO")(); src.attr("serialize")(pstream); object pbuffer = pstream.attr("getvalue")(); // deserialize C++ type from buffer diff --git a/core/package.xml b/core/package.xml index bb6e89b0..41dfc7c1 100644 --- a/core/package.xml +++ b/core/package.xml @@ -1,4 +1,4 @@ - + moveit_task_constructor_core 0.0.0 MoveIt Task Pipeline @@ -8,6 +8,8 @@ Robert Haschke catkin + python-setuptools + python3-setuptools eigen_conversions geometry_msgs diff --git a/core/python/src/moveit/python_tools/__init__.py b/core/python/src/moveit/python_tools/__init__.py index 940f46ad..c8118af5 100644 --- a/core/python/src/moveit/python_tools/__init__.py +++ b/core/python/src/moveit/python_tools/__init__.py @@ -1 +1 @@ -from _python_tools import * +from ._python_tools import * diff --git a/core/python/src/moveit/task_constructor/__init__.py b/core/python/src/moveit/task_constructor/__init__.py index 19c0f243..e69de29b 100644 --- a/core/python/src/moveit/task_constructor/__init__.py +++ b/core/python/src/moveit/task_constructor/__init__.py @@ -1,2 +0,0 @@ -from core import * -import stages diff --git a/core/python/src/moveit/task_constructor/core.py b/core/python/src/moveit/task_constructor/core.py index 65463a62..6eb86461 100644 --- a/core/python/src/moveit/task_constructor/core.py +++ b/core/python/src/moveit/task_constructor/core.py @@ -1 +1 @@ -from _core import * +from ._core import * diff --git a/core/python/src/moveit/task_constructor/stages.py b/core/python/src/moveit/task_constructor/stages.py index 56b4ce55..5e1bf68f 100644 --- a/core/python/src/moveit/task_constructor/stages.py +++ b/core/python/src/moveit/task_constructor/stages.py @@ -1 +1,2 @@ -from _stages import * +from ._core import Stage +from ._stages import * diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py index 18f4c161..2f1adb1f 100755 --- a/core/python/test/rostest_mtc.py +++ b/core/python/test/rostest_mtc.py @@ -1,6 +1,6 @@ #! /usr/bin/env python -# -*- coding: utf-8 -*- +from __future__ import print_function import unittest import rostest from moveit.python_tools import roscpp_init @@ -36,7 +36,7 @@ class Test(unittest.TestCase): self.assertEqual(len(task.solutions), 1) for s in task.solutions: - print s + print(s) s = task.solutions[0] task.execute(s) @@ -66,8 +66,6 @@ class Test(unittest.TestCase): if task.plan(): task.publish(task.solutions[0]) - rospy.sleep(100) - if __name__ == '__main__': roscpp_init("test_mtc") diff --git a/core/python/wrapper/src/core.cpp b/core/python/wrapper/src/core.cpp index 50ae017f..23cad504 100644 --- a/core/python/wrapper/src/core.cpp +++ b/core/python/wrapper/src/core.cpp @@ -46,7 +46,7 @@ void setForwardedProperties(Stage& self, const py::object& names) { std::set s; try { // handle string argument as single name - if (PyString_Check(names.ptr())) + if (PyBytes_Check(names.ptr())) s.emplace(names.cast()); else // expect iterable otherwise for (auto item : names) diff --git a/core/python/wrapper/src/properties.cpp b/core/python/wrapper/src/properties.cpp index 35aef78d..2531826f 100644 --- a/core/python/wrapper/src/properties.cpp +++ b/core/python/wrapper/src/properties.cpp @@ -94,12 +94,21 @@ boost::any PropertyConverterRegistry::fromPython(const py::object& po) { if (PyBool_Check(o)) return (o == Py_True); +#if PY_MAJOR_VERSION >= 3 + if (PyLong_Check(o)) + return PyLong_AS_LONG(o); +#else if (PyInt_Check(o)) return PyInt_AS_LONG(o); +#endif if (PyFloat_Check(o)) return PyFloat_AS_DOUBLE(o); +#if PY_MAJOR_VERSION >= 3 + if (PyUnicode_Check(o)) +#else if (PyString_Check(o)) - return std::string(PyString_AS_STRING(o)); +#endif + return py::cast(o); const std::string& ros_msg_name = rosMsgName(o); auto it = registry_singleton_.msg_names_.find(ros_msg_name); diff --git a/core/python/wrapper/src/ros_types.cpp b/core/python/wrapper/src/ros_types.cpp index 08a4c80f..e52ed0c7 100644 --- a/core/python/wrapper/src/ros_types.cpp +++ b/core/python/wrapper/src/ros_types.cpp @@ -18,7 +18,7 @@ py::object createMessage(const std::string& ros_msg_name) { bool convertible(const pybind11::handle& h, const char* ros_msg_name) { try { PyObject* o = h.attr("_type").ptr(); - return PyString_Check(o) && strcmp(PyString_AS_STRING(o), ros_msg_name) == 0; + return py::cast(o) == ros_msg_name; } catch (const std::exception& e) { return false; } diff --git a/core/setup.py b/core/setup.py index e49f4dfa..adab9f74 100644 --- a/core/setup.py +++ b/core/setup.py @@ -1,6 +1,6 @@ #!/usr/bin/env python -from distutils.core import setup +from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup(