diff --git a/c/coreslam.c b/c/coreslam.c index bcb2d7d..dd858f6 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -62,12 +62,6 @@ static double * double_alloc(int size) return (double *)safe_malloc(size * sizeof(double)); } -static float * float_alloc(int size) -{ - return (float *)safe_malloc(size * sizeof(float)); -} - - static void swap(int * a, int * b) { @@ -264,6 +258,13 @@ int * return (int *)safe_malloc(size * sizeof(int)); } +float * + float_alloc( + int size) +{ + return (float *)safe_malloc(size * sizeof(float)); +} + void map_init( map_t * map, diff --git a/c/coreslam.h b/c/coreslam.h index e3f1b6e..879a8ac 100644 --- a/c/coreslam.h +++ b/c/coreslam.h @@ -86,6 +86,10 @@ int * int_alloc( int size); +float * +float_alloc( + int size); + void map_init( map_t * map, diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index 0632f80..9ac1878 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -176,7 +176,8 @@ typedef struct PyObject_HEAD scan_t scan; - int * lidar_mm; + int * lidar_distances_mm; + float * lidar_angles_deg; } Scan; @@ -186,7 +187,8 @@ Scan_dealloc(Scan* self) { scan_free(&self->scan); - free(self->lidar_mm); + free(self->lidar_distances_mm); + free(self->lidar_angles_deg); Py_TYPE(self)->tp_free((PyObject*)self); } @@ -243,7 +245,8 @@ Scan_init(Scan *self, PyObject *args, PyObject *kwds) detection_margin, offset_mm); - self->lidar_mm = int_alloc(self->scan.size); + self->lidar_distances_mm = int_alloc(self->scan.size); + self->lidar_angles_deg = float_alloc(self->scan.size); return 0; } @@ -338,13 +341,13 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) // Extract LIDAR values from argument for (int k=0; klidar_mm[k] = (int)PyFloat_AsDouble(PyList_GetItem(py_lidar, k)); + self->lidar_distances_mm[k] = (int)PyFloat_AsDouble(PyList_GetItem(py_lidar, k)); } // Update the scan scan_update( &self->scan, - self->lidar_mm, + self->lidar_distances_mm, hole_width_mm, dxy_mm, dtheta_degrees);