diff --git a/examples/Makefile b/examples/Makefile index 30f824d..28a2d2c 100644 --- a/examples/Makefile +++ b/examples/Makefile @@ -47,7 +47,7 @@ cpptest: log2pgm javatest: Log2PGM.class java -classpath ../java:. -Djava.library.path=$(JAVADIR)/algorithms:$(JAVADIR)/components Log2PGM \ - $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) + $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) $(VIEWER) $(DATASET).pgm log2pgm: log2pgm.o diff --git a/python/breezyslam/algorithms.py b/python/breezyslam/algorithms.py index 0c4aeef..14d7d72 100644 --- a/python/breezyslam/algorithms.py +++ b/python/breezyslam/algorithms.py @@ -87,29 +87,30 @@ class CoreSLAM(object): # Initialize the map self.map = pybreezyslam.Map(map_size_pixels, map_size_meters) - def update(self, scans_mm, poseChange, should_update_map=True): + def update(self, scans_mm, pose_change, scan_angles_degrees=None, should_update_map=True): ''' Updates the scan and odometry, and calls the the implementing class's _updateMapAndPointcloud method with the specified pose change. scan_mm is a list of Lidar scan values, whose count is specified in the scan_size attribute of the Laser object passed to the CoreSlam constructor - poseChange is a tuple (dxy_mm, dtheta_degrees, dt_seconds) computed from odometry + pose_change is a tuple (dxy_mm, dtheta_degrees, dt_seconds) computed from odometry + scan_angles_degrees is an optional list of angles corresponding to the distances in scans_mm should_update_map flags for whether you want to update the map ''' # Convert pose change (dxy,dtheta,dt) to velocities (dxy/dt, dtheta/dt) for scan update - velocity_factor = (1 / poseChange[2]) if (poseChange[2] > 0) else 0 # units => units/sec - dxy_mm_dt = poseChange[0] * velocity_factor - dtheta_degrees_dt = poseChange[1] * velocity_factor + velocity_factor = (1 / pose_change[2]) if (pose_change[2] > 0) else 0 # units => units/sec + dxy_mm_dt = pose_change[0] * velocity_factor + dtheta_degrees_dt = pose_change[1] * velocity_factor velocities = (dxy_mm_dt, dtheta_degrees_dt) # Build a scan for computing distance to map, and one for updating map - self._scan_update(self.scan_for_mapbuild, scans_mm, velocities) - self._scan_update(self.scan_for_distance, scans_mm, velocities) + self._scan_update(self.scan_for_mapbuild, scans_mm, velocities, scan_angles_degrees) + self._scan_update(self.scan_for_distance, scans_mm, velocities, scan_angles_degrees) # Implementing class updates map and pointcloud - self._updateMapAndPointcloud(poseChange[0], poseChange[1], should_update_map) + self._updateMapAndPointcloud(pose_change[0], pose_change[1], should_update_map) def getmap(self, mapbytes): ''' @@ -136,9 +137,10 @@ class CoreSLAM(object): return self.__str__() - def _scan_update(self, scan, lidar, velocities): - - scan.update(scans_mm=lidar, hole_width_mm=self.hole_width_mm, velocities=velocities) + def _scan_update(self, scan, lidar, velocities, scan_angles_degrees): + + scan.update(scans_mm=lidar, hole_width_mm=self.hole_width_mm, + velocities=velocities, scan_angles_degrees=scan_angles_degrees) # SinglePositionSLAM class --------------------------------------------------------------------------------------------- @@ -251,13 +253,13 @@ class RMHC_SLAM(SinglePositionSLAM): self.sigma_theta_degrees = sigma_theta_degrees self.max_search_iter = max_search_iter - def update(self, scan_mm, angles_degrees=None, poseChange=None, should_update_map=True): + def update(self, scans_mm, pose_change=None, scan_angles_degrees=None, should_update_map=True): - if not poseChange: + if not pose_change: - poseChange = (0, 0, 0) + pose_change = (0, 0, 0) - CoreSLAM.update(self, scan_mm, poseChange, should_update_map) + CoreSLAM.update(self, scans_mm, pose_change, scan_angles_degrees, should_update_map) def _getNewPosition(self, start_position): ''' diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index 9671d78..1f34a81 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -268,13 +268,15 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) PyObject * py_lidar = NULL; double hole_width_mm = 0; PyObject * py_velocities = NULL; + PyObject * py_scan_angles_degrees = NULL; - static char* argnames[] = {"scans_mm", "hole_width_mm", "velocities", NULL}; + static char* argnames[] = {"scans_mm", "hole_width_mm", "velocities", "scan_angles_degrees", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, kwds,"Od|O", argnames, + if (!PyArg_ParseTupleAndKeywords(args, kwds,"Od|OO", argnames, &py_lidar, &hole_width_mm, - &py_velocities)) + &py_velocities, + &py_scan_angles_degrees)) { return null_on_raise_argument_exception("Scan", "update"); } @@ -285,36 +287,53 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) 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->scan.size) + + // Scan angles provided; run bozo-filter to match against lidar-list size + if (py_scan_angles_degrees != Py_None) + { + // Bozo filter on SCAN_ANGLES_DEGREES argument + if (!PyList_Check(py_scan_angles_degrees)) + { + return null_on_raise_argument_exception_with_details("Scan", "update", + "scan angles must be a list"); + } + + if (PyList_Size(py_lidar) != PyList_Size(py_scan_angles_degrees)) + { + return null_on_raise_argument_exception_with_details("Scan", "update", + "number of scan angles must equal number of scan distances"); + } + } + + // No scan angles provided; lidar-list size must match scan size + else if (PyList_Size(py_lidar) != self->scan.size) { return null_on_raise_argument_exception_with_details("Scan", "update", - "lidar size mismatch"); + "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"); + 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", + !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; @@ -322,15 +341,15 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) { self->lidar_mm[k] = PyFloat_AsDouble(PyList_GetItem(py_lidar, k)); } - + // Update the scan scan_update( - &self->scan, - self->lidar_mm, - hole_width_mm, - dxy_mm, - dtheta_degrees); - + &self->scan, + self->lidar_mm, + hole_width_mm, + dxy_mm, + dtheta_degrees); + Py_RETURN_NONE; } @@ -338,31 +357,31 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) 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 (dxy_mm/dt, dtheta_degrees/dt);\n"\ - "i.e., robot's (forward, rotational velocity) for improving the quality of the scan." + "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 (dxy_mm/dt, dtheta_degrees/dt);\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" \ + "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"\ +"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 +#if PY_MAJOR_VERSION < 3 PyObject_HEAD_INIT(NULL) - 0, // ob_size - #else + 0, // ob_size +#else PyVarObject_HEAD_INIT(NULL, 0) - #endif - "pybreezyslam.Scan", // tp_name +#endif + "pybreezyslam.Scan", // tp_name sizeof(Scan), // tp_basicsize 0, // tp_itemsize (destructor)Scan_dealloc, // tp_dealloc