diff --git a/bindings/python/README.md b/bindings/python/README.md new file mode 100644 index 00000000..53e024dd --- /dev/null +++ b/bindings/python/README.md @@ -0,0 +1,37 @@ +# 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 + +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. +* 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. +* ... diff --git a/bindings/python/_basetypes.pxd b/bindings/python/_basetypes.pxd new file mode 100644 index 00000000..325db618 --- /dev/null +++ b/bindings/python/_basetypes.pxd @@ -0,0 +1,328 @@ +from libcpp.string cimport string +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() + 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) + + Angle assign "operator="(Angle&) + + Angle flipped() + Angle &flip() + + #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) + 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: + Time() + + int64_t microseconds + + Time& assign "operator="(Time&) + + 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": + Time now() + + +cdef extern from "base/Eigen.hpp" namespace "base": + cdef cppclass Vector2d: + Vector2d() + Vector2d(double, double) + 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() + double norm() + double squaredNorm() + + cdef cppclass Vector3d: + Vector3d() + Vector3d(double, double, double) + 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() + double z() + double norm() + double squaredNorm() + + cdef cppclass Vector4d: + Vector4d() + Vector4d(double, double, double, double) + 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() + double z() + double norm() + double squaredNorm() + + 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() + + cdef cppclass Quaterniond: + Quaterniond() + Quaterniond(double, double, double, double) + Quaterniond& assign "operator="(Quaterniond&) + double w() + double x() + double y() + double z() + + +cdef extern from "base/TransformWithCovariance.hpp" namespace "base": + cdef cppclass TransformWithCovariance: + TransformWithCovariance() + TransformWithCovariance& assign "operator="(TransformWithCovariance&) + Vector3d translation + Quaterniond orientation + + +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 + 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": + JointState Position(double) + JointState Speed(double) + JointState Effort(double) + JointState Raw(double) + JointState Acceleration(double) + + +cdef extern from "base/NamedVector.hpp" namespace "base": + cdef cppclass NamedVector[T]: + vector[string] names + vector[T] elements + + NamedVector& assign "operator="(NamedVector&) + void resize(int) + int size() + bool empty() + void clear() + bool hasNames() + T getElementByName(string) + 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) + RigidBodyState& assign "operator="(RigidBodyState&) + Time time + string sourceFrame + string targetFrame + Vector3d position + Matrix3d cov_position + Quaterniond orientation + Matrix3d cov_orientation + Vector3d velocity + 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) + + Frame& assign "operator="(Frame&) + + 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() + + +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 + + +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() + LaserScan& assign "operator="(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() + IMUSensors& assign "operator="(IMUSensors&) + + Time time + Vector3d acc + Vector3d gyro + Vector3d mag + diff --git a/bindings/python/basetypes.pxd b/bindings/python/basetypes.pxd new file mode 100644 index 00000000..481fe7da --- /dev/null +++ b/bindings/python/basetypes.pxd @@ -0,0 +1,104 @@ +from libcpp cimport bool +from libcpp.vector cimport vector +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 + + +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 new file mode 100644 index 00000000..c8d60194 --- /dev/null +++ b/bindings/python/basetypes.pyx @@ -0,0 +1,1526 @@ +# distutils: language = c++ +cimport basetypes +from cython.operator cimport dereference as deref +from libc.stdint cimport uint8_t, uint16_t, uint32_t, uint64_t, int64_t +cimport numpy as np +import numpy as np + + +np.import_array() # must be here because we use the NumPy C API + + +cdef class Angle: + 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 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]) + 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 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] + 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__(a, b): + cdef Angle ret = Angle() + 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 + + 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) + + @staticmethod + def deg_to_rad(deg): + return _basetypes.deg2Rad(deg) + + @staticmethod + def normalize_rad(rad): + return _basetypes.normalizeRad(rad) + + @staticmethod + def from_rad(rad): + cdef Angle angle = Angle() + angle.thisptr[0] = _basetypes.fromRad(rad) + return angle + + @staticmethod + def from_deg(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 + + @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): + 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.Time() + self.delete_thisptr = True + + def __str__(self): + 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)) + + def _get_microseconds(self): + return self.thisptr.microseconds + + def _set_microseconds(self, int64_t microseconds): + self.thisptr.microseconds = microseconds + + 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 + + # TODO more factory methods + + 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 __floordiv__(Time self, int divider): + cdef Time time = Time() + time.thisptr[0] = deref(self.thisptr) / divider + return time + + def __ifloordiv__(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: + 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 str("[%.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 + 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) + 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() + + 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 + + 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 + + +cdef class Vector3d: + 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, double z=0.0): + self.thisptr = new _basetypes.Vector3d(x, y, z) + self.delete_thisptr = True + + def __str__(self): + 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)) + + 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) + 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() + + 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 + + def fromarray(self, np.ndarray[double, ndim=1] array): + cdef int i + 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 + + +cdef class Vector4d: + 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=0.0, v1=0.0, v2=0.0, v3=0.0): + self.thisptr = new _basetypes.Vector4d(v0, v1, v2, v3) + self.delete_thisptr = True + + def __str__(self): + 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)) + + 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) + 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 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 + + 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 + + +cdef class Matrix3d: + 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 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)) + + 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 + 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 + + 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] + + 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 + + +# TODO Vector6d, VectorXd, Pose + + +cdef class Quaterniond: + 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 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 str("[im=%.2f, real=(%.2f, %.2f, %.2f)]" % ( + 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(), + 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]) + + # TODO how can we modify a quaternion? + + +cdef class TransformWithCovariance: + 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.TransformWithCovariance() + self.delete_thisptr = True + + def __str__(self): + return str("(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 + translation.delete_thisptr = False + translation.thisptr = &self.thisptr.translation + return translation + + def _set_translation(self, Vector3d translation): + self.thisptr.translation = deref(translation.thisptr) + + translation = property(_get_translation, _set_translation) + + 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 orientation): + self.thisptr.orientation = deref(orientation.thisptr) + + orientation = property(_get_orientation, _set_orientation) + + # TODO covariance + + +cdef class JointState: + 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 __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 str("JointState [%s]" % ", ".join(parts)) + + def assign(self, JointState other): + self.thisptr.assign(deref(other.thisptr)) + + 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) + + 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() + self.thisptr[0] = _basetypes.Position(value) + return self + + @staticmethod + def Speed(double value): + cdef JointState self = JointState() + self.thisptr[0] = _basetypes.Speed(value) + return self + + @staticmethod + def Effort(double value): + cdef JointState self = JointState() + self.thisptr[0] = _basetypes.Effort(value) + return self + + @staticmethod + def Raw(double value): + cdef JointState self = JointState() + self.thisptr[0] = _basetypes.Raw(value) + return self + + @staticmethod + def Acceleration(double value): + cdef JointState self = JointState() + self.thisptr[0] = _basetypes.Acceleration(value) + return self + + +cdef class Joints: + 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 __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.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)) + + def size(self): + return self.thisptr.size() + + def resize(self, int size): + self.thisptr.resize(size) + + def has_names(self): + return self.thisptr.hasNames() + + 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() + 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, str name): + cdef string key = name.encode() + cdef JointState joint_state = JointState() + joint_state.thisptr[0] = self.thisptr.getElementByName(key) + return joint_state + + 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 + + +cdef class StringVectorReference: + def __cinit__(self): + self.thisptr = NULL + + def __dealloc__(self): + pass + + def __getitem__(self, int i): + return deref(self.thisptr)[i].decode() + + 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) + + def size(self): + return self.thisptr.size() + + +cdef class JointStateVectorReference: + 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 + + def __setitem__(self, int i, JointState v): + deref(self.thisptr)[i] = deref(v.thisptr) + + def resize(self, int i): + self.thisptr.resize(i) + + def size(self): + return self.thisptr.size() + + +cdef class RigidBodyState: + 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 __str__(self): + # TODO extend string representation? + 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)) + + 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_source_frame(self): + return self.thisptr.sourceFrame.decode() + + 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.decode() + + 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) + + 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) + + 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) + + +cdef class frame_mode_t: + 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.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 + 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 + + # TODO __str__ + + def assign(self, Frame 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) + + 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? + # 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: + 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 + + +cdef class Pointcloud: + 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 + + 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() + 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: + 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: + 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: + 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 __str__(self): + 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] + ", ...]" + 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)) + + 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: + 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: + 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: + 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 + + # TODO __str__ + + def assign(self, IMUSensors 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) + + 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) + + +# 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/setup.py b/bindings/python/setup.py new file mode 100644 index 00000000..c3cf6732 --- /dev/null +++ b/bindings/python/setup.py @@ -0,0 +1,62 @@ +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( + 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"] + } + ) diff --git a/bindings/python/test.py b/bindings/python/test.py new file mode 100644 index 00000000..6000acd9 --- /dev/null +++ b/bindings/python/test.py @@ -0,0 +1,636 @@ +import basetypes +import numpy as np +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 + + +def test_get_set_microseconds(): + t = basetypes.Time() + m = 1000023 + t.microseconds = m + 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 + assert_greater(mus, 0) + t.microseconds = mus + + +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_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) + 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_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) + 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(): + v = basetypes.Vector3d(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.Vector3d(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_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) + 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_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 + 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_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_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)]") + + +def test_transform_with_cov_ctor(): + basetypes.TransformWithCovariance() + + +def test_transform_set_get_translation(): + 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.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.JointState() + js.position = 5.0 + assert_equal(js.position, 5.0) + + +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(): + js = basetypes.JointState.Position(5.0) + assert_equal(js.position, 5.0) + js = basetypes.JointState.Speed(5.0) + assert_equal(js.speed, 5.0) + js = basetypes.JointState.Effort(5.0) + assert_equal(js.effort, 5.0) + js = basetypes.JointState.Raw(5.0) + assert_equal(js.raw, 5.0) + js = basetypes.JointState.Acceleration(5.0) + 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) + assert_false(j.has_names()) + 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_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) + rbs.time.microseconds = 500 + assert_equal(rbs.time.microseconds, 500) + 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.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.RigidBodyState() + assert_equal(rbs.target_frame, "") + rbs.target_frame = "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( + 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])) + + +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)) + + +def test_create_frame_rgb(): + 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) + 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)) + + +def test_create_frame_gray(): + 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) + 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() + + assert_equal(pcl.time.microseconds, 0) + + 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_laser_scan_str(): + ls = basetypes.LaserScan() + ls.min_range = 20 + ls.max_range = 30 + 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, 25, 25, ...], " + "remission=[0.0, 0.0, 0.0, 0.0, 0.0, ...]}" + ) + + +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) + + +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) + +def test_angle_constructors(): + angle = basetypes.Angle.from_rad(1) + assert_equal(angle.get_rad(), 1) + + angle = basetypes.Angle.from_deg(1) + assert_equal(angle.get_deg(), 1) + + angle = basetypes.Angle.min() + assert_true(angle.get_deg()< -179) + + angle = basetypes.Angle.max() + assert_true(angle.get_deg()> 179) + +def test_angle_comparisons(): + angle1 = basetypes.Angle.from_rad(1) + angle2 = basetypes.Angle.from_rad(0.9) + + 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_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.from_rad(1) == basetypes.Angle.from_rad(1)) + +def test_angle_operators(): + angle = basetypes.Angle.from_deg(1) + + sum = angle + angle + assert_equal(sum.get_deg(), 2) + + sum = angle - angle + assert_equal(sum.get_deg(), 0) + + angle += angle + assert_equal(angle.get_deg(), 2) + + angle -= angle + assert_equal(angle.get_deg(), 0) + + angle = basetypes.Angle.from_deg(1) + mul = angle * angle + assert_equal(mul.get_deg(), 0.017453292519943295) + + mul = angle * 2.0 + assert_equal(mul.get_deg(), 2.0) + + 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]') diff --git a/manifest.xml b/manifest.xml index bbcaea44..bd540632 100644 --- a/manifest.xml +++ b/manifest.xml @@ -13,11 +13,17 @@ + + + + + + stable