diff --git a/c/coreslam.c b/c/coreslam.c index c0e5fd8..bcb2d7d 100644 --- a/c/coreslam.c +++ b/c/coreslam.c @@ -235,17 +235,14 @@ static void int offset, int distance, int scanval, - laser_t laser, double horz_mm, double rotation) { int j; for (j=0; jspan; ++j) { - double k = (double)(offset*scan->span+j) * laser.detection_angle_degrees / - (laser.scan_size * scan->span - 1); - double angle = radians(-laser.detection_angle_degrees/2 + k * rotation); - + double k = (double)(offset*scan->span+j) * scan->detection_angle_degrees / (scan->size * scan->span - 1); + double angle = radians(-scan->detection_angle_degrees/2 + k * rotation); double x = distance * cos(angle) - k * horz_mm; double y = distance * sin(angle); @@ -381,16 +378,27 @@ void } } -void - scan_init( - scan_t * scan, - int size, - int span) +void scan_init( + scan_t * scan, + int span, + int size, + double scan_rate_hz, + double detection_angle_degrees, + double distance_no_detection_mm, + int detection_margin, + double offset_mm) { scan->x_mm = double_alloc(size*span); scan->y_mm = double_alloc(size*span); scan->value = int_alloc(size*span); scan->span = span; + + scan->size = size; + scan->rate_hz = scan_rate_hz; + scan->detection_angle_degrees = detection_angle_degrees; + scan->distance_no_detection_mm = distance_no_detection_mm; + scan->detection_margin = detection_margin; + scan->offset_mm = offset_mm; scan->npoints = 0; scan->obst_npoints = 0; @@ -421,16 +429,15 @@ void scan_string( } void - scan_update( +scan_update( scan_t * scan, int * lidar_mm, - laser_t laser, double hole_width_mm, double velocities_dxy_mm, double velocities_dtheta_degrees) { /* Take velocity into account */ - int degrees_per_second = (int)(laser.scan_rate_hz * 360); + int degrees_per_second = (int)(scan->rate_hz * 360); double horz_mm = velocities_dxy_mm / degrees_per_second; double rotation = 1 + velocities_dtheta_degrees / degrees_per_second; @@ -440,14 +447,14 @@ void scan->npoints = 0; scan->obst_npoints = 0; - for (i=laser.detection_margin+1; idetection_margin+1; isize-scan->detection_margin; ++i) { int lidar_value_mm = lidar_mm[i]; /* No obstacle */ if (lidar_value_mm == 0) { - scan_update_xy(scan, i, (int)laser.distance_no_detection_mm, NO_OBSTACLE, laser, horz_mm, rotation); + scan_update_xy(scan, i, (int)scan->distance_no_detection_mm, NO_OBSTACLE, horz_mm, rotation); } /* Obstacle */ @@ -457,7 +464,7 @@ void int j = 0; - scan_update_xy(scan, i, lidar_value_mm, OBSTACLE, laser, horz_mm, rotation); + scan_update_xy(scan, i, lidar_value_mm, OBSTACLE, horz_mm, rotation); /* Store obstacles separately for SSE */ for (j=oldstart; jnpoints; ++j) @@ -473,29 +480,11 @@ void } } -void - laser_string( - laser_t laser, - char * str) -{ - sprintf(str, - "scan_size=%d | scan_rate=%3.3f hz | detection_angle=%3.3f deg | " - "distance_no_detection=%7.4f mm | detection_margin=%d | offset=%4.4f m", - laser.scan_size, - laser.scan_rate_hz, - laser.detection_angle_degrees, - laser.distance_no_detection_mm, - laser.detection_margin, - laser.offset_mm - ); -} - position_t rmhc_position_search( position_t start_pos, map_t * map, scan_t * scan, - laser_t laser, double sigma_xy_mm, double sigma_theta_degrees, int max_search_iter, diff --git a/c/coreslam.h b/c/coreslam.h index 3c887a1..e3f1b6e 100644 --- a/c/coreslam.h +++ b/c/coreslam.h @@ -60,7 +60,14 @@ typedef struct scan_t int * value; int npoints; int span; - + + int size; /* number of rays per scan */ + double rate_hz; /* scans per second */ + double detection_angle_degrees; /* e.g. 240, 360 */ + double distance_no_detection_mm; /* default value when the laser returns 0 */ + int detection_margin; /* first scan element to consider */ + double offset_mm; /* position of the laser wrt center of rotation */ + /* for SSE */ float * obst_x_mm; float * obst_y_mm; @@ -68,19 +75,6 @@ typedef struct scan_t } scan_t; - -typedef struct laser_t -{ - int scan_size; /* number of points per scan */ - double scan_rate_hz; /* scans per second */ - double detection_angle_degrees; /* e.g. 240, 360 */ - double distance_no_detection_mm; /* default value when the laser returns 0 */ - int detection_margin; /* first scan element to consider */ - double offset_mm; /* position of the laser wrt center of rotation */ - -} laser_t; - - /* Exported functions ------------------------------------------------------- */ #ifdef __cplusplus @@ -116,8 +110,13 @@ map_update( void scan_init( scan_t * scan, + int span, int size, - int span); + double scan_rate_hz, + double detection_angle_degrees, + double distance_no_detection_mm, + int detection_margin, + double offset_mm); void scan_free( @@ -131,7 +130,6 @@ void scan_update( scan_t * scan, int * lidar_mm, - laser_t laser, double hole_width_mm, double velocities_dxy_mm, double velocities_dtheta_degrees); @@ -146,11 +144,6 @@ map_set( map_t * map, char * bytes); -void -laser_string( - laser_t laser, - char * str); - /* Returns -1 for infinity */ int distance_scan_to_map( @@ -165,7 +158,6 @@ rmhc_position_search( position_t start_pos, map_t * map, scan_t * scan, - laser_t laser, double sigma_xy_mm, double sigma_theta_degrees, int max_search_iter, diff --git a/cpp/Laser.hpp b/cpp/Laser.hpp index 3f3dc51..05aa81f 100644 --- a/cpp/Laser.hpp +++ b/cpp/Laser.hpp @@ -35,6 +35,15 @@ class Laser friend class SinglePositionSLAM; friend class RMHC_SLAM; friend class Scan; + +protected: + + int scan_size; /* number of points per scan */ + double scan_rate_hz; /* scans per second */ + double detection_angle_degrees; /* e.g. 240, 360 */ + double distance_no_detection_mm; /* default value when the laser returns 0 */ + int detection_margin; /* first scan element to consider */ + double offset_mm; /* position of the laser wrt center of rotation */ public: @@ -56,34 +65,53 @@ public: float distance_no_detection_mm, int detection_margin = 0, float offset_mm = 0. - ); + ) + + { + this->scan_size = scan_size; + this->scan_rate_hz = scan_rate_hz; + this->detection_angle_degrees = detection_angle_degrees; + this->distance_no_detection_mm = distance_no_detection_mm; + this->detection_margin = detection_margin; + this->offset_mm = offset_mm; + } /** * Builds an empty Laser object (all parameters zero). */ Laser(void); - /** - * Dealloates memory for this Laser object. - */ - ~Laser(void); - - - friend ostream& operator<< (ostream & out, Laser & laser); + friend ostream& operator<< (ostream & out, Laser & laser) + { + char str[512]; -private: - - struct laser_t * laser; - + sprintf(str, + "", + laser.scan_size, laser.scan_rate_hz, + laser.detection_angle_degrees, + laser.distance_no_detection_mm, + laser.detection_margin, + laser.offset_mm); + + out << str; + + return out; + } }; +/** + * A class for the Hokuyo URG-04LX laser. + */ class URG04LX : public Laser { public: /** - * Builds a Laser object from parameters based on the specifications for your + * Builds a URG04LX object. * Lidar unit. * @param detection_margin number of rays at edges of scan to ignore * @param offset_mm forward/backward offset of laser motor from robot center @@ -94,7 +122,7 @@ public: Laser(682, 10, 240, 4000, detection_margin, offset_mm) { } /** - * Builds an empty Laser object (all parameters zero). + * Builds an empty URG04LX object (all parameters zero). */ URG04LX(void) : Laser() {} diff --git a/cpp/Makefile b/cpp/Makefile index 8d8bf6a..a90ff00 100644 --- a/cpp/Makefile +++ b/cpp/Makefile @@ -50,9 +50,9 @@ all: libbreezyslam.$(LIBEXT) test: breezytest ./breezytest -libbreezyslam.$(LIBEXT): algorithms.o Scan.o Map.o Laser.o WheeledRobot.o \ +libbreezyslam.$(LIBEXT): algorithms.o Scan.o Map.o WheeledRobot.o \ coreslam.o coreslam_$(ARCH).o random.o ziggurat.o - g++ -O3 -shared algorithms.o Scan.o Map.o Laser.o WheeledRobot.o \ + g++ -O3 -shared algorithms.o Scan.o Map.o WheeledRobot.o \ coreslam.o coreslam_$(ARCH).o random.o ziggurat.o \ -o libbreezyslam.$(LIBEXT) -lm @@ -66,9 +66,6 @@ Scan.o: Scan.cpp Scan.hpp Velocities.hpp Laser.hpp ../c/coreslam.h Map.o: Map.cpp Map.hpp Position.hpp Scan.hpp ../c/coreslam.h g++ -O3 -I../c -c -Wall $(CFLAGS) Map.cpp -Laser.o: Laser.cpp Laser.hpp ../c/coreslam.h - g++ -O3 -I../c -c -Wall $(CFLAGS) Laser.cpp - WheeledRobot.o: WheeledRobot.cpp WheeledRobot.hpp g++ -O3 -I../c -c -Wall $(CFLAGS) WheeledRobot.cpp diff --git a/cpp/Position.hpp b/cpp/Position.hpp index b088919..84b44a4 100644 --- a/cpp/Position.hpp +++ b/cpp/Position.hpp @@ -28,7 +28,9 @@ using namespace std; /** * A class representing the position of a robot. -*/class Position +*/ + +class Position { friend class CoreSLAM; @@ -64,7 +66,8 @@ public: { char str[100]; - sprintf(str, "", + //sprintf(str, "", + sprintf(str, "", position.x_mm, position.y_mm, position.theta_degrees); out << str; diff --git a/cpp/Scan.cpp b/cpp/Scan.cpp index 597603b..57075c1 100644 --- a/cpp/Scan.cpp +++ b/cpp/Scan.cpp @@ -54,10 +54,15 @@ void Scan::init(Laser * laser, int span) { this->scan = new scan_t; - scan_init(this->scan, laser->laser->scan_size, span); - - this->laser = new laser_t; - memcpy(this->laser, laser->laser, sizeof(laser_t)); + scan_init( + this->scan, + span, + laser->scan_size, + laser->scan_rate_hz, + laser->detection_angle_degrees, + laser->distance_no_detection_mm, + laser->detection_margin, + laser->offset_mm); } Scan::~Scan(void) @@ -76,7 +81,6 @@ Scan::update( scan_update( this->scan, scanvals_mm, - *this->laser, hole_width_millimeters, velocities.dxy_mm, velocities.dtheta_degrees); @@ -86,13 +90,8 @@ Scan::update( int * scanvals_mm, double hole_width_millimeters) { - scan_update( - this->scan, - scanvals_mm, - *this->laser, - hole_width_millimeters, - 0, - 0); + Velocities zeroVelocities; + this->update(scanvals_mm, hole_width_millimeters, zeroVelocities); } ostream& operator<< (ostream & out, Scan & scan) diff --git a/cpp/Scan.hpp b/cpp/Scan.hpp index 7814e41..daa5506 100644 --- a/cpp/Scan.hpp +++ b/cpp/Scan.hpp @@ -96,8 +96,6 @@ private: struct scan_t * scan; - struct laser_t * laser; - void init(Laser * laser, int span); }; diff --git a/cpp/algorithms.cpp b/cpp/algorithms.cpp index f2fae2f..bfd7b09 100644 --- a/cpp/algorithms.cpp +++ b/cpp/algorithms.cpp @@ -99,6 +99,13 @@ void CoreSLAM::update(int * scan_mm, Velocities & velocities) this->updateMapAndPointcloud(velocities); } +void CoreSLAM::update(int * scan_mm) +{ + Velocities zero_velocities; + + this->update(scan_mm, zero_velocities); +} + void CoreSLAM::getmap(unsigned char * mapbytes) { @@ -130,15 +137,15 @@ void SinglePositionSLAM::updateMapAndPointcloud(Velocities & velocities) { // Start at current position Position start_pos = this->position; - + // Add effect of velocities start_pos.x_mm += velocities.dxy_mm * this->costheta(), start_pos.y_mm += velocities.dxy_mm * this->sintheta(), start_pos.theta_degrees += velocities.dtheta_degrees; // Add offset from laser - start_pos.x_mm += this->laser->laser->offset_mm * this->costheta(); - start_pos.y_mm += this->laser->laser->offset_mm * this->sintheta(); + start_pos.x_mm += this->laser->offset_mm * this->costheta(); + start_pos.y_mm += this->laser->offset_mm * this->sintheta(); // Get new position from implementing class Position new_position = this->getNewPosition(start_pos); @@ -148,8 +155,8 @@ void SinglePositionSLAM::updateMapAndPointcloud(Velocities & velocities) // Update the current position with this new position, adjusted by laser offset this->position = Position(new_position); - this->position.x_mm -= this->laser->laser->offset_mm * this->costheta(); - this->position.y_mm -= this->laser->laser->offset_mm * this->sintheta(); + this->position.x_mm -= this->laser->offset_mm * this->costheta(); + this->position.y_mm -= this->laser->offset_mm * this->sintheta(); } Position & SinglePositionSLAM::getpos(void) @@ -198,13 +205,6 @@ RMHC_SLAM::~RMHC_SLAM(void) free(this->randomizer); } -void RMHC_SLAM::update(int * scan_mm) -{ - Velocities zero_velocities; - - CoreSLAM::update(scan_mm, zero_velocities); -} - Position RMHC_SLAM::getNewPosition(Position & start_pos) { // Search for a new position if indicated @@ -219,7 +219,6 @@ Position RMHC_SLAM::getNewPosition(Position & start_pos) start_pos_c, this->map->map, this->scan_for_distance->scan, - *this->laser->laser, this->sigma_xy_mm, this->sigma_theta_degrees, this->max_search_iter, diff --git a/cpp/algorithms.hpp b/cpp/algorithms.hpp index 6428707..09913bd 100644 --- a/cpp/algorithms.hpp +++ b/cpp/algorithms.hpp @@ -83,6 +83,14 @@ public: void update(int * scan_mm, Velocities & velocities); + /** + * Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero velocities + * (no odometry). + * @param scan_mm Lidar scan values, whose count is specified in the scan_size + * attribute of the Laser object passed to the CoreSlam constructor + */ + void update(int * scan_mm); + /** * The quality of the map (0 through 255); default = 50 */ @@ -232,14 +240,7 @@ public: ~RMHC_SLAM(void); - /** - * Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero velocities - * (no odometry). - * @param scan_mm Lidar scan values, whose count is specified in the scan_size - * attribute of the Laser object passed to the CoreSlam constructor - */ - void update(int * scan_mm); - + /** * The standard deviation in millimeters of the Gaussian distribution of * the (X,Y) component of position in the particle filter; default = 100 diff --git a/examples/Makefile b/examples/Makefile index 5db3d7e..37e41cf 100644 --- a/examples/Makefile +++ b/examples/Makefile @@ -18,30 +18,41 @@ # Where you put the libbreezyslam library LIBDIR = /usr/local/lib +JAVADIR = ../java/edu/wlu/cs/levy/breezyslam + # Use EOG or your favorite image-display program VIEWER = eog # Set these for different experiments DATASET = exp2 -USE_ODOMETRY = 1 +USE_ODOMETRY = 0 RANDOM_SEED = 9999 -all: log2pgm +all: log2pgm Log2PGM.class pytest: ./log2pgm.py $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) - $(VIEWER) $(DATASET).pgm + $(VIEWER) $(DATASET).pgm ~/Desktop/$(DATASET).pgm + cpptest: log2pgm ./log2pgm $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) $(VIEWER) $(DATASET).pgm +javatest: Log2PGM.class + java -classpath ../java:. -Djava.library.path=$(JAVADIR)/algorithms:$(JAVADIR)/components Log2PGM \ + $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) + $(VIEWER) $(DATASET).pgm + log2pgm: log2pgm.o g++ -O3 -o log2pgm log2pgm.o -L$(LIBDIR) -lbreezyslam log2pgm.o: log2pgm.cpp g++ -O3 -c -I ../cpp log2pgm.cpp +Log2PGM.class: Log2PGM.java + javac -classpath ../java Log2PGM.java + $(DATASET).pgm: ./log2pgm.py $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) @@ -49,4 +60,4 @@ backup: cp -r .. ~/Documents/slam/bak-breezyslam clean: - rm -f log2pgm *.pyc *.pgm *.o *~ + rm -f log2pgm *.pyc *.pgm *.o *.class *~ diff --git a/examples/log2pgm.cpp b/examples/log2pgm.cpp index 20f03ec..feabd87 100644 --- a/examples/log2pgm.cpp +++ b/examples/log2pgm.cpp @@ -106,7 +106,6 @@ static void load_data( while (fgets(s, MAXLINE, fp)) { char * cp = strtok(s, " "); - long * odometry = new long [3]; odometry[0] = atol(cp); @@ -356,11 +355,11 @@ int main( int argc, const char** argv ) } else { - ((RMHC_SLAM *)slam)->update(lidar); + slam->update(lidar); } Position position = slam->getpos(); - + // Add new coordinates to trajectory double * v = new double[2]; v[0] = position.x_mm; diff --git a/examples/logdemo.m b/examples/logdemo.m index dbdf97f..b764357 100644 --- a/examples/logdemo.m +++ b/examples/logdemo.m @@ -3,6 +3,14 @@ % data from Paris Mines Tech and displays the map and robot % position in real time. % +% Usage: +% +% >> logdemo(dataset, [use_odometry], [random_seed]) +% +% Examples: +% +% >> logdemo('exp2') +% % For details see % % @inproceedings{coreslam-2010, @@ -84,7 +92,7 @@ for scanno = 1:size(scans, 1) % Update SLAM with lidar alone slam = slam.update(scans(scanno,:)); end - + % Get new position [x_mm, y_mm, theta_degrees] = slam.getpos(); @@ -110,7 +118,6 @@ for scanno = 1:size(scans, 1) x_pix = x_pix_r + mm2pix(x_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS); y_pix = y_pix_r + mm2pix(y_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS); - % Add robot image to map fill(x_pix, y_pix, 'r') drawnow diff --git a/examples/mines.py b/examples/mines.py index 04a711c..27e6d79 100644 --- a/examples/mines.py +++ b/examples/mines.py @@ -64,11 +64,11 @@ def load_data(datadir, dataset): break toks = s.split()[0:-1] # ignore '' - + odometry = (int(toks[0]), int(toks[2]), int(toks[3])) lidar = [int(tok) for tok in toks[24:]] - + scans.append(lidar) odometries.append(odometry) diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java index d0eca8a..458943d 100644 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java @@ -1,8 +1,24 @@ /** * DeterministicSLAM implements SinglePositionSLAM using by returning the starting position instead of searching * on it; i.e., using odometry alone. +* +* 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 . */ + package edu.wlu.cs.levy.breezyslam.algorithms; import edu.wlu.cs.levy.breezyslam.components.Position; diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile b/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile index 745d115..45b5a43 100644 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile @@ -1,3 +1,20 @@ +# Makefile for BreezySLAM algorithms in Java +# +# 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 . + BASEDIR = ../../../../../../.. JAVADIR = $(BASEDIR)/java CDIR = $(BASEDIR)/c diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java index 62b5815..46c11a8 100644 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java @@ -1,5 +1,20 @@ /** -* RMHCSLAM implements SinglePositionSLAM using random-mutation hill-climbing search on the starting position. +* RMHCSLAM implements SinglePositionSLAM using random-mutation hill-climbing search on the starting position. +* +* 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 . */ package edu.wlu.cs.levy.breezyslam.algorithms; diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java index 80b0924..c164b01 100644 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java @@ -1,10 +1,25 @@ /** -* SinglePositionSLAM is an abstract class that implements CoreSLAM using a point-cloud -* with a single point (particle, position). Implementing classes should provide the method +* SinglePositionSLAM is an abstract class that implements CoreSLAM using a point-cloud +* with a single point (particle, position). Implementing classes should provide the method * -* Position getNewPosition(Position start_position) +* Position getNewPosition(Position start_position) * -* to compute a new position based on searching from a starting position. +* to compute a new position based on searching from a starting position. +* +* 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 . */ package edu.wlu.cs.levy.breezyslam.algorithms; diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/CoreSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/CoreSLAM.java deleted file mode 100644 index b8481d4..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/CoreSLAM.java +++ /dev/null @@ -1,66 +0,0 @@ -package edu.wlu.cs.levy.breezyslam.algorithms; - -import edu.wlu.cs.levy.breezyslam.components.Laser; -import edu.wlu.cs.levy.breezyslam.components.Velocities; - -public class CoreSLAM { - - public CoreSLAM(Laser laser, int map_size_pixels, double map_size_meters) - { - /* - // Set default params - this->map_quality = DEFAULT_MAP_QUALITY; - this->hole_width_mm = DEFAULT_HOLE_WIDTH_MM; - - // Store laser for later - this->laser = new Laser(laser); - - // Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry - this->velocities = new Velocities(); - - // Initialize a scan for computing distance to map, and one for updating map - this->scan_for_mapbuild = this->scan_create(3); - this->scan_for_distance = this->scan_create(1); - - // Initialize the map - this->map = new Map(map_size_pixels, map_size_meters); - */ - } - - - public void update(int [] scan_mm, Velocities velocities) - { - /* - // Build a scan for computing distance to map, and one for updating map - this->scan_update(this->scan_for_mapbuild, scan_mm); - this->scan_update(this->scan_for_distance, scan_mm); - - // Update velocities - this->velocities->update(velocities.dxy_mm, - velocities.dtheta_degrees, - velocities.dt_seconds); - - // Implementing class updates map and pointcloud - this->updateMapAndPointcloud(velocities); - */ - } - - - public void getmap(byte [] mapbytes) - { - //this->map->get((char *)mapbytes); - } - - /* - Scan * CoreSLAM::scan_create(int span) - { - //return new Scan(this->laser, span); - } - - - void scan_update(Scan scan, int [] scan_mm) - { - //scan->update(scan_mm, this->hole_width_mm, *this->velocities); - } - */ -} diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/Makefile b/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/Makefile deleted file mode 100644 index 8918ed4..0000000 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/Makefile +++ /dev/null @@ -1,15 +0,0 @@ -JAVADIR = ../../../../../.. - -ALL = CoreSLAM.class - -all: $(ALL) - -CoreSLAM.class: CoreSLAM.java - javac -classpath $(JAVADIR):. CoreSLAM.java - -clean: - rm -f *.class *~ - -backup: - cp *.java bak - cp Makefile bak diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c b/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c index db97d3d..e2e8d8f 100644 --- a/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c @@ -1,3 +1,23 @@ +/* + * jnibreezyslam_algorithms.c Java Native Interface code for BreezySLAM algorithms + * + * 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 "../../../../../../../c/random.h" #include "../jni_utils.h" diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Laser.java b/java/edu/wlu/cs/levy/breezyslam/components/Laser.java index d1f4b23..aa77425 100644 --- a/java/edu/wlu/cs/levy/breezyslam/components/Laser.java +++ b/java/edu/wlu/cs/levy/breezyslam/components/Laser.java @@ -1,5 +1,30 @@ +/** +* +* BreezySLAM: Simple, efficient SLAM in Java +* +* Laser.java - Java 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 . +*/ + package edu.wlu.cs.levy.breezyslam.components; +/** +* A class for scanning laser rangefinder (Lidar) parameters. +*/ public class Laser { @@ -10,7 +35,18 @@ public class Laser protected int detection_margin; protected double offset_mm; - public Laser( + /** + * Builds a Laser object from parameters based on the specifications for your + * Lidar unit. + * @param scan_size number of rays per scan + * @param scan_rate_hz laser scan rate in Hertz + * @param detection_angle_degrees detection angle in degrees (e.g. 240, 360) + * @param detection_margin number of rays at edges of scan to ignore + * @param offset_mm forward/backward offset of laser motor from robot center + * @return a new Laser object + * + */ + public Laser( int scan_size, double scan_rate_hz, double detection_angle_degrees, @@ -26,7 +62,13 @@ public class Laser this.offset_mm = offset_mm; } - public Laser(Laser laser) + /** + * Builds a Laser object by copying another Laser object. + * Lidar unit. + * @param laser the other Laser object + * + */ + public Laser(Laser laser) { this.scan_size = laser.scan_size; this.scan_rate_hz = laser.scan_rate_hz; @@ -36,6 +78,10 @@ public class Laser this.offset_mm = laser.offset_mm; } + /** + * Returns a string representation of this Laser object. + */ + public String toString() { String format = "scan_size=%d | scan_rate=%3.3f hz | " + @@ -51,6 +97,10 @@ public class Laser } + /** + * Returns the offset of the laser in mm, from the center of the robot. + * + */ public double getOffsetMm() { return this.offset_mm; diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Makefile b/java/edu/wlu/cs/levy/breezyslam/components/Makefile index d037b4c..c034cc1 100644 --- a/java/edu/wlu/cs/levy/breezyslam/components/Makefile +++ b/java/edu/wlu/cs/levy/breezyslam/components/Makefile @@ -1,3 +1,20 @@ +# Makefile for BreezySLAM components in Java +# +# 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 . + BASEDIR = ../../../../../../.. JAVADIR = $(BASEDIR)/java CDIR = $(BASEDIR)/c diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Map.java b/java/edu/wlu/cs/levy/breezyslam/components/Map.java index 22e1da8..4bfa2f6 100644 --- a/java/edu/wlu/cs/levy/breezyslam/components/Map.java +++ b/java/edu/wlu/cs/levy/breezyslam/components/Map.java @@ -1,5 +1,30 @@ +/** +* +* BreezySLAM: Simple, efficient SLAM in Java +* +* Map.java - Java code for Map 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 . +*/ + package edu.wlu.cs.levy.breezyslam.components; +/** +* A class for maps used in SLAM. +*/ public class Map { static @@ -21,8 +46,17 @@ public class Map private long native_ptr; + /** + * Returns a string representation of this Map object. + */ public native String toString(); + /** + * Builds a square Map object. + * @param size_pixels size in pixels + * @param size_meters size in meters + * + */ public Map(int size_pixels, double size_meters) { this.init(size_pixels, size_meters); @@ -33,7 +67,8 @@ public class Map /** * Puts current map values into bytearray, which should of which should be of - * this->size map_size_pixels ^ 2. + * this.size map_size_pixels ^ 2. + * @param bytes byte array that gets the map values */ public native void get(byte [] bytes); @@ -50,6 +85,9 @@ public class Map this.update(scan, position.x_mm, position.y_mm, position.theta_degrees, quality, hole_width_mm); } + /** + * Returns the size of this map in meters. + */ public double sizeMeters() { return this.size_meters; diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Position.java b/java/edu/wlu/cs/levy/breezyslam/components/Position.java index 862a55e..8c504bc 100644 --- a/java/edu/wlu/cs/levy/breezyslam/components/Position.java +++ b/java/edu/wlu/cs/levy/breezyslam/components/Position.java @@ -1,38 +1,81 @@ +/** + * + * BreezySLAM: Simple, efficient SLAM in Java + * + * Position.java - Java code for Position 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 . + */ + package edu.wlu.cs.levy.breezyslam.components; +/** + * A class representing the position of a robot. + */ public class Position { - public double x_mm; - public double y_mm; - public double theta_degrees; - - public Position(double x_mm, double y_mm, double theta_degrees) + /** + * Distance of robot from left edge of map, in millimeters. + */ + public double x_mm; + + /** + * Distance of robot from top edge of map, in millimeters. + */ + public double y_mm; + + /** + * Clockwise rotation of robot with respect to three o'clock (east), in degrees. + */ + public double theta_degrees; + + /** + * Constructs a new position. + * @param x_mm X coordinate in millimeters + * @param y_mm Y coordinate in millimeters + * @param theta_degrees rotation angle in degrees + */ + public Position(double x_mm, double y_mm, double theta_degrees) { this.x_mm = x_mm; this.y_mm = y_mm; this.theta_degrees = theta_degrees; } - public Position(Position position) + /** + * Constructs a new Position object by copying another. + * @param the other Positon object + */ + + public Position(Position position) { this.x_mm = position.x_mm; this.y_mm = position.y_mm; this.theta_degrees = position.theta_degrees; - } + } + /** + * Returns a string representation of this Position object. + */ public String toString() { - //String format = ""; - String format = ""; + String format = ""; return String.format(format, this.x_mm, this.y_mm, this.theta_degrees); - - } - public static void main(String[] argv) - { - Position position = new Position(300, 400, 120); - System.out.println(position); - } + } } diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Scan.java b/java/edu/wlu/cs/levy/breezyslam/components/Scan.java index b14a3dc..631db78 100644 --- a/java/edu/wlu/cs/levy/breezyslam/components/Scan.java +++ b/java/edu/wlu/cs/levy/breezyslam/components/Scan.java @@ -1,5 +1,30 @@ +/** +* +* BreezySLAM: Simple, efficient SLAM in Java +* +* Scan.java - Java code for Scan 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 . +*/ + package edu.wlu.cs.levy.breezyslam.components; +/** +* A class for Lidar scans. +*/ public class Scan { static @@ -20,6 +45,9 @@ public class Scan public native String toString(); + /** + * Returns a string representation of this Scan object. + */ public native void update( int [] lidar_mm, double hole_width_mm, @@ -27,6 +55,12 @@ public class Scan double velocities_dtheta_degrees); + /** + * Builds a Scan object. + * @param laser laser parameters + * @param span supports spanning laser scan to cover the space better. + * + */ public Scan(Laser laser, int span) { this.init(span, @@ -38,6 +72,11 @@ public class Scan laser.offset_mm); } + /** + * Builds a Scan object. + * @param laser laser parameters + * + */ public Scan(Laser laser) { this(laser, 1); diff --git a/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java b/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java index 4696d47..053b174 100644 --- a/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java +++ b/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java @@ -1,13 +1,27 @@ package edu.wlu.cs.levy.breezyslam.components; +/** + * A class for the Hokuyo URG-04LX laser. + */ public class URG04LX extends Laser { - public URG04LX(int detection_margin, double offset_mm) + /** + * Builds a URG04LX object. + * Lidar unit. + * @param detection_margin number of rays at edges of scan to ignore + * @param offset_mm forward/backward offset of laser motor from robot center + * @return a new URG04LX object + * + */ + public URG04LX(int detection_margin, double offset_mm) { super(682, 10, 240, 4000, detection_margin, offset_mm); } + /** + * Builds a URG04LX object with zero detection margin, offset mm. + */ public URG04LX() { this(0, 0); diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Velocities.java b/java/edu/wlu/cs/levy/breezyslam/components/Velocities.java index 76e2855..081428d 100644 --- a/java/edu/wlu/cs/levy/breezyslam/components/Velocities.java +++ b/java/edu/wlu/cs/levy/breezyslam/components/Velocities.java @@ -1,7 +1,29 @@ +/** +* +* BreezySLAM: Simple, efficient SLAM in Java +* +* Velocities.java - Java code for Velocities 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 . +*/ + package edu.wlu.cs.levy.breezyslam.components; /** -* A class representing the forward and angular velocities of a robot. +* A class representing the forward and angular velocities of a robot as determined by odometry. */ public class Velocities { @@ -41,28 +63,52 @@ public class Velocities this.dtheta_degrees = dtheta_degrees * velocity_factor; } + /** + * Returns a string representation of this Velocities object. + */ public String toString() { return String.format(". + */ + #include "../jni_utils.h" #include "Map.h" diff --git a/java/edu/wlu/cs/levy/breezyslam/jni_utils.h b/java/edu/wlu/cs/levy/breezyslam/jni_utils.h index 079875a..acaa3a5 100644 --- a/java/edu/wlu/cs/levy/breezyslam/jni_utils.h +++ b/java/edu/wlu/cs/levy/breezyslam/jni_utils.h @@ -1,3 +1,25 @@ +/* + * jni_utils.h Utilities for Java Native Interface code + * + * 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 "../../../../../../c/coreslam.h" #include diff --git a/java/edu/wlu/cs/levy/breezyslam/robots/Makefile b/java/edu/wlu/cs/levy/breezyslam/robots/Makefile index f97b0d9..9dd81fe 100644 --- a/java/edu/wlu/cs/levy/breezyslam/robots/Makefile +++ b/java/edu/wlu/cs/levy/breezyslam/robots/Makefile @@ -1,3 +1,20 @@ +# Makefile for BreezySLAM robots in Java +# +# 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 . + BASEDIR = ../../../../../../.. JAVADIR = $(BASEDIR)/java JFLAGS = -Xlint diff --git a/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java b/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java index 8ccc871..a06766e 100644 --- a/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java +++ b/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java @@ -1,11 +1,11 @@ /** * - * BreezySLAM: Simple, efficient SLAM in C++ + * BreezySLAM: Simple, efficient SLAM in Java * * WheeledRobot.java - Java class for wheeled robots * * 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 diff --git a/matlab/CoreSLAM.m b/matlab/CoreSLAM.m index 8ed823e..ad17c74 100644 --- a/matlab/CoreSLAM.m +++ b/matlab/CoreSLAM.m @@ -96,15 +96,20 @@ classdef CoreSLAM % Calls the the implementing class's updateMapAndPointcloud() % method with the specified velocities. % - % slam = update(slam, scans_mm, velocities) + % slam = update(slam, scans_mm, [velocities]) % % scan_mm is a list of Lidar scan values, whose count is specified in the scan_size - % velocities is a list of velocities [dxy_mm, dtheta_degrees, dt_seconds] for odometry - + % velocities is an optional list of velocities [dxy_mm, dtheta_degrees, dt_seconds] for odometry + % Build a scan for computing distance to map, and one for updating map slam.scan_update(slam.scan_for_mapbuild, scans_mm) slam.scan_update(slam.scan_for_distance, scans_mm) - + + % Default to zero velocities + if nargin < 3 + velocities = [0, 0, 0]; + end + % Update velocities velocity_factor = 0; if velocities(3) > 0 diff --git a/matlab/Scan.m b/matlab/Scan.m index 4845738..4004572 100644 --- a/matlab/Scan.m +++ b/matlab/Scan.m @@ -19,7 +19,6 @@ classdef Scan properties (Access = {?Map, ?RMHC_SLAM}) c_scan - c_laser end methods @@ -34,14 +33,14 @@ classdef Scan % distance_no_detection_mm % distance_no_detection_mm % detection_margin - % offset_mm = offset_mm + % offset_mm = offset_mm % span (default=1) supports spanning laser scan to cover the space better if nargin < 2 span = 1; end - - [scan.c_scan, scan.c_laser] = mex_breezyslam('Scan_init', laser, span); + + scan.c_scan = mex_breezyslam('Scan_init', laser, span); end @@ -59,7 +58,7 @@ classdef Scan % hole_width_mm is the width of holes (obstacles, walls) in millimeters. % velocities is an optional list[dxy_mm, dtheta_degrees] % i.e., robot's (forward, rotational velocity) for improving the quality of the scan. - mex_breezyslam('Scan_update', scan.c_scan, scan.c_laser, int32(scans_mm), hole_width_mm, velocities) + mex_breezyslam('Scan_update', scan.c_scan, int32(scans_mm), hole_width_mm, velocities) end end diff --git a/matlab/mex_breezyslam.c b/matlab/mex_breezyslam.c index ad2d193..b74cbf3 100644 --- a/matlab/mex_breezyslam.c +++ b/matlab/mex_breezyslam.c @@ -83,16 +83,6 @@ static position_t _rhs2pos(const mxArray * prhs[], int index) 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[]) @@ -153,19 +143,29 @@ static void _map_get(mxArray *plhs[], const mxArray * prhs[]) 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); - - + + const mxArray * laser = prhs[1]; + int scan_size = (int)_get_field(laser, "scan_size"); + double scan_rate_hz = _get_field(laser, "scan_rate_hz"); + double detection_angle_degrees = _get_field(laser, "detection_angle_degrees"); + double distance_no_detection_mm = _get_field(laser, "distance_no_detection_mm"); + int detection_margin = (int)_get_field(laser, "detection_margin"); + double offset_mm = _get_field(laser, "offset_mm"); + + scan_init( + scan, + span, + scan_size, + scan_rate_hz, + detection_angle_degrees, + distance_no_detection_mm, + detection_margin, + offset_mm); + _insert_obj_lhs(plhs, scan, 0); - _insert_obj_lhs(plhs, laser, 1); } static void _scan_disp(const mxArray * prhs[]) @@ -183,17 +183,15 @@ 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[2]); - int scansize = (int)mxGetNumberOfElements(prhs[3]); + int * lidar_mm = (int *)mxGetPr(prhs[2]); - int * lidar_mm = (int *)mxGetPr(prhs[3]); + double hole_width_mm = mxGetScalar(prhs[3]); - double hole_width_mm = mxGetScalar(prhs[4]); + double * velocities = mxGetPr(prhs[4]); - double * velocities = mxGetPr(prhs[5]); - - scan_update(scan, lidar_mm, *laser, hole_width_mm, velocities[0], velocities[1]); + scan_update(scan, lidar_mm, hole_width_mm, velocities[0], velocities[1]); } static void _randomizer_init(mxArray *plhs[], const mxArray * prhs[]) @@ -215,7 +213,6 @@ static void _rmhcPositionSearch(mxArray *plhs[], const mxArray * prhs[]) scan_t * scan = _rhs2scan(prhs, 3); - laser_t laser; position_t new_pos; double sigma_xy_mm = mxGetScalar(prhs[5]); @@ -226,13 +223,10 @@ static void _rmhcPositionSearch(mxArray *plhs[], const mxArray * prhs[]) 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, diff --git a/matlab/mex_breezyslam.mexa64 b/matlab/mex_breezyslam.mexa64 index adaa9db..f69b7a1 100755 Binary files a/matlab/mex_breezyslam.mexa64 and b/matlab/mex_breezyslam.mexa64 differ diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index 6b9a423..e2cf701 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -47,26 +47,6 @@ Change log: #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 @@ -218,7 +198,6 @@ typedef struct { PyObject_HEAD - laser_t laser; scan_t scan; int * lidar_mm; @@ -253,6 +232,13 @@ Scan_init(Scan *self, PyObject *args, PyObject *kwds) static char* argnames[] = {"laser", "span", NULL}; + int scan_size = 0; + double scan_rate_hz = 0; + double detection_angle_degrees = 0; + double distance_no_detection_mm = 0; + int detection_margin = 0; + double offset_mm = 0; + if(!PyArg_ParseTupleAndKeywords(args, kwds,"O|i", argnames, &py_laser, &span)) @@ -260,14 +246,27 @@ Scan_init(Scan *self, PyObject *args, PyObject *kwds) return error_on_raise_argument_exception("Scan"); } - if (pylaser2claser(py_laser, &self->laser)) + if (!int_from_obj(py_laser, "scan_size", &scan_size) || + !double_from_obj(py_laser, "scan_rate_hz", &scan_rate_hz) || + !double_from_obj(py_laser, "detection_angle_degrees", &detection_angle_degrees) || + !double_from_obj(py_laser, "distance_no_detection_mm", &distance_no_detection_mm) || + !int_from_obj(py_laser, "detection_margin", &detection_margin) || + !double_from_obj(py_laser, "offset_mm", &offset_mm)) { 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); + + scan_init( + &self->scan, + span, + scan_size, + scan_rate_hz, + detection_angle_degrees, + distance_no_detection_mm, + detection_margin, + offset_mm); + + self->lidar_mm = int_alloc(self->scan.size); return 0; } @@ -311,7 +310,7 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) } // Bozo filter on LIDAR argument list size - if (PyList_Size(py_lidar) != self->laser.scan_size) + if (PyList_Size(py_lidar) != self->scan.size) { return null_on_raise_argument_exception_with_details("Scan", "update", "lidar size mismatch"); @@ -342,7 +341,7 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) // Extract LIDAR values from argument int k = 0; - for (k=0; klaser.scan_size; ++k) + for (k=0; kscan.size; ++k) { self->lidar_mm[k] = PyFloat_AsDouble(PyList_GetItem(py_lidar, k)); } @@ -351,7 +350,6 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds) scan_update( &self->scan, self->lidar_mm, - self->laser, hole_width_mm, dxy_mm, dtheta_degrees); @@ -794,19 +792,11 @@ rmhcPositionSearch(PyObject *self, PyObject *args) // 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, @@ -916,3 +906,4 @@ PyInit_pybreezyslam(void) } #endif +