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

Added Java support.

Browse Source
This commit is contained in:
Simon D. Levy
2014-10-26 17:46:47 -04:00
parent 721f75e2af
commit 08ab4a55bb
36 changed files with 630 additions and 311 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
c/coreslam.c
View File

@@ -235,17 +235,14 @@ static void
int offset, int offset,
int distance, int distance,
int scanval, int scanval,
laser_t laser,
double horz_mm, double horz_mm,
double rotation) double rotation)
{ {
int j; int j;
for (j=0; j<scan->span; ++j) for (j=0; j<scan->span; ++j)
{ {
double k = (double)(offset*scan->span+j) * laser.detection_angle_degrees / double k = (double)(offset*scan->span+j) * scan->detection_angle_degrees / (scan->size * scan->span - 1);
(laser.scan_size * scan->span - 1); double angle = radians(-scan->detection_angle_degrees/2 + k * rotation);
double angle = radians(-laser.detection_angle_degrees/2 + k * rotation);
double x = distance * cos(angle) - k * horz_mm; double x = distance * cos(angle) - k * horz_mm;
double y = distance * sin(angle); double y = distance * sin(angle);
@@ -381,17 +378,28 @@ void
} }
} }
void void scan_init(
scan_init(
scan_t * scan, scan_t * scan,
int span,
int size, int size,
int span) 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->x_mm = double_alloc(size*span);
scan->y_mm = double_alloc(size*span); scan->y_mm = double_alloc(size*span);
scan->value = int_alloc(size*span); scan->value = int_alloc(size*span);
scan->span = 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->npoints = 0;
scan->obst_npoints = 0; scan->obst_npoints = 0;
@@ -424,13 +432,12 @@ void
scan_update( scan_update(
scan_t * scan, scan_t * scan,
int * lidar_mm, int * lidar_mm,
laser_t laser,
double hole_width_mm, double hole_width_mm,
double velocities_dxy_mm, double velocities_dxy_mm,
double velocities_dtheta_degrees) double velocities_dtheta_degrees)
{ {
/* Take velocity into account */ /* 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 horz_mm = velocities_dxy_mm / degrees_per_second;
double rotation = 1 + velocities_dtheta_degrees / degrees_per_second; double rotation = 1 + velocities_dtheta_degrees / degrees_per_second;
@@ -440,14 +447,14 @@ void
scan->npoints = 0; scan->npoints = 0;
scan->obst_npoints = 0; scan->obst_npoints = 0;
for (i=laser.detection_margin+1; i<laser.scan_size-laser.detection_margin; ++i) for (i=scan->detection_margin+1; i<scan->size-scan->detection_margin; ++i)
{ {
int lidar_value_mm = lidar_mm[i]; int lidar_value_mm = lidar_mm[i];
/* No obstacle */ /* No obstacle */
if (lidar_value_mm == 0) 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 */ /* Obstacle */
@@ -457,7 +464,7 @@ void
int j = 0; 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 */ /* Store obstacles separately for SSE */
for (j=oldstart; j<scan->npoints; ++j) for (j=oldstart; j<scan->npoints; ++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 position_t
rmhc_position_search( rmhc_position_search(
position_t start_pos, position_t start_pos,
map_t * map, map_t * map,
scan_t * scan, scan_t * scan,
laser_t laser,
double sigma_xy_mm, double sigma_xy_mm,
double sigma_theta_degrees, double sigma_theta_degrees,
int max_search_iter, int max_search_iter,

34
c/coreslam.h
View File

@@ -61,6 +61,13 @@ typedef struct scan_t
int npoints; int npoints;
int span; 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 */ /* for SSE */
float * obst_x_mm; float * obst_x_mm;
float * obst_y_mm; float * obst_y_mm;
@@ -68,19 +75,6 @@ typedef struct scan_t
} 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 ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
#ifdef __cplusplus #ifdef __cplusplus
@@ -116,8 +110,13 @@ map_update(
void scan_init( void scan_init(
scan_t * scan, scan_t * scan,
int span,
int size, int size,
int span); double scan_rate_hz,
double detection_angle_degrees,
double distance_no_detection_mm,
int detection_margin,
double offset_mm);
void void
scan_free( scan_free(
@@ -131,7 +130,6 @@ void
scan_update( scan_update(
scan_t * scan, scan_t * scan,
int * lidar_mm, int * lidar_mm,
laser_t laser,
double hole_width_mm, double hole_width_mm,
double velocities_dxy_mm, double velocities_dxy_mm,
double velocities_dtheta_degrees); double velocities_dtheta_degrees);
@@ -146,11 +144,6 @@ map_set(
map_t * map, map_t * map,
char * bytes); char * bytes);
void
laser_string(
laser_t laser,
char * str);
/* Returns -1 for infinity */ /* Returns -1 for infinity */
int int
distance_scan_to_map( distance_scan_to_map(
@@ -165,7 +158,6 @@ rmhc_position_search(
position_t start_pos, position_t start_pos,
map_t * map, map_t * map,
scan_t * scan, scan_t * scan,
laser_t laser,
double sigma_xy_mm, double sigma_xy_mm,
double sigma_theta_degrees, double sigma_theta_degrees,
int max_search_iter, int max_search_iter,

52
cpp/Laser.hpp
View File

@@ -36,6 +36,15 @@ class Laser
friend class RMHC_SLAM; friend class RMHC_SLAM;
friend class Scan; 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: public:
/** /**
@@ -56,34 +65,53 @@ public:
float distance_no_detection_mm, float distance_no_detection_mm,
int detection_margin = 0, int detection_margin = 0,
float offset_mm = 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). * Builds an empty Laser object (all parameters zero).
*/ */
Laser(void); Laser(void);
/** friend ostream& operator<< (ostream & out, Laser & laser)
* Dealloates memory for this Laser object. {
*/ char str[512];
~Laser(void);
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 mm>",
laser.scan_size, laser.scan_rate_hz,
laser.detection_angle_degrees,
laser.distance_no_detection_mm,
laser.detection_margin,
laser.offset_mm);
friend ostream& operator<< (ostream & out, Laser & laser); out << str;
private:
struct laser_t * laser;
return out;
}
}; };
/**
* A class for the Hokuyo URG-04LX laser.
*/
class URG04LX : public Laser class URG04LX : public Laser
{ {
public: public:
/** /**
* Builds a Laser object from parameters based on the specifications for your * Builds a URG04LX object.
* Lidar unit. * Lidar unit.
* @param detection_margin number of rays at edges of scan to ignore * @param detection_margin number of rays at edges of scan to ignore
* @param offset_mm forward/backward offset of laser motor from robot center * @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) { } 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() {} URG04LX(void) : Laser() {}

7
cpp/Makefile
View File

@@ -50,9 +50,9 @@ all: libbreezyslam.$(LIBEXT)
test: breezytest test: breezytest
./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 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 \ coreslam.o coreslam_$(ARCH).o random.o ziggurat.o \
-o libbreezyslam.$(LIBEXT) -lm -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 Map.o: Map.cpp Map.hpp Position.hpp Scan.hpp ../c/coreslam.h
g++ -O3 -I../c -c -Wall $(CFLAGS) Map.cpp 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 WheeledRobot.o: WheeledRobot.cpp WheeledRobot.hpp
g++ -O3 -I../c -c -Wall $(CFLAGS) WheeledRobot.cpp g++ -O3 -I../c -c -Wall $(CFLAGS) WheeledRobot.cpp

7
cpp/Position.hpp
View File

@@ -28,7 +28,9 @@ using namespace std;
/** /**
* A class representing the position of a robot. * A class representing the position of a robot.
*/class Position */
class Position
{ {
friend class CoreSLAM; friend class CoreSLAM;
@@ -64,7 +66,8 @@ public:
{ {
char str[100]; char str[100];
sprintf(str, "<x = %7.0f mm y = %7.0f mm theta = %+3.3f degrees>", //sprintf(str, "<x = %7.0f mm y = %7.0f mm theta = %+3.3f degrees>",
sprintf(str, "<x = %f mm y = %f mm theta = %f degrees>",
position.x_mm, position.y_mm, position.theta_degrees); position.x_mm, position.y_mm, position.theta_degrees);
out << str; out << str;

23
cpp/Scan.cpp
View File

@@ -54,10 +54,15 @@ void Scan::init(Laser * laser, int span)
{ {
this->scan = new scan_t; this->scan = new scan_t;
scan_init(this->scan, laser->laser->scan_size, span); scan_init(
this->scan,
this->laser = new laser_t; span,
memcpy(this->laser, laser->laser, sizeof(laser_t)); 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) Scan::~Scan(void)
@@ -76,7 +81,6 @@ Scan::update(
scan_update( scan_update(
this->scan, this->scan,
scanvals_mm, scanvals_mm,
*this->laser,
hole_width_millimeters, hole_width_millimeters,
velocities.dxy_mm, velocities.dxy_mm,
velocities.dtheta_degrees); velocities.dtheta_degrees);
@@ -86,13 +90,8 @@ Scan::update(
int * scanvals_mm, int * scanvals_mm,
double hole_width_millimeters) double hole_width_millimeters)
{ {
scan_update( Velocities zeroVelocities;
this->scan, this->update(scanvals_mm, hole_width_millimeters, zeroVelocities);
scanvals_mm,
*this->laser,
hole_width_millimeters,
0,
0);
} }
ostream& operator<< (ostream & out, Scan & scan) ostream& operator<< (ostream & out, Scan & scan)

2
cpp/Scan.hpp
View File

@@ -96,8 +96,6 @@ private:
struct scan_t * scan; struct scan_t * scan;
struct laser_t * laser;
void init(Laser * laser, int span); void init(Laser * laser, int span);
}; };

23
cpp/algorithms.cpp
View File

@@ -99,6 +99,13 @@ void CoreSLAM::update(int * scan_mm, Velocities & velocities)
this->updateMapAndPointcloud(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) void CoreSLAM::getmap(unsigned char * mapbytes)
{ {
@@ -137,8 +144,8 @@ void SinglePositionSLAM::updateMapAndPointcloud(Velocities & velocities)
start_pos.theta_degrees += velocities.dtheta_degrees; start_pos.theta_degrees += velocities.dtheta_degrees;
// Add offset from laser // Add offset from laser
start_pos.x_mm += this->laser->laser->offset_mm * this->costheta(); start_pos.x_mm += this->laser->offset_mm * this->costheta();
start_pos.y_mm += this->laser->laser->offset_mm * this->sintheta(); start_pos.y_mm += this->laser->offset_mm * this->sintheta();
// Get new position from implementing class // Get new position from implementing class
Position new_position = this->getNewPosition(start_pos); 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 // Update the current position with this new position, adjusted by laser offset
this->position = Position(new_position); this->position = Position(new_position);
this->position.x_mm -= this->laser->laser->offset_mm * this->costheta(); this->position.x_mm -= this->laser->offset_mm * this->costheta();
this->position.y_mm -= this->laser->laser->offset_mm * this->sintheta(); this->position.y_mm -= this->laser->offset_mm * this->sintheta();
} }
Position & SinglePositionSLAM::getpos(void) Position & SinglePositionSLAM::getpos(void)
@@ -198,13 +205,6 @@ RMHC_SLAM::~RMHC_SLAM(void)
free(this->randomizer); 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) Position RMHC_SLAM::getNewPosition(Position & start_pos)
{ {
// Search for a new position if indicated // Search for a new position if indicated
@@ -219,7 +219,6 @@ Position RMHC_SLAM::getNewPosition(Position & start_pos)
start_pos_c, start_pos_c,
this->map->map, this->map->map,
this->scan_for_distance->scan, this->scan_for_distance->scan,
*this->laser->laser,
this->sigma_xy_mm, this->sigma_xy_mm,
this->sigma_theta_degrees, this->sigma_theta_degrees,
this->max_search_iter, this->max_search_iter,

15
cpp/algorithms.hpp
View File

@@ -83,6 +83,14 @@ public:
void update(int * scan_mm, Velocities & velocities); 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 <tt>scan_size</tt>
* 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 * The quality of the map (0 through 255); default = 50
*/ */
@@ -232,13 +240,6 @@ public:
~RMHC_SLAM(void); ~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 <tt>scan_size</tt>
* 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 standard deviation in millimeters of the Gaussian distribution of

19
examples/Makefile
View File

@@ -18,30 +18,41 @@
# Where you put the libbreezyslam library # Where you put the libbreezyslam library
LIBDIR = /usr/local/lib LIBDIR = /usr/local/lib
JAVADIR = ../java/edu/wlu/cs/levy/breezyslam
# Use EOG or your favorite image-display program # Use EOG or your favorite image-display program
VIEWER = eog VIEWER = eog
# Set these for different experiments # Set these for different experiments
DATASET = exp2 DATASET = exp2
USE_ODOMETRY = 1 USE_ODOMETRY = 0
RANDOM_SEED = 9999 RANDOM_SEED = 9999
all: log2pgm all: log2pgm Log2PGM.class
pytest: pytest:
./log2pgm.py $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) ./log2pgm.py $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED)
$(VIEWER) $(DATASET).pgm $(VIEWER) $(DATASET).pgm ~/Desktop/$(DATASET).pgm
cpptest: log2pgm cpptest: log2pgm
./log2pgm $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) ./log2pgm $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED)
$(VIEWER) $(DATASET).pgm $(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 log2pgm: log2pgm.o
g++ -O3 -o log2pgm log2pgm.o -L$(LIBDIR) -lbreezyslam g++ -O3 -o log2pgm log2pgm.o -L$(LIBDIR) -lbreezyslam
log2pgm.o: log2pgm.cpp log2pgm.o: log2pgm.cpp
g++ -O3 -c -I ../cpp log2pgm.cpp g++ -O3 -c -I ../cpp log2pgm.cpp
Log2PGM.class: Log2PGM.java
javac -classpath ../java Log2PGM.java
$(DATASET).pgm: $(DATASET).pgm:
./log2pgm.py $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED) ./log2pgm.py $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED)
@@ -49,4 +60,4 @@ backup:
cp -r .. ~/Documents/slam/bak-breezyslam cp -r .. ~/Documents/slam/bak-breezyslam
clean: clean:
rm -f log2pgm *.pyc *.pgm *.o *~ rm -f log2pgm *.pyc *.pgm *.o *.class *~

3
examples/log2pgm.cpp
View File

@@ -107,7 +107,6 @@ static void load_data(
{ {
char * cp = strtok(s, " "); char * cp = strtok(s, " ");
long * odometry = new long [3]; long * odometry = new long [3];
odometry[0] = atol(cp); odometry[0] = atol(cp);
skiptok(&cp); skiptok(&cp);
@@ -356,7 +355,7 @@ int main( int argc, const char** argv )
} }
else else
{ {
((RMHC_SLAM *)slam)->update(lidar); slam->update(lidar);
} }
Position position = slam->getpos(); Position position = slam->getpos();

9
examples/logdemo.m
View File

@@ -3,6 +3,14 @@
% data from Paris Mines Tech and displays the map and robot % data from Paris Mines Tech and displays the map and robot
% position in real time. % position in real time.
% %
% Usage:
%
% >> logdemo(dataset, [use_odometry], [random_seed])
%
% Examples:
%
% >> logdemo('exp2')
%
% For details see % For details see
% %
% @inproceedings{coreslam-2010, % @inproceedings{coreslam-2010,
@@ -110,7 +118,6 @@ for scanno = 1:size(scans, 1)
x_pix = x_pix_r + mm2pix(x_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS); 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); y_pix = y_pix_r + mm2pix(y_mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS);
% Add robot image to map % Add robot image to map
fill(x_pix, y_pix, 'r') fill(x_pix, y_pix, 'r')
drawnow drawnow

16
java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java
View File

@@ -1,8 +1,24 @@
/** /**
* DeterministicSLAM implements SinglePositionSLAM using by returning the starting position instead of searching * DeterministicSLAM implements SinglePositionSLAM using by returning the starting position instead of searching
* on it; i.e., using odometry alone. * 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 <http://www.gnu.org/licenses/>.
*/ */
package edu.wlu.cs.levy.breezyslam.algorithms; package edu.wlu.cs.levy.breezyslam.algorithms;
import edu.wlu.cs.levy.breezyslam.components.Position; import edu.wlu.cs.levy.breezyslam.components.Position;

17
java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile
View File

@@ -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 <http://www.gnu.org/licenses/>.
BASEDIR = ../../../../../../.. BASEDIR = ../../../../../../..
JAVADIR = $(BASEDIR)/java JAVADIR = $(BASEDIR)/java
CDIR = $(BASEDIR)/c CDIR = $(BASEDIR)/c

15
java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/ */
package edu.wlu.cs.levy.breezyslam.algorithms; package edu.wlu.cs.levy.breezyslam.algorithms;

15
java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java
View File

@@ -5,6 +5,21 @@
* 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 <http://www.gnu.org/licenses/>.
*/ */
package edu.wlu.cs.levy.breezyslam.algorithms; package edu.wlu.cs.levy.breezyslam.algorithms;

66
java/edu/wlu/cs/levy/breezyslam/algorithms/bak/CoreSLAM.java
View File

@@ -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);
}
*/
}

15
java/edu/wlu/cs/levy/breezyslam/algorithms/bak/Makefile
View File

@@ -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

20
java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c
View File

@@ -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 <http:#www.gnu.org/licenses/>.
*/
#include "../../../../../../../c/random.h" #include "../../../../../../../c/random.h"
#include "../jni_utils.h" #include "../jni_utils.h"

50
java/edu/wlu/cs/levy/breezyslam/components/Laser.java
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
package edu.wlu.cs.levy.breezyslam.components; package edu.wlu.cs.levy.breezyslam.components;
/**
* A class for scanning laser rangefinder (Lidar) parameters.
*/
public class Laser public class Laser
{ {
@@ -10,6 +35,17 @@ public class Laser
protected int detection_margin; protected int detection_margin;
protected double offset_mm; protected double offset_mm;
/**
* 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( public Laser(
int scan_size, int scan_size,
double scan_rate_hz, double scan_rate_hz,
@@ -26,6 +62,12 @@ public class Laser
this.offset_mm = offset_mm; this.offset_mm = offset_mm;
} }
/**
* Builds a Laser object by copying another Laser object.
* Lidar unit.
* @param laser the other Laser object
*
*/
public Laser(Laser laser) public Laser(Laser laser)
{ {
this.scan_size = laser.scan_size; this.scan_size = laser.scan_size;
@@ -36,6 +78,10 @@ public class Laser
this.offset_mm = laser.offset_mm; this.offset_mm = laser.offset_mm;
} }
/**
* Returns a string representation of this Laser object.
*/
public String toString() public String toString()
{ {
String format = "scan_size=%d | scan_rate=%3.3f hz | " + 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() public double getOffsetMm()
{ {
return this.offset_mm; return this.offset_mm;

17
java/edu/wlu/cs/levy/breezyslam/components/Makefile
View File

@@ -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 <http://www.gnu.org/licenses/>.
BASEDIR = ../../../../../../.. BASEDIR = ../../../../../../..
JAVADIR = $(BASEDIR)/java JAVADIR = $(BASEDIR)/java
CDIR = $(BASEDIR)/c CDIR = $(BASEDIR)/c

40
java/edu/wlu/cs/levy/breezyslam/components/Map.java
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
package edu.wlu.cs.levy.breezyslam.components; package edu.wlu.cs.levy.breezyslam.components;
/**
* A class for maps used in SLAM.
*/
public class Map public class Map
{ {
static static
@@ -21,8 +46,17 @@ public class Map
private long native_ptr; private long native_ptr;
/**
* Returns a string representation of this Map object.
*/
public native String toString(); 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) public Map(int size_pixels, double size_meters)
{ {
this.init(size_pixels, 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 * 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); 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); 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() public double sizeMeters()
{ {
return this.size_meters; return this.size_meters;

59
java/edu/wlu/cs/levy/breezyslam/components/Position.java
View File

@@ -1,12 +1,54 @@
/**
*
* 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 <http://www.gnu.org/licenses/>.
*/
package edu.wlu.cs.levy.breezyslam.components; package edu.wlu.cs.levy.breezyslam.components;
/**
* A class representing the position of a robot.
*/
public class Position public class Position
{ {
/**
* Distance of robot from left edge of map, in millimeters.
*/
public double x_mm; public double x_mm;
/**
* Distance of robot from top edge of map, in millimeters.
*/
public double y_mm; public double y_mm;
/**
* Clockwise rotation of robot with respect to three o'clock (east), in degrees.
*/
public double theta_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) public Position(double x_mm, double y_mm, double theta_degrees)
{ {
this.x_mm = x_mm; this.x_mm = x_mm;
@@ -14,6 +56,11 @@ public class Position
this.theta_degrees = theta_degrees; this.theta_degrees = theta_degrees;
} }
/**
* Constructs a new Position object by copying another.
* @param the other Positon object
*/
public Position(Position position) public Position(Position position)
{ {
this.x_mm = position.x_mm; this.x_mm = position.x_mm;
@@ -21,18 +68,14 @@ public class Position
this.theta_degrees = position.theta_degrees; this.theta_degrees = position.theta_degrees;
} }
/**
* Returns a string representation of this Position object.
*/
public String toString() public String toString()
{ {
//String format = "<x = %7.0f mm y = %7.0f mm theta = %+3.3f degrees>"; String format = "<x = %7.0f mm y = %7.0f mm theta = %+3.3f degrees>";
String format = "<x = %f mm y = %f mm theta = %f degrees>";
return String.format(format, this.x_mm, this.y_mm, this.theta_degrees); 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);
}
} }

39
java/edu/wlu/cs/levy/breezyslam/components/Scan.java
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
package edu.wlu.cs.levy.breezyslam.components; package edu.wlu.cs.levy.breezyslam.components;
/**
* A class for Lidar scans.
*/
public class Scan public class Scan
{ {
static static
@@ -20,6 +45,9 @@ public class Scan
public native String toString(); public native String toString();
/**
* Returns a string representation of this Scan object.
*/
public native void update( public native void update(
int [] lidar_mm, int [] lidar_mm,
double hole_width_mm, double hole_width_mm,
@@ -27,6 +55,12 @@ public class Scan
double velocities_dtheta_degrees); 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) public Scan(Laser laser, int span)
{ {
this.init(span, this.init(span,
@@ -38,6 +72,11 @@ public class Scan
laser.offset_mm); laser.offset_mm);
} }
/**
* Builds a Scan object.
* @param laser laser parameters
*
*/
public Scan(Laser laser) public Scan(Laser laser)
{ {
this(laser, 1); this(laser, 1);

14
java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java
View File

@@ -1,13 +1,27 @@
package edu.wlu.cs.levy.breezyslam.components; package edu.wlu.cs.levy.breezyslam.components;
/**
* A class for the Hokuyo URG-04LX laser.
*/
public class URG04LX extends Laser public class URG04LX extends Laser
{ {
/**
* 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) public URG04LX(int detection_margin, double offset_mm)
{ {
super(682, 10, 240, 4000, detection_margin, offset_mm); super(682, 10, 240, 4000, detection_margin, offset_mm);
} }
/**
* Builds a URG04LX object with zero detection margin, offset mm.
*/
public URG04LX() public URG04LX()
{ {
this(0, 0); this(0, 0);

48
java/edu/wlu/cs/levy/breezyslam/components/Velocities.java
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
package edu.wlu.cs.levy.breezyslam.components; 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 public class Velocities
{ {
@@ -41,28 +63,52 @@ public class Velocities
this.dtheta_degrees = dtheta_degrees * velocity_factor; this.dtheta_degrees = dtheta_degrees * velocity_factor;
} }
/**
* Returns a string representation of this Velocities object.
*/
public String toString() public String toString()
{ {
return String.format("<dxy=%7.0f mm dtheta = %+3.3f degrees dt = %f s", return String.format("<dxy=%7.0f mm dtheta = %+3.3f degrees dt = %f s",
this.dxy_mm, this.dtheta_degrees, this.dt_seconds); this.dxy_mm, this.dtheta_degrees, this.dt_seconds);
} }
/**
* Returns the forward component of this Velocities object.
*/
public double getDxyMm() public double getDxyMm()
{ {
return this.dxy_mm; return this.dxy_mm;
} }
/**
* Returns the angular component of this Velocities object.
*/
public double getDthetaDegrees() public double getDthetaDegrees()
{ {
return this.dtheta_degrees; return this.dtheta_degrees;
} }
/**
* Returns the time component of this Velocities object.
*/
public double getDtSeconds() public double getDtSeconds()
{ {
return this.dt_seconds; return this.dt_seconds;
} }
/**
* Forward component of velocity, in mm to be divided by time in seconds.
*/
protected double dxy_mm; protected double dxy_mm;
/**
* Angular component of velocity, in mm to be divided by time in seconds.
*/
protected double dtheta_degrees; protected double dtheta_degrees;
/**
* Time in seconds between successive velocity measurements.
*/
protected double dt_seconds; protected double dt_seconds;
} }

19
java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c
View File

@@ -1,3 +1,22 @@
/*
* jnibreezyslam_components.c Java Native Interface code for BreezySLAM components
*
* 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 <http:#www.gnu.org/licenses/>.
*/
#include "../jni_utils.h" #include "../jni_utils.h"
#include "Map.h" #include "Map.h"

22
java/edu/wlu/cs/levy/breezyslam/jni_utils.h
View File

@@ -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 <http:#www.gnu.org/licenses/>.
*/
#include <stdlib.h>
#include <stdio.h>
#include "../../../../../../c/coreslam.h" #include "../../../../../../c/coreslam.h"
#include <jni.h> #include <jni.h>

17
java/edu/wlu/cs/levy/breezyslam/robots/Makefile
View File

@@ -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 <http://www.gnu.org/licenses/>.
BASEDIR = ../../../../../../.. BASEDIR = ../../../../../../..
JAVADIR = $(BASEDIR)/java JAVADIR = $(BASEDIR)/java
JFLAGS = -Xlint JFLAGS = -Xlint

4
java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java
View File

@@ -1,11 +1,11 @@
/** /**
* *
* BreezySLAM: Simple, efficient SLAM in C++ * BreezySLAM: Simple, efficient SLAM in Java
* *
* WheeledRobot.java - Java class for wheeled robots * WheeledRobot.java - Java class for wheeled robots
* *
* Copyright (C) 2014 Simon D. Levy * Copyright (C) 2014 Simon D. Levy
*
* This code is free software: you can redistribute it and/or modify * This code is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as * it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation, either version 3 of the * published by the Free Software Foundation, either version 3 of the

9
matlab/CoreSLAM.m
View File

@@ -96,15 +96,20 @@ classdef CoreSLAM
% Calls the the implementing class's updateMapAndPointcloud() % Calls the the implementing class's updateMapAndPointcloud()
% method with the specified velocities. % 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 % 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 % 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_mapbuild, scans_mm)
slam.scan_update(slam.scan_for_distance, 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 % Update velocities
velocity_factor = 0; velocity_factor = 0;
if velocities(3) > 0 if velocities(3) > 0

5
matlab/Scan.m
View File

@@ -19,7 +19,6 @@ classdef Scan
properties (Access = {?Map, ?RMHC_SLAM}) properties (Access = {?Map, ?RMHC_SLAM})
c_scan c_scan
c_laser
end end
methods methods
@@ -41,7 +40,7 @@ classdef Scan
span = 1; span = 1;
end end
[scan.c_scan, scan.c_laser] = mex_breezyslam('Scan_init', laser, span); scan.c_scan = mex_breezyslam('Scan_init', laser, span);
end end
@@ -59,7 +58,7 @@ classdef Scan
% hole_width_mm is the width of holes (obstacles, walls) in millimeters. % hole_width_mm is the width of holes (obstacles, walls) in millimeters.
% velocities is an optional list[dxy_mm, dtheta_degrees] % velocities is an optional list[dxy_mm, dtheta_degrees]
% 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.
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
end end

48
matlab/mex_breezyslam.c
View File

@@ -83,16 +83,6 @@ static position_t _rhs2pos(const mxArray * prhs[], int index)
return position; 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 ------------------------------------------------------- */ /* Class methods ------------------------------------------------------- */
static void _map_init(mxArray *plhs[], const mxArray * prhs[]) 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[]) 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)); scan_t * scan = (scan_t *)mxMalloc(sizeof(scan_t));
int span = (int)mxGetScalar(prhs[2]); int span = (int)mxGetScalar(prhs[2]);
_rhs2laser(laser, prhs, 1); const mxArray * laser = prhs[1];
int scan_size = (int)_get_field(laser, "scan_size");
scan_init(scan, laser->scan_size, span); 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, scan, 0);
_insert_obj_lhs(plhs, laser, 1);
} }
static void _scan_disp(const mxArray * prhs[]) static void _scan_disp(const mxArray * prhs[])
@@ -183,17 +183,15 @@ static void _scan_update(const mxArray * prhs[])
{ {
scan_t * scan = _rhs2scan(prhs, 1); 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, hole_width_mm, velocities[0], velocities[1]);
scan_update(scan, lidar_mm, *laser, hole_width_mm, velocities[0], velocities[1]);
} }
static void _randomizer_init(mxArray *plhs[], const mxArray * prhs[]) 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); scan_t * scan = _rhs2scan(prhs, 3);
laser_t laser;
position_t new_pos; position_t new_pos;
double sigma_xy_mm = mxGetScalar(prhs[5]); 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]); void * randomizer = (void *)(long)mxGetScalar(prhs[8]);
_rhs2laser(&laser, prhs, 4);
new_pos = rmhc_position_search( new_pos = rmhc_position_search(
start_pos, start_pos,
map, map,
scan, scan,
laser,
sigma_xy_mm, sigma_xy_mm,
sigma_theta_degrees, sigma_theta_degrees,
max_search_iter, max_search_iter,

BIN
matlab/mex_breezyslam.mexa64
View File

Binary file not shown.

61
python/pybreezyslam.c
View File

@@ -47,26 +47,6 @@ Change log:
#include "../c/random.h" #include "../c/random.h"
#include "pyextension_utils.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 ------------------------------------------------------------- // Position class -------------------------------------------------------------
typedef struct typedef struct
@@ -218,7 +198,6 @@ typedef struct
{ {
PyObject_HEAD PyObject_HEAD
laser_t laser;
scan_t scan; scan_t scan;
int * lidar_mm; int * lidar_mm;
@@ -253,6 +232,13 @@ Scan_init(Scan *self, PyObject *args, PyObject *kwds)
static char* argnames[] = {"laser", "span", NULL}; 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, if(!PyArg_ParseTupleAndKeywords(args, kwds,"O|i", argnames,
&py_laser, &py_laser,
&span)) &span))
@@ -260,14 +246,27 @@ Scan_init(Scan *self, PyObject *args, PyObject *kwds)
return error_on_raise_argument_exception("Scan"); 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"); return error_on_raise_argument_exception("Scan");
} }
scan_init(&self->scan, self->laser.scan_size, span); 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->laser.scan_size); self->lidar_mm = int_alloc(self->scan.size);
return 0; return 0;
} }
@@ -311,7 +310,7 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
} }
// Bozo filter on LIDAR argument list size // 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", return null_on_raise_argument_exception_with_details("Scan", "update",
"lidar size mismatch"); "lidar size mismatch");
@@ -342,7 +341,7 @@ Scan_update(Scan *self, PyObject *args, PyObject *kwds)
// Extract LIDAR values from argument // Extract LIDAR values from argument
int k = 0; int k = 0;
for (k=0; k<self->laser.scan_size; ++k) for (k=0; k<self->scan.size; ++k)
{ {
self->lidar_mm[k] = PyFloat_AsDouble(PyList_GetItem(py_lidar, 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( scan_update(
&self->scan, &self->scan,
self->lidar_mm, self->lidar_mm,
self->laser,
hole_width_mm, hole_width_mm,
dxy_mm, dxy_mm,
dtheta_degrees); dtheta_degrees);
@@ -794,19 +792,11 @@ rmhcPositionSearch(PyObject *self, PyObject *args)
// Convert Python objects to C structures // Convert Python objects to C structures
position_t start_pos = pypos2cpos(py_start_pos); 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 = position_t likeliest_position =
rmhc_position_search( rmhc_position_search(
start_pos, start_pos,
&py_map->map, &py_map->map,
&py_scan->scan, &py_scan->scan,
c_laser,
sigma_xy_mm, sigma_xy_mm,
sigma_theta_degrees, sigma_theta_degrees,
max_search_iter, max_search_iter,
@@ -916,3 +906,4 @@ PyInit_pybreezyslam(void)
} }
#endif #endif