From b5ec23d209e5fbbed815192562271c9cb6ef809d Mon Sep 17 00:00:00 2001 From: "Simon D. Levy" Date: Fri, 8 Jun 2018 21:57:09 -0400 Subject: [PATCH] Removed obsolete files referencing laser_t datatype --- cpp/Laser.cpp | 85 ---------- matlab/mex_breezyslam.mexa64 | Bin 23458 -> 23466 bytes matlab/mex_coreslam.c | 300 ----------------------------------- 3 files changed, 385 deletions(-) delete mode 100644 cpp/Laser.cpp delete mode 100644 matlab/mex_coreslam.c diff --git a/cpp/Laser.cpp b/cpp/Laser.cpp deleted file mode 100644 index 946e1fe..0000000 --- a/cpp/Laser.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/** -* -* BreezySLAM: Simple, efficient SLAM in C++ -* -* Laser.cpp - C++ code for Laser model class -* -* 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 . -*/ - -#include -#include - -#include -#include -using namespace std; - -#include "Laser.hpp" -#include "coreslam.h" - - -Laser::Laser( - int scanSize, - float scanRateHz, - float detection_angle_degrees, - float distance_no_detection_mm, - int detection_margin, - float offset_mm - ) -{ - this->laser = new laser_t; - - this->laser->scan_size = scanSize; - this->laser->scan_rate_hz = scanRateHz; - this->laser->detection_angle_degrees = detection_angle_degrees; - this->laser->distance_no_detection_mm = distance_no_detection_mm; - this->laser->detection_margin = detection_margin; - this->laser->offset_mm = offset_mm; -} - - -Laser::Laser(void) -{ - this->laser = new laser_t; - - this->laser->scan_size = 0; - this->laser->scan_rate_hz = 0; - this->laser->detection_angle_degrees = 0; - this->laser->distance_no_detection_mm = 0; - this->laser->detection_margin = 0; - this->laser->offset_mm = 0; -} - - -Laser::~Laser(void) -{ - delete this->laser; -} - -ostream& operator<< (ostream & out, Laser & laser) -{ - char str[512]; - char lstr[512]; - - laser_string(*laser.laser, lstr); - - sprintf(str, "<%s>", lstr); - - out << str; - - return out; -} - diff --git a/matlab/mex_breezyslam.mexa64 b/matlab/mex_breezyslam.mexa64 index f69b7a1b2c7b4246390edcf0b30c5b1f0bbe13d7..0e9425666089018199330fdca8ff0426ea80a9e7 100755 GIT binary patch delta 2085 zcmZ9N4N%i%9LN999|jGD|7g5z;0BeBF~-XnFCxYfH<(2M=lL=^C^aeXw6rAcQn8e9 z(e6gS>QL`WE6d)!{u%T_jFLpr)5;3O6vCHra*51$t$*MDHY?+9zt8jh-k;x|hrQX( z-fw5_RAz5A<}2UM=lp+fw=U_5F31XO{vl0gZ^RV2(}sG6v$xjIUiopkZ*NUs{ZNlM z#)_j@E2XOvv}>nu+^m+OyS#5nk~ASi+xjb7mi60Rxl5={wT{1%M8{Pd*|W4Lx=NRm z#Bm*R5$}oXk~DfTl;iBK&hft_3I1C8A$kXEq*b@iVcB%>_WY$Gi5yqqJmWoi!=Jfn zTfp&>HlRKjRJR4xV?p)jLA4v|ik96nko|Pf+IEYzGiV*w(sFeyKd7Z<^;2v)U03g5 zWwbpeiEr0XS4=Z2r1IEWek?(JCw7D}Exj238naSL!W5-0p5rEjI)#qP6V@^}^&~jL zU1ca)!O>KWg~if*O(C-oX%b=p2d09ex2(v|M=>9CxrQt7u4eO zi3b>aU96mv6vB5$(X!-1{^uxqFFBcgK>L%QV2NU3%HzEH!%1><^x~Zpf@+x?xy@VX zt2;-8ZzBFv>5B;Z=q{1JprnOU&3uniY@Hg$__0vhJZ%kYrZ~gK>WJI?d6xAfkFCF6 zn5U?M#%nqyPhJ)23zyAx_rpBD+q(nWdLi{N++vMz!+JlOxc_j_#Sz&TPG=2i>|KgW zbrlaPNH^VAjUzV=3QcZwb9NF|dQNECfK7$-vbW{W ze;S&H{PI|S4UtFWKi7~C_v3)a4B);}bv7m7a63x=JOq}%W{ z{SFp@DdPwPE5X&^TjwQdCs=$zlKR1;7vYZ!bP7xbBQBvq&9^3_XfQP|8 za9lBi<3_Mh;bz8MMyFu%6%+&)T$QBt;8)-_@EGU;6R)9Bu#ygD=kVMh@s??)=bt%4y*_18OTet8Jag~AJ*M83VDc>xtYvI(;*+CC6N0m zB`;BN1jF1-U*zi9mo$)TU|$o<)3e`T)3aVGfgGX*kU!CLkbSfr@)&jJ8JLGg^9(5i z6OvSe>%x^psb1hqBg6PPlf&^p2EQC!1qP_5?0h|oq3V2t!sL@AD@EoeB;3LLPI6p2 zK6~JpiErwVY-F9{_xV36UM{GjwwdesU1m{xw-BPSnk@|r8y;WTki9P3nvrGAuo{!C z83h?uaYl)aiH-LR^Sq>^g|>`{U1m8k$L%uH7F%9~C%E;P>8LGF`R~uArYv9S&JOsx zMMqgV&rG!4o{tAUU@vByNHeRLb%>5xkFxBWIRoAS8Nx+zHM=)}xqVzQ0Kq}C&$GibFD$4MZTdIU|DkWm6AkZ7{~@7`gX+|0ed zcb|{{^M3BWeww{}nsubI)tzaM@H-Ap``zi>cFyojcHrSYX&LK@>YmwBbMpP^i`#FN zIXiEBv@7jI@|5%yD~x0#lqp8XUW(_q()N|f@+*p>ObQ0)pu4?((B;iLL)GG+cvn3A zM?Ap(NlzNqC$uc)xOdee^7XrlGWJa<$GN;`Cw^2E#MjY);TZdfb}#rPdzvmSaO`+2 zj^o^(FXS8dH09PKfxzOR`BKo_6)>+1nokAI9WcAw-%$be{Ghk%C*HF`uPu&t8clpg z933+Lmig$e@fdrajz%r!!(!=N)FIYJC8kaM${6XmX_T=#YFqR?J3vX%@!<`NIBrs? z)-_N`^j_wte?(V>wcKK5UUxpF#MoI4RmLo5`-x&|BL8YqW2>BrYQme+oY-2nupmma zdADlISKU7;RO;77N&Dk2GImQUk6#?Z8w9FPT+XKmbRsd4-K7hOzhT>@L2BQfJB>#xK)R_mV%dPiUd# zK*h$nnoj*7bew~=!Uo+DSmcbG>dK4ywVJ#7VYn-Mm#KwKR-wrslVSn;Y}%uCm=)5htX7?Ow4%v|?>+ZWn6Jv=nLX+~X^ znOec>YszL9RGkY`L6ryYt0V?w_G>bt{xB$S8daAn%2z9F^qr@!v;-2<6Xx~c5PCjz zdv42hv@E?k?HaB`>$S#*#=}~#31+S_Pj&U+;35-=^7hXHOO!)r)0Z17;Fww~i1UxP zjI*gCL5zBdA~MYO&vBiC36GDcWvx!%?p#&sgerAv(DSLwJO4TL{uQkVeL-khO^;`k zN3Vgi$kXTfuw}H_GdNi!q+?%1eKmcMk@3hcu+$h+Dp3tLd=GSiH^7}>%D0Nr0cL|I!L8tB zunC;dt>8F9N5skP1GB-)(r~wvuc)W|l)0 zXj5ocj)i%s8~xpM2JJx_LQE5d=bBkNrJ+4erMXMlqx2N|yQmv&Go69u6&gb;P)y!Z z_6Hisi_=}jV%E^MJTuXpDRMID znKgc%FE>-2(;C59RXO~u^s+NQLJU5NR{G4DAO62@bW*V++;0nn`lTm|OL&${10hEG zn=2U$J>n{4`zWcjke!sCDBZ$J@9PFqv^Io`{X-!3O_|=R+ S@D-`EqAip;q{YAbi|)Vgv`bR} diff --git a/matlab/mex_coreslam.c b/matlab/mex_coreslam.c deleted file mode 100644 index b4b9b3e..0000000 --- a/matlab/mex_coreslam.c +++ /dev/null @@ -1,300 +0,0 @@ -/* - * mex_coreslam.c : C extensions for BreezySLAM in Matlab - * - * 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 . - */ - -#include "mex.h" - -#include "../c/coreslam.h" -#include "../c/random.h" - -#define MAXSTR 100 - -/* Helpers ------------------------------------------------------------- */ - -static int _streq(char * s, const char * t) -{ - return !strcmp(s, t); -} - -static void _insert_obj_lhs(mxArray *plhs[], void * obj, int pos) -{ - long * outptr = NULL; - - plhs[pos] = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); - - outptr = (long *) mxGetPr(plhs[pos]); - - mexMakeMemoryPersistent(obj); - - *outptr = (long)obj; -} - -static double _get_field(const mxArray * pm, const char * fieldname) -{ - mxArray * field_array_ptr = mxGetField(pm, 0, fieldname); - - return mxGetScalar(field_array_ptr); -} - -static long _rhs2ptr(const mxArray * prhs[], int index) -{ - long * inptr = (long *) mxGetPr(prhs[index]); - - return *inptr; -} - -static scan_t * _rhs2scan(const mxArray * prhs[], int index) -{ - long inptr = _rhs2ptr(prhs, index); - - return (scan_t *)inptr; -} - -static map_t * _rhs2map(const mxArray * prhs[], int index) -{ - long inptr = _rhs2ptr(prhs, index); - - return (map_t *)inptr; -} - -static position_t _rhs2pos(const mxArray * prhs[], int index) -{ - position_t position; - - position.x_mm = _get_field(prhs[index], "x_mm"); - position.y_mm = _get_field(prhs[index], "y_mm"); - position.theta_degrees = _get_field(prhs[index], "theta_degrees"); - - return position; -} - -static void _rhs2laser(laser_t * laser, const mxArray * prhs[], int index) -{ - laser->scan_size = (int)_get_field(prhs[index], "scan_size"); - laser->scan_rate_hz = _get_field(prhs[index], "scan_rate_hz"); - laser->detection_angle_degrees = _get_field(prhs[index], "detection_angle_degrees"); - laser->distance_no_detection_mm = _get_field(prhs[index], "distance_no_detection_mm"); - laser->detection_margin = (int)_get_field(prhs[index], "detection_margin"); - laser->offset_mm = _get_field(prhs[index], "offset_mm"); -} - -/* Class methods ------------------------------------------------------- */ - -static void _map_init(mxArray *plhs[], const mxArray * prhs[]) -{ - - int size_pixels = (int)mxGetScalar(prhs[1]); - - double size_meters = mxGetScalar(prhs[2]); - - map_t * map = (map_t *)mxMalloc(sizeof(map_t)); - - map_init(map, size_pixels, size_meters); - - _insert_obj_lhs(plhs, map, 0); -} - -static void _map_disp(const mxArray * prhs[]) -{ - char str[MAXSTR]; - - map_t * map = _rhs2map(prhs, 1); - - map_string(*map, str); - - printf("%s\n", str); -} - -static void _map_update(const mxArray * prhs[]) -{ - map_t * map = _rhs2map(prhs, 1); - - scan_t * scan = _rhs2scan(prhs, 2); - - position_t position = _rhs2pos(prhs, 3); - - int map_quality = (int)mxGetScalar(prhs[4]); - - double hole_width_mm = mxGetScalar(prhs[5]); - - map_update(map, scan, position, map_quality, hole_width_mm); -} - -static void _map_get(mxArray *plhs[], const mxArray * prhs[]) -{ - map_t * map = _rhs2map(prhs, 1); - - unsigned char * pointer = NULL; - - plhs[0] = mxCreateNumericMatrix(map->size_pixels, map->size_pixels, - mxUINT8_CLASS, mxREAL); - - pointer = (unsigned char *)mxGetPr(plhs[0]); - - map_get(map, pointer); -} - - -static void _scan_init(mxArray *plhs[], const mxArray * prhs[]) -{ - - laser_t * laser = (laser_t *)mxMalloc(sizeof(laser_t)); - - scan_t * scan = (scan_t *)mxMalloc(sizeof(scan_t)); - - int span = (int)mxGetScalar(prhs[2]); - - _rhs2laser(laser, prhs, 1); - - scan_init(scan, laser->scan_size, span); - - - _insert_obj_lhs(plhs, scan, 0); - _insert_obj_lhs(plhs, laser, 1); -} - -static void _scan_disp(const mxArray * prhs[]) -{ - char str[MAXSTR]; - - scan_t * scan = _rhs2scan(prhs, 1); - - scan_string(*scan, str); - - printf("%s\n", str); -} - -static void _scan_update(const mxArray * prhs[]) -{ - scan_t * scan = _rhs2scan(prhs, 1); - - laser_t * laser = (laser_t *)_rhs2ptr(prhs, 2); - - int scansize = (int)mxGetNumberOfElements(prhs[3]); - - int * lidar_mm = (int *)mxGetPr(prhs[3]); - - double hole_width_mm = mxGetScalar(prhs[4]); - - double * velocities = mxGetPr(prhs[5]); - - scan_update(scan, lidar_mm, *laser, hole_width_mm, velocities[0], velocities[1]); -} - -static void _randomizer_init(mxArray *plhs[], const mxArray * prhs[]) -{ - int seed = (int)mxGetScalar(prhs[1]); - - void * r = mxMalloc(random_size()); - - random_init(r, seed); - - _insert_obj_lhs(plhs, r, 0); -} - -static void _rmhcPositionSearch(mxArray *plhs[], const mxArray * prhs[]) -{ - position_t start_pos = _rhs2pos(prhs, 1); - - map_t * map = _rhs2map(prhs, 2); - - scan_t * scan = _rhs2scan(prhs, 3); - - laser_t laser; - position_t new_pos; - - double sigma_xy_mm = mxGetScalar(prhs[5]); - - double sigma_theta_degrees = mxGetScalar(prhs[6]); - - int max_search_iter = (int)mxGetScalar(prhs[7]); - - void * randomizer = (void *)(long)mxGetScalar(prhs[8]); - - _rhs2laser(&laser, prhs, 4); - - new_pos = rmhc_position_search( - start_pos, - map, - scan, - laser, - sigma_xy_mm, - sigma_theta_degrees, - max_search_iter, - randomizer); - - plhs[0] = mxCreateDoubleScalar(new_pos.x_mm); - plhs[1] = mxCreateDoubleScalar(new_pos.y_mm); - plhs[2] = mxCreateDoubleScalar(new_pos.theta_degrees); -} - -/* The gateway function ------------------------------------------------ */ -void mexFunction( int nlhs, mxArray *plhs[], - int nrhs, const mxArray * prhs[]) -{ - - char methodname[MAXSTR]; - - mxGetString(prhs[0], methodname, 100); - - if (_streq(methodname, "Map_init")) - { - _map_init(plhs, prhs); - } - - else if (_streq(methodname, "Map_disp")) - { - _map_disp(prhs); - } - - else if (_streq(methodname, "Map_update")) - { - _map_update(prhs); - } - - else if (_streq(methodname, "Map_get")) - { - _map_get(plhs, prhs); - } - - else if (_streq(methodname, "Scan_init")) - { - _scan_init(plhs, prhs); - } - - else if (_streq(methodname, "Scan_disp")) - { - _scan_disp(prhs); - } - - else if (_streq(methodname, "Scan_update")) - { - _scan_update(prhs); - } - - else if (_streq(methodname, "Randomizer_init")) - { - _randomizer_init(plhs, prhs); - } - - else if (_streq(methodname, "rmhcPositionSearch")) - { - _rmhcPositionSearch(plhs, prhs); - } -} -