diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c
new file mode 100644
index 0000000..a039c97
--- /dev/null
+++ b/python/pybreezyslam.c
@@ -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 .
+
+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
+#include
+
+#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; klaser.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
+