vato007/breezyslam
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Begin supporting scan angles argument to Scan.update()

Browse Source
This commit is contained in:
simondlevy
2018-07-04 14:26:55 -04:00
parent 2811620c6d
commit 5c3b00ec9c
3 changed files with 74 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/Makefile
View File

@@ -47,7 +47,7 @@ cpptest: log2pgm
javatest: Log2PGM.class javatest: Log2PGM.class
java -classpath ../java:. -Djava.library.path=$(JAVADIR)/algorithms:$(JAVADIR)/components Log2PGM \ java -classpath ../java:. -Djava.library.path=$(JAVADIR)/algorithms:$(JAVADIR)/components Log2PGM \
$(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED)
$(VIEWER) $(DATASET).pgm $(VIEWER) $(DATASET).pgm
log2pgm: log2pgm.o log2pgm: log2pgm.o

32
python/breezyslam/algorithms.py
View File

@@ -87,29 +87,30 @@ class CoreSLAM(object):
# Initialize the map # Initialize the map
self.map = pybreezyslam.Map(map_size_pixels, map_size_meters) 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 Updates the scan and odometry, and calls the the implementing class's _updateMapAndPointcloud method with
the specified pose change. the specified pose change.
scan_mm is a list of Lidar scan values, whose count is specified in the scan_size 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 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 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 # 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 velocity_factor = (1 / pose_change[2]) if (pose_change[2] > 0) else 0 # units => units/sec
dxy_mm_dt = poseChange[0] * velocity_factor dxy_mm_dt = pose_change[0] * velocity_factor
dtheta_degrees_dt = poseChange[1] * velocity_factor dtheta_degrees_dt = pose_change[1] * velocity_factor
velocities = (dxy_mm_dt, dtheta_degrees_dt) velocities = (dxy_mm_dt, dtheta_degrees_dt)
# Build a scan for computing distance to map, and one for updating map # 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_mapbuild, scans_mm, velocities, scan_angles_degrees)
self._scan_update(self.scan_for_distance, scans_mm, velocities) self._scan_update(self.scan_for_distance, scans_mm, velocities, scan_angles_degrees)
# Implementing class updates map and pointcloud # 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): def getmap(self, mapbytes):
''' '''
@@ -136,9 +137,10 @@ class CoreSLAM(object):
return self.__str__() return self.__str__()
def _scan_update(self, scan, lidar, 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.update(scans_mm=lidar, hole_width_mm=self.hole_width_mm,
velocities=velocities, scan_angles_degrees=scan_angles_degrees)
# SinglePositionSLAM class --------------------------------------------------------------------------------------------- # SinglePositionSLAM class ---------------------------------------------------------------------------------------------
@@ -251,13 +253,13 @@ class RMHC_SLAM(SinglePositionSLAM):
self.sigma_theta_degrees = sigma_theta_degrees self.sigma_theta_degrees = sigma_theta_degrees
self.max_search_iter = max_search_iter 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): def _getNewPosition(self, start_position):
''' '''

93
python/pybreezyslam.c
View File

@@ -268,13 +268,15 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
PyObject * py_lidar = NULL; PyObject * py_lidar = NULL;
double hole_width_mm = 0; double hole_width_mm = 0;
PyObject * py_velocities = NULL; 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, &py_lidar,
&hole_width_mm, &hole_width_mm,
&py_velocities)) &py_velocities,
&py_scan_angles_degrees))
{ {
return null_on_raise_argument_exception("Scan", "update"); 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", return null_on_raise_argument_exception_with_details("Scan", "update",
"lidar must be a list"); "lidar must be a list");
} }
// Bozo filter on LIDAR argument list size // Scan angles provided; run bozo-filter to match against lidar-list size
if (PyList_Size(py_lidar) != self->scan.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", return null_on_raise_argument_exception_with_details("Scan", "update",
"lidar size mismatch"); "lidar size mismatch");
} }
// Default to no velocities // Default to no velocities
double dxy_mm = 0; double dxy_mm = 0;
double dtheta_degrees = 0; double dtheta_degrees = 0;
// Bozo filter on velocities tuple // Bozo filter on velocities tuple
if (py_velocities) if (py_velocities)
{ {
if (!PyTuple_Check(py_velocities)) if (!PyTuple_Check(py_velocities))
{ {
return null_on_raise_argument_exception_with_details("Scan", "update", return null_on_raise_argument_exception_with_details("Scan", "update",
"velocities must be a tuple"); "velocities must be a tuple");
} }
if (!double_from_tuple(py_velocities, 0, &dxy_mm) || if (!double_from_tuple(py_velocities, 0, &dxy_mm) ||
!double_from_tuple(py_velocities, 1, &dtheta_degrees)) !double_from_tuple(py_velocities, 1, &dtheta_degrees))
{ {
return null_on_raise_argument_exception_with_details("Scan", "update", return null_on_raise_argument_exception_with_details("Scan", "update",
"velocities tuple must contain at least two numbers"); "velocities tuple must contain at least two numbers");
} }
} }
// Extract LIDAR values from argument // Extract LIDAR values from argument
int k = 0; 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)); self->lidar_mm[k] = PyFloat_AsDouble(PyList_GetItem(py_lidar, k));
} }
// Update the scan // Update the scan
scan_update( scan_update(
&self->scan, &self->scan,
self->lidar_mm, self->lidar_mm,
hole_width_mm, hole_width_mm,
dxy_mm, dxy_mm,
dtheta_degrees); dtheta_degrees);
Py_RETURN_NONE; Py_RETURN_NONE;
} }
@@ -338,31 +357,31 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
static PyMethodDef Scan_methods[] = static PyMethodDef Scan_methods[] =
{ {
{"update", (PyCFunction)Scan_update, METH_VARARGS | METH_KEYWORDS, {"update", (PyCFunction)Scan_update, METH_VARARGS | METH_KEYWORDS,
"Scan.update(scans_mm, hole_width_mm, velocities=None) updates scan.\n"\ "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"\ "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"\ "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"\ "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." "i.e., robot's (forward, rotational velocity) for improving the quality of the scan."
}, },
{NULL} // Sentinel {NULL} // Sentinel
}; };
#define TP_DOC_SCAN \ #define TP_DOC_SCAN \
"A class for Lidar scans.\n" \ "A class for Lidar scans.\n" \
"Scan.__init__(laser, span=1)\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" " span supports spanning laser scan to cover the space better"
static PyTypeObject pybreezyslam_ScanType = static PyTypeObject pybreezyslam_ScanType =
{ {
#if PY_MAJOR_VERSION < 3 #if PY_MAJOR_VERSION < 3
PyObject_HEAD_INIT(NULL) PyObject_HEAD_INIT(NULL)
0, // ob_size 0, // ob_size
#else #else
PyVarObject_HEAD_INIT(NULL, 0) PyVarObject_HEAD_INIT(NULL, 0)
#endif #endif
"pybreezyslam.Scan", // tp_name "pybreezyslam.Scan", // tp_name
sizeof(Scan), // tp_basicsize sizeof(Scan), // tp_basicsize
0, // tp_itemsize 0, // tp_itemsize
(destructor)Scan_dealloc, // tp_dealloc (destructor)Scan_dealloc, // tp_dealloc