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