Create pybreezyslam.c
This commit is contained in:
919
python/pybreezyslam.c
Normal file
919
python/pybreezyslam.c
Normal file
@@ -0,0 +1,919 @@
|
|||||||
|
/*
|
||||||
|
pypybreezyslam.c : C extensions for BreezySLAM in Python
|
||||||
|
|
||||||
|
Copyright (C) 2014 Simon D. Levy
|
||||||
|
|
||||||
|
This code is free software: you can redistribute it and/or modify
|
||||||
|
it under the terms of the GNU Lesser General Public License as
|
||||||
|
published by the Free Software Foundation, either version 3 of the
|
||||||
|
License, or (at your option) any later version.
|
||||||
|
|
||||||
|
This code is distributed in the hope that it will be useful,
|
||||||
|
but WITHOUT ANY WARRANTY without even the implied warranty of
|
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
GNU General Public License for more details.
|
||||||
|
|
||||||
|
You should have received a copy of the GNU Lesser General Public License
|
||||||
|
along with this code. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
Change log:
|
||||||
|
|
||||||
|
07-FEB-2014 : Simon D. Levy - Initial release
|
||||||
|
|
||||||
|
17-FEB-2014 : SDL - Prohibit non-positive random seed
|
||||||
|
28-FEB-2014 : SDL - Check for null return in Robot.computeVelocities()
|
||||||
|
01-MAR-2014 : SDL - Moved module code to in __init__.py
|
||||||
|
14-MAR_2014 : SDL - Changed mm_per_pixel to pixels_per_meter
|
||||||
|
- No more robot object passed to __init__(); velocities
|
||||||
|
sent directly into update()
|
||||||
|
16-MAR_2014 : SDL - Changed #include to ../pybreezyslam
|
||||||
|
23-MAR-2014 : SDL - Changed millimeters to meters
|
||||||
|
31-MAR-2014 : SDL - Improved documetnation for Laser class
|
||||||
|
- Made all units explicit
|
||||||
|
30-APR-2014 : SDL - Migrated CoreSLAM algorithm to pure Python
|
||||||
|
- Added Position, Map, Scan classes
|
||||||
|
- Added distanceScanToMap method
|
||||||
|
04-MAY-2014 : SDL - Changed back from meters to mm
|
||||||
|
03-JUN-2014 : SDL - Made distanceScanToMap() return -1 for infinity
|
||||||
|
23-JUL-2014 : SDL - Simplified API for Laser
|
||||||
|
08-AUG-2014: SDL - Moved Laser to pure Python
|
||||||
|
13-AUG-2014: SDL - Fixed Scan.update() bug in Python3
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <Python.h>
|
||||||
|
#include <structmember.h>
|
||||||
|
|
||||||
|
#include "../c/coreslam.h"
|
||||||
|
#include "../c/random.h"
|
||||||
|
#include "pyextension_utils.h"
|
||||||
|
|
||||||
|
// Helpers ---------------------------------------------------------------------
|
||||||
|
|
||||||
|
static int pylaser2claser(PyObject * py_laser, laser_t * c_laser)
|
||||||
|
{
|
||||||
|
if (
|
||||||
|
!int_from_obj(py_laser, "scan_size", &c_laser->scan_size) ||
|
||||||
|
!double_from_obj(py_laser, "scan_rate_hz", &c_laser->scan_rate_hz) ||
|
||||||
|
!double_from_obj(py_laser, "detection_angle_degrees", &c_laser->detection_angle_degrees) ||
|
||||||
|
!double_from_obj(py_laser, "distance_no_detection_mm", &c_laser->distance_no_detection_mm) ||
|
||||||
|
!int_from_obj(py_laser, "detection_margin", &c_laser->detection_margin) ||
|
||||||
|
!double_from_obj(py_laser, "offset_mm", &c_laser->offset_mm)
|
||||||
|
)
|
||||||
|
{
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* success */
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Position class -------------------------------------------------------------
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
PyObject_HEAD
|
||||||
|
|
||||||
|
double x_mm;
|
||||||
|
double y_mm;
|
||||||
|
double theta_degrees;
|
||||||
|
|
||||||
|
} Position;
|
||||||
|
|
||||||
|
static void Position_dealloc(Position* self)
|
||||||
|
{
|
||||||
|
Py_TYPE(self)->tp_free((PyObject*)self);
|
||||||
|
}
|
||||||
|
|
||||||
|
static PyObject * Position_new(PyTypeObject *type, PyObject *args, PyObject *kwds)
|
||||||
|
{
|
||||||
|
Position *self;
|
||||||
|
|
||||||
|
self = (Position *)type->tp_alloc(type, 0);
|
||||||
|
|
||||||
|
return (PyObject *)self;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int Position_init(Position *self, PyObject *args, PyObject *kwds)
|
||||||
|
{
|
||||||
|
if (!PyArg_ParseTuple(args,"ddd",
|
||||||
|
&self->x_mm,
|
||||||
|
&self->y_mm,
|
||||||
|
&self->theta_degrees))
|
||||||
|
{
|
||||||
|
return error_on_raise_argument_exception("Position");
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static PyObject * Position_str(Position *self)
|
||||||
|
{
|
||||||
|
char str[100];
|
||||||
|
|
||||||
|
sprintf(str, "Position: x = %7.0f mm y = %7.0f mm theta = %+04.0f degrees",
|
||||||
|
self->x_mm, self->y_mm, self->theta_degrees);
|
||||||
|
|
||||||
|
return PyUnicode_FromString(str);
|
||||||
|
}
|
||||||
|
|
||||||
|
PyObject * Position_copy(Position *self, PyObject *args, PyObject *kwds);
|
||||||
|
|
||||||
|
static PyMethodDef Position_methods[] =
|
||||||
|
{
|
||||||
|
{"copy", (PyCFunction)Position_copy, METH_VARARGS,
|
||||||
|
"Returns a mutable copy of this position."},
|
||||||
|
{NULL} // Sentinel
|
||||||
|
};
|
||||||
|
|
||||||
|
static PyMemberDef Position_members[] = {
|
||||||
|
{"x_mm", T_DOUBLE, offsetof(Position, x_mm), 0,
|
||||||
|
"Distance of robot from left edge of map, in millimeters"},
|
||||||
|
{"y_mm", T_DOUBLE, offsetof(Position, y_mm), 0,
|
||||||
|
"Distance of robot from top edge of map, in millimeters"},
|
||||||
|
{"theta_degrees", T_DOUBLE, offsetof(Position, theta_degrees), 0,
|
||||||
|
"Clockwise rotation of robot with respect to three o'clock (east), in degrees"},
|
||||||
|
{NULL} /* Sentinel */
|
||||||
|
};
|
||||||
|
|
||||||
|
#define TP_DOC_POSITION \
|
||||||
|
"A class representing the position (pose; location and orientation) of a robot.\n" \
|
||||||
|
"See data descriptors for details.\n"\
|
||||||
|
"Position.__init__(x_mm, y_mm, theta_degrees)"
|
||||||
|
|
||||||
|
static PyTypeObject pybreezyslam_PositionType =
|
||||||
|
{
|
||||||
|
#if PY_MAJOR_VERSION < 3
|
||||||
|
PyObject_HEAD_INIT(NULL)
|
||||||
|
0, // ob_size
|
||||||
|
#else
|
||||||
|
PyVarObject_HEAD_INIT(NULL, 0)
|
||||||
|
#endif
|
||||||
|
"pypybreezyslam.Position", // tp_name
|
||||||
|
sizeof(Position), // tp_basicsize
|
||||||
|
0, // tp_itemsize
|
||||||
|
(destructor)Position_dealloc, // tp_dealloc
|
||||||
|
0, // tp_print
|
||||||
|
0, // tp_getattr
|
||||||
|
0, // tp_setattr
|
||||||
|
0, // tp_compare
|
||||||
|
(reprfunc)Position_str, // tp_repr
|
||||||
|
0, // tp_as_number
|
||||||
|
0, // tp_as_sequence
|
||||||
|
0, // tp_as_positionping
|
||||||
|
0, // tp_hash
|
||||||
|
0, // tp_call
|
||||||
|
(reprfunc)Position_str, // tp_str
|
||||||
|
0, // tp_getattro
|
||||||
|
0, // tp_setattro
|
||||||
|
0, // tp_as_buffer
|
||||||
|
Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, // tp_flags
|
||||||
|
TP_DOC_POSITION, // tp_doc
|
||||||
|
0, // tp_traverse
|
||||||
|
0, // tp_clear
|
||||||
|
0, // tp_richcompare
|
||||||
|
0, // tp_weaklistoffset
|
||||||
|
0, // tp_iter
|
||||||
|
0, // tp_iternext
|
||||||
|
Position_methods, // tp_methods
|
||||||
|
Position_members, // tp_members
|
||||||
|
0, // tp_getset
|
||||||
|
0, // tp_base
|
||||||
|
0, // tp_dict
|
||||||
|
0, // tp_descr_get
|
||||||
|
0, // tp_descr_set
|
||||||
|
0, // tp_dictoffset
|
||||||
|
(initproc)Position_init, // tp_init
|
||||||
|
0, // tp_alloc
|
||||||
|
Position_new, // tp_new
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
PyObject * Position_copy(Position *self, PyObject *args, PyObject *kwds)
|
||||||
|
{
|
||||||
|
PyObject * argList = Py_BuildValue("ddd",
|
||||||
|
self->x_mm, self->y_mm, self->theta_degrees);
|
||||||
|
PyObject * new_position =
|
||||||
|
PyObject_CallObject((PyObject *) &pybreezyslam_PositionType, argList);
|
||||||
|
Py_DECREF(argList);
|
||||||
|
|
||||||
|
return new_position;
|
||||||
|
}
|
||||||
|
|
||||||
|
static position_t pypos2cpos(Position * pypos)
|
||||||
|
{
|
||||||
|
position_t cpos;
|
||||||
|
|
||||||
|
cpos.x_mm = pypos->x_mm;
|
||||||
|
cpos.y_mm = pypos->y_mm;
|
||||||
|
cpos.theta_degrees = pypos->theta_degrees;
|
||||||
|
|
||||||
|
return cpos;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Scan class ------------------------------------------------------------
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
PyObject_HEAD
|
||||||
|
|
||||||
|
laser_t laser;
|
||||||
|
scan_t scan;
|
||||||
|
int * lidar_mm;
|
||||||
|
|
||||||
|
} Scan;
|
||||||
|
|
||||||
|
|
||||||
|
static void
|
||||||
|
Scan_dealloc(Scan* self)
|
||||||
|
{
|
||||||
|
scan_free(&self->scan);
|
||||||
|
|
||||||
|
free(self->lidar_mm);
|
||||||
|
|
||||||
|
Py_TYPE(self)->tp_free((PyObject*)self);
|
||||||
|
}
|
||||||
|
|
||||||
|
static PyObject *
|
||||||
|
Scan_new(PyTypeObject *type, PyObject *args, PyObject *kwds)
|
||||||
|
{
|
||||||
|
Scan *self;
|
||||||
|
|
||||||
|
self = (Scan *)type->tp_alloc(type, 0);
|
||||||
|
|
||||||
|
return (PyObject *)self;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int
|
||||||
|
Scan_init(Scan *self, PyObject *args, PyObject *kwds)
|
||||||
|
{
|
||||||
|
PyObject * py_laser = NULL;
|
||||||
|
int span = 1;
|
||||||
|
|
||||||
|
static char* argnames[] = {"laser", "span", NULL};
|
||||||
|
|
||||||
|
if(!PyArg_ParseTupleAndKeywords(args, kwds,"O|i", argnames,
|
||||||
|
&py_laser,
|
||||||
|
&span))
|
||||||
|
{
|
||||||
|
return error_on_raise_argument_exception("Scan");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (pylaser2claser(py_laser, &self->laser))
|
||||||
|
{
|
||||||
|
return error_on_raise_argument_exception("Scan");
|
||||||
|
}
|
||||||
|
|
||||||
|
scan_init(&self->scan, self->laser.scan_size, span);
|
||||||
|
|
||||||
|
self->lidar_mm = int_alloc(self->laser.scan_size);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static PyObject *
|
||||||
|
Scan_str(Scan *self)
|
||||||
|
{
|
||||||
|
char scanstr[100];
|
||||||
|
scan_string(self->scan, scanstr);
|
||||||
|
|
||||||
|
char str[200];
|
||||||
|
sprintf(str, "Scan: %s", scanstr);
|
||||||
|
|
||||||
|
return PyUnicode_FromString(str);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static PyObject *
|
||||||
|
Scan_update(Scan *self, PyObject *args, PyObject *kwds)
|
||||||
|
{
|
||||||
|
PyObject * py_lidar = NULL;
|
||||||
|
double hole_width_mm = 0;
|
||||||
|
PyObject * py_velocities = NULL;
|
||||||
|
|
||||||
|
static char* argnames[] = {"scans_mm", "hole_width_mm", "velocities", NULL};
|
||||||
|
|
||||||
|
if (!PyArg_ParseTupleAndKeywords(args, kwds,"Od|O", argnames,
|
||||||
|
&py_lidar,
|
||||||
|
&hole_width_mm,
|
||||||
|
&py_velocities))
|
||||||
|
{
|
||||||
|
return null_on_raise_argument_exception("Scan", "update");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Bozo filter on LIDAR argument
|
||||||
|
if (!PyList_Check(py_lidar))
|
||||||
|
{
|
||||||
|
return null_on_raise_argument_exception_with_details("Scan", "update",
|
||||||
|
"lidar must be a list");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Bozo filter on LIDAR argument list size
|
||||||
|
if (PyList_Size(py_lidar) != self->laser.scan_size)
|
||||||
|
{
|
||||||
|
return null_on_raise_argument_exception_with_details("Scan", "update",
|
||||||
|
"lidar size mismatch");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Default to no velocities
|
||||||
|
double dxy_mm = 0;
|
||||||
|
double dtheta_degrees = 0;
|
||||||
|
|
||||||
|
// Bozo filter on velocities tuple
|
||||||
|
if (py_velocities)
|
||||||
|
{
|
||||||
|
if (!PyTuple_Check(py_velocities))
|
||||||
|
{
|
||||||
|
return null_on_raise_argument_exception_with_details("Scan", "update",
|
||||||
|
"velocities must be a tuple");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!double_from_tuple(py_velocities, 0, &dxy_mm) ||
|
||||||
|
!double_from_tuple(py_velocities, 1, &dtheta_degrees))
|
||||||
|
{
|
||||||
|
return null_on_raise_argument_exception_with_details("Scan", "update",
|
||||||
|
"velocities tuple must contain at least two numbers");
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Extract LIDAR values from argument
|
||||||
|
int k = 0;
|
||||||
|
for (k=0; k<self->laser.scan_size; ++k)
|
||||||
|
{
|
||||||
|
self->lidar_mm[k] = PyFloat_AsDouble(PyList_GetItem(py_lidar, k));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Update the scan
|
||||||
|
scan_update(
|
||||||
|
&self->scan,
|
||||||
|
self->lidar_mm,
|
||||||
|
self->laser,
|
||||||
|
hole_width_mm,
|
||||||
|
dxy_mm,
|
||||||
|
dtheta_degrees);
|
||||||
|
|
||||||
|
Py_RETURN_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static PyMethodDef Scan_methods[] =
|
||||||
|
{
|
||||||
|
{"update", (PyCFunction)Scan_update, METH_VARARGS | METH_KEYWORDS,
|
||||||
|
"Scan.update(scans_mm, hole_width_mm, velocities=None) updates scan.\n"\
|
||||||
|
"scans_mm is a list of integers representing scanned distances in mm.\n"\
|
||||||
|
"hole_width_mm is the width of holes (obstacles, walls) in millimeters.\n"\
|
||||||
|
"velocities is an optional tuple containing at least dxy_mm, dtheta_degrees;\n"\
|
||||||
|
"i.e., robot's (forward, rotational velocity) for improving the quality of the scan."
|
||||||
|
},
|
||||||
|
{NULL} // Sentinel
|
||||||
|
};
|
||||||
|
|
||||||
|
#define TP_DOC_SCAN \
|
||||||
|
"A class for Lidar scans.\n" \
|
||||||
|
"Scan.__init__(laser, span=1)\n"\
|
||||||
|
"laser is a Laser object containing parameters of your laser rangefinder (Lidar)\n"\
|
||||||
|
" span supports spanning laser scan to cover the space better"
|
||||||
|
|
||||||
|
|
||||||
|
static PyTypeObject pybreezyslam_ScanType =
|
||||||
|
{
|
||||||
|
#if PY_MAJOR_VERSION < 3
|
||||||
|
PyObject_HEAD_INIT(NULL)
|
||||||
|
0, // ob_size
|
||||||
|
#else
|
||||||
|
PyVarObject_HEAD_INIT(NULL, 0)
|
||||||
|
#endif
|
||||||
|
"pypybreezyslam.Scan", // tp_name
|
||||||
|
sizeof(Scan), // tp_basicsize
|
||||||
|
0, // tp_itemsize
|
||||||
|
(destructor)Scan_dealloc, // tp_dealloc
|
||||||
|
0, // tp_print
|
||||||
|
0, // tp_getattr
|
||||||
|
0, // tp_setattr
|
||||||
|
0, // tp_compare
|
||||||
|
(reprfunc)Scan_str, // tp_repr
|
||||||
|
0, // tp_as_number
|
||||||
|
0, // tp_as_sequence
|
||||||
|
0, // tp_as_positionping
|
||||||
|
0, // tp_hash
|
||||||
|
0, // tp_call
|
||||||
|
(reprfunc)Scan_str, // tp_str
|
||||||
|
0, // tp_getattro
|
||||||
|
0, // tp_setattro
|
||||||
|
0, // tp_as_buffer
|
||||||
|
Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, // tp_flags
|
||||||
|
TP_DOC_SCAN, // tp_doc
|
||||||
|
0, // tp_traverse
|
||||||
|
0, // tp_clear
|
||||||
|
0, // tp_richcompare
|
||||||
|
0, // tp_weaklistoffset
|
||||||
|
0, // tp_iter
|
||||||
|
0, // tp_iternext
|
||||||
|
Scan_methods, // tp_methods
|
||||||
|
0, // tp_members
|
||||||
|
0, // tp_getset
|
||||||
|
0, // tp_base
|
||||||
|
0, // tp_dict
|
||||||
|
0, // tp_descr_get
|
||||||
|
0, // tp_descr_set
|
||||||
|
0, // tp_dictoffset
|
||||||
|
(initproc)Scan_init, // tp_init
|
||||||
|
0, // tp_alloc
|
||||||
|
Scan_new, // tp_new
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
// Map class ------------------------------------------------------------
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
PyObject_HEAD
|
||||||
|
|
||||||
|
map_t map;
|
||||||
|
|
||||||
|
} Map;
|
||||||
|
|
||||||
|
// Helper for Map.__init__(), Map.set()
|
||||||
|
static int bad_mapbytes(PyObject * py_mapbytes, int size_pixels, const char * methodname)
|
||||||
|
{
|
||||||
|
if (!PyByteArray_Check(py_mapbytes))
|
||||||
|
{
|
||||||
|
return error_on_raise_argument_exception_with_details("Map", methodname,
|
||||||
|
"argument is not a byte array");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (PyByteArray_GET_SIZE(py_mapbytes) != (size_pixels * size_pixels))
|
||||||
|
{
|
||||||
|
return error_on_raise_argument_exception_with_details("Map", methodname,
|
||||||
|
"mapbytes are wrong size");
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void
|
||||||
|
Map_dealloc(Map* self)
|
||||||
|
{
|
||||||
|
map_free(&self->map);
|
||||||
|
|
||||||
|
Py_TYPE(self)->tp_free((PyObject*)self);
|
||||||
|
}
|
||||||
|
|
||||||
|
static PyObject *
|
||||||
|
Map_new(PyTypeObject *type, PyObject *args, PyObject *kwds)
|
||||||
|
{
|
||||||
|
Map *self;
|
||||||
|
|
||||||
|
self = (Map *)type->tp_alloc(type, 0);
|
||||||
|
|
||||||
|
return (PyObject *)self;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int
|
||||||
|
Map_init(Map *self, PyObject *args, PyObject *kwds)
|
||||||
|
{
|
||||||
|
int size_pixels;
|
||||||
|
double size_meters;
|
||||||
|
PyObject * py_bytes = NULL;
|
||||||
|
|
||||||
|
static char * argnames[] = {"size_pixels", "size_meters", "bytes", NULL};
|
||||||
|
|
||||||
|
if(!PyArg_ParseTupleAndKeywords(args, kwds,"id|O", argnames,
|
||||||
|
&size_pixels,
|
||||||
|
&size_meters,
|
||||||
|
&py_bytes))
|
||||||
|
{
|
||||||
|
return error_on_raise_argument_exception("Map");
|
||||||
|
}
|
||||||
|
|
||||||
|
map_init(&self->map, size_pixels, size_meters);
|
||||||
|
|
||||||
|
if (py_bytes && !bad_mapbytes(py_bytes, size_pixels, "__init__"))
|
||||||
|
{
|
||||||
|
map_set(&self->map, PyByteArray_AsString(py_bytes));
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static PyObject *
|
||||||
|
Map_str(Map * self)
|
||||||
|
{
|
||||||
|
char mapstr[100];
|
||||||
|
map_string(self->map, mapstr);
|
||||||
|
|
||||||
|
char str[200];
|
||||||
|
sprintf(str, "Map: %s", mapstr);
|
||||||
|
|
||||||
|
return PyUnicode_FromString(str);
|
||||||
|
}
|
||||||
|
|
||||||
|
static PyObject *
|
||||||
|
Map_get(Map * self, PyObject * args, PyObject * kwds)
|
||||||
|
{
|
||||||
|
PyObject * py_mapbytes = NULL;
|
||||||
|
|
||||||
|
if (!PyArg_ParseTuple(args, "O", &py_mapbytes))
|
||||||
|
{
|
||||||
|
return null_on_raise_argument_exception("Map", "get");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (bad_mapbytes(py_mapbytes, self->map.size_pixels, "get"))
|
||||||
|
{
|
||||||
|
Py_RETURN_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
map_get(&self->map, PyByteArray_AsString(py_mapbytes));
|
||||||
|
|
||||||
|
Py_RETURN_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
static PyObject *
|
||||||
|
Map_update(Map *self, PyObject *args, PyObject *kwds)
|
||||||
|
{
|
||||||
|
Scan * py_scan = NULL;
|
||||||
|
Position * py_position = NULL;
|
||||||
|
int map_quality = 0;
|
||||||
|
double hole_width_mm = 0;
|
||||||
|
|
||||||
|
if (!PyArg_ParseTuple(args, "OOid",
|
||||||
|
&py_scan,
|
||||||
|
&py_position,
|
||||||
|
&map_quality,
|
||||||
|
&hole_width_mm))
|
||||||
|
{
|
||||||
|
return null_on_raise_argument_exception("Map", "update");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (error_on_check_argument_type((PyObject *)py_scan, &pybreezyslam_ScanType, 0,
|
||||||
|
"pybreezyslam.Scan", "Map", "update") ||
|
||||||
|
error_on_check_argument_type((PyObject *)py_position, &pybreezyslam_PositionType, 0,
|
||||||
|
"pybreezyslam.Position", "Map", "update"))
|
||||||
|
{
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
position_t position = pypos2cpos(py_position);
|
||||||
|
|
||||||
|
map_update(
|
||||||
|
&self->map,
|
||||||
|
&py_scan->scan,
|
||||||
|
position,
|
||||||
|
map_quality,
|
||||||
|
hole_width_mm);
|
||||||
|
|
||||||
|
Py_RETURN_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
static PyMethodDef Map_methods[] =
|
||||||
|
{
|
||||||
|
{"update", (PyCFunction)Map_update, METH_VARARGS,
|
||||||
|
"Map.update(Scan, Position, quality, hole_width_mm) updates map based on scan and position.\n"\
|
||||||
|
"Quality from 0 through 255 determines integration speed of scan into map.\n"\
|
||||||
|
"Hole width determines width of obstacles (walls)."
|
||||||
|
},
|
||||||
|
{"get", (PyCFunction)Map_get, METH_VARARGS,
|
||||||
|
"Map.get(bytearray) fills byte array with map pixels, where bytearray length is square of size of map."
|
||||||
|
},
|
||||||
|
{NULL} // Sentinel
|
||||||
|
};
|
||||||
|
|
||||||
|
#define TP_DOC_MAP \
|
||||||
|
"A class for maps used in SLAM.\n"\
|
||||||
|
"Map.__init__(size_pixels, scale_mm_per_pixel, bytes=None)"
|
||||||
|
|
||||||
|
|
||||||
|
static PyTypeObject pybreezyslam_MapType =
|
||||||
|
{
|
||||||
|
#if PY_MAJOR_VERSION < 3
|
||||||
|
PyObject_HEAD_INIT(NULL)
|
||||||
|
0, // ob_size
|
||||||
|
#else
|
||||||
|
PyVarObject_HEAD_INIT(NULL, 0)
|
||||||
|
#endif
|
||||||
|
"pypybreezyslam.Map", // tp_name
|
||||||
|
sizeof(Map), // tp_basicsize
|
||||||
|
0, // tp_itemsize
|
||||||
|
(destructor)Map_dealloc, // tp_dealloc
|
||||||
|
0, // tp_print
|
||||||
|
0, // tp_getattr
|
||||||
|
0, // tp_setattr
|
||||||
|
0, // tp_compare
|
||||||
|
(reprfunc)Map_str, // tp_repr
|
||||||
|
0, // tp_as_number
|
||||||
|
0, // tp_as_sequence
|
||||||
|
0, // tp_as_positionping
|
||||||
|
0, // tp_hash
|
||||||
|
0, // tp_call
|
||||||
|
(reprfunc)Map_str, // tp_str
|
||||||
|
0, // tp_getattro
|
||||||
|
0, // tp_setattro
|
||||||
|
0, // tp_as_buffer
|
||||||
|
Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, // tp_flags
|
||||||
|
TP_DOC_MAP, // tp_doc
|
||||||
|
0, // tp_traverse
|
||||||
|
0, // tp_clear
|
||||||
|
0, // tp_richcompare
|
||||||
|
0, // tp_weaklistoffset
|
||||||
|
0, // tp_iter
|
||||||
|
0, // tp_iternext
|
||||||
|
Map_methods, // tp_methods
|
||||||
|
0, // tp_members
|
||||||
|
0, // tp_getset
|
||||||
|
0, // tp_base
|
||||||
|
0, // tp_dict
|
||||||
|
0, // tp_descr_get
|
||||||
|
0, // tp_descr_set
|
||||||
|
0, // tp_dictoffset
|
||||||
|
(initproc)Map_init, // tp_init
|
||||||
|
0, // tp_alloc
|
||||||
|
Map_new, // tp_new
|
||||||
|
};
|
||||||
|
|
||||||
|
// Randomizer class ------------------------------------------------------------
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
PyObject_HEAD
|
||||||
|
|
||||||
|
void * randomizer;
|
||||||
|
|
||||||
|
} Randomizer;
|
||||||
|
|
||||||
|
|
||||||
|
static void
|
||||||
|
Randomizer_dealloc(Randomizer* self)
|
||||||
|
{
|
||||||
|
random_free(self->randomizer);
|
||||||
|
|
||||||
|
Py_TYPE(self)->tp_free((PyObject*)self);
|
||||||
|
}
|
||||||
|
|
||||||
|
static PyObject *
|
||||||
|
Randomizer_new(PyTypeObject *type, PyObject *args, PyObject *kwds)
|
||||||
|
{
|
||||||
|
Randomizer *self;
|
||||||
|
|
||||||
|
self = (Randomizer *)type->tp_alloc(type, 0);
|
||||||
|
|
||||||
|
return (PyObject *)self;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int
|
||||||
|
Randomizer_init(Randomizer *self, PyObject *args, PyObject *kwds)
|
||||||
|
{
|
||||||
|
int seed;
|
||||||
|
|
||||||
|
if (!PyArg_ParseTuple(args, "i", &seed))
|
||||||
|
{
|
||||||
|
return error_on_raise_argument_exception("Randomizer");
|
||||||
|
}
|
||||||
|
|
||||||
|
self->randomizer = random_init(seed);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
#define TP_DOC_RANDOMIZER \
|
||||||
|
""
|
||||||
|
|
||||||
|
static PyTypeObject pybreezyslam_RandomizerType =
|
||||||
|
{
|
||||||
|
#if PY_MAJOR_VERSION < 3
|
||||||
|
PyObject_HEAD_INIT(NULL)
|
||||||
|
0, // ob_size
|
||||||
|
#else
|
||||||
|
PyVarObject_HEAD_INIT(NULL, 0)
|
||||||
|
#endif
|
||||||
|
"pypybreezyslam.Randomizer", // tp_name
|
||||||
|
sizeof(Randomizer), // tp_basicsize
|
||||||
|
0, // tp_itemsize
|
||||||
|
(destructor)Randomizer_dealloc, // tp_dealloc
|
||||||
|
0, // tp_print
|
||||||
|
0, // tp_getattr
|
||||||
|
0, // tp_setattr
|
||||||
|
0, // tp_compare
|
||||||
|
0, // tp_repr
|
||||||
|
0, // tp_as_number
|
||||||
|
0, // tp_as_sequence
|
||||||
|
0, // tp_as_positionping
|
||||||
|
0, // tp_hash
|
||||||
|
0, // tp_call
|
||||||
|
0, // tp_str
|
||||||
|
0, // tp_getattro
|
||||||
|
0, // tp_setattro
|
||||||
|
0, // tp_as_buffer
|
||||||
|
Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, // tp_flags
|
||||||
|
TP_DOC_RANDOMIZER, // tp_doc
|
||||||
|
0, // tp_traverse
|
||||||
|
0, // tp_clear
|
||||||
|
0, // tp_richcompare
|
||||||
|
0, // tp_weaklistoffset
|
||||||
|
0, // tp_iter
|
||||||
|
0, // tp_iternext
|
||||||
|
0, // tp_methods
|
||||||
|
0, // tp_members
|
||||||
|
0, // tp_getset
|
||||||
|
0, // tp_base
|
||||||
|
0, // tp_dict
|
||||||
|
0, // tp_descr_get
|
||||||
|
0, // tp_descr_set
|
||||||
|
0, // tp_dictoffset
|
||||||
|
(initproc)Randomizer_init, // tp_init
|
||||||
|
0, // tp_alloc
|
||||||
|
Randomizer_new, // tp_new
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
// pypybreezyslam module ------------------------------------------------------------
|
||||||
|
|
||||||
|
|
||||||
|
static PyObject *
|
||||||
|
distanceScanToMap(PyObject *self, PyObject *args)
|
||||||
|
{
|
||||||
|
Map * py_map = NULL;
|
||||||
|
Scan * py_scan = NULL;
|
||||||
|
Position * py_position = NULL;
|
||||||
|
|
||||||
|
// Extract Python objects for map, scan, and position
|
||||||
|
if (!PyArg_ParseTuple(args, "OOO", &py_map, &py_scan, &py_position))
|
||||||
|
{
|
||||||
|
return null_on_raise_argument_exception("breezyslam", "distanceScanToMap");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check object types
|
||||||
|
if (error_on_check_argument_type((PyObject *)py_map, &pybreezyslam_MapType, 0,
|
||||||
|
"pybreezyslam.Map", "pybreezyslam", "distanceScanToMap") ||
|
||||||
|
error_on_check_argument_type((PyObject *)py_scan, &pybreezyslam_ScanType, 1,
|
||||||
|
"pybreezyslam.Scan", "pybreezyslam", "distanceScanToMap") ||
|
||||||
|
error_on_check_argument_type((PyObject *)py_position, &pybreezyslam_PositionType, 2,
|
||||||
|
"pybreezyslam.Position", "pybreezyslam", "distanceScanToMap"))
|
||||||
|
{
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Translate position object from Python to C
|
||||||
|
position_t c_position = pypos2cpos(py_position);
|
||||||
|
|
||||||
|
// Run C version and return Python integer
|
||||||
|
return PyLong_FromLong(distance_scan_to_map(&py_map->map, &py_scan->scan, c_position));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Called internally, so minimal type-checking on arguments
|
||||||
|
static PyObject *
|
||||||
|
rmhcPositionSearch(PyObject *self, PyObject *args)
|
||||||
|
{
|
||||||
|
Position * py_start_pos = NULL;
|
||||||
|
Map * py_map = NULL;
|
||||||
|
Scan * py_scan = NULL;
|
||||||
|
PyObject * py_laser = NULL;
|
||||||
|
double sigma_xy_mm = 0;
|
||||||
|
double sigma_theta_degrees = 0;
|
||||||
|
int max_search_iter = 0;
|
||||||
|
Randomizer * py_randomizer = NULL;
|
||||||
|
|
||||||
|
// Extract Python objects for map, scan, and position
|
||||||
|
if (!PyArg_ParseTuple(args, "OOOOddiO",
|
||||||
|
&py_start_pos,
|
||||||
|
&py_map,
|
||||||
|
&py_scan,
|
||||||
|
&py_laser,
|
||||||
|
&sigma_xy_mm,
|
||||||
|
&sigma_theta_degrees,
|
||||||
|
&max_search_iter,
|
||||||
|
&py_randomizer))
|
||||||
|
{
|
||||||
|
return null_on_raise_argument_exception("breezyslam.algorithms", "rmhcPositionSearch");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Convert Python objects to C structures
|
||||||
|
position_t start_pos = pypos2cpos(py_start_pos);
|
||||||
|
|
||||||
|
// Convert Python laser object to C struct
|
||||||
|
laser_t c_laser;
|
||||||
|
if (pylaser2claser(py_laser, &c_laser))
|
||||||
|
{
|
||||||
|
return null_on_raise_argument_exception("breezyslam", "rmhcPositionSearch");
|
||||||
|
}
|
||||||
|
|
||||||
|
position_t likeliest_position =
|
||||||
|
rmhc_position_search(
|
||||||
|
start_pos,
|
||||||
|
&py_map->map,
|
||||||
|
&py_scan->scan,
|
||||||
|
c_laser,
|
||||||
|
sigma_xy_mm,
|
||||||
|
sigma_theta_degrees,
|
||||||
|
max_search_iter,
|
||||||
|
py_randomizer->randomizer);
|
||||||
|
|
||||||
|
|
||||||
|
// Convert C position back to Python object
|
||||||
|
PyObject * argList = Py_BuildValue("ddd",
|
||||||
|
likeliest_position.x_mm,
|
||||||
|
likeliest_position.y_mm,
|
||||||
|
likeliest_position.theta_degrees);
|
||||||
|
PyObject * py_likeliest_position =
|
||||||
|
PyObject_CallObject((PyObject *) &pybreezyslam_PositionType, argList);
|
||||||
|
Py_DECREF(argList);
|
||||||
|
|
||||||
|
return py_likeliest_position;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static PyMethodDef module_methods[] =
|
||||||
|
{
|
||||||
|
{"distanceScanToMap", distanceScanToMap, METH_VARARGS,
|
||||||
|
"distanceScanToMap(map, scan, position)\n"
|
||||||
|
"Computes distance between a scan and map, given hypothetical position, to support particle filtering.\n"\
|
||||||
|
"Returns -1 for infinity.\n"\
|
||||||
|
"map is a breezyslam.components.Map object\n"\
|
||||||
|
"scan is a breezyslam.components.Scan object\n"\
|
||||||
|
"position is a breezyslam.components.Position object\n"\
|
||||||
|
},
|
||||||
|
{"rmhcPositionSearch", rmhcPositionSearch, METH_VARARGS,
|
||||||
|
"rmhcPositionSearch(startpos, map, scan, laser, sigma_xy_mm, max_iter, randomizer)\n"
|
||||||
|
"Internal use only."
|
||||||
|
},
|
||||||
|
{NULL, NULL, 0, NULL} /* Sentinel */
|
||||||
|
};
|
||||||
|
|
||||||
|
static void add_classes(PyObject * module)
|
||||||
|
{
|
||||||
|
add_class(module, &pybreezyslam_ScanType, "Scan");
|
||||||
|
add_class(module, &pybreezyslam_MapType, "Map");
|
||||||
|
add_class(module, &pybreezyslam_PositionType, "Position");
|
||||||
|
add_class(module, &pybreezyslam_RandomizerType, "Randomizer");
|
||||||
|
}
|
||||||
|
|
||||||
|
static int types_are_ready(void)
|
||||||
|
{
|
||||||
|
return
|
||||||
|
type_is_ready(&pybreezyslam_ScanType) &&
|
||||||
|
type_is_ready(&pybreezyslam_MapType) &&
|
||||||
|
type_is_ready(&pybreezyslam_PositionType) &&
|
||||||
|
type_is_ready(&pybreezyslam_RandomizerType);
|
||||||
|
}
|
||||||
|
|
||||||
|
#if PY_MAJOR_VERSION < 3
|
||||||
|
|
||||||
|
PyMODINIT_FUNC
|
||||||
|
initpybreezyslam(void)
|
||||||
|
{
|
||||||
|
if (!types_are_ready()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
PyObject * module = Py_InitModule("pybreezyslam", module_methods);
|
||||||
|
|
||||||
|
if (module == NULL)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
add_classes(module);
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
static PyModuleDef moduledef =
|
||||||
|
{
|
||||||
|
PyModuleDef_HEAD_INIT,
|
||||||
|
"pybreezyslam",
|
||||||
|
"BreezySLAM module",
|
||||||
|
-1, // m_size
|
||||||
|
module_methods,
|
||||||
|
NULL, // m_reload
|
||||||
|
NULL, // m_traverse
|
||||||
|
NULL, // m_clear
|
||||||
|
NULL // m_free
|
||||||
|
};
|
||||||
|
|
||||||
|
PyMODINIT_FUNC
|
||||||
|
PyInit_pybreezyslam(void)
|
||||||
|
{
|
||||||
|
if (!types_are_ready())
|
||||||
|
{
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
PyObject* module = PyModule_Create(&moduledef);
|
||||||
|
|
||||||
|
if (module == NULL)
|
||||||
|
{
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
add_classes(module);
|
||||||
|
|
||||||
|
return module;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
Reference in New Issue
Block a user