From 3770c10349cb14ea2d9bc038b0ad4d68669872bd Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Mon, 10 Jul 2017 16:45:03 +0200 Subject: [PATCH 01/39] Cython wrapper for Time and TransformWithCovariance --- bindings/python/_basetypes.pxd | 35 +++++++++++ bindings/python/basetypes.pyx | 112 +++++++++++++++++++++++++++++++++ bindings/python/setup.py | 50 +++++++++++++++ bindings/python/test.py | 37 +++++++++++ 4 files changed, 234 insertions(+) create mode 100644 bindings/python/_basetypes.pxd create mode 100644 bindings/python/basetypes.pyx create mode 100644 bindings/python/setup.py create mode 100644 bindings/python/test.py diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd new file mode 100644 index 00000000..111da87a --- /dev/null +++ b/bindings/python/_basetypes.pxd @@ -0,0 +1,35 @@ +from libcpp.string cimport string + + +cdef extern from "base/Time.hpp" namespace "base": + cdef cppclass Time: + Time() + int microseconds + + +cdef extern from "base/Eigen.hpp" namespace "base": + cdef cppclass Vector3d: + Vector3d() + Vector3d(double, double, double) + Vector3d(Vector3d&) + double* data() + int rows() + double& get "operator()"(int rows) + double x() + double y() + double z() + cdef cppclass Quaterniond: + Quaterniond() + Quaterniond(double, double, double, double) + double w() + double x() + double y() + double z() + + +cdef extern from "base/TransformWithCovariance.hpp" namespace "base": + cdef cppclass TransformWithCovariance: + TransformWithCovariance() + Vector3d translation + Quaterniond orientation + diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx new file mode 100644 index 00000000..837f2d8c --- /dev/null +++ b/bindings/python/basetypes.pyx @@ -0,0 +1,112 @@ +# distutils: language = c++ +cimport _basetypes +from cython.operator cimport dereference as deref +from libcpp.string cimport string + + +cdef class PyTime: + cdef _basetypes.Time* thisptr + + def __cinit__(self): + self.thisptr = NULL + + def __dealloc__(self): + if self.thisptr != NULL: + del self.thisptr + + def __init__(self): + self.thisptr = new _basetypes.Time() + + def _get_microseconds(self): + return self.thisptr.microseconds + + def _set_microseconds(self, int microseconds): + self.thisptr.microseconds = microseconds + + microseconds = property(_get_microseconds, _set_microseconds) + + +cdef class PyVector3d: + cdef _basetypes.Vector3d* thisptr + + def __cinit__(self): + self.thisptr = NULL + + def __dealloc__(self): + if self.thisptr != NULL: + del self.thisptr + + def __init__(self, double x=0.0, double y=0.0, double z=0.0): + self.thisptr = new _basetypes.Vector3d(x, y, z) + + def __str__(self): + return "[%.2f, %.2f, %.2f]" % (self.thisptr.get(0), + self.thisptr.get(1), + self.thisptr.get(2)) + + def x(self): + return self.thisptr.x() + + def y(self): + return self.thisptr.y() + + def z(self): + return self.thisptr.z() + + +cdef class PyQuaterniond: + cdef _basetypes.Quaterniond* thisptr + + def __cinit__(self): + self.thisptr = NULL + + def __dealloc__(self): + if self.thisptr != NULL: + del self.thisptr + + def __init__(self, double w=1.0, double x=0.0, double y=0.0, double z=0.0): + self.thisptr = new _basetypes.Quaterniond(w, x, y, z) + + def __str__(self): + return "[im=%.2f, real=(%.2f, %.2f, %.2f)]" % ( + self.thisptr.w(), self.thisptr.x(), self.thisptr.y(), + self.thisptr.z()) + + +cdef class PyTransformWithCovariance: + cdef _basetypes.TransformWithCovariance* thisptr + + def __cinit__(self): + self.thisptr = NULL + + def __dealloc__(self): + if self.thisptr != NULL: + del self.thisptr + + def __init__(self): + self.thisptr = new _basetypes.TransformWithCovariance() + + def _get_translation(self): + translation = PyVector3d() + translation.thisptr[0] = self.thisptr.translation + return translation + + def _set_translation(self, PyVector3d translation): + self.thisptr.translation = deref(translation.thisptr) + + translation = property(_get_translation, _set_translation) + + def _get_orientation(self): + orientation = PyQuaterniond() + orientation.thisptr[0] = self.thisptr.orientation + return orientation + + def _set_orientation(self, PyQuaterniond orientation): + self.thisptr.orientation = deref(orientation.thisptr) + + orientation = property(_get_orientation, _set_orientation) + + def __str__(self): + return "(translation=%s, orientation=%s)" % (self.translation, + self.orientation) + diff --git a/bindings/python/setup.py b/bindings/python/setup.py new file mode 100644 index 00000000..de46612b --- /dev/null +++ b/bindings/python/setup.py @@ -0,0 +1,50 @@ +from distutils.core import setup +from distutils.extension import Extension +from distutils.sysconfig import get_config_vars +from Cython.Build import cythonize +import numpy +import os + + +def strict_prototypes_workaround(): + # Workaround to remove '-Wstrict-prototypes' from compiler invocation + opt = get_config_vars('OPT')[0] + os.environ['OPT'] = " ".join(flag for flag in opt.split() + if flag != '-Wstrict-prototypes') + + +if __name__ == '__main__': + strict_prototypes_workaround() + + extensions = [ + Extension( + "basetypes", + [ + "basetypes.pyx" + ], + include_dirs=[ + ".", + numpy.get_include(), + os.environ.get("AUTOPROJ_CURRENT_ROOT") + "/install/include/", + "/usr/include/eigen3/" # TODO + ], + library_dirs=[ + os.environ.get("AUTOPROJ_CURRENT_ROOT") + "/install/lib/" + ], + libraries=[ + "base-types" + ], + define_macros=[ + ("NDEBUG",), + ], + extra_compile_args=[ + "-std=c++11", + "-O3", + # disable warnings caused by Cython using the deprecated + # NumPy C-API + "-Wno-cpp", "-Wno-unused-function" + ], + language="c++" + ) + ] + setup(ext_modules=cythonize(extensions)) diff --git a/bindings/python/test.py b/bindings/python/test.py new file mode 100644 index 00000000..fd933a10 --- /dev/null +++ b/bindings/python/test.py @@ -0,0 +1,37 @@ +import basetypes +from nose.tools import assert_equal, assert_raises_regexp + + +def test_get_set_microseconds(): + t = basetypes.PyTime() + m = 1000023 + t.microseconds = m + assert_equal(t.microseconds, m) + + +def test_vector3d_ctor(): + v = basetypes.PyVector3d(1.0, 2.0, 3.0) + assert_equal(str(v), "[1.00, 2.00, 3.00]") + + +def test_quaterniond_ctor(): + q = basetypes.PyQuaterniond(1.0, 0.0, 0.0, 0.0) + assert_equal(str(q), "[im=1.00, real=(0.00, 0.00, 0.00)]") + + +def test_transform_with_cov_ctor(): + basetypes.PyTransformWithCovariance() + + +def test_transform_set_get_translation(): + p = basetypes.PyTransformWithCovariance() + t = basetypes.PyVector3d(1.0, 2.0, 3.0) + p.translation = t + assert_equal(str(p.translation), "[1.00, 2.00, 3.00]") + + +def test_transform_set_get_orientation(): + p = basetypes.PyTransformWithCovariance() + o = basetypes.PyQuaterniond(1.0, 0.0, 0.0, 0.0) + p.orientation = o + assert_equal(str(p.orientation), "[im=1.00, real=(0.00, 0.00, 0.00)]") From 662f7290a5feb2c93d7c046d3e0c033dea91cc06 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Mon, 10 Jul 2017 16:59:54 +0200 Subject: [PATCH 02/39] Don't copy data --- bindings/python/basetypes.pyx | 33 +++++++++++++++++++++++++-------- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 837f2d8c..18e5710e 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -2,20 +2,24 @@ cimport _basetypes from cython.operator cimport dereference as deref from libcpp.string cimport string +from libcpp cimport bool cdef class PyTime: cdef _basetypes.Time* thisptr + cdef bool delete_thisptr def __cinit__(self): self.thisptr = NULL + self.delete_thisptr = False def __dealloc__(self): - if self.thisptr != NULL: + if self.thisptr != NULL and self.delete_thisptr: del self.thisptr def __init__(self): self.thisptr = new _basetypes.Time() + self.delete_thisptr = True def _get_microseconds(self): return self.thisptr.microseconds @@ -28,16 +32,19 @@ cdef class PyTime: cdef class PyVector3d: cdef _basetypes.Vector3d* thisptr + cdef bool delete_thisptr def __cinit__(self): self.thisptr = NULL + self.delete_thisptr = False def __dealloc__(self): - if self.thisptr != NULL: + if self.thisptr != NULL and self.delete_thisptr: del self.thisptr def __init__(self, double x=0.0, double y=0.0, double z=0.0): self.thisptr = new _basetypes.Vector3d(x, y, z) + self.delete_thisptr = True def __str__(self): return "[%.2f, %.2f, %.2f]" % (self.thisptr.get(0), @@ -56,16 +63,19 @@ cdef class PyVector3d: cdef class PyQuaterniond: cdef _basetypes.Quaterniond* thisptr + cdef bool delete_thisptr def __cinit__(self): self.thisptr = NULL + self.delete_thisptr = False def __dealloc__(self): - if self.thisptr != NULL: + if self.thisptr != NULL and self.delete_thisptr: del self.thisptr def __init__(self, double w=1.0, double x=0.0, double y=0.0, double z=0.0): self.thisptr = new _basetypes.Quaterniond(w, x, y, z) + self.delete_thisptr = True def __str__(self): return "[im=%.2f, real=(%.2f, %.2f, %.2f)]" % ( @@ -75,20 +85,25 @@ cdef class PyQuaterniond: cdef class PyTransformWithCovariance: cdef _basetypes.TransformWithCovariance* thisptr + cdef bool delete_thisptr def __cinit__(self): self.thisptr = NULL + self.delete_thisptr = False def __dealloc__(self): - if self.thisptr != NULL: + if self.thisptr != NULL and self.delete_thisptr: del self.thisptr def __init__(self): self.thisptr = new _basetypes.TransformWithCovariance() + self.delete_thisptr = True def _get_translation(self): - translation = PyVector3d() - translation.thisptr[0] = self.thisptr.translation + cdef PyVector3d translation = PyVector3d() + del translation.thisptr + translation.delete_thisptr = False + translation.thisptr = &self.thisptr.translation return translation def _set_translation(self, PyVector3d translation): @@ -97,8 +112,10 @@ cdef class PyTransformWithCovariance: translation = property(_get_translation, _set_translation) def _get_orientation(self): - orientation = PyQuaterniond() - orientation.thisptr[0] = self.thisptr.orientation + cdef PyQuaterniond orientation = PyQuaterniond() + del orientation.thisptr + orientation.delete_thisptr = False + orientation.thisptr = &self.thisptr.orientation return orientation def _set_orientation(self, PyQuaterniond orientation): From bf671eb2008184e10dc5c0b6da43c21bc0b02ae0 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Mon, 10 Jul 2017 17:51:05 +0200 Subject: [PATCH 03/39] More vector types --- bindings/python/_basetypes.pxd | 28 +++++++ bindings/python/basetypes.pyx | 137 +++++++++++++++++++++++++++++++-- bindings/python/test.py | 39 +++++++++- 3 files changed, 197 insertions(+), 7 deletions(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index 111da87a..3b82dff7 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -8,6 +8,18 @@ cdef extern from "base/Time.hpp" namespace "base": cdef extern from "base/Eigen.hpp" namespace "base": + cdef cppclass Vector2d: + Vector2d() + Vector2d(double, double) + Vector2d(Vector2d&) + double* data() + int rows() + double& get "operator()"(int rows) + double x() + double y() + double norm() + double squaredNorm() + cdef cppclass Vector3d: Vector3d() Vector3d(double, double, double) @@ -18,6 +30,22 @@ cdef extern from "base/Eigen.hpp" namespace "base": double x() double y() double z() + double norm() + double squaredNorm() + + cdef cppclass Vector4d: + Vector4d() + Vector4d(double, double, double, double) + Vector4d(Vector4d&) + double* data() + int rows() + double& get "operator()"(int rows) + double x() + double y() + double z() + double norm() + double squaredNorm() + cdef cppclass Quaterniond: Quaterniond() Quaterniond(double, double, double, double) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 18e5710e..88df6753 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -30,6 +30,58 @@ cdef class PyTime: microseconds = property(_get_microseconds, _set_microseconds) +cdef class PyVector2d: + cdef _basetypes.Vector2d* thisptr + cdef bool delete_thisptr + + def __cinit__(self): + self.thisptr = NULL + self.delete_thisptr = False + + def __dealloc__(self): + if self.thisptr != NULL and self.delete_thisptr: + del self.thisptr + + def __init__(self, double x=0.0, double y=0.0): + self.thisptr = new _basetypes.Vector2d(x, y) + self.delete_thisptr = True + + def __str__(self): + return "[%.2f, %.2f]" % (self.thisptr.get(0), self.thisptr.get(1)) + + def __getitem__(self, int i): + if i < 0 or i > 1: + raise KeyError("index must be 0 or 1 but was %d" % i) + return self.thisptr.data()[i] + + def __setitem__(self, int i, double v): + if i < 0 or i > 1: + raise KeyError("index must be 0 or 1 but was %d" % i) + self.thisptr.data()[i] = v + + def _get_x(self): + return self.thisptr.data()[0] + + def _set_x(self, double x): + self.thisptr.data()[0] = x + + x = property(_get_x, _set_x) + + def _get_y(self): + return self.thisptr.data()[1] + + def _set_y(self, double y): + self.thisptr.data()[1] = y + + y = property(_get_y, _set_y) + + def norm(self): + return self.thisptr.norm() + + def squared_norm(self): + return self.thisptr.squaredNorm() + + cdef class PyVector3d: cdef _basetypes.Vector3d* thisptr cdef bool delete_thisptr @@ -51,14 +103,87 @@ cdef class PyVector3d: self.thisptr.get(1), self.thisptr.get(2)) - def x(self): - return self.thisptr.x() + def __getitem__(self, int i): + if i < 0 or i > 2: + raise KeyError("index must be 0, 1 or 2 but was %d" % i) + return self.thisptr.data()[i] + + def __setitem__(self, int i, double v): + if i < 0 or i > 2: + raise KeyError("index must be 0, 1 or 2 but was %d" % i) + self.thisptr.data()[i] = v + + def _get_x(self): + return self.thisptr.data()[0] + + def _set_x(self, double x): + self.thisptr.data()[0] = x + + x = property(_get_x, _set_x) + + def _get_y(self): + return self.thisptr.data()[1] + + def _set_y(self, double y): + self.thisptr.data()[1] = y + + y = property(_get_y, _set_y) + + def _get_z(self): + return self.thisptr.data()[2] + + def _set_z(self, double z): + self.thisptr.data()[2] = z + + z = property(_get_z, _set_z) + + def norm(self): + return self.thisptr.norm() + + def squared_norm(self): + return self.thisptr.squaredNorm() + + +cdef class PyVector4d: + cdef _basetypes.Vector4d* thisptr + cdef bool delete_thisptr + + def __cinit__(self): + self.thisptr = NULL + self.delete_thisptr = False + + def __dealloc__(self): + if self.thisptr != NULL and self.delete_thisptr: + del self.thisptr + + def __init__(self, v0, v1, v2, v3): + self.thisptr = new _basetypes.Vector4d(v0, v1, v2, v3) + self.delete_thisptr = True + + def __str__(self): + return "[%.2f, %.2f, %.2f, %.2f]" % (self.thisptr.get(0), + self.thisptr.get(1), + self.thisptr.get(2), + self.thisptr.get(3)) + + def __getitem__(self, int i): + if i < 0 or i > 3: + raise KeyError("index must be in [0, 3] but was %d" % i) + return self.thisptr.data()[i] + + def __setitem__(self, int i, double v): + if i < 0 or i > 3: + raise KeyError("index must be in [0, 3] but was %d" % i) + self.thisptr.data()[i] = v + + def norm(self): + return self.thisptr.norm() + + def squared_norm(self): + return self.thisptr.squaredNorm() - def y(self): - return self.thisptr.y() - def z(self): - return self.thisptr.z() +# TODO Vector6d, VectorXd cdef class PyQuaterniond: diff --git a/bindings/python/test.py b/bindings/python/test.py index fd933a10..df6f0d5f 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -1,5 +1,5 @@ import basetypes -from nose.tools import assert_equal, assert_raises_regexp +from nose.tools import assert_equal, assert_raises_regexp, assert_almost_equal def test_get_set_microseconds(): @@ -9,11 +9,48 @@ def test_get_set_microseconds(): assert_equal(t.microseconds, m) +def test_vector2d_ctor(): + v = basetypes.PyVector2d(1.0, 2.0) + assert_equal(str(v), "[1.00, 2.00]") + + def test_vector3d_ctor(): v = basetypes.PyVector3d(1.0, 2.0, 3.0) assert_equal(str(v), "[1.00, 2.00, 3.00]") +def test_vector4d_ctor(): + v = basetypes.PyVector4d(1.0, 2.0, 3.0, 4.0) + assert_equal(str(v), "[1.00, 2.00, 3.00, 4.00]") + + +def test_vector3d_get_set_data(): + v = basetypes.PyVector3d(1.0, 2.0, 3.0) + v.x = 5.0 + v.y = 6.0 + v.z = 7.0 + assert_equal(v.x, 5.0) + assert_equal(v.y, 6.0) + assert_equal(v.z, 7.0) + + +def test_vector3d_array_access(): + v = basetypes.PyVector3d(1.0, 2.0, 3.0) + assert_equal(v[0], 1.0) + v[1] = 4.0 + assert_equal(v[1], 4.0) + assert_raises_regexp(KeyError, "index must be", lambda i: v[i], -1) + def assign(i): + v[i] = 5.0 + assert_raises_regexp(KeyError, "index must be", assign, 3) + + +def test_norms(): + v = basetypes.PyVector3d(1.0, 2.0, 3.0) + assert_almost_equal(v.norm(), 3.741657387) + assert_equal(v.squared_norm(), 14.0) + + def test_quaterniond_ctor(): q = basetypes.PyQuaterniond(1.0, 0.0, 0.0, 0.0) assert_equal(str(q), "[im=1.00, real=(0.00, 0.00, 0.00)]") From b390ae6f1c6b7f194507dffe7a0272d7c4c60183 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Mon, 10 Jul 2017 18:18:07 +0200 Subject: [PATCH 04/39] JointState --- bindings/python/_basetypes.pxd | 17 +++++++ bindings/python/basetypes.pyx | 86 ++++++++++++++++++++++++++++++++++ bindings/python/test.py | 43 +++++++++++++++++ 3 files changed, 146 insertions(+) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index 3b82dff7..7feb1473 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -61,3 +61,20 @@ cdef extern from "base/TransformWithCovariance.hpp" namespace "base": Vector3d translation Quaterniond orientation + +cdef extern from "base/JointState.hpp" namespace "base": + cdef cppclass JointState: + JointState() + double position + double speed + double effort + double raw + double acceleration + + +cdef extern from "base/JointState.hpp" namespace "base::JointState": + JointState Position(double) + JointState Speed(double) + JointState Effort(double) + JointState Raw(double) + JointState Acceleration(double) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 88df6753..8f21e9bd 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -252,3 +252,89 @@ cdef class PyTransformWithCovariance: return "(translation=%s, orientation=%s)" % (self.translation, self.orientation) + +cdef class PyJointState: + cdef _basetypes.JointState* thisptr + cdef bool delete_thisptr + + def __cinit__(self): + self.thisptr = NULL + self.delete_thisptr = False + + def __dealloc__(self): + if self.thisptr != NULL and self.delete_thisptr: + del self.thisptr + + def __init__(self): + self.thisptr = new _basetypes.JointState() + self.delete_thisptr = True + + def _get_position(self): + return self.thisptr.position + + def _set_position(self, double value): + self.thisptr.position = value + + position = property(_get_position, _set_position) + + def _get_speed(self): + return self.thisptr.speed + + def _set_speed(self, double value): + self.thisptr.speed = value + + speed = property(_get_speed, _set_speed) + + def _get_effort(self): + return self.thisptr.effort + + def _set_effort(self, double value): + self.thisptr.effort = value + + effort = property(_get_effort, _set_effort) + + def _get_raw(self): + return self.thisptr.raw + + def _set_raw(self, double value): + self.thisptr.raw = value + + raw = property(_get_raw, _set_raw) + + def _get_acceleration(self): + return self.thisptr.acceleration + + def _set_acceleration(self, double value): + self.thisptr.acceleration = value + + acceleration = property(_get_acceleration, _set_acceleration) + + @staticmethod + def Position(double value): + cdef PyJointState self = PyJointState() + self.thisptr[0] = _basetypes.Position(value) + return self + + @staticmethod + def Speed(double value): + cdef PyJointState self = PyJointState() + self.thisptr[0] = _basetypes.Speed(value) + return self + + @staticmethod + def Effort(double value): + cdef PyJointState self = PyJointState() + self.thisptr[0] = _basetypes.Effort(value) + return self + + @staticmethod + def Raw(double value): + cdef PyJointState self = PyJointState() + self.thisptr[0] = _basetypes.Raw(value) + return self + + @staticmethod + def Acceleration(double value): + cdef PyJointState self = PyJointState() + self.thisptr[0] = _basetypes.Acceleration(value) + return self diff --git a/bindings/python/test.py b/bindings/python/test.py index df6f0d5f..da119500 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -72,3 +72,46 @@ def test_transform_set_get_orientation(): o = basetypes.PyQuaterniond(1.0, 0.0, 0.0, 0.0) p.orientation = o assert_equal(str(p.orientation), "[im=1.00, real=(0.00, 0.00, 0.00)]") + + +def test_joint_state_get_set_position(): + js = basetypes.PyJointState() + js.position = 5.0 + assert_equal(js.position, 5.0) + + +def test_joint_state_get_set_speed(): + js = basetypes.PyJointState() + js.speed = 5.0 + assert_equal(js.speed, 5.0) + + +def test_joint_state_get_set_effort(): + js = basetypes.PyJointState() + js.effort = 5.0 + assert_equal(js.effort, 5.0) + + +def test_joint_state_get_set_raw(): + js = basetypes.PyJointState() + js.raw = 5.0 + assert_equal(js.raw, 5.0) + + +def test_joint_state_get_set_acceleration(): + js = basetypes.PyJointState() + js.acceleration = 5.0 + assert_equal(js.acceleration, 5.0) + + +def test_joint_state_factories(): + js = basetypes.PyJointState.Position(5.0) + assert_equal(js.position, 5.0) + js = basetypes.PyJointState.Speed(5.0) + assert_equal(js.speed, 5.0) + js = basetypes.PyJointState.Effort(5.0) + assert_equal(js.effort, 5.0) + js = basetypes.PyJointState.Raw(5.0) + assert_equal(js.raw, 5.0) + js = basetypes.PyJointState.Acceleration(5.0) + assert_equal(js.acceleration, 5.0) From 27b1f3232538969d78466a20f0c15991bde69e40 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Mon, 10 Jul 2017 18:32:23 +0200 Subject: [PATCH 05/39] Partial wrapper of rigid body state --- bindings/python/_basetypes.pxd | 15 ++++++++++++ bindings/python/basetypes.pyx | 45 ++++++++++++++++++++++++++++++++++ bindings/python/test.py | 25 +++++++++++++++++++ 3 files changed, 85 insertions(+) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index 7feb1473..ee371324 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -78,3 +78,18 @@ cdef extern from "base/JointState.hpp" namespace "base::JointState": JointState Effort(double) JointState Raw(double) JointState Acceleration(double) + + +cdef extern from "base/samples/RigidBodyState.hpp" namespace "base::samples": + cdef cppclass RigidBodyState: + RigidBodyState(bool) + Time time + string sourceFrame + string targetFrame + Vector3d position + #Matrix3d cov_position TODO + #Matrix3d cov_orientatio TODO + Vector3d velocity + #Matrix3d cov_velocity TODO + Vector3d angular_velocity + #Matrix3d cov_angular_velocity TODO diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 8f21e9bd..0165296e 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -338,3 +338,48 @@ cdef class PyJointState: cdef PyJointState self = PyJointState() self.thisptr[0] = _basetypes.Acceleration(value) return self + + +cdef class PyRigidBodyState: + cdef _basetypes.RigidBodyState* thisptr + cdef bool delete_thisptr + + def __cinit__(self): + self.thisptr = NULL + self.delete_thisptr = False + + def __dealloc__(self): + if self.thisptr != NULL and self.delete_thisptr: + del self.thisptr + + def __init__(self, bool do_invalidation=True): + self.thisptr = new _basetypes.RigidBodyState(do_invalidation) + self.delete_thisptr = True + + def _get_time(self): + cdef PyTime time = PyTime() + del time.thisptr + time.thisptr = &self.thisptr.time + time.delete_thisptr = False + return time + + def _set_time(self, PyTime time): + self.thisptr.time = deref(time.thisptr) + + time = property(_get_time, _set_time) + + def _get_source_frame(self): + return self.thisptr.sourceFrame + + def _set_source_frame(self, string value): + self.thisptr.sourceFrame = value + + source_frame = property(_get_source_frame, _set_source_frame) + + def _get_target_frame(self): + return self.thisptr.targetFrame + + def _set_target_frame(self, string value): + self.thisptr.targetFrame = value + + target_frame = property(_get_target_frame, _set_target_frame) diff --git a/bindings/python/test.py b/bindings/python/test.py index da119500..209fd33b 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -115,3 +115,28 @@ def test_joint_state_factories(): assert_equal(js.raw, 5.0) js = basetypes.PyJointState.Acceleration(5.0) assert_equal(js.acceleration, 5.0) + + +def test_rigid_body_state_get_set_time(): + rbs = basetypes.PyRigidBodyState() + assert_equal(rbs.time.microseconds, 0) + rbs.time.microseconds = 500 + assert_equal(rbs.time.microseconds, 500) + time = basetypes.PyTime() + time.microseconds = 1000 + rbs.time = time + assert_equal(rbs.time.microseconds, 1000) + + +def test_rigid_body_state_get_set_source_frame(): + rbs = basetypes.PyRigidBodyState() + assert_equal(rbs.source_frame, "") + rbs.source_frame = "source_frame" + assert_equal(rbs.source_frame, "source_frame") + + +def test_rigid_body_state_get_set_target_frame(): + rbs = basetypes.PyRigidBodyState() + assert_equal(rbs.target_frame, "") + rbs.target_frame = "target_frame" + assert_equal(rbs.target_frame, "target_frame") From 9761caf6a9870c77390913220a43f026de32d1d8 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Mon, 10 Jul 2017 18:38:44 +0200 Subject: [PATCH 06/39] Add todos --- bindings/python/basetypes.pyx | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 0165296e..1d08f682 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -81,6 +81,8 @@ cdef class PyVector2d: def squared_norm(self): return self.thisptr.squaredNorm() + # TODO add toarray / fromarray + cdef class PyVector3d: cdef _basetypes.Vector3d* thisptr @@ -143,6 +145,8 @@ cdef class PyVector3d: def squared_norm(self): return self.thisptr.squaredNorm() + # TODO add toarray / fromarray + cdef class PyVector4d: cdef _basetypes.Vector4d* thisptr @@ -182,8 +186,10 @@ cdef class PyVector4d: def squared_norm(self): return self.thisptr.squaredNorm() + # TODO add toarray / fromarray -# TODO Vector6d, VectorXd + +# TODO Vector6d, VectorXd, Point, Pose cdef class PyQuaterniond: @@ -207,6 +213,8 @@ cdef class PyQuaterniond: self.thisptr.w(), self.thisptr.x(), self.thisptr.y(), self.thisptr.z()) + # TODO add toarray / fromarray + cdef class PyTransformWithCovariance: cdef _basetypes.TransformWithCovariance* thisptr @@ -340,6 +348,9 @@ cdef class PyJointState: return self +# TODO Joints + + cdef class PyRigidBodyState: cdef _basetypes.RigidBodyState* thisptr cdef bool delete_thisptr @@ -383,3 +394,6 @@ cdef class PyRigidBodyState: self.thisptr.targetFrame = value target_frame = property(_get_target_frame, _set_target_frame) + + +# TODO DistanceImage, Frame, LaserScan, IMUSensors, PointCloud From 86cf719cca613a260bfd44d3d1bb7cc9e58bb25e Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 11 Jul 2017 11:57:31 +0200 Subject: [PATCH 07/39] Add Matrix3d --- bindings/python/_basetypes.pxd | 13 +++- bindings/python/basetypes.pyx | 110 ++++++++++++++++++++++++++------- bindings/python/test.py | 66 ++++++++++++-------- 3 files changed, 136 insertions(+), 53 deletions(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index ee371324..35f80436 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -14,7 +14,7 @@ cdef extern from "base/Eigen.hpp" namespace "base": Vector2d(Vector2d&) double* data() int rows() - double& get "operator()"(int rows) + double& get "operator()"(int row) double x() double y() double norm() @@ -26,7 +26,7 @@ cdef extern from "base/Eigen.hpp" namespace "base": Vector3d(Vector3d&) double* data() int rows() - double& get "operator()"(int rows) + double& get "operator()"(int row) double x() double y() double z() @@ -39,13 +39,20 @@ cdef extern from "base/Eigen.hpp" namespace "base": Vector4d(Vector4d&) double* data() int rows() - double& get "operator()"(int rows) + double& get "operator()"(int row) double x() double y() double z() double norm() double squaredNorm() + cdef cppclass Matrix3d: + Matrix3d() + double* data() + double& get "operator()"(int row, int col) + int rows() + int cols() + cdef cppclass Quaterniond: Quaterniond() Quaterniond(double, double, double, double) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 1d08f682..70583c00 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -3,9 +3,11 @@ cimport _basetypes from cython.operator cimport dereference as deref from libcpp.string cimport string from libcpp cimport bool +cimport numpy as np +import numpy as np -cdef class PyTime: +cdef class Time: cdef _basetypes.Time* thisptr cdef bool delete_thisptr @@ -30,7 +32,7 @@ cdef class PyTime: microseconds = property(_get_microseconds, _set_microseconds) -cdef class PyVector2d: +cdef class Vector2d: cdef _basetypes.Vector2d* thisptr cdef bool delete_thisptr @@ -84,7 +86,7 @@ cdef class PyVector2d: # TODO add toarray / fromarray -cdef class PyVector3d: +cdef class Vector3d: cdef _basetypes.Vector3d* thisptr cdef bool delete_thisptr @@ -148,7 +150,7 @@ cdef class PyVector3d: # TODO add toarray / fromarray -cdef class PyVector4d: +cdef class Vector4d: cdef _basetypes.Vector4d* thisptr cdef bool delete_thisptr @@ -165,10 +167,10 @@ cdef class PyVector4d: self.delete_thisptr = True def __str__(self): - return "[%.2f, %.2f, %.2f, %.2f]" % (self.thisptr.get(0), - self.thisptr.get(1), - self.thisptr.get(2), - self.thisptr.get(3)) + return "[%.2f, %.2f, %.2f, %.2f]" % ( + self.thisptr.get(0), self.thisptr.get(1), + self.thisptr.get(2), self.thisptr.get(3) + ) def __getitem__(self, int i): if i < 0 or i > 3: @@ -189,10 +191,72 @@ cdef class PyVector4d: # TODO add toarray / fromarray +cdef class Matrix3d: + cdef _basetypes.Matrix3d* thisptr + cdef bool delete_thisptr + + def __cinit__(self): + self.thisptr = NULL + self.delete_thisptr = False + + def __dealloc__(self): + if self.thisptr != NULL and self.delete_thisptr: + del self.thisptr + + def __init__(self, np.ndarray[double, ndim=2] array=None): + self.thisptr = new _basetypes.Matrix3d() + self.delete_thisptr = True + cdef int i + cdef int j + if array is not None: + for i in range(3): + for j in range(3): + self.thisptr.data()[3 * j + i] = array[i, j] + + def __str__(self): + return ("[%.2f, %.2f, %.2f]," + "[%.2f, %.2f, %.2f]," + "[%.2f, %.2f, %.2f]") % ( + self.thisptr.get(0, 0), self.thisptr.get(0, 1), self.thisptr.get(0, 2), + self.thisptr.get(1, 0), self.thisptr.get(1, 1), self.thisptr.get(1, 2), + self.thisptr.get(2, 0), self.thisptr.get(2, 1), self.thisptr.get(2, 2), + ) + + def __getitem__(self, tuple indices): + cdef int i + cdef int j + i, j = indices + if i < 0 or i > 3 or j < 0 or j > 3: + raise KeyError("indices must be in [0, 3] but were (%d, %d)" + % (i, j)) + return self.thisptr.get(i, j) + + def __setitem__(self, tuple indices, double v): + cdef int i + cdef int j + i, j = indices + if i < 0 or i > 3 or j < 0 or j > 3: + raise KeyError("indices must be in [0, 3] but were (%d, %d)" + % (i, j)) + # Assumes column-major order! + self.thisptr.data()[3 * j + i] = v + + def toarray(self): + cdef np.ndarray[double, ndim=2] array = np.empty((3, 3)) + cdef int i + cdef int j + for i in range(3): + for j in range(3): + array[i, j] = self.thisptr.data()[3 * j + i] + return array + + # TODO add fromarray + + # TODO Vector6d, VectorXd, Point, Pose -cdef class PyQuaterniond: +cdef class Quaterniond: cdef _basetypes.Quaterniond* thisptr cdef bool delete_thisptr @@ -216,7 +280,7 @@ cdef class PyQuaterniond: # TODO add toarray / fromarray -cdef class PyTransformWithCovariance: +cdef class TransformWithCovariance: cdef _basetypes.TransformWithCovariance* thisptr cdef bool delete_thisptr @@ -233,25 +297,25 @@ cdef class PyTransformWithCovariance: self.delete_thisptr = True def _get_translation(self): - cdef PyVector3d translation = PyVector3d() + cdef Vector3d translation = Vector3d() del translation.thisptr translation.delete_thisptr = False translation.thisptr = &self.thisptr.translation return translation - def _set_translation(self, PyVector3d translation): + def _set_translation(self, Vector3d translation): self.thisptr.translation = deref(translation.thisptr) translation = property(_get_translation, _set_translation) def _get_orientation(self): - cdef PyQuaterniond orientation = PyQuaterniond() + cdef Quaterniond orientation = Quaterniond() del orientation.thisptr orientation.delete_thisptr = False orientation.thisptr = &self.thisptr.orientation return orientation - def _set_orientation(self, PyQuaterniond orientation): + def _set_orientation(self, Quaterniond orientation): self.thisptr.orientation = deref(orientation.thisptr) orientation = property(_get_orientation, _set_orientation) @@ -261,7 +325,7 @@ cdef class PyTransformWithCovariance: self.orientation) -cdef class PyJointState: +cdef class JointState: cdef _basetypes.JointState* thisptr cdef bool delete_thisptr @@ -319,31 +383,31 @@ cdef class PyJointState: @staticmethod def Position(double value): - cdef PyJointState self = PyJointState() + cdef JointState self = JointState() self.thisptr[0] = _basetypes.Position(value) return self @staticmethod def Speed(double value): - cdef PyJointState self = PyJointState() + cdef JointState self = JointState() self.thisptr[0] = _basetypes.Speed(value) return self @staticmethod def Effort(double value): - cdef PyJointState self = PyJointState() + cdef JointState self = JointState() self.thisptr[0] = _basetypes.Effort(value) return self @staticmethod def Raw(double value): - cdef PyJointState self = PyJointState() + cdef JointState self = JointState() self.thisptr[0] = _basetypes.Raw(value) return self @staticmethod def Acceleration(double value): - cdef PyJointState self = PyJointState() + cdef JointState self = JointState() self.thisptr[0] = _basetypes.Acceleration(value) return self @@ -351,7 +415,7 @@ cdef class PyJointState: # TODO Joints -cdef class PyRigidBodyState: +cdef class RigidBodyState: cdef _basetypes.RigidBodyState* thisptr cdef bool delete_thisptr @@ -368,13 +432,13 @@ cdef class PyRigidBodyState: self.delete_thisptr = True def _get_time(self): - cdef PyTime time = PyTime() + cdef Time time = Time() del time.thisptr time.thisptr = &self.thisptr.time time.delete_thisptr = False return time - def _set_time(self, PyTime time): + def _set_time(self, Time time): self.thisptr.time = deref(time.thisptr) time = property(_get_time, _set_time) diff --git a/bindings/python/test.py b/bindings/python/test.py index 209fd33b..8c83908b 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -1,31 +1,33 @@ import basetypes +import numpy as np from nose.tools import assert_equal, assert_raises_regexp, assert_almost_equal +from numpy.testing import assert_array_almost_equal def test_get_set_microseconds(): - t = basetypes.PyTime() + t = basetypes.Time() m = 1000023 t.microseconds = m assert_equal(t.microseconds, m) def test_vector2d_ctor(): - v = basetypes.PyVector2d(1.0, 2.0) + v = basetypes.Vector2d(1.0, 2.0) assert_equal(str(v), "[1.00, 2.00]") def test_vector3d_ctor(): - v = basetypes.PyVector3d(1.0, 2.0, 3.0) + v = basetypes.Vector3d(1.0, 2.0, 3.0) assert_equal(str(v), "[1.00, 2.00, 3.00]") def test_vector4d_ctor(): - v = basetypes.PyVector4d(1.0, 2.0, 3.0, 4.0) + v = basetypes.Vector4d(1.0, 2.0, 3.0, 4.0) assert_equal(str(v), "[1.00, 2.00, 3.00, 4.00]") def test_vector3d_get_set_data(): - v = basetypes.PyVector3d(1.0, 2.0, 3.0) + v = basetypes.Vector3d(1.0, 2.0, 3.0) v.x = 5.0 v.y = 6.0 v.z = 7.0 @@ -35,7 +37,7 @@ def test_vector3d_get_set_data(): def test_vector3d_array_access(): - v = basetypes.PyVector3d(1.0, 2.0, 3.0) + v = basetypes.Vector3d(1.0, 2.0, 3.0) assert_equal(v[0], 1.0) v[1] = 4.0 assert_equal(v[1], 4.0) @@ -45,98 +47,108 @@ def assign(i): assert_raises_regexp(KeyError, "index must be", assign, 3) +def test_matrix3d_get_set_data(): + m = basetypes.Matrix3d() + m[0, 1] = 1.0 + assert_equal(m[0, 1], 1.0) + random_state = np.random.RandomState(13) + r = random_state.randn(3, 3) + m = basetypes.Matrix3d(r) + assert_array_almost_equal(m.toarray(), r) + + def test_norms(): - v = basetypes.PyVector3d(1.0, 2.0, 3.0) + v = basetypes.Vector3d(1.0, 2.0, 3.0) assert_almost_equal(v.norm(), 3.741657387) assert_equal(v.squared_norm(), 14.0) def test_quaterniond_ctor(): - q = basetypes.PyQuaterniond(1.0, 0.0, 0.0, 0.0) + q = basetypes.Quaterniond(1.0, 0.0, 0.0, 0.0) assert_equal(str(q), "[im=1.00, real=(0.00, 0.00, 0.00)]") def test_transform_with_cov_ctor(): - basetypes.PyTransformWithCovariance() + basetypes.TransformWithCovariance() def test_transform_set_get_translation(): - p = basetypes.PyTransformWithCovariance() - t = basetypes.PyVector3d(1.0, 2.0, 3.0) + p = basetypes.TransformWithCovariance() + t = basetypes.Vector3d(1.0, 2.0, 3.0) p.translation = t assert_equal(str(p.translation), "[1.00, 2.00, 3.00]") def test_transform_set_get_orientation(): - p = basetypes.PyTransformWithCovariance() - o = basetypes.PyQuaterniond(1.0, 0.0, 0.0, 0.0) + p = basetypes.TransformWithCovariance() + o = basetypes.Quaterniond(1.0, 0.0, 0.0, 0.0) p.orientation = o assert_equal(str(p.orientation), "[im=1.00, real=(0.00, 0.00, 0.00)]") def test_joint_state_get_set_position(): - js = basetypes.PyJointState() + js = basetypes.JointState() js.position = 5.0 assert_equal(js.position, 5.0) def test_joint_state_get_set_speed(): - js = basetypes.PyJointState() + js = basetypes.JointState() js.speed = 5.0 assert_equal(js.speed, 5.0) def test_joint_state_get_set_effort(): - js = basetypes.PyJointState() + js = basetypes.JointState() js.effort = 5.0 assert_equal(js.effort, 5.0) def test_joint_state_get_set_raw(): - js = basetypes.PyJointState() + js = basetypes.JointState() js.raw = 5.0 assert_equal(js.raw, 5.0) def test_joint_state_get_set_acceleration(): - js = basetypes.PyJointState() + js = basetypes.JointState() js.acceleration = 5.0 assert_equal(js.acceleration, 5.0) def test_joint_state_factories(): - js = basetypes.PyJointState.Position(5.0) + js = basetypes.JointState.Position(5.0) assert_equal(js.position, 5.0) - js = basetypes.PyJointState.Speed(5.0) + js = basetypes.JointState.Speed(5.0) assert_equal(js.speed, 5.0) - js = basetypes.PyJointState.Effort(5.0) + js = basetypes.JointState.Effort(5.0) assert_equal(js.effort, 5.0) - js = basetypes.PyJointState.Raw(5.0) + js = basetypes.JointState.Raw(5.0) assert_equal(js.raw, 5.0) - js = basetypes.PyJointState.Acceleration(5.0) + js = basetypes.JointState.Acceleration(5.0) assert_equal(js.acceleration, 5.0) def test_rigid_body_state_get_set_time(): - rbs = basetypes.PyRigidBodyState() + rbs = basetypes.RigidBodyState() assert_equal(rbs.time.microseconds, 0) rbs.time.microseconds = 500 assert_equal(rbs.time.microseconds, 500) - time = basetypes.PyTime() + time = basetypes.Time() time.microseconds = 1000 rbs.time = time assert_equal(rbs.time.microseconds, 1000) def test_rigid_body_state_get_set_source_frame(): - rbs = basetypes.PyRigidBodyState() + rbs = basetypes.RigidBodyState() assert_equal(rbs.source_frame, "") rbs.source_frame = "source_frame" assert_equal(rbs.source_frame, "source_frame") def test_rigid_body_state_get_set_target_frame(): - rbs = basetypes.PyRigidBodyState() + rbs = basetypes.RigidBodyState() assert_equal(rbs.target_frame, "") rbs.target_frame = "target_frame" assert_equal(rbs.target_frame, "target_frame") From 345f5b0aa96b2c90e693326807fd13b1fcff10c7 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 11 Jul 2017 13:57:44 +0200 Subject: [PATCH 08/39] Expose more RBS members --- bindings/python/_basetypes.pxd | 5 +-- bindings/python/basetypes.pyx | 62 ++++++++++++++++++++++++++++++++-- bindings/python/test.py | 27 +++++++++++++++ 3 files changed, 89 insertions(+), 5 deletions(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index 35f80436..b05d336f 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -94,8 +94,9 @@ cdef extern from "base/samples/RigidBodyState.hpp" namespace "base::samples": string sourceFrame string targetFrame Vector3d position - #Matrix3d cov_position TODO - #Matrix3d cov_orientatio TODO + Matrix3d cov_position + Quaterniond orientation + #Matrix3d cov_orientation TODO Vector3d velocity #Matrix3d cov_velocity TODO Vector3d angular_velocity diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 70583c00..605e3674 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -147,7 +147,14 @@ cdef class Vector3d: def squared_norm(self): return self.thisptr.squaredNorm() - # TODO add toarray / fromarray + def toarray(self): + cdef np.ndarray[double, ndim=1] array = np.empty(3) + cdef int i + for i in range(3): + array[i] = self.thisptr.data()[i] + return array + + # TODO add fromarray cdef class Vector4d: @@ -250,7 +257,12 @@ cdef class Matrix3d: array[i, j] = self.thisptr.data()[3 * j + i] return array - # TODO add fromarray + def fromarray(self, np.ndarray[double, ndim=2] array): + cdef int i + cdef int j + for i in range(3): + for j in range(3): + self.thisptr.data()[3 * j + i] = array[i, j] # TODO Vector6d, VectorXd, Point, Pose @@ -277,7 +289,14 @@ cdef class Quaterniond: self.thisptr.w(), self.thisptr.x(), self.thisptr.y(), self.thisptr.z()) - # TODO add toarray / fromarray + def toarray(self): + cdef np.ndarray[double, ndim=1] array = np.array([ + self.thisptr.w(), self.thisptr.x(), self.thisptr.y(), + self.thisptr.z()]) + return array + + def fromarray(self, np.ndarray[double, ndim=1] array): + self.thisptr[0] = _basetypes.Quaterniond(array[0], array[1], array[2], array[3]) cdef class TransformWithCovariance: @@ -459,5 +478,42 @@ cdef class RigidBodyState: target_frame = property(_get_target_frame, _set_target_frame) + def _get_position(self): + cdef Vector3d position = Vector3d() + del position.thisptr + position.delete_thisptr = False + position.thisptr = &self.thisptr.position + return position + + def _set_position(self, Vector3d value): + self.thisptr.position = deref(value.thisptr) + + position = property(_get_position, _set_position) + + def _get_cov_position(self): + cdef Matrix3d cov_position = Matrix3d() + del cov_position.thisptr + cov_position.delete_thisptr = False + cov_position.thisptr = &self.thisptr.cov_position + return cov_position + + def _set_cov_position(self, Matrix3d value): + self.thisptr.cov_position = deref(value.thisptr) + + cov_position = property(_get_cov_position, _set_cov_position) + + def _get_orientation(self): + cdef Quaterniond orientation = Quaterniond() + del orientation.thisptr + orientation.delete_thisptr = False + orientation.thisptr = &self.thisptr.orientation + return orientation + + def _set_orientation(self, Quaterniond value): + self.thisptr.orientation = deref(value.thisptr) + + orientation = property(_get_orientation, _set_orientation) + + # TODO other properties # TODO DistanceImage, Frame, LaserScan, IMUSensors, PointCloud diff --git a/bindings/python/test.py b/bindings/python/test.py index 8c83908b..72a4cc85 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -152,3 +152,30 @@ def test_rigid_body_state_get_set_target_frame(): assert_equal(rbs.target_frame, "") rbs.target_frame = "target_frame" assert_equal(rbs.target_frame, "target_frame") + + +def test_rigid_body_state_get_set_position(): + rbs = basetypes.RigidBodyState() + assert_array_almost_equal( + rbs.position.toarray(), np.array([np.nan, np.nan, np.nan])) + rbs.position.x = 1.0 + rbs.position.y = 2.0 + rbs.position.z = 3.0 + assert_array_almost_equal(rbs.position.toarray(), np.array([1, 2, 3])) + + +def test_rigid_body_state_get_set_cov_position(): + rbs = basetypes.RigidBodyState() + assert_array_almost_equal( + rbs.cov_position.toarray(), np.ones((3, 3)) * np.nan) + rbs.cov_position.fromarray(np.eye(3)) + assert_array_almost_equal(rbs.cov_position.toarray(), np.eye(3)) + + +def test_rigid_body_state_get_set_orientation(): + rbs = basetypes.RigidBodyState() + assert_array_almost_equal( + rbs.orientation.toarray(), np.array([np.nan, np.nan, np.nan, np.nan])) + rbs.orientation.fromarray(np.array([1.0, 2.0, 3.0, 4.0])) + assert_array_almost_equal( + rbs.orientation.toarray(), np.array([1.0, 2.0, 3.0, 4.0])) From b885de997a12293462fc045bba25dfefc1b0fc10 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 11 Jul 2017 18:03:11 +0200 Subject: [PATCH 09/39] Expose all members of RBS --- bindings/python/_basetypes.pxd | 6 ++-- bindings/python/basetypes.pyx | 61 +++++++++++++++++++++++++++++++++- bindings/python/test.py | 44 ++++++++++++++++++++++++ 3 files changed, 107 insertions(+), 4 deletions(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index b05d336f..00830176 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -96,8 +96,8 @@ cdef extern from "base/samples/RigidBodyState.hpp" namespace "base::samples": Vector3d position Matrix3d cov_position Quaterniond orientation - #Matrix3d cov_orientation TODO + Matrix3d cov_orientation Vector3d velocity - #Matrix3d cov_velocity TODO + Matrix3d cov_velocity Vector3d angular_velocity - #Matrix3d cov_angular_velocity TODO + Matrix3d cov_angular_velocity diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 605e3674..402f9bf9 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -514,6 +514,65 @@ cdef class RigidBodyState: orientation = property(_get_orientation, _set_orientation) - # TODO other properties + def _get_cov_orientation(self): + cdef Matrix3d cov_orientation = Matrix3d() + del cov_orientation.thisptr + cov_orientation.delete_thisptr = False + cov_orientation.thisptr = &self.thisptr.cov_orientation + return cov_orientation + + def _set_cov_orientation(self, Matrix3d value): + self.thisptr.cov_orientation = deref(value.thisptr) + + cov_orientation = property(_get_cov_orientation, _set_cov_orientation) + + def _get_velocity(self): + cdef Vector3d velocity = Vector3d() + del velocity.thisptr + velocity.delete_thisptr = False + velocity.thisptr = &self.thisptr.velocity + return velocity + + def _set_velocity(self, Vector3d value): + self.thisptr.velocity = deref(value.thisptr) + + velocity = property(_get_velocity, _set_velocity) + + def _get_cov_velocity(self): + cdef Matrix3d cov_velocity = Matrix3d() + del cov_velocity.thisptr + cov_velocity.delete_thisptr = False + cov_velocity.thisptr = &self.thisptr.cov_velocity + return cov_velocity + + def _set_cov_velocity(self, Matrix3d value): + self.thisptr.cov_velocity = deref(value.thisptr) + + cov_velocity = property(_get_cov_velocity, _set_cov_velocity) + + def _get_angular_velocity(self): + cdef Vector3d angular_velocity = Vector3d() + del angular_velocity.thisptr + angular_velocity.delete_thisptr = False + angular_velocity.thisptr = &self.thisptr.angular_velocity + return angular_velocity + + def _set_angular_velocity(self, Vector3d value): + self.thisptr.angular_velocity = deref(value.thisptr) + + angular_velocity = property(_get_angular_velocity, _set_angular_velocity) + + def _get_cov_angular_velocity(self): + cdef Matrix3d cov_angular_velocity = Matrix3d() + del cov_angular_velocity.thisptr + cov_angular_velocity.delete_thisptr = False + cov_angular_velocity.thisptr = &self.thisptr.cov_angular_velocity + return cov_angular_velocity + + def _set_cov_angular_velocity(self, Matrix3d value): + self.thisptr.cov_angular_velocity = deref(value.thisptr) + + cov_angular_velocity = property( + _get_cov_angular_velocity, _set_cov_angular_velocity) # TODO DistanceImage, Frame, LaserScan, IMUSensors, PointCloud diff --git a/bindings/python/test.py b/bindings/python/test.py index 72a4cc85..6757f225 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -179,3 +179,47 @@ def test_rigid_body_state_get_set_orientation(): rbs.orientation.fromarray(np.array([1.0, 2.0, 3.0, 4.0])) assert_array_almost_equal( rbs.orientation.toarray(), np.array([1.0, 2.0, 3.0, 4.0])) + + +def test_rigid_body_state_get_set_cov_orientation(): + rbs = basetypes.RigidBodyState() + assert_array_almost_equal( + rbs.cov_orientation.toarray(), np.ones((3, 3)) * np.nan) + rbs.cov_orientation.fromarray(np.eye(3)) + assert_array_almost_equal(rbs.cov_orientation.toarray(), np.eye(3)) + + +def test_rigid_body_state_get_set_velocity(): + rbs = basetypes.RigidBodyState() + assert_array_almost_equal( + rbs.velocity.toarray(), np.array([np.nan, np.nan, np.nan])) + rbs.velocity.x = 1.0 + rbs.velocity.y = 2.0 + rbs.velocity.z = 3.0 + assert_array_almost_equal(rbs.velocity.toarray(), np.array([1, 2, 3])) + + +def test_rigid_body_state_get_set_cov_velocity(): + rbs = basetypes.RigidBodyState() + assert_array_almost_equal( + rbs.cov_velocity.toarray(), np.ones((3, 3)) * np.nan) + rbs.cov_velocity.fromarray(np.eye(3)) + assert_array_almost_equal(rbs.cov_velocity.toarray(), np.eye(3)) + + +def test_rigid_body_state_get_set_angular_velocity(): + rbs = basetypes.RigidBodyState() + assert_array_almost_equal( + rbs.angular_velocity.toarray(), np.array([np.nan, np.nan, np.nan])) + rbs.angular_velocity.x = 1.0 + rbs.angular_velocity.y = 2.0 + rbs.angular_velocity.z = 3.0 + assert_array_almost_equal(rbs.angular_velocity.toarray(), np.array([1, 2, 3])) + + +def test_rigid_body_state_get_set_cov_angular_velocity(): + rbs = basetypes.RigidBodyState() + assert_array_almost_equal( + rbs.cov_angular_velocity.toarray(), np.ones((3, 3)) * np.nan) + rbs.cov_angular_velocity.fromarray(np.eye(3)) + assert_array_almost_equal(rbs.cov_angular_velocity.toarray(), np.eye(3)) From 1f2fac354eefe89df8a05ef5f92b10fade26d92f Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 11 Jul 2017 18:17:24 +0200 Subject: [PATCH 10/39] Expose some functions of Joints --- bindings/python/_basetypes.pxd | 22 ++++++++++++++++++++++ bindings/python/basetypes.pyx | 27 ++++++++++++++++++++++++++- bindings/python/test.py | 11 ++++++++++- 3 files changed, 58 insertions(+), 2 deletions(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index 00830176..1527c7c7 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -1,4 +1,6 @@ from libcpp.string cimport string +from libcpp.vector cimport vector +from libcpp cimport bool cdef extern from "base/Time.hpp" namespace "base": @@ -87,6 +89,26 @@ cdef extern from "base/JointState.hpp" namespace "base::JointState": JointState Acceleration(double) +cdef extern from "base/NamedVector.hpp" namespace "base": + cdef cppclass NamedVector[T]: + vector[string] names + vector[T] elements + + bool hasNames() + T getElementByName(string) + void resize(int) + int size() + bool empty() + void clear() + int mapNameToIndex(string name) + + +cdef extern from "base/samples/Joints.hpp" namespace "base::samples": + cdef cppclass Joints(NamedVector[JointState]): + Joints() + Time time + + cdef extern from "base/samples/RigidBodyState.hpp" namespace "base::samples": cdef cppclass RigidBodyState: RigidBodyState(bool) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 402f9bf9..86aa4d7b 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -431,7 +431,32 @@ cdef class JointState: return self -# TODO Joints +cdef class Joints: + cdef _basetypes.Joints* thisptr + cdef bool delete_thisptr + + def __cinit__(self): + self.thisptr = NULL + self.delete_thisptr = False + + def __dealloc__(self): + if self.thisptr != NULL and self.delete_thisptr: + del self.thisptr + + def __init__(self): + self.thisptr = new _basetypes.Joints() + self.delete_thisptr = True + + def size(self): + return self.thisptr.size() + + def resize(self, int size): + self.thisptr.resize(size) + + def has_names(self): + return self.thisptr.hasNames() + + # TODO expose some useful functions cdef class RigidBodyState: diff --git a/bindings/python/test.py b/bindings/python/test.py index 6757f225..8906f8ed 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -1,6 +1,6 @@ import basetypes import numpy as np -from nose.tools import assert_equal, assert_raises_regexp, assert_almost_equal +from nose.tools import assert_equal, assert_raises_regexp, assert_almost_equal, assert_false, assert_true from numpy.testing import assert_array_almost_equal @@ -129,6 +129,15 @@ def test_joint_state_factories(): assert_equal(js.acceleration, 5.0) +def test_joints_resize(): + j = basetypes.Joints() + assert_equal(j.size(), 0) + assert_false(j.has_names()) + j.resize(5) + assert_equal(j.size(), 5) + assert_false(j.has_names()) + + def test_rigid_body_state_get_set_time(): rbs = basetypes.RigidBodyState() assert_equal(rbs.time.microseconds, 0) From 0e8418d5b5cfc32963f7828ae4fa807b7646e24c Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 12 Jul 2017 10:20:04 +0200 Subject: [PATCH 11/39] Add element access for Joints --- bindings/python/_basetypes.pxd | 14 +++++- bindings/python/basetypes.pyx | 88 ++++++++++++++++++++++++++++++++++ bindings/python/test.py | 21 ++++++++ 3 files changed, 121 insertions(+), 2 deletions(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index 1527c7c7..1c95b7d1 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -79,6 +79,16 @@ cdef extern from "base/JointState.hpp" namespace "base": double effort double raw double acceleration + bool hasPosition() + bool hasSpeed() + bool hasEffort() + bool hasRaw() + bool hasAcceleration() + bool isPosition() + bool isSpeed() + bool isEffort() + bool isRaw() + bool isAcceleration() cdef extern from "base/JointState.hpp" namespace "base::JointState": @@ -94,12 +104,12 @@ cdef extern from "base/NamedVector.hpp" namespace "base": vector[string] names vector[T] elements - bool hasNames() - T getElementByName(string) void resize(int) int size() bool empty() void clear() + bool hasNames() + T getElementByName(string) int mapNameToIndex(string name) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 86aa4d7b..82506f24 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -2,6 +2,7 @@ cimport _basetypes from cython.operator cimport dereference as deref from libcpp.string cimport string +from libcpp.vector cimport vector from libcpp cimport bool cimport numpy as np import numpy as np @@ -400,6 +401,36 @@ cdef class JointState: acceleration = property(_get_acceleration, _set_acceleration) + def has_position(self): + return self.thisptr.hasPosition() + + def has_speed(self): + return self.thisptr.hasSpeed() + + def has_effort(self): + return self.thisptr.hasEffort() + + def has_raw(self): + return self.thisptr.hasRaw() + + def has_acceleration(self): + return self.thisptr.hasAcceleration() + + def has_position(self): + return self.thisptr.hasPosition() + + def is_speed(self): + return self.thisptr.isSpeed() + + def is_effort(self): + return self.thisptr.isEffort() + + def is_raw(self): + return self.thisptr.isRaw() + + def is_acceleration(self): + return self.thisptr.isAcceleration() + @staticmethod def Position(double value): cdef JointState self = JointState() @@ -456,9 +487,66 @@ cdef class Joints: def has_names(self): return self.thisptr.hasNames() + def clear(self): + self.thisptr.clear() + + @property + def names(self): + cdef StringVectorReference names = StringVectorReference() + names.thisptr = &self.thisptr.names + return names + + @property + def elements(self): + cdef JointStateVectorReference elements = JointStateVectorReference() + elements.thisptr = &self.thisptr.elements + return elements + + def __getitem__(self, string name): + cdef JointState joint_state = JointState() + joint_state.thisptr[0] = self.thisptr.getElementByName(name) + return joint_state + + def __setitem__(self, string name, JointState joint_state): + cdef int i = self.thisptr.mapNameToIndex(name) + self.thisptr.elements[i] = deref(joint_state.thisptr) + # TODO expose some useful functions +cdef class StringVectorReference: + cdef vector[string]* thisptr + + def __cinit__(self): + self.thisptr = NULL + + def __dealloc__(self): + pass + + def __getitem__(self, int i): + return deref(self.thisptr)[i] + + def __setitem__(self, int i, string s): + deref(self.thisptr)[i] = s + + +cdef class JointStateVectorReference: + cdef vector[_basetypes.JointState]* thisptr + + def __cinit__(self): + self.thisptr = NULL + + def __dealloc__(self): + pass + + def __getitem__(self, int i): + cdef JointState joint_state = JointState() + del joint_state.thisptr + joint_state.delete_thisptr = False + joint_state.thisptr = &deref(self.thisptr)[i] + return joint_state + + cdef class RigidBodyState: cdef _basetypes.RigidBodyState* thisptr cdef bool delete_thisptr diff --git a/bindings/python/test.py b/bindings/python/test.py index 8906f8ed..51a2e638 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -136,6 +136,27 @@ def test_joints_resize(): j.resize(5) assert_equal(j.size(), 5) assert_false(j.has_names()) + j.clear() + assert_equal(j.size(), 0) + + +def test_joints_access(): + j = basetypes.Joints() + j.resize(1) + + assert_equal(j.names[0], "") + assert_false(j.elements[0].has_position()) + assert_false(j.elements[0].has_speed()) + assert_false(j.elements[0].has_effort()) + assert_false(j.elements[0].has_raw()) + assert_false(j.elements[0].has_acceleration()) + + j.names[0] = "test_name" + j.elements[0].position = 1.0 + assert_equal(j.names[0], "test_name") + assert_true(j.elements[0].has_position()) + + assert_equal(j["test_name"].position, 1.0) def test_rigid_body_state_get_set_time(): From c1846f96d0f3910ba74cc0813a23b55e4ea49dc5 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 12 Jul 2017 14:08:11 +0200 Subject: [PATCH 12/39] Wrap Frame --- bindings/python/_basetypes.pxd | 50 +++++++++++++ bindings/python/basetypes.pyx | 127 +++++++++++++++++++++++++++++++-- bindings/python/test.py | 17 ++++- 3 files changed, 189 insertions(+), 5 deletions(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index 1c95b7d1..40ba8e87 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -1,6 +1,7 @@ from libcpp.string cimport string from libcpp.vector cimport vector from libcpp cimport bool +from libc.stdint cimport uint8_t, uint16_t, uint32_t cdef extern from "base/Time.hpp" namespace "base": @@ -133,3 +134,52 @@ cdef extern from "base/samples/RigidBodyState.hpp" namespace "base::samples": Matrix3d cov_velocity Vector3d angular_velocity Matrix3d cov_angular_velocity + + +cdef extern from "base/samples/Frame.hpp" namespace "base::samples::frame": + cdef enum frame_mode_t: + MODE_UNDEFINED, + MODE_GRAYSCALE, + MODE_RGB, + MODE_UYVY, + MODE_BGR, + MODE_BGR32, + RAW_MODES, + MODE_BAYER, + MODE_BAYER_RGGB, + MODE_BAYER_GRBG, + MODE_BAYER_BGGR, + MODE_BAYER_GBRG, + COMPRESSED_MODES, + MODE_PJPG, + MODE_JPEG, + MODE_PNG + + cdef enum frame_status_t: + STATUS_EMPTY, + STATUS_VALID, + STATUS_INVALID + + cdef cppclass frame_size_t: + int width + int height + + cdef cppclass Frame: + Frame() + Frame(int width, int height, int depth, frame_mode_t mode, int val, int sizeInBytes) + Frame(Frame other, bool bcopy) + + Time time + Time received_time + vector[uint8_t] image + + frame_size_t size + + uint32_t getChannelCount() + uint32_t getPixelSize() + uint32_t getRowSize() + uint32_t getNumberOfBytes() + uint32_t getPixelCount() + uint32_t getDataDepth() + uint16_t getWidth() + uint16_t getHeight() diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 82506f24..778a28e7 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -1,11 +1,12 @@ # distutils: language = c++ -cimport _basetypes from cython.operator cimport dereference as deref from libcpp.string cimport string from libcpp.vector cimport vector from libcpp cimport bool +from libc.stdint cimport uint8_t, uint16_t, uint32_t, uint64_t cimport numpy as np import numpy as np +cimport _basetypes cdef class Time: @@ -511,8 +512,6 @@ cdef class Joints: cdef int i = self.thisptr.mapNameToIndex(name) self.thisptr.elements[i] = deref(joint_state.thisptr) - # TODO expose some useful functions - cdef class StringVectorReference: cdef vector[string]* thisptr @@ -688,4 +687,124 @@ cdef class RigidBodyState: cov_angular_velocity = property( _get_cov_angular_velocity, _set_cov_angular_velocity) -# TODO DistanceImage, Frame, LaserScan, IMUSensors, PointCloud + +""" TODO +cdef class frame_mode_t: + MODE_UNDEFINED = _basetypes.MODE_UNDEFINED + MODE_GRAYSCALE = _basetypes.MODE_GRAYSCALE + MODE_RGB = _basetypes.MODE_RGB + MODE_UYVY = _basetypes.MODE_UYVY + MODE_BGR = _basetypes.MODE_BGR + MODE_RGB32 = _basetypes.MODE_RGB32 + RAW_MODES = _basetypes.RAW_MODES + MODE_BAYER = _basetypes.MODE_BAYER + MODE_BAYER_RGGB = _basetypes.MODE_BAYER_RGGB + MODE_BAYER_GRBG = _basetypes.MODE_BAYER_GRBG + MODE_BAYER_BGGR = _basetypes.MODE_BAYER_BGGR + MODE_BAYER_GBRG = _basetypes.MODE_BAYER_GBRG + COMPRESSED_MODES = _basetypes.COMPRESSED_MODES + MODE_PJPG = _basetypes.MODE_PJPG + MODE_JPEG = _basetypes.MODE_JPEG + MODE_PNG = _basetypes.MODE_PNG + + +cdef class frame_status_t: + STATUS_EMPTY = _basetypes.STATUS_EMPTY + STATUS_VALID = _basetypes.STATUS_VALID + STATUS_INVALID = _basetypes.STATUS_INVALID +""" + + +cdef class Frame: + cdef _basetypes.Frame* thisptr + cdef bool delete_thisptr + + def __cinit__(self): + self.thisptr = NULL + self.delete_thisptr = False + + def __dealloc__(self): + if self.thisptr != NULL and self.delete_thisptr: + del self.thisptr + + def __init__(self, width=None, height=None, depth=None, mode=None, val=None, + size_in_bytes=None): + if width is None: + self.thisptr = new _basetypes.Frame() + else: + self.thisptr = new _basetypes.Frame(width, height, depth, mode, val, size_in_bytes) + self.delete_thisptr = True + + def _get_time(self): + cdef Time time = Time() + del time.thisptr + time.thisptr = &self.thisptr.time + time.delete_thisptr = False + return time + + def _set_time(self, Time time): + self.thisptr.time = deref(time.thisptr) + + time = property(_get_time, _set_time) + + def _get_received_time(self): + cdef Time received_time = Time() + del received_time.thisptr + received_time.thisptr = &self.thisptr.received_time + received_time.delete_thisptr = False + return received_time + + def _set_received_time(self, Time received_time): + self.thisptr.received_time = deref(received_time.thisptr) + + received_time = property(_get_received_time, _set_received_time) + + def _get_image(self): # TODO can we return a reference? + cdef int n_channels = self.thisptr.getChannelCount() + dimensions = (self.thisptr.size.width, self.thisptr.size.height) + if n_channels > 1: + dimensions += (n_channels,) + + cdef uint32_t depth = self.thisptr.getDataDepth() + if depth == 1: + dtype = np.uint8 + elif depth == 2: + dtype = np.uint16 + elif depth == 4: + dtype = np.uint32 + elif depth == 8: + dtype = np.uint64 + else: + raise ValueError("Cannot handle depth of %d" % depth) + + cdef np.ndarray image + image = np.empty(dimensions, dtype=dtype) + cdef uint32_t i + for i in range(self.thisptr.getNumberOfBytes()): + image.data[i] = self.thisptr.image[i] + return image + + def _set_image(self, np.ndarray image): + cdef uint32_t i + for i in range(self.thisptr.getNumberOfBytes()): + self.thisptr.image[i] = image.data[i] + + image = property(_get_image, _set_image) + + def get_width(self): + return self.thisptr.getWidth() + + def get_height(self): + return self.thisptr.getHeight() + + def get_channel_count(self): + return self.thisptr.getChannelCount() + + def get_data_depth(self): + return self.thisptr.getDataDepth() + + +# TODO FramePair + + +# TODO Frame, LaserScan, IMUSensors, PointCloud diff --git a/bindings/python/test.py b/bindings/python/test.py index 51a2e638..9a96530f 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -1,7 +1,7 @@ import basetypes import numpy as np from nose.tools import assert_equal, assert_raises_regexp, assert_almost_equal, assert_false, assert_true -from numpy.testing import assert_array_almost_equal +from numpy.testing import assert_array_equal, assert_array_almost_equal def test_get_set_microseconds(): @@ -253,3 +253,18 @@ def test_rigid_body_state_get_set_cov_angular_velocity(): rbs.cov_angular_velocity.toarray(), np.ones((3, 3)) * np.nan) rbs.cov_angular_velocity.fromarray(np.eye(3)) assert_array_almost_equal(rbs.cov_angular_velocity.toarray(), np.eye(3)) + + +def test_create_frame_rgb(): + RGB = 2 + VALID = 1 + frame = basetypes.Frame(800, 600, 1, RGB, VALID, 800 * 600 * 3) + assert_equal(frame.get_width(), 800) + assert_equal(frame.get_height(), 600) + assert_equal(frame.get_channel_count(), 3) + assert_equal(frame.get_data_depth(), 1) + image = frame.image + assert_array_equal(image.shape, (800, 600, 3)) + assert_equal(image.dtype, np.uint8) + frame.image = np.ones(image.shape, dtype=np.uint8) + assert_true(np.all(frame.image == 1)) From ca23b9e2cfb48572866eecb2b74a1e773c522523 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 12 Jul 2017 16:38:24 +0200 Subject: [PATCH 13/39] LaserScan and IMUSensors --- bindings/python/_basetypes.pxd | 54 ++++++ bindings/python/basetypes.pyx | 296 ++++++++++++++++++++++++++++++++- bindings/python/test.py | 74 +++++++++ 3 files changed, 421 insertions(+), 3 deletions(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index 40ba8e87..70a75a0a 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -7,8 +7,15 @@ from libc.stdint cimport uint8_t, uint16_t, uint32_t cdef extern from "base/Time.hpp" namespace "base": cdef cppclass Time: Time() + int microseconds + bool isNull() + + +cdef extern from "base/Time.hpp" namespace "base::Time": + Time now() + cdef extern from "base/Eigen.hpp" namespace "base": cdef cppclass Vector2d: @@ -183,3 +190,50 @@ cdef extern from "base/samples/Frame.hpp" namespace "base::samples::frame": uint32_t getDataDepth() uint16_t getWidth() uint16_t getHeight() + + +cdef extern from "base/samples/Pointcloud.hpp" namespace "base::samples": + cdef cppclass Pointcloud: + Pointcloud() + + Time time + vector[Vector3d] points + vector[Vector4d] colors + + +cdef extern from "base/samples/LaserScan.hpp" namespace "base::samples": + cdef enum LASER_RANGE_ERRORS: # TODO pyx + TOO_FAR + TOO_NEAR + MEASUREMENT_ERROR + OTHER_RANGE_ERRORS + MAX_RANGE_ERROR + + cdef cppclass LaserScan: + LaserScan() + + Time time + double start_angle + double angular_resolution + double speed + vector[uint32_t] ranges + uint32_t minRange + uint32_t maxRange + vector[float] remission + + bool isValidBeam(unsigned int) + void reset() + bool isRangeValid(uint32_t) + #convertScanToPointCloud(vector[T], Affine3d, bool) TODO wrap Affine3d + #bool getPointFromScanBeamXForward(unsigned int i, Vector3d point) TODO + #bool getPointFromScanBeam(unsigned int i, Vector3d point) TODO + + +cdef extern from "base/samples/IMUSensors.hpp" namespace "base::samples": + cdef cppclass IMUSensors: + IMUSensors() + + Time time + Vector3d acc + Vector3d gyro + Vector3d mag diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 778a28e7..c870598d 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -33,6 +33,15 @@ cdef class Time: microseconds = property(_get_microseconds, _set_microseconds) + def is_null(self): + return self.thisptr.isNull() + + @staticmethod + def now(): + cdef Time time = Time() + time.thisptr[0] = _basetypes.now() + return time + cdef class Vector2d: cdef _basetypes.Vector2d* thisptr @@ -171,7 +180,7 @@ cdef class Vector4d: if self.thisptr != NULL and self.delete_thisptr: del self.thisptr - def __init__(self, v0, v1, v2, v3): + def __init__(self, v0=0.0, v1=0.0, v2=0.0, v3=0.0): self.thisptr = new _basetypes.Vector4d(v0, v1, v2, v3) self.delete_thisptr = True @@ -197,7 +206,14 @@ cdef class Vector4d: def squared_norm(self): return self.thisptr.squaredNorm() - # TODO add toarray / fromarray + def toarray(self): + cdef np.ndarray[double, ndim=1] array = np.empty(4) + cdef int i + for i in range(4): + array[i] = self.thisptr.data()[i] + return array + + # TODO add fromarray cdef class Matrix3d: @@ -528,6 +544,9 @@ cdef class StringVectorReference: def __setitem__(self, int i, string s): deref(self.thisptr)[i] = s + def size(self): + return self.thisptr.size() + cdef class JointStateVectorReference: cdef vector[_basetypes.JointState]* thisptr @@ -545,6 +564,9 @@ cdef class JointStateVectorReference: joint_state.thisptr = &deref(self.thisptr)[i] return joint_state + def size(self): + return self.thisptr.size() + cdef class RigidBodyState: cdef _basetypes.RigidBodyState* thisptr @@ -807,4 +829,272 @@ cdef class Frame: # TODO FramePair -# TODO Frame, LaserScan, IMUSensors, PointCloud +cdef class Pointcloud: + cdef _basetypes.Pointcloud* thisptr + cdef bool delete_thisptr + + def __cinit__(self): + self.thisptr = NULL + self.delete_thisptr = False + + def __dealloc__(self): + if self.thisptr != NULL and self.delete_thisptr: + del self.thisptr + + def __init__(self): + self.thisptr = new _basetypes.Pointcloud() + self.delete_thisptr = True + + @property + def points(self): + cdef Vector3dVectorReference points = Vector3dVectorReference() + points.thisptr = &self.thisptr.points + return points + + @property + def colors(self): + cdef Vector4dVectorReference colors = Vector4dVectorReference() + colors.thisptr = &self.thisptr.colors + return colors + + +cdef class Vector3dVectorReference: + cdef vector[_basetypes.Vector3d]* thisptr + + def __cinit__(self): + self.thisptr = NULL + + def __dealloc__(self): + pass + + def __getitem__(self, int i): + cdef Vector3d v = Vector3d() + del v.thisptr + v.delete_thisptr = False + v.thisptr = &deref(self.thisptr)[i] + return v + + def resize(self, int i): + self.thisptr.resize(i) + + def size(self): + return self.thisptr.size() + + +cdef class Vector4dVectorReference: + cdef vector[_basetypes.Vector4d]* thisptr + + def __cinit__(self): + self.thisptr = NULL + + def __dealloc__(self): + pass + + def __getitem__(self, int i): + cdef Vector4d v = Vector4d() + del v.thisptr + v.delete_thisptr = False + v.thisptr = &deref(self.thisptr)[i] + return v + + def resize(self, int i): + self.thisptr.resize(i) + + def size(self): + return self.thisptr.size() + + +cdef class LaserScan: + cdef _basetypes.LaserScan* thisptr + cdef bool delete_thisptr + + def __cinit__(self): + self.thisptr = NULL + self.delete_thisptr = False + + def __dealloc__(self): + if self.thisptr != NULL and self.delete_thisptr: + del self.thisptr + + def __init__(self): + self.thisptr = new _basetypes.LaserScan() + self.delete_thisptr = True + + def _get_time(self): + cdef Time time = Time() + del time.thisptr + time.thisptr = &self.thisptr.time + time.delete_thisptr = False + return time + + def _set_time(self, Time time): + self.thisptr.time = deref(time.thisptr) + + time = property(_get_time, _set_time) + + def _get_start_angle(self): + return self.thisptr.start_angle + + def _set_start_angle(self, double start_angle): + self.thisptr.start_angle = start_angle + + start_angle = property(_get_start_angle, _set_start_angle) + + def _get_angular_resolution(self): + return self.thisptr.angular_resolution + + def _set_angular_resolution(self, double angular_resolution): + self.thisptr.angular_resolution = angular_resolution + + angular_resolution = property(_get_angular_resolution, _set_angular_resolution) + + def _get_speed(self): + return self.thisptr.speed + + def _set_speed(self, double speed): + self.thisptr.speed = speed + + speed = property(_get_speed, _set_speed) + + @property + def ranges(self): + cdef UInt32VectorReference ranges = UInt32VectorReference() + ranges.thisptr = &self.thisptr.ranges + return ranges + + def _get_min_range(self): + return self.thisptr.minRange + + def _set_min_range(self, uint32_t min_range): + self.thisptr.minRange = min_range + + min_range = property(_get_min_range, _set_min_range) + + def _get_max_range(self): + return self.thisptr.maxRange + + def _set_max_range(self, uint32_t max_range): + self.thisptr.maxRange = max_range + + max_range = property(_get_max_range, _set_max_range) + + @property + def remission(self): + cdef FloatVectorReference remission = FloatVectorReference() + remission.thisptr = &self.thisptr.remission + return remission + + def is_valid_beam(self, unsigned int i): + return self.thisptr.isValidBeam(i) + + def is_range_valid(self, uint32_t r): + return self.thisptr.isRangeValid(r) + + +cdef class UInt32VectorReference: + cdef vector[uint32_t]* thisptr + + def __cinit__(self): + self.thisptr = NULL + + def __dealloc__(self): + pass + + def __getitem__(self, int i): + return deref(self.thisptr)[i] + + def __setitem__(self, int i, uint32_t v): + deref(self.thisptr)[i] = v + + def resize(self, int i): + self.thisptr.resize(i) + + def size(self): + return self.thisptr.size() + + +cdef class FloatVectorReference: + cdef vector[float]* thisptr + + def __cinit__(self): + self.thisptr = NULL + + def __dealloc__(self): + pass + + def __getitem__(self, int i): + return deref(self.thisptr)[i] + + def __setitem__(self, int i, float v): + deref(self.thisptr)[i] = v + + def resize(self, int i): + self.thisptr.resize(i) + + def size(self): + return self.thisptr.size() + + +cdef class IMUSensors: + cdef _basetypes.IMUSensors* thisptr + cdef bool delete_thisptr + + def __cinit__(self): + self.thisptr = NULL + self.delete_thisptr = False + + def __dealloc__(self): + if self.thisptr != NULL and self.delete_thisptr: + del self.thisptr + + def __init__(self): + self.thisptr = new _basetypes.IMUSensors() + self.delete_thisptr = True + + def _get_time(self): + cdef Time time = Time() + del time.thisptr + time.thisptr = &self.thisptr.time + time.delete_thisptr = False + return time + + def _set_time(self, Time time): + self.thisptr.time = deref(time.thisptr) + + time = property(_get_time, _set_time) + + def _get_acc(self): + cdef Vector3d acc = Vector3d() + del acc.thisptr + acc.delete_thisptr = False + acc.thisptr = &self.thisptr.acc + return acc + + def _set_acc(self, Vector3d value): + self.thisptr.acc = deref(value.thisptr) + + acc = property(_get_acc, _set_acc) + + def _get_gyro(self): + cdef Vector3d gyro = Vector3d() + del gyro.thisptr + gyro.delete_thisptr = False + gyro.thisptr = &self.thisptr.gyro + return gyro + + def _set_gyro(self, Vector3d value): + self.thisptr.gyro = deref(value.thisptr) + + gyro = property(_get_gyro, _set_gyro) + + def _get_mag(self): + cdef Vector3d mag = Vector3d() + del mag.thisptr + mag.delete_thisptr = False + mag.thisptr = &self.thisptr.mag + return mag + + def _set_mag(self, Vector3d value): + self.thisptr.mag = deref(value.thisptr) + + mag = property(_get_mag, _set_mag) diff --git a/bindings/python/test.py b/bindings/python/test.py index 9a96530f..c219ffa0 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -268,3 +268,77 @@ def test_create_frame_rgb(): assert_equal(image.dtype, np.uint8) frame.image = np.ones(image.shape, dtype=np.uint8) assert_true(np.all(frame.image == 1)) + + +def test_create_frame_gray(): + GRAYSCALE = 1 + VALID = 1 + frame = basetypes.Frame(800, 600, 1, GRAYSCALE, VALID, 800 * 600 * 1) + assert_equal(frame.get_width(), 800) + assert_equal(frame.get_height(), 600) + assert_equal(frame.get_channel_count(), 1) + assert_equal(frame.get_data_depth(), 1) + image = frame.image + assert_array_equal(image.shape, (800, 600)) + assert_equal(image.dtype, np.uint8) + frame.image = np.ones(image.shape, dtype=np.uint8) + assert_true(np.all(frame.image == 1)) + + +def test_create_pointcloud(): + pcl = basetypes.Pointcloud() + + pcl.points.resize(100) + point = pcl.points[0] + point.x = 1.0 + point.y = 2.0 + point.z = 3.0 + assert_equal(pcl.points.size(), 100) + assert_array_equal(pcl.points[0].toarray(), (1.0, 2.0, 3.0)) + + pcl.colors.resize(100) + color = pcl.colors[0] + color[0] = 255.0 + color[1] = 255.0 + color[2] = 255.0 + color[3] = 255.0 + assert_equal(pcl.colors.size(), 100) + assert_array_equal(pcl.colors[0].toarray(), (255.0, 255.0, 255.0, 255.0)) + + +def test_laser_scan(): + ls = basetypes.LaserScan() + + time = basetypes.Time.now() + ls.time = time + assert_equal(ls.time.microseconds, time.microseconds) + + ls.min_range = 20 + ls.max_range = 30 + + ls.ranges.resize(10) + ls.remission.resize(10) + for i in range(10): + ls.ranges[i] = 25 + ls.remission[i] = 0.0 + ls.ranges[5] = 10 + assert_false(ls.is_valid_beam(5)) + assert_true(ls.is_valid_beam(0)) + assert_false(ls.is_range_valid(500)) + assert_true(ls.is_range_valid(25)) + + +def test_imu_sensors(): + imu = basetypes.IMUSensors() + + imu.time = basetypes.Time.now() + assert_false(imu.time.is_null()) + + imu.acc.x = 1.0 + assert_equal(imu.acc.x, 1.0) + + imu.gyro.x = 2.0 + assert_equal(imu.gyro.x, 2.0) + + imu.mag.x = 3.0 + assert_equal(imu.mag.x, 3.0) From 8065c90dc4148293a7ea3a893da73286715c61fb Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 12 Jul 2017 18:25:57 +0200 Subject: [PATCH 14/39] Implement some operators --- bindings/python/_basetypes.pxd | 19 +++++++ bindings/python/basetypes.pyx | 94 +++++++++++++++++++++++++++++++++- bindings/python/test.py | 87 ++++++++++++++++++++++++++++--- 3 files changed, 191 insertions(+), 9 deletions(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index 70a75a0a..b873199a 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -10,7 +10,26 @@ cdef extern from "base/Time.hpp" namespace "base": int microseconds + bool operator<(Time) + bool operator>(Time) + bool operator==(Time) + bool operator!=(Time) + bool operator>=(Time) + bool operator<=(Time) + Time operator-(Time) + Time operator+(Time) + Time operator/(int) + Time operator*(double) + bool isNull() + string toString(Resolution, string) + + +cdef extern from "base/Time.hpp" namespace "base::Time": + cdef enum Resolution: + Seconds + Milliseconds + Microseconds cdef extern from "base/Time.hpp" namespace "base::Time": diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index c870598d..e0f6a9b2 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -9,6 +9,9 @@ import numpy as np cimport _basetypes +np.import_array() # must be here because we use the NumPy C API + + cdef class Time: cdef _basetypes.Time* thisptr cdef bool delete_thisptr @@ -25,6 +28,9 @@ cdef class Time: self.thisptr = new _basetypes.Time() self.delete_thisptr = True + def __str__(self): + return self.thisptr.toString(_basetypes.Resolution.Microseconds, "%Y%m%d-%H:%M:%S") + def _get_microseconds(self): return self.thisptr.microseconds @@ -42,6 +48,58 @@ cdef class Time: time.thisptr[0] = _basetypes.now() return time + def __richcmp__(Time self, Time other, int op): + if op == 0: + return deref(self.thisptr) < deref(other.thisptr) + elif op == 1: + return deref(self.thisptr) <= deref(other.thisptr) + elif op == 2: + return deref(self.thisptr) == deref(other.thisptr) + elif op == 3: + return deref(self.thisptr) != deref(other.thisptr) + elif op == 4: + return deref(self.thisptr) > deref(other.thisptr) + elif op == 5: + return deref(self.thisptr) >= deref(other.thisptr) + else: + raise ValueError("Unknown comparison operation %d" % op) + + def __add__(Time self, Time other): + cdef Time time = Time() + time.thisptr[0] = deref(self.thisptr) + deref(other.thisptr) + return time + + def __iadd__(Time self, Time other): + self.thisptr[0] = deref(self.thisptr) + deref(other.thisptr) + return self + + def __sub__(Time self, Time other): + cdef Time time = Time() + time.thisptr[0] = deref(self.thisptr) - deref(other.thisptr) + return time + + def __isub__(Time self, Time other): + self.thisptr[0] = deref(self.thisptr) - deref(other.thisptr) + return self + + def __div__(Time self, int divider): + cdef Time time = Time() + time.thisptr[0] = deref(self.thisptr) / divider + return time + + def __idiv__(Time self, int divider): + self.thisptr[0] = deref(self.thisptr) / divider + return self + + def __mul__(Time self, double factor): + cdef Time time = Time() + time.thisptr[0] = deref(self.thisptr) * factor + return time + + def __imul__(Time self, double factor): + self.thisptr[0] = deref(self.thisptr) * factor + return self + cdef class Vector2d: cdef _basetypes.Vector2d* thisptr @@ -62,6 +120,12 @@ cdef class Vector2d: def __str__(self): return "[%.2f, %.2f]" % (self.thisptr.get(0), self.thisptr.get(1)) + def __array__(self, dtype=None): + cdef np.npy_intp shape[1] + shape[0] = 2 + return np.PyArray_SimpleNewFromData( + 1, shape, np.NPY_DOUBLE, self.thisptr.data()) + def __getitem__(self, int i): if i < 0 or i > 1: raise KeyError("index must be 0 or 1 but was %d" % i) @@ -94,7 +158,14 @@ cdef class Vector2d: def squared_norm(self): return self.thisptr.squaredNorm() - # TODO add toarray / fromarray + def toarray(self): + cdef np.ndarray[double, ndim=1] array = np.empty(2) + cdef int i + for i in range(2): + array[i] = self.thisptr.data()[i] + return array + + # TODO add fromarray cdef class Vector3d: @@ -118,6 +189,12 @@ cdef class Vector3d: self.thisptr.get(1), self.thisptr.get(2)) + def __array__(self, dtype=None): + cdef np.npy_intp shape[1] + shape[0] = 3 + return np.PyArray_SimpleNewFromData( + 1, shape, np.NPY_DOUBLE, self.thisptr.data()) + def __getitem__(self, int i): if i < 0 or i > 2: raise KeyError("index must be 0, 1 or 2 but was %d" % i) @@ -190,6 +267,12 @@ cdef class Vector4d: self.thisptr.get(2), self.thisptr.get(3) ) + def __array__(self, dtype=None): + cdef np.npy_intp shape[1] + shape[0] = 4 + return np.PyArray_SimpleNewFromData( + 1, shape, np.NPY_DOUBLE, self.thisptr.data()) + def __getitem__(self, int i): if i < 0 or i > 3: raise KeyError("index must be in [0, 3] but was %d" % i) @@ -247,6 +330,13 @@ cdef class Matrix3d: self.thisptr.get(2, 0), self.thisptr.get(2, 1), self.thisptr.get(2, 2), ) + def __array__(self, dtype=None): + cdef np.npy_intp shape[2] + shape[0] = 3 + shape[1] = 3 + return np.PyArray_SimpleNewFromData( + 2, shape, np.NPY_DOUBLE, self.thisptr.data()).T + def __getitem__(self, tuple indices): cdef int i cdef int j @@ -283,7 +373,7 @@ cdef class Matrix3d: self.thisptr.data()[3 * j + i] = array[i, j] -# TODO Vector6d, VectorXd, Point, Pose +# TODO Vector6d, VectorXd, Pose cdef class Quaterniond: diff --git a/bindings/python/test.py b/bindings/python/test.py index c219ffa0..c4b391a7 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -11,19 +11,70 @@ def test_get_set_microseconds(): assert_equal(t.microseconds, m) +def test_time_operators(): + t1 = basetypes.Time() + t1.microseconds = 0 + t2 = basetypes.Time() + t2.microseconds = 1 + t3 = basetypes.Time() + t3.microseconds = 1 + t4 = basetypes.Time() + t4.microseconds = 2 + + assert_true(t1 < t2 < t4) + assert_true(t4 > t2 > t1) + assert_true(t1 != t2) + assert_false(t2 != t3) + assert_true(t2 == t3) + assert_false(t1 == t2) + assert_true(t2 >= t3) + assert_false(t2 > t3) + assert_true(t2 <= t3) + assert_false(t2 < t3) + + assert_true(t2 + t2 == t4) + assert_true(t4 - t2 == t2) + assert_true(t4 / 2 == t2) + assert_true(t2 * 2 == t4) + + t5 = basetypes.Time() + t5.microseconds = 10 + t5 /= 2 + assert_equal(t5.microseconds, 5) + t5 -= t2 * 4 + assert_equal(t5, t2) + t5 *= 2 + assert_equal(t5, t4) + t5 += t1 + assert_equal(t5, t4) + + def test_vector2d_ctor(): v = basetypes.Vector2d(1.0, 2.0) assert_equal(str(v), "[1.00, 2.00]") +def test_vector2d_as_ndarray(): + random_state = np.random.RandomState(843) + r = random_state.randn(2, 2) + v = basetypes.Vector2d(3.23, 2.24) + rv = r.dot(np.asarray(v)) + rv2 = r.dot(v.toarray()) + assert_array_almost_equal(rv, rv2) + + def test_vector3d_ctor(): v = basetypes.Vector3d(1.0, 2.0, 3.0) assert_equal(str(v), "[1.00, 2.00, 3.00]") -def test_vector4d_ctor(): - v = basetypes.Vector4d(1.0, 2.0, 3.0, 4.0) - assert_equal(str(v), "[1.00, 2.00, 3.00, 4.00]") +def test_vector3d_as_ndarray(): + random_state = np.random.RandomState(843) + r = random_state.randn(3, 3) + v = basetypes.Vector3d(3.23, 2.24, 3.63) + rv = r.dot(np.asarray(v)) + rv2 = r.dot(v.toarray()) + assert_array_almost_equal(rv, rv2) def test_vector3d_get_set_data(): @@ -47,6 +98,26 @@ def assign(i): assert_raises_regexp(KeyError, "index must be", assign, 3) +def test_norms(): + v = basetypes.Vector3d(1.0, 2.0, 3.0) + assert_almost_equal(v.norm(), 3.741657387) + assert_equal(v.squared_norm(), 14.0) + + +def test_vector4d_ctor(): + v = basetypes.Vector4d(1.0, 2.0, 3.0, 4.0) + assert_equal(str(v), "[1.00, 2.00, 3.00, 4.00]") + + +def test_vector4d_as_ndarray(): + random_state = np.random.RandomState(843) + r = random_state.randn(4, 4) + v = basetypes.Vector4d(3.23, 2.24, 3.63, 2.05) + rv = r.dot(np.asarray(v)) + rv2 = r.dot(v.toarray()) + assert_array_almost_equal(rv, rv2) + + def test_matrix3d_get_set_data(): m = basetypes.Matrix3d() m[0, 1] = 1.0 @@ -57,10 +128,12 @@ def test_matrix3d_get_set_data(): assert_array_almost_equal(m.toarray(), r) -def test_norms(): - v = basetypes.Vector3d(1.0, 2.0, 3.0) - assert_almost_equal(v.norm(), 3.741657387) - assert_equal(v.squared_norm(), 14.0) +def test_matrix3d_array_access(): + random_state = np.random.RandomState(843) + m = basetypes.Matrix3d() + r = random_state.randn(3, 3) + m.fromarray(r) + assert_array_equal(np.asarray(m), r) def test_quaterniond_ctor(): From e7f6d8947e483654b1c7007374ab23e75e5ea8c2 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 12 Jul 2017 18:44:49 +0200 Subject: [PATCH 15/39] Organize todos --- bindings/python/basetypes.pyx | 103 +++++++++++++++++++++++++--------- bindings/python/test.py | 12 ++-- 2 files changed, 81 insertions(+), 34 deletions(-) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index e0f6a9b2..6bb492d7 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -48,6 +48,8 @@ cdef class Time: time.thisptr[0] = _basetypes.now() return time + # TODO more factory methods + def __richcmp__(Time self, Time other, int op): if op == 0: return deref(self.thisptr) < deref(other.thisptr) @@ -165,7 +167,7 @@ cdef class Vector2d: array[i] = self.thisptr.data()[i] return array - # TODO add fromarray + # TODO add operators, fromarray cdef class Vector3d: @@ -242,7 +244,7 @@ cdef class Vector3d: array[i] = self.thisptr.data()[i] return array - # TODO add fromarray + # TODO add operators, fromarray cdef class Vector4d: @@ -296,7 +298,7 @@ cdef class Vector4d: array[i] = self.thisptr.data()[i] return array - # TODO add fromarray + # TODO add operators, fromarray cdef class Matrix3d: @@ -372,6 +374,8 @@ cdef class Matrix3d: for j in range(3): self.thisptr.data()[3 * j + i] = array[i, j] + # TODO operators + # TODO Vector6d, VectorXd, Pose @@ -406,6 +410,8 @@ cdef class Quaterniond: def fromarray(self, np.ndarray[double, ndim=1] array): self.thisptr[0] = _basetypes.Quaterniond(array[0], array[1], array[2], array[3]) + # TODO how can we modify a quaternion? + cdef class TransformWithCovariance: cdef _basetypes.TransformWithCovariance* thisptr @@ -423,6 +429,10 @@ cdef class TransformWithCovariance: self.thisptr = new _basetypes.TransformWithCovariance() self.delete_thisptr = True + def __str__(self): + return "(translation=%s, orientation=%s)" % (self.translation, + self.orientation) + def _get_translation(self): cdef Vector3d translation = Vector3d() del translation.thisptr @@ -447,9 +457,7 @@ cdef class TransformWithCovariance: orientation = property(_get_orientation, _set_orientation) - def __str__(self): - return "(translation=%s, orientation=%s)" % (self.translation, - self.orientation) + # TODO covariance cdef class JointState: @@ -618,6 +626,8 @@ cdef class Joints: cdef int i = self.thisptr.mapNameToIndex(name) self.thisptr.elements[i] = deref(joint_state.thisptr) + # TODO factory methods + cdef class StringVectorReference: cdef vector[string]* thisptr @@ -800,34 +810,33 @@ cdef class RigidBodyState: _get_cov_angular_velocity, _set_cov_angular_velocity) -""" TODO cdef class frame_mode_t: - MODE_UNDEFINED = _basetypes.MODE_UNDEFINED - MODE_GRAYSCALE = _basetypes.MODE_GRAYSCALE - MODE_RGB = _basetypes.MODE_RGB - MODE_UYVY = _basetypes.MODE_UYVY - MODE_BGR = _basetypes.MODE_BGR - MODE_RGB32 = _basetypes.MODE_RGB32 - RAW_MODES = _basetypes.RAW_MODES - MODE_BAYER = _basetypes.MODE_BAYER - MODE_BAYER_RGGB = _basetypes.MODE_BAYER_RGGB - MODE_BAYER_GRBG = _basetypes.MODE_BAYER_GRBG - MODE_BAYER_BGGR = _basetypes.MODE_BAYER_BGGR - MODE_BAYER_GBRG = _basetypes.MODE_BAYER_GBRG - COMPRESSED_MODES = _basetypes.COMPRESSED_MODES - MODE_PJPG = _basetypes.MODE_PJPG - MODE_JPEG = _basetypes.MODE_JPEG - MODE_PNG = _basetypes.MODE_PNG + MODE_UNDEFINED = _basetypes.frame_mode_t.MODE_UNDEFINED + MODE_GRAYSCALE = _basetypes.frame_mode_t.MODE_GRAYSCALE + MODE_RGB = _basetypes.frame_mode_t.MODE_RGB + MODE_UYVY = _basetypes.frame_mode_t.MODE_UYVY + MODE_BGR = _basetypes.frame_mode_t.MODE_BGR + #MODE_RGB32 = _basetypes.frame_mode_t.MODE_RGB32 # TODO I don't know why but this value is "not known" + RAW_MODES = _basetypes.frame_mode_t.RAW_MODES + MODE_BAYER = _basetypes.frame_mode_t.MODE_BAYER + MODE_BAYER_RGGB = _basetypes.frame_mode_t.MODE_BAYER_RGGB + MODE_BAYER_GRBG = _basetypes.frame_mode_t.MODE_BAYER_GRBG + MODE_BAYER_BGGR = _basetypes.frame_mode_t.MODE_BAYER_BGGR + MODE_BAYER_GBRG = _basetypes.frame_mode_t.MODE_BAYER_GBRG + COMPRESSED_MODES = _basetypes.frame_mode_t.COMPRESSED_MODES + MODE_PJPG = _basetypes.frame_mode_t.MODE_PJPG + MODE_JPEG = _basetypes.frame_mode_t.MODE_JPEG + MODE_PNG = _basetypes.frame_mode_t.MODE_PNG cdef class frame_status_t: - STATUS_EMPTY = _basetypes.STATUS_EMPTY - STATUS_VALID = _basetypes.STATUS_VALID - STATUS_INVALID = _basetypes.STATUS_INVALID -""" + STATUS_EMPTY = _basetypes.frame_status_t.STATUS_EMPTY + STATUS_VALID = _basetypes.frame_status_t.STATUS_VALID + STATUS_INVALID = _basetypes.frame_status_t.STATUS_INVALID cdef class Frame: + # TODO frame attributes cdef _basetypes.Frame* thisptr cdef bool delete_thisptr @@ -871,7 +880,9 @@ cdef class Frame: received_time = property(_get_received_time, _set_received_time) - def _get_image(self): # TODO can we return a reference? + def _get_image(self): + # TODO can we return a reference? + # TODO what if it is compressed? cdef int n_channels = self.thisptr.getChannelCount() dimensions = (self.thisptr.size.width, self.thisptr.size.height) if n_channels > 1: @@ -1188,3 +1199,39 @@ cdef class IMUSensors: self.thisptr.mag = deref(value.thisptr) mag = property(_get_mag, _set_mag) + + +# TODO missing types: +# typedefs: Position, Point, Orientation +# Angle !!! +# (CircularBuffer) +# Eigen::Matrix3/4/6/Xd +# Eigen::Affine3d +# Eigen::Isometry3d +# JointLimitRange +# JointLimits +# JointsTrajectory +# JointTransform +# Pose / Pose2D +# (Spline) +# Temperature +# TimeMark +# Timeout +# Trajectory +# TwistWithCovariance +# Waypoint +# Wrench +# ~~commands/Joints~~ +# commands/LinearAngular6DCommand +# commands/Motion2D +# (commands/Speed6D) +# samples/BodyState - what is the difference to RigidBodyState??? +# samples/CommandSamples +# samples/DepthMap !!! +# samples/DistanceImage +# samples/Pressure +# (samples/RigidBodyAcceleration) +# (samples/Sonar) +# (samples/SonarBeam) +# samples/Wrench +# samples/Wrenches diff --git a/bindings/python/test.py b/bindings/python/test.py index c4b391a7..de834c16 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -329,9 +329,9 @@ def test_rigid_body_state_get_set_cov_angular_velocity(): def test_create_frame_rgb(): - RGB = 2 - VALID = 1 - frame = basetypes.Frame(800, 600, 1, RGB, VALID, 800 * 600 * 3) + frame = basetypes.Frame( + 800, 600, 1, basetypes.frame_mode_t.MODE_RGB, + basetypes.frame_status_t.STATUS_VALID, 800 * 600 * 3) assert_equal(frame.get_width(), 800) assert_equal(frame.get_height(), 600) assert_equal(frame.get_channel_count(), 3) @@ -344,9 +344,9 @@ def test_create_frame_rgb(): def test_create_frame_gray(): - GRAYSCALE = 1 - VALID = 1 - frame = basetypes.Frame(800, 600, 1, GRAYSCALE, VALID, 800 * 600 * 1) + frame = basetypes.Frame( + 800, 600, 1, basetypes.frame_mode_t.MODE_GRAYSCALE, + basetypes.frame_status_t.STATUS_VALID, 800 * 600 * 1) assert_equal(frame.get_width(), 800) assert_equal(frame.get_height(), 600) assert_equal(frame.get_channel_count(), 1) From 4c0734c34cae38ec6aeb65f97a911ffb0fcbfa5f Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 12 Jul 2017 18:50:06 +0200 Subject: [PATCH 16/39] Add readme --- bindings/python/README.md | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 bindings/python/README.md diff --git a/bindings/python/README.md b/bindings/python/README.md new file mode 100644 index 00000000..d5ea6a97 --- /dev/null +++ b/bindings/python/README.md @@ -0,0 +1,10 @@ +# Python bindings for base-types + +You can build this prototype with + + python setup.py build_ext -i + +There are still some open issues, e.g. +* How can we handle compressed Frame objects (e.g. JPG, PNG)? +* How can we make vectors / matricies look and feel more like numpy arrays? +* ... \ No newline at end of file From a63c4f94d3dba19d2a0864eca88e1742bc1aa219 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 12 Jul 2017 18:52:27 +0200 Subject: [PATCH 17/39] Update README.md --- bindings/python/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/bindings/python/README.md b/bindings/python/README.md index d5ea6a97..3df561f5 100644 --- a/bindings/python/README.md +++ b/bindings/python/README.md @@ -7,4 +7,5 @@ You can build this prototype with There are still some open issues, e.g. * How can we handle compressed Frame objects (e.g. JPG, PNG)? * How can we make vectors / matricies look and feel more like numpy arrays? +* You can find todos all over the files. * ... \ No newline at end of file From c02fea910c4f0a02716e60c4e089daa5470ccaef Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Wed, 26 Jul 2017 17:35:47 +0200 Subject: [PATCH 18/39] Update installation instructions --- bindings/python/README.md | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/bindings/python/README.md b/bindings/python/README.md index 3df561f5..af42fcb6 100644 --- a/bindings/python/README.md +++ b/bindings/python/README.md @@ -1,11 +1,24 @@ # Python bindings for base-types +## Install + You can build this prototype with python setup.py build_ext -i +For now, you have to add this folder to your PYTHONPATH environment variable: + + export PYTHONPATH=$PYTHONPATH: + +You could also install the bindings to some location that is already in the +Python path: + + python setup.py install --prefix + +## Note + There are still some open issues, e.g. * How can we handle compressed Frame objects (e.g. JPG, PNG)? * How can we make vectors / matricies look and feel more like numpy arrays? * You can find todos all over the files. -* ... \ No newline at end of file +* ... From 12f5cf3e46ae2d2ce33d737eff0691e5c84bbd42 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Thu, 27 Jul 2017 10:37:00 +0200 Subject: [PATCH 19/39] Add Vector3d.fromarray() --- bindings/python/basetypes.pyx | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 6bb492d7..c4723f52 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -244,7 +244,12 @@ cdef class Vector3d: array[i] = self.thisptr.data()[i] return array - # TODO add operators, fromarray + def fromarray(self, np.ndarray[double, ndim=1] array): + cdef int i + for i in range(3): + self.thisptr.data()[3 * i] = array[i] + + # TODO add operators cdef class Vector4d: From dffde7719b36c73ccf0023362f95f0982498e999 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Thu, 27 Jul 2017 14:24:49 +0200 Subject: [PATCH 20/39] Fix Vector3d.fromarray() --- bindings/python/basetypes.pyx | 2 +- bindings/python/test.py | 9 +++++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index c4723f52..32df8bee 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -247,7 +247,7 @@ cdef class Vector3d: def fromarray(self, np.ndarray[double, ndim=1] array): cdef int i for i in range(3): - self.thisptr.data()[3 * i] = array[i] + self.thisptr.data()[i] = array[i] # TODO add operators diff --git a/bindings/python/test.py b/bindings/python/test.py index de834c16..1f41a54f 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -68,6 +68,15 @@ def test_vector3d_ctor(): assert_equal(str(v), "[1.00, 2.00, 3.00]") +def test_vector3d_fromarray(): + v = basetypes.Vector3d() + a = np.array([-2.32, 2.42, 54.23]) + v.fromarray(a) + assert_equal(v[0], a[0]) + assert_equal(v[1], a[1]) + assert_equal(v[2], a[2]) + + def test_vector3d_as_ndarray(): random_state = np.random.RandomState(843) r = random_state.randn(3, 3) From 48ab2d326018a95f108e0442276599f64bc42a13 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Thu, 27 Jul 2017 16:01:12 +0200 Subject: [PATCH 21/39] Use correct type for timestamp --- bindings/python/_basetypes.pxd | 4 ++-- bindings/python/basetypes.pyx | 4 ++-- bindings/python/test.py | 9 ++++++++- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index b873199a..44a2b8c3 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -1,14 +1,14 @@ from libcpp.string cimport string from libcpp.vector cimport vector from libcpp cimport bool -from libc.stdint cimport uint8_t, uint16_t, uint32_t +from libc.stdint cimport uint8_t, uint16_t, uint32_t, int64_t cdef extern from "base/Time.hpp" namespace "base": cdef cppclass Time: Time() - int microseconds + int64_t microseconds bool operator<(Time) bool operator>(Time) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 32df8bee..ff375c29 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -3,7 +3,7 @@ from cython.operator cimport dereference as deref from libcpp.string cimport string from libcpp.vector cimport vector from libcpp cimport bool -from libc.stdint cimport uint8_t, uint16_t, uint32_t, uint64_t +from libc.stdint cimport uint8_t, uint16_t, uint32_t, uint64_t, int64_t cimport numpy as np import numpy as np cimport _basetypes @@ -34,7 +34,7 @@ cdef class Time: def _get_microseconds(self): return self.thisptr.microseconds - def _set_microseconds(self, int microseconds): + def _set_microseconds(self, int64_t microseconds): self.thisptr.microseconds = microseconds microseconds = property(_get_microseconds, _set_microseconds) diff --git a/bindings/python/test.py b/bindings/python/test.py index 1f41a54f..12e8b0df 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -1,6 +1,6 @@ import basetypes import numpy as np -from nose.tools import assert_equal, assert_raises_regexp, assert_almost_equal, assert_false, assert_true +from nose.tools import assert_equal, assert_raises_regexp, assert_almost_equal, assert_false, assert_true, assert_greater from numpy.testing import assert_array_equal, assert_array_almost_equal @@ -11,6 +11,13 @@ def test_get_set_microseconds(): assert_equal(t.microseconds, m) +def test_no_overflow(): + t = basetypes.Time.now() + mus = t.microseconds + assert_greater(mus, 0) + t.microseconds = mus + + def test_time_operators(): t1 = basetypes.Time() t1.microseconds = 0 From 1b7005102390fabbb25500c44a633d03c3e011cf Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Fri, 28 Jul 2017 13:56:08 +0200 Subject: [PATCH 22/39] Add dependencies --- manifest.xml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/manifest.xml b/manifest.xml index bbcaea44..8c2d4799 100644 --- a/manifest.xml +++ b/manifest.xml @@ -19,5 +19,10 @@ + + + + + stable From 931ad08be5d8f1b83285feaca99a33117295c2fe Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 1 Aug 2017 11:19:22 +0200 Subject: [PATCH 23/39] Update installation instructions --- bindings/python/README.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/bindings/python/README.md b/bindings/python/README.md index af42fcb6..53e024dd 100644 --- a/bindings/python/README.md +++ b/bindings/python/README.md @@ -15,6 +15,19 @@ Python path: python setup.py install --prefix +The more convenient way is to use autoproj. You must add three lines to +your manifest: + + package_sets: + ... + - type: git + url: git@git.hb.dfki.de:InFuse/infuse-package_set.git + + layout: + ... + - tools/infuselog2cpp + ... + ## Note There are still some open issues, e.g. From d167a43e9f596ee2ea627f6fc7d68b2e1636cb23 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 1 Aug 2017 11:54:58 +0200 Subject: [PATCH 24/39] Extract Cython declaration file --- bindings/python/basetypes.pxd | 98 +++++++++++++++++++++++++++++++++++ bindings/python/basetypes.pyx | 59 +-------------------- 2 files changed, 99 insertions(+), 58 deletions(-) create mode 100644 bindings/python/basetypes.pxd diff --git a/bindings/python/basetypes.pxd b/bindings/python/basetypes.pxd new file mode 100644 index 00000000..889aa2f5 --- /dev/null +++ b/bindings/python/basetypes.pxd @@ -0,0 +1,98 @@ +from libcpp cimport bool +from libcpp.vector cimport vector +from libcpp.string cimport string +from libc.stdint cimport uint32_t +cimport _basetypes + +cdef class Time: + cdef _basetypes.Time* thisptr + cdef bool delete_thisptr + + +cdef class Vector2d: + cdef _basetypes.Vector2d* thisptr + cdef bool delete_thisptr + + +cdef class Vector3d: + cdef _basetypes.Vector3d* thisptr + cdef bool delete_thisptr + + +cdef class Vector4d: + cdef _basetypes.Vector4d* thisptr + cdef bool delete_thisptr + + +cdef class Matrix3d: + cdef _basetypes.Matrix3d* thisptr + cdef bool delete_thisptr + + +cdef class Quaterniond: + cdef _basetypes.Quaterniond* thisptr + cdef bool delete_thisptr + + +cdef class TransformWithCovariance: + cdef _basetypes.TransformWithCovariance* thisptr + cdef bool delete_thisptr + + +cdef class JointState: + cdef _basetypes.JointState* thisptr + cdef bool delete_thisptr + + +cdef class Joints: + cdef _basetypes.Joints* thisptr + cdef bool delete_thisptr + + +cdef class StringVectorReference: + cdef vector[string]* thisptr + + +cdef class JointStateVectorReference: + cdef vector[_basetypes.JointState]* thisptr + + +cdef class RigidBodyState: + cdef _basetypes.RigidBodyState* thisptr + cdef bool delete_thisptr + + +cdef class Frame: + cdef _basetypes.Frame* thisptr + cdef bool delete_thisptr + + +cdef class Pointcloud: + cdef _basetypes.Pointcloud* thisptr + cdef bool delete_thisptr + + +cdef class Vector3dVectorReference: + cdef vector[_basetypes.Vector3d]* thisptr + + +cdef class Vector4dVectorReference: + cdef vector[_basetypes.Vector4d]* thisptr + + +cdef class LaserScan: + cdef _basetypes.LaserScan* thisptr + cdef bool delete_thisptr + + +cdef class UInt32VectorReference: + cdef vector[uint32_t]* thisptr + + +cdef class FloatVectorReference: + cdef vector[float]* thisptr + + +cdef class IMUSensors: + cdef _basetypes.IMUSensors* thisptr + cdef bool delete_thisptr diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index ff375c29..be47b7ed 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -1,21 +1,15 @@ # distutils: language = c++ +cimport basetypes from cython.operator cimport dereference as deref -from libcpp.string cimport string -from libcpp.vector cimport vector -from libcpp cimport bool from libc.stdint cimport uint8_t, uint16_t, uint32_t, uint64_t, int64_t cimport numpy as np import numpy as np -cimport _basetypes np.import_array() # must be here because we use the NumPy C API cdef class Time: - cdef _basetypes.Time* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -104,9 +98,6 @@ cdef class Time: cdef class Vector2d: - cdef _basetypes.Vector2d* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -171,9 +162,6 @@ cdef class Vector2d: cdef class Vector3d: - cdef _basetypes.Vector3d* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -253,9 +241,6 @@ cdef class Vector3d: cdef class Vector4d: - cdef _basetypes.Vector4d* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -307,9 +292,6 @@ cdef class Vector4d: cdef class Matrix3d: - cdef _basetypes.Matrix3d* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -386,9 +368,6 @@ cdef class Matrix3d: cdef class Quaterniond: - cdef _basetypes.Quaterniond* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -419,9 +398,6 @@ cdef class Quaterniond: cdef class TransformWithCovariance: - cdef _basetypes.TransformWithCovariance* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -466,9 +442,6 @@ cdef class TransformWithCovariance: cdef class JointState: - cdef _basetypes.JointState* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -583,9 +556,6 @@ cdef class JointState: cdef class Joints: - cdef _basetypes.Joints* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -635,8 +605,6 @@ cdef class Joints: cdef class StringVectorReference: - cdef vector[string]* thisptr - def __cinit__(self): self.thisptr = NULL @@ -654,8 +622,6 @@ cdef class StringVectorReference: cdef class JointStateVectorReference: - cdef vector[_basetypes.JointState]* thisptr - def __cinit__(self): self.thisptr = NULL @@ -674,9 +640,6 @@ cdef class JointStateVectorReference: cdef class RigidBodyState: - cdef _basetypes.RigidBodyState* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -842,9 +805,6 @@ cdef class frame_status_t: cdef class Frame: # TODO frame attributes - cdef _basetypes.Frame* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -936,9 +896,6 @@ cdef class Frame: cdef class Pointcloud: - cdef _basetypes.Pointcloud* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -965,8 +922,6 @@ cdef class Pointcloud: cdef class Vector3dVectorReference: - cdef vector[_basetypes.Vector3d]* thisptr - def __cinit__(self): self.thisptr = NULL @@ -988,8 +943,6 @@ cdef class Vector3dVectorReference: cdef class Vector4dVectorReference: - cdef vector[_basetypes.Vector4d]* thisptr - def __cinit__(self): self.thisptr = NULL @@ -1011,9 +964,6 @@ cdef class Vector4dVectorReference: cdef class LaserScan: - cdef _basetypes.LaserScan* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False @@ -1098,8 +1048,6 @@ cdef class LaserScan: cdef class UInt32VectorReference: - cdef vector[uint32_t]* thisptr - def __cinit__(self): self.thisptr = NULL @@ -1120,8 +1068,6 @@ cdef class UInt32VectorReference: cdef class FloatVectorReference: - cdef vector[float]* thisptr - def __cinit__(self): self.thisptr = NULL @@ -1142,9 +1088,6 @@ cdef class FloatVectorReference: cdef class IMUSensors: - cdef _basetypes.IMUSensors* thisptr - cdef bool delete_thisptr - def __cinit__(self): self.thisptr = NULL self.delete_thisptr = False From 9edf872d747655ed00048e19477e32511e5fcda0 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 1 Aug 2017 12:06:01 +0200 Subject: [PATCH 25/39] Install Cython declarations --- bindings/python/setup.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/bindings/python/setup.py b/bindings/python/setup.py index de46612b..c3cf6732 100644 --- a/bindings/python/setup.py +++ b/bindings/python/setup.py @@ -47,4 +47,16 @@ def strict_prototypes_workaround(): language="c++" ) ] - setup(ext_modules=cythonize(extensions)) + setup( + name="basetypes", + ext_modules=cythonize(extensions), + description="Python bindings for Rock base-types", + version="0.0.1", + maintainer="Alexander Fabisch", + maintainer_email="Alexander.Fabisch@dfki.de", + packages=[""], + package_dir={"": "."}, + package_data={ + "": ["_basetypes.pxd", "basetypes.pxd", "basetypes.pyx"] + } + ) From 561e0f30c8a1fad59c97abf687d0c46d65f71d9c Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 8 Aug 2017 10:28:14 +0200 Subject: [PATCH 26/39] Make resize for members of Joints possible --- bindings/python/basetypes.pyx | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index be47b7ed..5bd308e9 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -617,6 +617,9 @@ cdef class StringVectorReference: def __setitem__(self, int i, string s): deref(self.thisptr)[i] = s + def resize(self, int i): + self.thisptr.resize(i) + def size(self): return self.thisptr.size() @@ -635,6 +638,9 @@ cdef class JointStateVectorReference: joint_state.thisptr = &deref(self.thisptr)[i] return joint_state + def resize(self, int i): + self.thisptr.resize(i) + def size(self): return self.thisptr.size() From 3b0388f95339a86a97a03e3201ad919edab01a54 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 8 Aug 2017 10:33:16 +0200 Subject: [PATCH 27/39] Expose Joints::time --- bindings/python/basetypes.pyx | 12 ++++++++++++ bindings/python/test.py | 5 +++++ 2 files changed, 17 insertions(+) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 5bd308e9..62473d3d 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -580,6 +580,18 @@ cdef class Joints: def clear(self): self.thisptr.clear() + def _get_time(self): + cdef Time time = Time() + del time.thisptr + time.thisptr = &self.thisptr.time + time.delete_thisptr = False + return time + + def _set_time(self, Time time): + self.thisptr.time = deref(time.thisptr) + + time = property(_get_time, _set_time) + @property def names(self): cdef StringVectorReference names = StringVectorReference() diff --git a/bindings/python/test.py b/bindings/python/test.py index 12e8b0df..304b70e9 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -218,6 +218,11 @@ def test_joint_state_factories(): assert_equal(js.acceleration, 5.0) +def test_joints_has_time(): + j = basetypes.Joints() + assert_true(hasattr(j, "time")) + + def test_joints_resize(): j = basetypes.Joints() assert_equal(j.size(), 0) From 8bf5f2e3cadf4771285f84523fdbf76c76db9e03 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 8 Aug 2017 10:41:28 +0200 Subject: [PATCH 28/39] Enable setting of joint state vector elements --- bindings/python/basetypes.pyx | 3 +++ 1 file changed, 3 insertions(+) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 62473d3d..1f968bf7 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -650,6 +650,9 @@ cdef class JointStateVectorReference: joint_state.thisptr = &deref(self.thisptr)[i] return joint_state + def __setitem__(self, int i, JointState v): + deref(self.thisptr)[i] = deref(v.thisptr) + def resize(self, int i): self.thisptr.resize(i) From 922682a3415d0d780ada79767ad1aaedfcc4f502 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 8 Aug 2017 13:31:14 +0200 Subject: [PATCH 29/39] Implement some __str__ functions --- bindings/python/basetypes.pyx | 38 ++++++++++++++++++++++++++++++++++- bindings/python/test.py | 33 +++++++++++++++++++++++++++++- 2 files changed, 69 insertions(+), 2 deletions(-) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 1f968bf7..17337794 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -23,7 +23,9 @@ cdef class Time: self.delete_thisptr = True def __str__(self): - return self.thisptr.toString(_basetypes.Resolution.Microseconds, "%Y%m%d-%H:%M:%S") + return ("" + % self.thisptr.toString(_basetypes.Resolution.Microseconds, + "%Y%m%d-%H:%M:%S")) def _get_microseconds(self): return self.thisptr.microseconds @@ -454,6 +456,20 @@ cdef class JointState: self.thisptr = new _basetypes.JointState() self.delete_thisptr = True + def __str__(self): + parts = [] + if self.thisptr.hasPosition(): + parts.append("position=%g" % self.thisptr.position) + if self.thisptr.hasSpeed(): + parts.append("speed=%g" % self.thisptr.speed) + if self.thisptr.hasEffort(): + parts.append("effort=%g" % self.thisptr.effort) + if self.thisptr.hasRaw(): + parts.append("raw=%g" % self.thisptr.raw) + if self.thisptr.hasAcceleration(): + parts.append("acceleration=%g" % self.thisptr.acceleration) + return "JointState [%s]" % ", ".join(parts) + def _get_position(self): return self.thisptr.position @@ -568,6 +584,16 @@ cdef class Joints: self.thisptr = new _basetypes.Joints() self.delete_thisptr = True + def __str__(self): + parts = [] + # TODO Is this really a requirement? Can we ensure this? + assert self.thisptr.elements.size() == self.thisptr.names.size() + cdef unsigned int i + for i in range(self.thisptr.elements.size()): + parts.append("%s: %s" % (self.thisptr.names[i], + self.elements[i])) + return "Joints %s {%s}" % (self.time, ", ".join(parts)) + def size(self): return self.thisptr.size() @@ -673,6 +699,10 @@ cdef class RigidBodyState: self.thisptr = new _basetypes.RigidBodyState(do_invalidation) self.delete_thisptr = True + def __str__(self): + # TODO extend string representation? + return "RigidBodyState {%s, source_frame=%s, target_frame=%s, ...}" % (self.time, self.thisptr.sourceFrame, self.thisptr.targetFrame) + def _get_time(self): cdef Time time = Time() del time.thisptr @@ -842,6 +872,8 @@ cdef class Frame: self.thisptr = new _basetypes.Frame(width, height, depth, mode, val, size_in_bytes) self.delete_thisptr = True + # TODO __str__ + def _get_time(self): cdef Time time = Time() del time.thisptr @@ -997,6 +1029,8 @@ cdef class LaserScan: self.thisptr = new _basetypes.LaserScan() self.delete_thisptr = True + # TODO __str__ + def _get_time(self): cdef Time time = Time() del time.thisptr @@ -1121,6 +1155,8 @@ cdef class IMUSensors: self.thisptr = new _basetypes.IMUSensors() self.delete_thisptr = True + # TODO __str__ + def _get_time(self): cdef Time time = Time() del time.thisptr diff --git a/bindings/python/test.py b/bindings/python/test.py index 304b70e9..775d37c1 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -1,6 +1,6 @@ import basetypes import numpy as np -from nose.tools import assert_equal, assert_raises_regexp, assert_almost_equal, assert_false, assert_true, assert_greater +from nose.tools import assert_equal, assert_raises_regexp, assert_almost_equal, assert_false, assert_true, assert_greater, assert_regexp_matches from numpy.testing import assert_array_equal, assert_array_almost_equal @@ -11,6 +11,11 @@ def test_get_set_microseconds(): assert_equal(t.microseconds, m) +def test_time_str(): + t = basetypes.Time.now() + assert_regexp_matches(str(t), "") + + def test_no_overflow(): t = basetypes.Time.now() mus = t.microseconds @@ -185,24 +190,28 @@ def test_joint_state_get_set_speed(): js = basetypes.JointState() js.speed = 5.0 assert_equal(js.speed, 5.0) + assert_equal(str(js), "JointState [speed=5]") def test_joint_state_get_set_effort(): js = basetypes.JointState() js.effort = 5.0 assert_equal(js.effort, 5.0) + assert_equal(str(js), "JointState [effort=5]") def test_joint_state_get_set_raw(): js = basetypes.JointState() js.raw = 5.0 assert_equal(js.raw, 5.0) + assert_equal(str(js), "JointState [raw=5]") def test_joint_state_get_set_acceleration(): js = basetypes.JointState() js.acceleration = 5.0 assert_equal(js.acceleration, 5.0) + assert_equal(str(js), "JointState [acceleration=5]") def test_joint_state_factories(): @@ -253,6 +262,19 @@ def test_joints_access(): assert_equal(j["test_name"].position, 1.0) +def test_joints_str(): + j = basetypes.Joints() + j.names.resize(2) + j.names[0] = "j1" + j.names[1] = "j2" + j.elements.resize(2) + j.elements[0].position = 1.0 + j.elements[1].position = 2.0 + assert_equal( + str(j), "Joints " + "{j1: JointState [position=1], j2: JointState [position=2]}") + + def test_rigid_body_state_get_set_time(): rbs = basetypes.RigidBodyState() assert_equal(rbs.time.microseconds, 0) @@ -278,6 +300,15 @@ def test_rigid_body_state_get_set_target_frame(): assert_equal(rbs.target_frame, "target_frame") +def test_rigid_body_state_str(): + rbs = basetypes.RigidBodyState() + rbs.source_frame = "source" + rbs.target_frame = "target" + assert_equal( + str(rbs), "RigidBodyState {, " + "source_frame=source, target_frame=target, ...}") + + def test_rigid_body_state_get_set_position(): rbs = basetypes.RigidBodyState() assert_array_almost_equal( From 22a0792200746bab497db9eecf1999684aa0f9f7 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 8 Aug 2017 13:44:41 +0200 Subject: [PATCH 30/39] String representation of LaserScan --- bindings/python/basetypes.pyx | 10 +++++++++- bindings/python/test.py | 17 +++++++++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 17337794..0d969371 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -1029,7 +1029,15 @@ cdef class LaserScan: self.thisptr = new _basetypes.LaserScan() self.delete_thisptr = True - # TODO __str__ + def __str__(self): + ranges = [] + cdef unsigned int i + for i in range(self.ranges.size()): + ranges.append(self.ranges[i]) + remission = [] + for i in range(self.remission.size()): + remission.append(self.remission[i]) + return "LaserScan {%s, min_range=%s, max_range=%s, ranges=%s, remission=%s}" % (self.time, self.thisptr.minRange, self.thisptr.maxRange, ranges, remission) def _get_time(self): cdef Time time = Time() diff --git a/bindings/python/test.py b/bindings/python/test.py index 775d37c1..01c5ace6 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -453,6 +453,23 @@ def test_laser_scan(): assert_true(ls.is_range_valid(25)) +def test_laser_scan_str(): + ls = basetypes.LaserScan() + ls.min_range = 20 + ls.max_range = 30 + ls.ranges.resize(3) + ls.remission.resize(3) + for i in range(3): + ls.ranges[i] = 25 + ls.remission[i] = 0.0 + ls.ranges[1] = 10 + assert_equal( + str(ls), + "LaserScan {, min_range=20, max_range=30, ranges=[25, 10, 25], " + "remission=[0.0, 0.0, 0.0]}" + ) + + def test_imu_sensors(): imu = basetypes.IMUSensors() From bd1714023cbfd322742bff409181096d9cb552f8 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Tue, 8 Aug 2017 13:49:05 +0200 Subject: [PATCH 31/39] Cap output of LaserScan --- bindings/python/basetypes.pyx | 4 ++++ bindings/python/test.py | 10 +++++----- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 0d969371..ffd60f07 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -1034,9 +1034,13 @@ cdef class LaserScan: cdef unsigned int i for i in range(self.ranges.size()): ranges.append(self.ranges[i]) + if len(ranges) > 5: + ranges = str(ranges[:5])[:-1] + ", ...]" remission = [] for i in range(self.remission.size()): remission.append(self.remission[i]) + if len(remission) > 5: + remission = str(remission[:5])[:-1] + ", ...]" return "LaserScan {%s, min_range=%s, max_range=%s, ranges=%s, remission=%s}" % (self.time, self.thisptr.minRange, self.thisptr.maxRange, ranges, remission) def _get_time(self): diff --git a/bindings/python/test.py b/bindings/python/test.py index 01c5ace6..49138258 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -457,16 +457,16 @@ def test_laser_scan_str(): ls = basetypes.LaserScan() ls.min_range = 20 ls.max_range = 30 - ls.ranges.resize(3) - ls.remission.resize(3) - for i in range(3): + ls.ranges.resize(6) + ls.remission.resize(6) + for i in range(6): ls.ranges[i] = 25 ls.remission[i] = 0.0 ls.ranges[1] = 10 assert_equal( str(ls), - "LaserScan {, min_range=20, max_range=30, ranges=[25, 10, 25], " - "remission=[0.0, 0.0, 0.0]}" + "LaserScan {, min_range=20, max_range=30, ranges=[25, 10, 25, 25, 25, ...], " + "remission=[0.0, 0.0, 0.0, 0.0, 0.0, ...]}" ) From d8fb7a81007fb411451307c37b3d9e328380a2ea Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Fri, 1 Sep 2017 11:23:55 +0200 Subject: [PATCH 32/39] Define assign function for each type --- bindings/python/_basetypes.pxd | 27 +++++++++ bindings/python/basetypes.pyx | 106 +++++++++++++++++++++++++++++++++ bindings/python/test.py | 45 +++++++++++++- 3 files changed, 177 insertions(+), 1 deletion(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index 44a2b8c3..f2304279 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -10,6 +10,8 @@ cdef extern from "base/Time.hpp" namespace "base": int64_t microseconds + Time& assign "operator="(Time&) + bool operator<(Time) bool operator>(Time) bool operator==(Time) @@ -43,6 +45,9 @@ cdef extern from "base/Eigen.hpp" namespace "base": Vector2d(Vector2d&) double* data() int rows() + Vector2d& assign "operator="(Vector2d&) + bool operator==(Vector2d&) + bool operator!=(Vector2d&) double& get "operator()"(int row) double x() double y() @@ -55,6 +60,9 @@ cdef extern from "base/Eigen.hpp" namespace "base": Vector3d(Vector3d&) double* data() int rows() + Vector3d& assign "operator="(Vector3d&) + bool operator==(Vector3d&) + bool operator!=(Vector3d&) double& get "operator()"(int row) double x() double y() @@ -68,6 +76,9 @@ cdef extern from "base/Eigen.hpp" namespace "base": Vector4d(Vector4d&) double* data() int rows() + Vector4d& assign "operator="(Vector4d&) + bool operator==(Vector4d&) + bool operator!=(Vector4d&) double& get "operator()"(int row) double x() double y() @@ -78,6 +89,9 @@ cdef extern from "base/Eigen.hpp" namespace "base": cdef cppclass Matrix3d: Matrix3d() double* data() + Matrix3d& assign "operator="(Matrix3d&) + bool operator==(Matrix3d&) + bool operator!=(Matrix3d&) double& get "operator()"(int row, int col) int rows() int cols() @@ -85,6 +99,7 @@ cdef extern from "base/Eigen.hpp" namespace "base": cdef cppclass Quaterniond: Quaterniond() Quaterniond(double, double, double, double) + Quaterniond& assign "operator="(Quaterniond&) double w() double x() double y() @@ -94,6 +109,7 @@ cdef extern from "base/Eigen.hpp" namespace "base": cdef extern from "base/TransformWithCovariance.hpp" namespace "base": cdef cppclass TransformWithCovariance: TransformWithCovariance() + TransformWithCovariance& assign "operator="(TransformWithCovariance&) Vector3d translation Quaterniond orientation @@ -101,6 +117,9 @@ cdef extern from "base/TransformWithCovariance.hpp" namespace "base": cdef extern from "base/JointState.hpp" namespace "base": cdef cppclass JointState: JointState() + JointState& assign "operator="(JointState&) + bool operator==(JointState&) + bool operator!=(JointState&) double position double speed double effort @@ -131,6 +150,7 @@ cdef extern from "base/NamedVector.hpp" namespace "base": vector[string] names vector[T] elements + NamedVector& assign "operator="(NamedVector&) void resize(int) int size() bool empty() @@ -149,6 +169,7 @@ cdef extern from "base/samples/Joints.hpp" namespace "base::samples": cdef extern from "base/samples/RigidBodyState.hpp" namespace "base::samples": cdef cppclass RigidBodyState: RigidBodyState(bool) + RigidBodyState& assign "operator="(RigidBodyState&) Time time string sourceFrame string targetFrame @@ -195,6 +216,8 @@ cdef extern from "base/samples/Frame.hpp" namespace "base::samples::frame": Frame(int width, int height, int depth, frame_mode_t mode, int val, int sizeInBytes) Frame(Frame other, bool bcopy) + Frame& assign "operator="(Frame&) + Time time Time received_time vector[uint8_t] image @@ -215,6 +238,8 @@ cdef extern from "base/samples/Pointcloud.hpp" namespace "base::samples": cdef cppclass Pointcloud: Pointcloud() + Pointcloud& assign "operator="(Pointcloud&) + Time time vector[Vector3d] points vector[Vector4d] colors @@ -230,6 +255,7 @@ cdef extern from "base/samples/LaserScan.hpp" namespace "base::samples": cdef cppclass LaserScan: LaserScan() + LaserScan& assign "operator="(LaserScan&) Time time double start_angle @@ -251,6 +277,7 @@ cdef extern from "base/samples/LaserScan.hpp" namespace "base::samples": cdef extern from "base/samples/IMUSensors.hpp" namespace "base::samples": cdef cppclass IMUSensors: IMUSensors() + IMUSensors& assign "operator="(IMUSensors&) Time time Vector3d acc diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index ffd60f07..bbe6fb63 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -27,6 +27,9 @@ cdef class Time: % self.thisptr.toString(_basetypes.Resolution.Microseconds, "%Y%m%d-%H:%M:%S")) + def assign(self, Time other): + self.thisptr.assign(deref(other.thisptr)) + def _get_microseconds(self): return self.thisptr.microseconds @@ -115,6 +118,9 @@ cdef class Vector2d: def __str__(self): return "[%.2f, %.2f]" % (self.thisptr.get(0), self.thisptr.get(1)) + def assign(self, Vector2d other): + self.thisptr.assign(deref(other.thisptr)) + def __array__(self, dtype=None): cdef np.npy_intp shape[1] shape[0] = 2 @@ -160,6 +166,22 @@ cdef class Vector2d: array[i] = self.thisptr.data()[i] return array + def __richcmp__(Vector2d self, Vector2d other, int op): + if op == 0: + raise NotImplementedError("<") + elif op == 1: + raise NotImplementedError("<=") + elif op == 2: + return deref(self.thisptr) == deref(other.thisptr) + elif op == 3: + return deref(self.thisptr) != deref(other.thisptr) + elif op == 4: + raise NotImplementedError(">") + elif op == 5: + raise NotImplementedError(">=") + else: + raise ValueError("Unknown comparison operation %d" % op) + # TODO add operators, fromarray @@ -181,6 +203,9 @@ cdef class Vector3d: self.thisptr.get(1), self.thisptr.get(2)) + def assign(self, Vector3d other): + self.thisptr.assign(deref(other.thisptr)) + def __array__(self, dtype=None): cdef np.npy_intp shape[1] shape[0] = 3 @@ -239,6 +264,22 @@ cdef class Vector3d: for i in range(3): self.thisptr.data()[i] = array[i] + def __richcmp__(Vector3d self, Vector3d other, int op): + if op == 0: + raise NotImplementedError("<") + elif op == 1: + raise NotImplementedError("<=") + elif op == 2: + return deref(self.thisptr) == deref(other.thisptr) + elif op == 3: + return deref(self.thisptr) != deref(other.thisptr) + elif op == 4: + raise NotImplementedError(">") + elif op == 5: + raise NotImplementedError(">=") + else: + raise ValueError("Unknown comparison operation %d" % op) + # TODO add operators @@ -261,6 +302,9 @@ cdef class Vector4d: self.thisptr.get(2), self.thisptr.get(3) ) + def assign(self, Vector4d other): + self.thisptr.assign(deref(other.thisptr)) + def __array__(self, dtype=None): cdef np.npy_intp shape[1] shape[0] = 4 @@ -290,6 +334,22 @@ cdef class Vector4d: array[i] = self.thisptr.data()[i] return array + def __richcmp__(Vector4d self, Vector4d other, int op): + if op == 0: + raise NotImplementedError("<") + elif op == 1: + raise NotImplementedError("<=") + elif op == 2: + return deref(self.thisptr) == deref(other.thisptr) + elif op == 3: + return deref(self.thisptr) != deref(other.thisptr) + elif op == 4: + raise NotImplementedError(">") + elif op == 5: + raise NotImplementedError(">=") + else: + raise ValueError("Unknown comparison operation %d" % op) + # TODO add operators, fromarray @@ -321,6 +381,9 @@ cdef class Matrix3d: self.thisptr.get(2, 0), self.thisptr.get(2, 1), self.thisptr.get(2, 2), ) + def assign(self, Matrix3d other): + self.thisptr.assign(deref(other.thisptr)) + def __array__(self, dtype=None): cdef np.npy_intp shape[2] shape[0] = 3 @@ -363,6 +426,22 @@ cdef class Matrix3d: for j in range(3): self.thisptr.data()[3 * j + i] = array[i, j] + def __richcmp__(Matrix3d self, Matrix3d other, int op): + if op == 0: + raise NotImplementedError("<") + elif op == 1: + raise NotImplementedError("<=") + elif op == 2: + return deref(self.thisptr) == deref(other.thisptr) + elif op == 3: + return deref(self.thisptr) != deref(other.thisptr) + elif op == 4: + raise NotImplementedError(">") + elif op == 5: + raise NotImplementedError(">=") + else: + raise ValueError("Unknown comparison operation %d" % op) + # TODO operators @@ -387,6 +466,9 @@ cdef class Quaterniond: self.thisptr.w(), self.thisptr.x(), self.thisptr.y(), self.thisptr.z()) + def assign(self, Quaterniond other): + self.thisptr.assign(deref(other.thisptr)) + def toarray(self): cdef np.ndarray[double, ndim=1] array = np.array([ self.thisptr.w(), self.thisptr.x(), self.thisptr.y(), @@ -416,6 +498,9 @@ cdef class TransformWithCovariance: return "(translation=%s, orientation=%s)" % (self.translation, self.orientation) + def assign(self, TransformWithCovariance other): + self.thisptr.assign(deref(other.thisptr)) + def _get_translation(self): cdef Vector3d translation = Vector3d() del translation.thisptr @@ -470,6 +555,9 @@ cdef class JointState: parts.append("acceleration=%g" % self.thisptr.acceleration) return "JointState [%s]" % ", ".join(parts) + def assign(self, JointState other): + self.thisptr.assign(deref(other.thisptr)) + def _get_position(self): return self.thisptr.position @@ -594,6 +682,9 @@ cdef class Joints: self.elements[i])) return "Joints %s {%s}" % (self.time, ", ".join(parts)) + def assign(self, Joints other): + self.thisptr.assign(deref(other.thisptr)) + def size(self): return self.thisptr.size() @@ -703,6 +794,9 @@ cdef class RigidBodyState: # TODO extend string representation? return "RigidBodyState {%s, source_frame=%s, target_frame=%s, ...}" % (self.time, self.thisptr.sourceFrame, self.thisptr.targetFrame) + def assign(self, RigidBodyState other): + self.thisptr.assign(deref(other.thisptr)) + def _get_time(self): cdef Time time = Time() del time.thisptr @@ -874,6 +968,9 @@ cdef class Frame: # TODO __str__ + def assign(self, Frame other): + self.thisptr.assign(deref(other.thisptr)) + def _get_time(self): cdef Time time = Time() del time.thisptr @@ -961,6 +1058,9 @@ cdef class Pointcloud: self.thisptr = new _basetypes.Pointcloud() self.delete_thisptr = True + def assign(self, Pointcloud other): + self.thisptr.assign(deref(other.thisptr)) + @property def points(self): cdef Vector3dVectorReference points = Vector3dVectorReference() @@ -1043,6 +1143,9 @@ cdef class LaserScan: remission = str(remission[:5])[:-1] + ", ...]" return "LaserScan {%s, min_range=%s, max_range=%s, ranges=%s, remission=%s}" % (self.time, self.thisptr.minRange, self.thisptr.maxRange, ranges, remission) + def assign(self, LaserScan other): + self.thisptr.assign(deref(other.thisptr)) + def _get_time(self): cdef Time time = Time() del time.thisptr @@ -1169,6 +1272,9 @@ cdef class IMUSensors: # TODO __str__ + def assign(self, IMUSensors other): + self.thisptr.assign(deref(other.thisptr)) + def _get_time(self): cdef Time time = Time() del time.thisptr diff --git a/bindings/python/test.py b/bindings/python/test.py index 49138258..dbbf0b7e 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -1,6 +1,8 @@ import basetypes import numpy as np -from nose.tools import assert_equal, assert_raises_regexp, assert_almost_equal, assert_false, assert_true, assert_greater, assert_regexp_matches +from nose.tools import assert_equal, assert_not_equal, assert_raises_regexp, \ + assert_almost_equal, assert_false, assert_true, assert_greater, \ + assert_regexp_matches from numpy.testing import assert_array_equal, assert_array_almost_equal @@ -61,11 +63,27 @@ def test_time_operators(): assert_equal(t5, t4) +def test_time_assign(): + t1 = basetypes.Time() + t2 = basetypes.Time.now() + assert_not_equal(t1, t2) + t1.assign(t2) + assert_equal(t1, t2) + + def test_vector2d_ctor(): v = basetypes.Vector2d(1.0, 2.0) assert_equal(str(v), "[1.00, 2.00]") +def test_vector2d_assign(): + obj1 = basetypes.Vector2d() + obj2 = basetypes.Vector2d(1.0, 2.0) + assert_not_equal(obj1, obj2) + obj1.assign(obj2) + assert_equal(obj1, obj2) + + def test_vector2d_as_ndarray(): random_state = np.random.RandomState(843) r = random_state.randn(2, 2) @@ -119,6 +137,14 @@ def assign(i): assert_raises_regexp(KeyError, "index must be", assign, 3) +def test_vector3d_assign(): + obj1 = basetypes.Vector3d() + obj2 = basetypes.Vector3d(1.0, 2.0, 3.0) + assert_not_equal(obj1, obj2) + obj1.assign(obj2) + assert_equal(obj1, obj2) + + def test_norms(): v = basetypes.Vector3d(1.0, 2.0, 3.0) assert_almost_equal(v.norm(), 3.741657387) @@ -139,6 +165,14 @@ def test_vector4d_as_ndarray(): assert_array_almost_equal(rv, rv2) +def test_vector4d_assign(): + obj1 = basetypes.Vector4d() + obj2 = basetypes.Vector4d(1.0, 2.0, 3.0) + assert_not_equal(obj1, obj2) + obj1.assign(obj2) + assert_equal(obj1, obj2) + + def test_matrix3d_get_set_data(): m = basetypes.Matrix3d() m[0, 1] = 1.0 @@ -157,6 +191,15 @@ def test_matrix3d_array_access(): assert_array_equal(np.asarray(m), r) +def test_matrix3d_assign(): + obj1 = basetypes.Matrix3d() + obj2 = basetypes.Matrix3d() + obj2.fromarray(np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]])) + assert_not_equal(obj1, obj2) + obj1.assign(obj2) + assert_equal(obj1, obj2) + + def test_quaterniond_ctor(): q = basetypes.Quaterniond(1.0, 0.0, 0.0, 0.0) assert_equal(str(q), "[im=1.00, real=(0.00, 0.00, 0.00)]") From 25fc1180fe9406ee94516912428cce35a0abf555 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Fri, 1 Sep 2017 15:19:51 +0200 Subject: [PATCH 33/39] Expose Pointcloud::time --- bindings/python/basetypes.pyx | 12 ++++++++++++ bindings/python/test.py | 2 ++ 2 files changed, 14 insertions(+) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index bbe6fb63..909c6a12 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -1061,6 +1061,18 @@ cdef class Pointcloud: def assign(self, Pointcloud other): self.thisptr.assign(deref(other.thisptr)) + def _get_time(self): + cdef Time time = Time() + del time.thisptr + time.thisptr = &self.thisptr.time + time.delete_thisptr = False + return time + + def _set_time(self, Time time): + self.thisptr.time = deref(time.thisptr) + + time = property(_get_time, _set_time) + @property def points(self): cdef Vector3dVectorReference points = Vector3dVectorReference() diff --git a/bindings/python/test.py b/bindings/python/test.py index dbbf0b7e..fcd3950d 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -456,6 +456,8 @@ def test_create_frame_gray(): def test_create_pointcloud(): pcl = basetypes.Pointcloud() + assert_equal(pcl.time.microseconds, 0) + pcl.points.resize(100) point = pcl.points[0] point.x = 1.0 From 56fa683b98bd50268b88ce425988bb5dd70c7544 Mon Sep 17 00:00:00 2001 From: Docker Date: Wed, 30 Aug 2017 13:10:05 +0000 Subject: [PATCH 34/39] started to create bindings for Angle --- bindings/python/_basetypes.pxd | 41 +++++++++++ bindings/python/basetypes.pyx | 121 +++++++++++++++++++++++++++++++++ bindings/python/test.py | 65 ++++++++++++++++++ 3 files changed, 227 insertions(+) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index f2304279..17e1454f 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -3,6 +3,46 @@ from libcpp.vector cimport vector from libcpp cimport bool from libc.stdint cimport uint8_t, uint16_t, uint32_t, int64_t +cdef extern from "base/Angle.hpp" namespace "base": + cdef cppclass Angle: + Angle() + + double getDeg() + bool isApprox(Angle, double) + bool isApprox(Angle) + + + bool operator==(Angle) + bool operator<(Angle) + bool operator>(Angle) + bool operator<=(Angle) + bool operator>=(Angle) + + Angle operator+(Angle) + Angle operator-(Angle) + Angle operator*(Angle) + Angle operator*(double) + + #void operator=(Angle) + + #Angle flipped() + #Angle &flip() + + #static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b); + #static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b, const base::Vector3d& positive); + #static inline Angle operator*( double a, Angle b ) + #std::ostream& operator << (std::ostream& os, Angle angle); + # and more + +cdef extern from "base/Angle.hpp" namespace "base::Angle": + double rad2Deg(double) + double deg2Rad( double) + double normalizeRad( double) + Angle fromRad( double ) + Angle fromDeg( double ) + Angle unknown() + Angle Min() + Angle Max() cdef extern from "base/Time.hpp" namespace "base": cdef cppclass Time: @@ -283,3 +323,4 @@ cdef extern from "base/samples/IMUSensors.hpp" namespace "base::samples": Vector3d acc Vector3d gyro Vector3d mag + diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 909c6a12..e69d4183 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -9,6 +9,127 @@ import numpy as np np.import_array() # must be here because we use the NumPy C API + +cdef class Angle: + cdef _basetypes.Angle* thisptr + cdef bool delete_thisptr + + def __cinit__(self, Angle other=None): + self.thisptr = NULL + self.delete_thisptr = False + + def __dealloc__(self): + if self.thisptr != NULL and self.delete_thisptr: + del self.thisptr + + def __init__(self, Angle other=None): + self.thisptr = new _basetypes.Angle() + self.delete_thisptr = True + if other and type(other) is Angle: + self.thisptr[0] = other.thisptr[0] + + def getDeg(self): + return self.thisptr.getDeg() + + def isApprox(self, Angle other, prec=None): + if prec is None: + return self.thisptr.isApprox(other.thisptr[0]) + else: + return self.thisptr.isApprox(other.thisptr[0], prec) + + def __richcmp__(Angle self, Angle other, int op): + if op == 0: + return deref(self.thisptr) < deref(other.thisptr) + elif op == 1: + return deref(self.thisptr) <= deref(other.thisptr) + elif op == 2: + return deref(self.thisptr) == deref(other.thisptr) + elif op == 3: + return not deref(self.thisptr) == deref(other.thisptr) + elif op == 4: + return deref(self.thisptr) > deref(other.thisptr) + elif op == 5: + return deref(self.thisptr) >= deref(other.thisptr) + else: + raise ValueError("Unknown comparison operation %d" % op) + + def __add__(Angle self, Angle other): + cdef Angle ret = Angle() + ret.thisptr[0] = self.thisptr[0] + other.thisptr[0] + return ret + + def __sub__(Angle self, Angle other): + cdef Angle ret = Angle() + ret.thisptr[0] = self.thisptr[0] - other.thisptr[0] + return ret + + def __iadd__(Angle self, Angle other): + self.thisptr[0] = self.thisptr[0] + other.thisptr[0] + return self + + def __isub__(Angle self, Angle other): + self.thisptr[0] = self.thisptr[0] - other.thisptr[0] + return self + + def __mul__(Angle self, Angle other): + cdef Angle ret = Angle() + + return ret + + def __mul__(Angle self, factor): + cdef Angle ret = Angle() + if isinstance(factor, Angle): + ret.thisptr[0] = self.thisptr[0] * (factor).thisptr[0] + elif isinstance(factor, float): + ret.thisptr[0] = deref(self.thisptr) * factor + else: + raise TypeError("Angle multiplication only accept types Angle and float") + return ret + + #__lshift__ x, y object << operato + @staticmethod + def rad2Deg(rad): + return _basetypes.rad2Deg(rad) + + @staticmethod + def deg2Rad(deg): + return _basetypes.deg2Rad(deg) + + @staticmethod + def normalizeRad(rad): + return _basetypes.normalizeRad(rad) + + @staticmethod + def fromRad(rad): + cdef Angle angle = Angle() + angle.thisptr[0] = _basetypes.fromRad(rad) + return angle + + @staticmethod + def fromDeg(deg): + cdef Angle angle = Angle() + angle.thisptr[0] = _basetypes.fromDeg(deg) + return angle + + @staticmethod + def unknown(): + cdef Angle angle = Angle() + angle.thisptr[0] = _basetypes.unknown() + return angle + + @staticmethod + def Min(): + cdef Angle angle = Angle() + angle.thisptr[0] = _basetypes.Min() + return angle + + @staticmethod + def Max(): + cdef Angle angle = Angle() + angle.thisptr[0] = _basetypes.Max() + return angle + + cdef class Time: def __cinit__(self): self.thisptr = NULL diff --git a/bindings/python/test.py b/bindings/python/test.py index fcd3950d..bf8b5048 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -529,3 +529,68 @@ def test_imu_sensors(): imu.mag.x = 3.0 assert_equal(imu.mag.x, 3.0) + + +def test_angle(): + assert_equal(basetypes.Angle.rad2Deg(1),57.29577951308232) + assert_equal(basetypes.Angle.deg2Rad(57.29577951308232), 1) + assert_equal(basetypes.Angle.normalizeRad(7),0.7168146928204138) + + angle = basetypes.Angle.fromRad(1) + assert_equal(angle.getDeg(), 57.29577951308232) + + angle = basetypes.Angle.fromDeg(1) + assert_equal(angle.getDeg(), 1) + + angle = basetypes.Angle.Min() + assert_true(angle.getDeg()< -179) + + angle = basetypes.Angle.Max() + assert_true(angle.getDeg()> 179) + + angle1 = basetypes.Angle.fromRad(1) + angle2 = basetypes.Angle.fromRad(0.9) + + assert_true(angle1.isApprox(angle2,0.2)) + assert_false(angle1.isApprox(angle2)) + assert_true(angle1.isApprox(angle1)) + + assert_true(angle1>angle2) + assert_false(angle2>angle1) + assert_false(angle1 > angle1) + assert_true(angle1 >= angle2) + assert_true(angle1 >= angle1) + + assert_false(angle1 < angle2) + assert_true(angle2 < angle1) + assert_false(angle1 < angle1) + assert_false(angle1 <= angle2) + assert_true(angle1 <= angle1) + + assert_true(angle1 == angle1) + assert_false(angle1 == angle2) + assert_false(angle1 != angle1) + assert_true(angle1 != angle2) + assert_true(basetypes.Angle.fromRad(1) == basetypes.Angle.fromRad(1)) + + angle = basetypes.Angle.fromDeg(1) + sum = angle + angle + assert_equal(sum.getDeg(), 2) + sum = angle - angle + assert_equal(sum.getDeg(), 0) + angle += angle + assert_equal(angle.getDeg(), 2) + angle -= angle + assert_equal(angle.getDeg(), 0) + angle = basetypes.Angle.fromDeg(1) + mul = angle * angle + assert_equal(mul.getDeg(), 0.017453292519943295) + mul = angle * 2.0 + assert_equal(mul.getDeg(), 2.0) + + angle1 = basetypes.Angle.fromDeg(1) + angle2 = angle1 + angle2+= basetypes.Angle.fromDeg(1) + assert_true(angle1 != angle2) + + angle = basetypes.Angle() \ No newline at end of file From b18cfade0e4518eebb77b6cbe10ac8e22bac4057 Mon Sep 17 00:00:00 2001 From: Enno Roehrig Date: Wed, 6 Sep 2017 09:14:18 +0000 Subject: [PATCH 35/39] coding style and splitted tests into groups --- bindings/python/basetypes.pyx | 23 ++++------ bindings/python/test.py | 86 +++++++++++++++++++---------------- 2 files changed, 56 insertions(+), 53 deletions(-) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index e69d4183..a9819b57 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -28,10 +28,10 @@ cdef class Angle: if other and type(other) is Angle: self.thisptr[0] = other.thisptr[0] - def getDeg(self): + def get_deg(self): return self.thisptr.getDeg() - def isApprox(self, Angle other, prec=None): + def is_approx(self, Angle other, prec=None): if prec is None: return self.thisptr.isApprox(other.thisptr[0]) else: @@ -71,11 +71,6 @@ cdef class Angle: self.thisptr[0] = self.thisptr[0] - other.thisptr[0] return self - def __mul__(Angle self, Angle other): - cdef Angle ret = Angle() - - return ret - def __mul__(Angle self, factor): cdef Angle ret = Angle() if isinstance(factor, Angle): @@ -88,25 +83,25 @@ cdef class Angle: #__lshift__ x, y object << operato @staticmethod - def rad2Deg(rad): + def rad_to_deg(rad): return _basetypes.rad2Deg(rad) @staticmethod - def deg2Rad(deg): + def deg_to_rad(deg): return _basetypes.deg2Rad(deg) @staticmethod - def normalizeRad(rad): + def normalize_rad(rad): return _basetypes.normalizeRad(rad) @staticmethod - def fromRad(rad): + def from_rad(rad): cdef Angle angle = Angle() angle.thisptr[0] = _basetypes.fromRad(rad) return angle @staticmethod - def fromDeg(deg): + def from_deg(deg): cdef Angle angle = Angle() angle.thisptr[0] = _basetypes.fromDeg(deg) return angle @@ -118,13 +113,13 @@ cdef class Angle: return angle @staticmethod - def Min(): + def min(): cdef Angle angle = Angle() angle.thisptr[0] = _basetypes.Min() return angle @staticmethod - def Max(): + def max(): cdef Angle angle = Angle() angle.thisptr[0] = _basetypes.Max() return angle diff --git a/bindings/python/test.py b/bindings/python/test.py index bf8b5048..c1b9cf62 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -531,66 +531,74 @@ def test_imu_sensors(): assert_equal(imu.mag.x, 3.0) -def test_angle(): - assert_equal(basetypes.Angle.rad2Deg(1),57.29577951308232) - assert_equal(basetypes.Angle.deg2Rad(57.29577951308232), 1) - assert_equal(basetypes.Angle.normalizeRad(7),0.7168146928204138) +def test_angle_conversions(): + assert_equal(basetypes.Angle.rad_to_deg(1), 57.29577951308232) + assert_equal(basetypes.Angle.deg_to_rad(57.29577951308232), 1) + assert_equal(basetypes.Angle.normalize_rad(7), 0.7168146928204138) - angle = basetypes.Angle.fromRad(1) - assert_equal(angle.getDeg(), 57.29577951308232) +def test_angle_constructors(): + angle = basetypes.Angle.from_rad(1) + assert_equal(angle.get_deg(), 57.29577951308232) - angle = basetypes.Angle.fromDeg(1) - assert_equal(angle.getDeg(), 1) + angle = basetypes.Angle.from_deg(1) + assert_equal(angle.get_deg(), 1) - angle = basetypes.Angle.Min() - assert_true(angle.getDeg()< -179) + angle = basetypes.Angle.min() + assert_true(angle.get_deg()< -179) - angle = basetypes.Angle.Max() - assert_true(angle.getDeg()> 179) + angle = basetypes.Angle.max() + assert_true(angle.get_deg()> 179) - angle1 = basetypes.Angle.fromRad(1) - angle2 = basetypes.Angle.fromRad(0.9) +def test_angle_comparisons(): + angle1 = basetypes.Angle.from_rad(1) + angle2 = basetypes.Angle.from_rad(0.9) - assert_true(angle1.isApprox(angle2,0.2)) - assert_false(angle1.isApprox(angle2)) - assert_true(angle1.isApprox(angle1)) + assert_true(angle1.is_approx(angle2, 0.2)) + assert_false(angle1.is_approx(angle2)) + assert_true(angle1.is_approx(angle1)) - assert_true(angle1>angle2) - assert_false(angle2>angle1) - assert_false(angle1 > angle1) - assert_true(angle1 >= angle2) - assert_true(angle1 >= angle1) + assert_true( angle1 > angle2) + assert_false(angle2 > angle1) + assert_false(angle1 > angle1) + assert_true( angle1 >= angle2) + assert_true( angle1 >= angle1) assert_false(angle1 < angle2) - assert_true(angle2 < angle1) + assert_true( angle2 < angle1) assert_false(angle1 < angle1) assert_false(angle1 <= angle2) - assert_true(angle1 <= angle1) + assert_true( angle1 <= angle1) - assert_true(angle1 == angle1) + assert_true( angle1 == angle1) assert_false(angle1 == angle2) assert_false(angle1 != angle1) - assert_true(angle1 != angle2) - assert_true(basetypes.Angle.fromRad(1) == basetypes.Angle.fromRad(1)) + assert_true( angle1 != angle2) + assert_true(basetypes.Angle.from_rad(1) == basetypes.Angle.from_rad(1)) + +def test_angle_operators(): + angle = basetypes.Angle.from_deg(1) - angle = basetypes.Angle.fromDeg(1) sum = angle + angle - assert_equal(sum.getDeg(), 2) + assert_equal(sum.get_deg(), 2) + sum = angle - angle - assert_equal(sum.getDeg(), 0) + assert_equal(sum.get_deg(), 0) + angle += angle - assert_equal(angle.getDeg(), 2) + assert_equal(angle.get_deg(), 2) + angle -= angle - assert_equal(angle.getDeg(), 0) - angle = basetypes.Angle.fromDeg(1) + assert_equal(angle.get_deg(), 0) + + angle = basetypes.Angle.from_deg(1) mul = angle * angle - assert_equal(mul.getDeg(), 0.017453292519943295) + assert_equal(mul.get_deg(), 0.017453292519943295) + mul = angle * 2.0 - assert_equal(mul.getDeg(), 2.0) + assert_equal(mul.get_deg(), 2.0) - angle1 = basetypes.Angle.fromDeg(1) +def test_angle_allocation(): + angle1 = basetypes.Angle.from_deg(1) angle2 = angle1 - angle2+= basetypes.Angle.fromDeg(1) + angle2+= basetypes.Angle.from_deg(1) assert_true(angle1 != angle2) - - angle = basetypes.Angle() \ No newline at end of file From 55eb1db9a6030879f29c81dbfe64461e1a8fcbd6 Mon Sep 17 00:00:00 2001 From: Enno Roehrig Date: Wed, 6 Sep 2017 13:11:09 +0000 Subject: [PATCH 36/39] finished Angle --- bindings/python/_basetypes.pxd | 52 ++++++++++++++++++---------------- bindings/python/basetypes.pyx | 41 +++++++++++++++++++++------ bindings/python/test.py | 40 +++++++++++++++++++++++--- 3 files changed, 96 insertions(+), 37 deletions(-) diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd index 17e1454f..325db618 100644 --- a/bindings/python/_basetypes.pxd +++ b/bindings/python/_basetypes.pxd @@ -8,41 +8,43 @@ cdef extern from "base/Angle.hpp" namespace "base": Angle() double getDeg() - bool isApprox(Angle, double) - bool isApprox(Angle) - - - bool operator==(Angle) - bool operator<(Angle) - bool operator>(Angle) - bool operator<=(Angle) - bool operator>=(Angle) - - Angle operator+(Angle) - Angle operator-(Angle) - Angle operator*(Angle) + double getRad() + bool isApprox(Angle&, double) + bool isApprox(Angle&) + + bool operator==(Angle&) + bool operator<(Angle&) + bool operator>(Angle&) + bool operator<=(Angle&) + bool operator>=(Angle&) + + Angle operator+(Angle&) + Angle operator-(Angle&) + Angle operator*(Angle&) Angle operator*(double) + Angle operator*(double, Angle) - #void operator=(Angle) + Angle assign "operator="(Angle&) - #Angle flipped() - #Angle &flip() + Angle flipped() + Angle &flip() - #static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b); - #static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b, const base::Vector3d& positive); - #static inline Angle operator*( double a, Angle b ) - #std::ostream& operator << (std::ostream& os, Angle angle); - # and more + #std::ostream& operator << (std::ostream& os, Angle angle); reimplemented + #Angle operator*(double, Angle) used Angle operator*(double, Angle) instead + #AngleSegment cdef extern from "base/Angle.hpp" namespace "base::Angle": double rad2Deg(double) - double deg2Rad( double) - double normalizeRad( double) - Angle fromRad( double ) - Angle fromDeg( double ) + double deg2Rad(double) + double normalizeRad(double) + Angle fromRad(double) + Angle fromDeg(double) Angle unknown() Angle Min() Angle Max() + Angle vectorToVector(Vector3d&, Vector3d&) + Angle vectorToVector(Vector3d&, Vector3d&, Vector3d&) + cdef extern from "base/Time.hpp" namespace "base": cdef cppclass Time: diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index a9819b57..305514b5 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -8,8 +8,6 @@ import numpy as np np.import_array() # must be here because we use the NumPy C API - - cdef class Angle: cdef _basetypes.Angle* thisptr cdef bool delete_thisptr @@ -31,6 +29,9 @@ cdef class Angle: def get_deg(self): return self.thisptr.getDeg() + def get_rad(self): + return self.thisptr.getRad() + def is_approx(self, Angle other, prec=None): if prec is None: return self.thisptr.isApprox(other.thisptr[0]) @@ -53,6 +54,9 @@ cdef class Angle: else: raise ValueError("Unknown comparison operation %d" % op) + def assign(self,Angle other): + self.thisptr.assign(other.thisptr[0]) + def __add__(Angle self, Angle other): cdef Angle ret = Angle() ret.thisptr[0] = self.thisptr[0] + other.thisptr[0] @@ -71,17 +75,30 @@ cdef class Angle: self.thisptr[0] = self.thisptr[0] - other.thisptr[0] return self - def __mul__(Angle self, factor): + def __mul__(a, b): cdef Angle ret = Angle() - if isinstance(factor, Angle): - ret.thisptr[0] = self.thisptr[0] * (factor).thisptr[0] - elif isinstance(factor, float): - ret.thisptr[0] = deref(self.thisptr) * factor + if isinstance(a, Angle) and isinstance(b, Angle): + ret.thisptr[0] = (b).thisptr[0] * (b).thisptr[0] + elif isinstance(a, float) and isinstance(b, Angle): + ret.thisptr[0] = (b).thisptr[0] * a + elif isinstance(a, Angle) and isinstance(b, float): + ret.thisptr[0] = (a).thisptr[0] * b else: raise TypeError("Angle multiplication only accept types Angle and float") return ret - #__lshift__ x, y object << operato + def __str__(self): + return "%f [%3.1fdeg]" % (self.get_rad(), self.get_deg()) + + def flipped(self): + cdef Angle ret = Angle() + ret.thisptr[0] = self.thisptr[0].flipped() + return ret + + def flip(self): + self.thisptr.flip() + return self + @staticmethod def rad_to_deg(rad): return _basetypes.rad2Deg(rad) @@ -124,6 +141,14 @@ cdef class Angle: angle.thisptr[0] = _basetypes.Max() return angle + @staticmethod + def vector_to_vector(Vector3d a, Vector3d b, Vector3d positive=None): + cdef Angle ret = Angle() + if positive: + ret.thisptr[0] = _basetypes.vectorToVector(a.thisptr[0], b.thisptr[0], positive.thisptr[0]) + else: + ret.thisptr[0] = _basetypes.vectorToVector(a.thisptr[0], b.thisptr[0]) + return ret cdef class Time: def __cinit__(self): diff --git a/bindings/python/test.py b/bindings/python/test.py index c1b9cf62..6111ca19 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -538,7 +538,7 @@ def test_angle_conversions(): def test_angle_constructors(): angle = basetypes.Angle.from_rad(1) - assert_equal(angle.get_deg(), 57.29577951308232) + assert_equal(angle.get_rad(), 1) angle = basetypes.Angle.from_deg(1) assert_equal(angle.get_deg(), 1) @@ -597,8 +597,40 @@ def test_angle_operators(): mul = angle * 2.0 assert_equal(mul.get_deg(), 2.0) -def test_angle_allocation(): - angle1 = basetypes.Angle.from_deg(1) - angle2 = angle1 + mul = 2.0 * angle + assert_equal(mul.get_deg(), 2.0) + +def test_angle_assign(): + angle1 = basetypes.Angle.from_deg(2) + angle2 = basetypes.Angle() + angle2.assign(angle1) + assert_true(angle1 == angle2) angle2+= basetypes.Angle.from_deg(1) assert_true(angle1 != angle2) + +def test_angle_flipping(): + angle1 = basetypes.Angle.from_deg(1) + angle2 = angle1.flip() + assert_true(angle1 == angle2) + assert_equal(angle1.get_deg(), -179) + + angle1 = basetypes.Angle.from_deg(1) + angle2 = angle1.flipped() + assert_false(angle1 == angle2) + assert_equal(angle1.get_deg(), 1) + assert_equal(angle2.get_deg(), -179) + +def test_angle_vector_to_vector(): + v1 = basetypes.Vector3d(0,0,1) + v2 = basetypes.Vector3d(0, -1, 0) + pos_dir = basetypes.Vector3d(-1, -1, -1) + + angle1 = basetypes.Angle.vector_to_vector(v1, v2) + assert_equal(angle1.get_deg(), 90) + + angle1 = basetypes.Angle.vector_to_vector(v1, v2, pos_dir) + assert_equal(angle1.get_deg(), -90) + +def test_angle_to_str(): + angle1 = basetypes.Angle.from_deg(123.456) + assert_equal(str(angle1), '2.154714 [123.5deg]') \ No newline at end of file From 039437311d63aaedff9d54082155b2db460ed383 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Mon, 11 Sep 2017 14:08:56 +0200 Subject: [PATCH 37/39] Move declaration of Angle to .pxd --- bindings/python/basetypes.pxd | 6 ++++++ bindings/python/basetypes.pyx | 4 +--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/bindings/python/basetypes.pxd b/bindings/python/basetypes.pxd index 889aa2f5..481fe7da 100644 --- a/bindings/python/basetypes.pxd +++ b/bindings/python/basetypes.pxd @@ -4,6 +4,12 @@ from libcpp.string cimport string from libc.stdint cimport uint32_t cimport _basetypes + +cdef class Angle: + cdef _basetypes.Angle* thisptr + cdef bool delete_thisptr + + cdef class Time: cdef _basetypes.Time* thisptr cdef bool delete_thisptr diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 305514b5..57180ab1 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -8,10 +8,8 @@ import numpy as np np.import_array() # must be here because we use the NumPy C API -cdef class Angle: - cdef _basetypes.Angle* thisptr - cdef bool delete_thisptr +cdef class Angle: def __cinit__(self, Angle other=None): self.thisptr = NULL self.delete_thisptr = False From c26dbbb6f5afb0592cd58350a142485b27365f50 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Mon, 16 Oct 2017 11:38:53 +0200 Subject: [PATCH 38/39] Fix Python 3 compatibility issues --- bindings/python/basetypes.pyx | 89 ++++++++++++++++++++--------------- bindings/python/test.py | 6 +-- 2 files changed, 55 insertions(+), 40 deletions(-) diff --git a/bindings/python/basetypes.pyx b/bindings/python/basetypes.pyx index 57180ab1..c8d60194 100644 --- a/bindings/python/basetypes.pyx +++ b/bindings/python/basetypes.pyx @@ -162,9 +162,9 @@ cdef class Time: self.delete_thisptr = True def __str__(self): - return ("" - % self.thisptr.toString(_basetypes.Resolution.Microseconds, - "%Y%m%d-%H:%M:%S")) + cdef bytes text = self.thisptr.toString( + _basetypes.Resolution.Microseconds, "%Y%m%d-%H:%M:%S") + return str("" % text.decode()) def assign(self, Time other): self.thisptr.assign(deref(other.thisptr)) @@ -222,12 +222,12 @@ cdef class Time: self.thisptr[0] = deref(self.thisptr) - deref(other.thisptr) return self - def __div__(Time self, int divider): + def __floordiv__(Time self, int divider): cdef Time time = Time() time.thisptr[0] = deref(self.thisptr) / divider return time - def __idiv__(Time self, int divider): + def __ifloordiv__(Time self, int divider): self.thisptr[0] = deref(self.thisptr) / divider return self @@ -255,7 +255,7 @@ cdef class Vector2d: self.delete_thisptr = True def __str__(self): - return "[%.2f, %.2f]" % (self.thisptr.get(0), self.thisptr.get(1)) + return str("[%.2f, %.2f]" % (self.thisptr.get(0), self.thisptr.get(1))) def assign(self, Vector2d other): self.thisptr.assign(deref(other.thisptr)) @@ -338,9 +338,9 @@ cdef class Vector3d: self.delete_thisptr = True def __str__(self): - return "[%.2f, %.2f, %.2f]" % (self.thisptr.get(0), - self.thisptr.get(1), - self.thisptr.get(2)) + return str("[%.2f, %.2f, %.2f]" % ( + self.thisptr.get(0), self.thisptr.get(1), + self.thisptr.get(2))) def assign(self, Vector3d other): self.thisptr.assign(deref(other.thisptr)) @@ -436,10 +436,10 @@ cdef class Vector4d: self.delete_thisptr = True def __str__(self): - return "[%.2f, %.2f, %.2f, %.2f]" % ( + return str("[%.2f, %.2f, %.2f, %.2f]" % ( self.thisptr.get(0), self.thisptr.get(1), self.thisptr.get(2), self.thisptr.get(3) - ) + )) def assign(self, Vector4d other): self.thisptr.assign(deref(other.thisptr)) @@ -512,13 +512,13 @@ cdef class Matrix3d: self.thisptr.data()[3 * j + i] = array[i, j] def __str__(self): - return ("[%.2f, %.2f, %.2f]," - "[%.2f, %.2f, %.2f]," - "[%.2f, %.2f, %.2f]") % ( + return str("[%.2f, %.2f, %.2f],\n" + "[%.2f, %.2f, %.2f],\n" + "[%.2f, %.2f, %.2f]" % ( self.thisptr.get(0, 0), self.thisptr.get(0, 1), self.thisptr.get(0, 2), self.thisptr.get(1, 0), self.thisptr.get(1, 1), self.thisptr.get(1, 2), self.thisptr.get(2, 0), self.thisptr.get(2, 1), self.thisptr.get(2, 2), - ) + )) def assign(self, Matrix3d other): self.thisptr.assign(deref(other.thisptr)) @@ -601,9 +601,9 @@ cdef class Quaterniond: self.delete_thisptr = True def __str__(self): - return "[im=%.2f, real=(%.2f, %.2f, %.2f)]" % ( + return str("[im=%.2f, real=(%.2f, %.2f, %.2f)]" % ( self.thisptr.w(), self.thisptr.x(), self.thisptr.y(), - self.thisptr.z()) + self.thisptr.z())) def assign(self, Quaterniond other): self.thisptr.assign(deref(other.thisptr)) @@ -634,8 +634,8 @@ cdef class TransformWithCovariance: self.delete_thisptr = True def __str__(self): - return "(translation=%s, orientation=%s)" % (self.translation, - self.orientation) + return str("(translation=%s, orientation=%s)" % (self.translation, + self.orientation)) def assign(self, TransformWithCovariance other): self.thisptr.assign(deref(other.thisptr)) @@ -692,7 +692,7 @@ cdef class JointState: parts.append("raw=%g" % self.thisptr.raw) if self.thisptr.hasAcceleration(): parts.append("acceleration=%g" % self.thisptr.acceleration) - return "JointState [%s]" % ", ".join(parts) + return str("JointState [%s]" % ", ".join(parts)) def assign(self, JointState other): self.thisptr.assign(deref(other.thisptr)) @@ -817,9 +817,8 @@ cdef class Joints: assert self.thisptr.elements.size() == self.thisptr.names.size() cdef unsigned int i for i in range(self.thisptr.elements.size()): - parts.append("%s: %s" % (self.thisptr.names[i], - self.elements[i])) - return "Joints %s {%s}" % (self.time, ", ".join(parts)) + parts.append("%s: %s" % (self.names[i], self.elements[i])) + return str("Joints %s {%s}" % (self.time, ", ".join(parts))) def assign(self, Joints other): self.thisptr.assign(deref(other.thisptr)) @@ -860,13 +859,15 @@ cdef class Joints: elements.thisptr = &self.thisptr.elements return elements - def __getitem__(self, string name): + def __getitem__(self, str name): + cdef string key = name.encode() cdef JointState joint_state = JointState() - joint_state.thisptr[0] = self.thisptr.getElementByName(name) + joint_state.thisptr[0] = self.thisptr.getElementByName(key) return joint_state - def __setitem__(self, string name, JointState joint_state): - cdef int i = self.thisptr.mapNameToIndex(name) + def __setitem__(self, str name, JointState joint_state): + cdef string key = name.encode() + cdef int i = self.thisptr.mapNameToIndex(key) self.thisptr.elements[i] = deref(joint_state.thisptr) # TODO factory methods @@ -880,10 +881,11 @@ cdef class StringVectorReference: pass def __getitem__(self, int i): - return deref(self.thisptr)[i] + return deref(self.thisptr)[i].decode() - def __setitem__(self, int i, string s): - deref(self.thisptr)[i] = s + def __setitem__(self, int i, str s): + cdef string value = s.encode() + deref(self.thisptr)[i] = value def resize(self, int i): self.thisptr.resize(i) @@ -931,7 +933,8 @@ cdef class RigidBodyState: def __str__(self): # TODO extend string representation? - return "RigidBodyState {%s, source_frame=%s, target_frame=%s, ...}" % (self.time, self.thisptr.sourceFrame, self.thisptr.targetFrame) + return str("RigidBodyState {%s, source_frame=%s, target_frame=%s, ...}" + % (self.time, self.source_frame, self.target_frame)) def assign(self, RigidBodyState other): self.thisptr.assign(deref(other.thisptr)) @@ -949,17 +952,19 @@ cdef class RigidBodyState: time = property(_get_time, _set_time) def _get_source_frame(self): - return self.thisptr.sourceFrame + return self.thisptr.sourceFrame.decode() - def _set_source_frame(self, string value): + def _set_source_frame(self, str source_frame): + cdef string value = source_frame.encode() self.thisptr.sourceFrame = value source_frame = property(_get_source_frame, _set_source_frame) def _get_target_frame(self): - return self.thisptr.targetFrame + return self.thisptr.targetFrame.decode() - def _set_target_frame(self, string value): + def _set_target_frame(self, str target_frame): + cdef string value = target_frame.encode() self.thisptr.targetFrame = value target_frame = property(_get_target_frame, _set_target_frame) @@ -1281,18 +1286,28 @@ cdef class LaserScan: self.delete_thisptr = True def __str__(self): - ranges = [] cdef unsigned int i + + ranges = [] for i in range(self.ranges.size()): ranges.append(self.ranges[i]) if len(ranges) > 5: ranges = str(ranges[:5])[:-1] + ", ...]" + else: + ranges = str(ranges) + remission = [] for i in range(self.remission.size()): remission.append(self.remission[i]) if len(remission) > 5: remission = str(remission[:5])[:-1] + ", ...]" - return "LaserScan {%s, min_range=%s, max_range=%s, ranges=%s, remission=%s}" % (self.time, self.thisptr.minRange, self.thisptr.maxRange, ranges, remission) + else: + remission = str(remission) + + return str( + "LaserScan {%s, min_range=%s, max_range=%s, ranges=%s, remission=%s}" + % (self.time, str(self.thisptr.minRange), + str(self.thisptr.maxRange), ranges, remission)) def assign(self, LaserScan other): self.thisptr.assign(deref(other.thisptr)) diff --git a/bindings/python/test.py b/bindings/python/test.py index 6111ca19..6000acd9 100644 --- a/bindings/python/test.py +++ b/bindings/python/test.py @@ -48,12 +48,12 @@ def test_time_operators(): assert_true(t2 + t2 == t4) assert_true(t4 - t2 == t2) - assert_true(t4 / 2 == t2) + assert_true(t4 // 2 == t2) assert_true(t2 * 2 == t4) t5 = basetypes.Time() t5.microseconds = 10 - t5 /= 2 + t5 //= 2 assert_equal(t5.microseconds, 5) t5 -= t2 * 4 assert_equal(t5, t2) @@ -633,4 +633,4 @@ def test_angle_vector_to_vector(): def test_angle_to_str(): angle1 = basetypes.Angle.from_deg(123.456) - assert_equal(str(angle1), '2.154714 [123.5deg]') \ No newline at end of file + assert_equal(str(angle1), '2.154714 [123.5deg]') From 38dfd9b9c81e735c88d6398941ad387358084d9c Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Mon, 8 Jan 2018 13:25:38 +0100 Subject: [PATCH 39/39] Add base-types as a dep --- manifest.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/manifest.xml b/manifest.xml index 8c2d4799..bd540632 100644 --- a/manifest.xml +++ b/manifest.xml @@ -13,6 +13,7 @@ +