diff --git a/README.md b/README.md index 179405a..8222f00 100644 --- a/README.md +++ b/README.md @@ -5,8 +5,6 @@ BreezySLAM

-NOTE: This branch (legacy) contains legacy support for Matlab, and Java, languages that I am no longer supporting in the [master](https://github.com/simondlevy/BreezySLAM) branch. If you're working in Python or C++, you should be using the master branch. - Simple, efficient, open-source package for Simultaneous Localization and Mapping in Python and C++ This repository contains everything you need to diff --git a/examples/Log2PGM.java b/examples/Log2PGM.java new file mode 100644 index 0000000..47d2100 --- /dev/null +++ b/examples/Log2PGM.java @@ -0,0 +1,363 @@ +/* + Log2PGM.java : BreezySLAM demo. Reads logfile with odometry and scan data from + Paris Mines Tech and produces a .PGM image file showing robot path + and final map. + + For details see + + @inproceedings{, + author = {Bruno Steux and Oussama El Hamzaoui}, + title = {SinglePositionSLAM: a SLAM Algorithm in less than 200 lines of C code}, + booktitle = {11th International Conference on Control, Automation, Robotics and Vision, ICARCV 2010, Singapore, 7-10 + December 2010, Proceedings}, + pages = {1975-1979}, + publisher = {IEEE}, + year = {2010} + } + + 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 . + */ + +import edu.wlu.cs.levy.breezyslam.components.Map; +import edu.wlu.cs.levy.breezyslam.components.Scan; +import edu.wlu.cs.levy.breezyslam.components.Position; +import edu.wlu.cs.levy.breezyslam.components.URG04LX; +import edu.wlu.cs.levy.breezyslam.components.PoseChange; + +import edu.wlu.cs.levy.breezyslam.robots.WheeledRobot; + +import edu.wlu.cs.levy.breezyslam.algorithms.RMHCSLAM; +import edu.wlu.cs.levy.breezyslam.algorithms.DeterministicSLAM; +import edu.wlu.cs.levy.breezyslam.algorithms.SinglePositionSLAM; + +import java.io.BufferedReader; +import java.io.FileReader; +import java.io.BufferedWriter; +import java.io.FileWriter; +import java.io.IOException; +import java.util.Vector; + +public class Log2PGM +{ + private static int MAP_SIZE_PIXELS = 800; + private static double MAP_SIZE_METERS = 32; + private static int SCAN_SIZE = 682; + + private SinglePositionSLAM slam; + + private boolean use_odometry; + private int random_seed; + private Rover robot; + + static int coords2index(double x, double y) + { + return (int)(y * MAP_SIZE_PIXELS + x); + } + + int mm2pix(double mm) + { + return (int)(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS)); + } + + + // Class for Mines verison of URG-04LX Lidar ----------------------------------- + + private class MinesURG04LX extends URG04LX + { + public MinesURG04LX() + { + super( 70, // detectionMargin + 145); // offsetMillimeters + } + } + + // Class for MinesRover custom robot ------------------------------------------- + + private class Rover extends WheeledRobot + { + public Rover() + { + super(77, // wheelRadiusMillimeters + 165); // halfAxleLengthMillimeters + } + + protected WheeledRobot.WheelOdometry extractOdometry( + double timestamp, + double leftWheelOdometry, + double rightWheelOdometry) + { + // Convert microseconds to seconds, ticks to angles + return new WheeledRobot.WheelOdometry( + timestamp / 1e6, + ticksToDegrees(leftWheelOdometry), + ticksToDegrees(rightWheelOdometry)); + } + + protected String descriptorString() + { + return String.format("ticks_per_cycle=%d", this.TICKS_PER_CYCLE); + } + + + private double ticksToDegrees(double ticks) + { + return ticks * (180. / this.TICKS_PER_CYCLE); + } + + private int TICKS_PER_CYCLE = 2000; + } + + // Progress-bar class ---------------------------------------------------------------- + // Adapted from http://code.activestate.com/recipes/168639-progress-bar-class/ + // Downloaded 12 January 2014 + + private class ProgressBar + { + public ProgressBar(int minValue, int maxValue, int totalWidth) + { + this.progBar = "[]"; + this.min = minValue; + this.max = maxValue; + this.span = maxValue - minValue; + this.width = totalWidth; + this.amount = 0; // When amount == max, we are 100% done + this.updateAmount(0); // Build progress bar string + } + + void updateAmount(int newAmount) + { + if (newAmount < this.min) + { + newAmount = this.min; + } + if (newAmount > this.max) + { + newAmount = this.max; + } + + this.amount = newAmount; + + // Figure out the new percent done, round to an integer + float diffFromMin = (float)(this.amount - this.min); + int percentDone = (int)java.lang.Math.round((diffFromMin / (float)this.span) * 100.0); + + // Figure out how many hash bars the percentage should be + int allFull = this.width - 2; + int numHashes = (int)java.lang.Math.round((percentDone / 100.0) * allFull); + + // Build a progress bar with hashes and spaces + this.progBar = "["; + this.addToProgBar("#", numHashes); + this.addToProgBar(" ", allFull-numHashes); + this.progBar += "]"; + + // Figure out where to put the percentage, roughly centered + int percentPlace = (this.progBar.length() / 2) - ((int)(java.lang.Math.log10(percentDone+1)) + 1); + String percentString = String.format("%d%%", percentDone); + + // Put it there + this.progBar = this.progBar.substring(0,percentPlace) + percentString + this.progBar.substring(percentPlace+percentString.length()); + } + + String str() + { + return this.progBar; + } + + private String progBar; + private int min; + private int max; + private int span; + private int width; + private int amount; + + private void addToProgBar(String s, int n) + { + for (int k=0; k 0 ? + new RMHCSLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed) : + new DeterministicSLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS); + + this.use_odometry = use_odometry; + this.random_seed = random_seed; + } + + public byte [] processData(Vector scans, Vector odometries) + { + // Report what we're doing + int nscans = scans.size(); + ProgressBar progbar = new ProgressBar(0, nscans, 80); + + System.out.printf("Processing %d scans with%s odometry / with%s particle filter...\n", + nscans, this.use_odometry ? "" : "out", this.random_seed > 0 ? "" : "out"); + + Vector trajectory = new Vector(); + + for (int scanno=0; scanno "); + System.err.println("Example: java log2pgm exp2 1 9999"); + System.exit(1); + } + + // Grab input args + String dataset = argv[0]; + boolean use_odometry = Integer.parseInt(argv[1]) == 0 ? false : true; + int random_seed = argv.length > 2 ? Integer.parseInt(argv[2]) : 0; + + // Load the data from the file + + Vector scans = new Vector(); + Vector odometries = new Vector();; + + String filename = dataset + ".dat"; + System.out.printf("Loading data from %s ... \n", filename); + + BufferedReader input = null; + + try + { + FileReader fstream = new FileReader(filename); + input = new BufferedReader(fstream); + while (true) + { + String line = input.readLine(); + if (line == null) + { + break; + } + + String [] toks = line.split(" +"); + + long [] odometry = new long [3]; + odometry[0] = Long.parseLong(toks[0]); + odometry[1] = Long.parseLong(toks[2]); + odometry[2] = Long.parseLong(toks[3]); + odometries.add(odometry); + + int [] scan = new int [SCAN_SIZE]; + for (int k=0; k. + + properties (Access = 'private') + TICKS_PER_CYCLE = 2000; + end + + methods (Access = 'private') + + function degrees = ticks_to_degrees(obj, ticks) + + degrees = ticks * (180. / obj.TICKS_PER_CYCLE); + + end + + end + + methods (Access = 'public') + + function robot = MinesRover() + + robot = robot@WheeledRobot(77, 165); + + end + + function disp(obj) + % Displays information about this MinesRover + + fprintf('<%s ticks_per_cycle=%d>\n', obj.str(), obj.TICKS_PER_CYCLE) + + end + + function [poseChange, obj] = computePoseChange(obj, odometry) + + [poseChange, obj] = computePoseChange@WheeledRobot(obj, odometry(1), odometry(2), odometry(3)); + + end + + function [timestampSeconds, leftWheelDegrees, rightWheelDegrees] = ... + extractOdometry(obj, timestamp, leftWheel, rightWheel) + + % Convert microseconds to seconds + timestampSeconds = timestamp / 1e6; + + % Convert ticks to angles + leftWheelDegrees = obj.ticks_to_degrees(leftWheel); + rightWheelDegrees = obj.ticks_to_degrees(rightWheel); + + end + + end + +end + diff --git a/examples/logdemo.m b/examples/logdemo.m new file mode 100644 index 0000000..c59e336 --- /dev/null +++ b/examples/logdemo.m @@ -0,0 +1,175 @@ +% +% logdemo.m : BreezySLAM Matlab demo. Reads logfile with odometry and scan +% 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, +% author = {Bruno Steux and Oussama El Hamzaoui}, +% title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code}, +% booktitle = {11th International Conference on Control, Automation, +% Robotics and Vision, ICARCV 2010, Singapore, 7-10 +% December 2010, Proceedings}, +% pages = {1975-1979}, +% publisher = {IEEE}, +% year = {2010} +% } +% +% 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 . + +function logdemo(dataset, use_odometry, seed) + +% Params + +MAP_SIZE_PIXELS = 800; +MAP_SIZE_METERS = 32; +ROBOT_SIZE_PIXELS = 10; + +% Grab input args + +if nargin < 2 + use_odometry = 0; +end + +if nargin < 3 + seed = 0; +end + +% Load data from file +[times, scans, odometries] = load_data(dataset); + +% Build a robot model if we want odometry +robot = []; +if use_odometry + robot = MinesRover(); +end + +% Create a CoreSLAM object with laser params and optional robot object +if seed + slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, seed); +else + slam = Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS); +end + +% Initialize previous time for delay +prevTime = 0; + +% Loop over scans +for scanno = 1:size(scans, 1) + + if use_odometry + + % Convert odometry to velocities + [velocities,robot] = robot.computePoseChange(odometries(scanno, :)); + + % Update SLAM with lidar and velocities + slam = slam.update(scans(scanno,:), velocities); + + else + + % Update SLAM with lidar alone + slam = slam.update(scans(scanno,:)); + end + + % Get new position + [x_mm, y_mm, theta_degrees] = slam.getpos(); + + % Get current map + map = slam.getmap(); + + % Display map + hold off + image(map/4) % Keep bytes in [0,64] for colormap + axis('square') + colormap('gray') + hold on + + % Generate a polyline to represent the robot + [x_pix, y_pix] = robot_polyline(ROBOT_SIZE_PIXELS); + + % Rotate the polyline based on the robot's angular rotation + theta_radians = pi * theta_degrees / 180; + x_pix_r = x_pix * cos(theta_radians) - y_pix * sin(theta_radians); + y_pix_r = x_pix * sin(theta_radians) + y_pix * cos(theta_radians); + + % Add the robot's position as offset to the polyline + 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 + + % Delay based on current time in microseconds + currTime = times(scanno); + if scanno > 1 + pause((currTime-prevTime)/1e6) + end + prevTime = times(scanno); + + +end + +% Function to generate a x, y points for a polyline to display the robot +% Currently we just generate a triangle. +function [x_pix, y_pix] = robot_polyline(robot_size) +HEIGHT_RATIO = 1.25; +x_pix = [-robot_size, robot_size, -robot_size]; +y_pix = [-robot_size/HEIGHT_RATIO, 0 , robot_size/HEIGHT_RATIO]; + +% Function to convert millimeters to pixels ------------------------------- + +function pix = mm2pix(mm, MAP_SIZE_METERS, MAP_SIZE_PIXELS) +pix = floor(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS)); + + +% Function to load all from file ------------------------------------------ +% Each line in the file has the format: +% +% TIMESTAMP ... Q1 Q1 ... Distances +% (usec) (mm) +% 0 ... 2 3 ... 24 ... +% +%where Q1, Q2 are odometry values + +function [times, scans,odometries] = load_data(dataset) + +data = load([dataset '.dat']); + +times = data(:,1); +scans = data(:,25:end-1); % avoid final ' ' +odometries = data(:,[1,3,4]); + +% Function to build a Laser data structure for the Hokuyo URG04LX used for +% collecting the logfile data. + +function laser = MinesLaser() + +laser.scan_size = 682; +laser.scan_rate_hz = 10; +laser.detection_angle_degrees = 240; +laser.distance_no_detection_mm = 4000; +laser.detection_margin = 70; +laser.offset_mm = 145; diff --git a/examples/rpslam.py b/examples/rpslam.py index 84b7140..f1099ed 100755 --- a/examples/rpslam.py +++ b/examples/rpslam.py @@ -30,6 +30,9 @@ from rplidar import RPLidar as Lidar from pltslamshow import SlamShow +from scipy.interpolate import interp1d +import numpy as np + if __name__ == '__main__': # Connect to Lidar unit @@ -52,36 +55,33 @@ if __name__ == '__main__': while True: - try: + # Extrat (quality, angle, distance) triples from current scan + items = [item for item in next(iterator)] - # Extrat (quality, angle, distance) triples from current scan - items = [item for item in next(iterator)] + # Extract distances and angles from triples + distances = [item[2] for item in items] + angles = [item[1] for item in items] - # Extract distances and angles from triples - distances = [item[2] for item in items] - angles = [item[1] for item in items] - print(angles) - - except KeyboardInterrupt: - break + # Interpolate to get 360 angles from 0 through 359, and corresponding distances + f = interp1d(angles, distances, fill_value='extrapolate') + distances = list(f(np.arange(360))) # Update SLAM with current Lidar scan, using third element of (quality, angle, distance) triples - #slam.update([item[2] for item in next(iterator)]) + slam.update(distances) # Get current robot position - #x, y, theta = slam.getpos() + x, y, theta = slam.getpos() # Get current map bytes as grayscale - #slam.getmap(mapbytes) + slam.getmap(mapbytes) - #display.displayMap(mapbytes) + display.displayMap(mapbytes) - #display.setPose(x, y, theta) + display.setPose(x, y, theta) - # Exit on ESCape - #key = display.refresh() - #if key != None and (key&0x1A): - # exit(0) + # Exit on window close + if not display.refresh(): + exit(0) lidar.stop() lidar.disconnect() diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java new file mode 100644 index 0000000..a57a162 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java @@ -0,0 +1,133 @@ +/** +* +* CoreSLAM.java abstract Java class for CoreSLAM algorithm in BreezySLAM +* +* 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.Laser; +import edu.wlu.cs.levy.breezyslam.components.PoseChange; +import edu.wlu.cs.levy.breezyslam.components.Map; +import edu.wlu.cs.levy.breezyslam.components.Scan; + +/** +* CoreSLAM is an abstract class that uses the classes Position, Map, Scan, and Laser +* to run variants of the simple CoreSLAM (tinySLAM) algorithm described in +* 

+*     @inproceedings{coreslam-2010,
+*       author    = {Bruno Steux and Oussama El Hamzaoui}, 
+*       title     = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code},  
+*       booktitle = {11th International Conference on Control, Automation,   
+*                    Robotics and Vision, ICARCV 2010, Singapore, 7-10  
+*                    December 2010, Proceedings},  
+*       pages     = {1975-1979},  
+*       publisher = {IEEE},  
+*       year      = {2010}
+*     }
+*
+* Implementing classes should provide the method +* +* void updateMapAndPointcloud(int * scan_mm, PoseChange & poseChange) +* +* to update the map and point-cloud (particle cloud). +* +*/ +public abstract class CoreSLAM { + + /** + * The quality of the map (0 through 255) + */ + public int map_quality = 50; + + /** + * The width in millimeters of each "hole" in the map (essentially, wall width) + */ + public double hole_width_mm = 600; + + protected Laser laser; + + protected PoseChange poseChange; + + protected Map map; + + protected Scan scan_for_mapbuild; + protected Scan scan_for_distance; + + public CoreSLAM(Laser laser, int map_size_pixels, double map_size_meters) + { + // Set default params + this.laser = new Laser(laser); + + // Initialize poseChange (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry + this.poseChange = new PoseChange(); + + // 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); + } + + private Scan scan_create(int span) + { + return new Scan(this.laser, span); + } + + private void scan_update(Scan scan, int [] scan_mm) + { + scan.update(scan_mm, this.hole_width_mm, this.poseChange); + } + + + public void update(int [] scan_mm, PoseChange poseChange) + { + // 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 poseChange + this.poseChange.update(poseChange.getDxyMm(), poseChange.getDthetaDegrees(), poseChange.getDtSeconds()); + + // Implementing class updates map and pointcloud + this.updateMapAndPointcloud(poseChange); + } + + /** + * Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero poseChange + * (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 + */ + public void update(int [] scan_mm) + { + PoseChange zero_poseChange = new PoseChange(); + + this.update(scan_mm, zero_poseChange); + } + + + protected abstract void updateMapAndPointcloud(PoseChange poseChange); + + public void getmap(byte [] mapbytes) + { + this.map.get(mapbytes); + } + +} diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java new file mode 100644 index 0000000..458943d --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java @@ -0,0 +1,52 @@ +/** +* 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; +import edu.wlu.cs.levy.breezyslam.components.Laser; + +public class DeterministicSLAM extends SinglePositionSLAM +{ + + /** + * Creates a DeterministicSLAM object. + * @param laser a Laser object containing parameters for your Lidar equipment + * @param map_size_pixels the size of the desired map (map is square) + * @param map_size_meters the size of the area to be mapped, in meters + * @return a new CoreSLAM object + */ + public DeterministicSLAM(Laser laser, int map_size_pixels, double map_size_meters) + { + super(laser, map_size_pixels, map_size_meters); + } + + /** + * Returns a new position identical to the starting position. Called automatically by + * SinglePositionSLAM::updateMapAndPointcloud() + * @param start_pos the starting position + */ + protected Position getNewPosition(Position start_position) + { + return new Position(start_position.x_mm, start_position.y_mm, start_position.theta_degrees); + } + +} // DeterministicSLAM diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile b/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile new file mode 100644 index 0000000..45b5a43 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile @@ -0,0 +1,90 @@ +# 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 +JFLAGS = -Xlint +JDKINC = -I /usr/lib/jvm/java-7-openjdk-amd64/include +#JDKINC = -I /opt/jdk1.7.0_67/include -I /opt/jdk1.7.0_67/include/linux + +# Set library extension based on OS +ifeq ("$(shell uname)","Darwin") + LIBEXT = dylib +else ifeq ("$(shell uname)","Linux") + CFLAGS = -fPIC + LIBEXT = so +else + LIBEXT = dll +endif + +# Set SIMD compile params based on architecture +ifeq ("$(ARCH)","armv7l") + SIMD_FLAGS = -mfpu=neon +else ifeq ("$(ARCH)","i686") + SIMD_FLAGS = -msse3 +else + ARCH = sisd +endif + + +ALL = libjnibreezyslam_algorithms.$(LIBEXT) CoreSLAM.class SinglePositionSLAM.class DeterministicSLAM.class RMHCSLAM.class + +all: $(ALL) + +libjnibreezyslam_algorithms.$(LIBEXT): jnibreezyslam_algorithms.o coreslam.o random.o ziggurat.o coreslam_$(ARCH).o + gcc -shared -Wl,-soname,libjnibreezyslam_algorithms.so -o libjnibreezyslam_algorithms.so jnibreezyslam_algorithms.o \ + coreslam.o coreslam_$(ARCH).o random.o ziggurat.o + +jnibreezyslam_algorithms.o: jnibreezyslam_algorithms.c RMHCSLAM.h ../jni_utils.h + gcc $(JDKINC) -fPIC -c jnibreezyslam_algorithms.c + + +CoreSLAM.class: CoreSLAM.java + javac -classpath $(JAVADIR):. CoreSLAM.java + + +SinglePositionSLAM.class: SinglePositionSLAM.java + javac -classpath $(JAVADIR):. SinglePositionSLAM.java + +DeterministicSLAM.class: DeterministicSLAM.java + javac -classpath $(JAVADIR):. DeterministicSLAM.java + +RMHCSLAM.class: RMHCSLAM.java + javac -classpath $(JAVADIR):. RMHCSLAM.java + +RMHCSLAM.h: RMHCSLAM.class + javah -o RMHCSLAM.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.algorithms.RMHCSLAM + +coreslam.o: $(CDIR)/coreslam.c $(CDIR)/coreslam.h + gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/coreslam.c + +coreslam_$(ARCH).o: $(CDIR)/coreslam_$(ARCH).c $(CDIR)/coreslam.h + gcc -O3 -c -Wall $(CFLAGS) $(SIMD_FLAGS) $(CDIR)/coreslam_$(ARCH).c + +random.o: $(CDIR)/random.c + gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/random.c + +ziggurat.o: $(CDIR)/ziggurat.c + gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/ziggurat.c + +clean: + rm -f *.class *.o *.h *.$(LIBEXT) *~ + +backup: + cp *.java bak + cp Makefile bak diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java new file mode 100644 index 0000000..06269ab --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java @@ -0,0 +1,103 @@ +/** +* 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; + +import edu.wlu.cs.levy.breezyslam.components.Position; +import edu.wlu.cs.levy.breezyslam.components.Laser; +import edu.wlu.cs.levy.breezyslam.components.PoseChange; +import edu.wlu.cs.levy.breezyslam.components.Map; +import edu.wlu.cs.levy.breezyslam.components.Scan; + +public class RMHCSLAM extends SinglePositionSLAM +{ + private static final double DEFAULT_SIGMA_XY_MM = 100; + private static final double DEFAULT_SIGMA_THETA_DEGREES = 20; + private static final int DEFAULT_MAX_SEARCH_ITER = 1000; + + static + { + System.loadLibrary("jnibreezyslam_algorithms"); + } + + private native void init(int random_seed); + + private native Object positionSearch( + Position start_pos, + Map map, + Scan scan, + double sigma_xy_mm, + double sigma_theta_degrees, + int max_search_iter); + + + private long native_ptr; + + /** + * Creates an RMHCSLAM object. + * @param laser a Laser object containing parameters for your Lidar equipment + * @param map_size_pixels the size of the desired map (map is square) + * @param map_size_meters the size of the area to be mapped, in meters + * @param random_seed seed for psuedorandom number generator in particle filter + * @return a new CoreSLAM object + */ + public RMHCSLAM(Laser laser, int map_size_pixels, double map_size_meters, int random_seed) + { + super(laser, map_size_pixels, map_size_meters); + + this.init(random_seed); + } + + /** + * The standard deviation in millimeters of the Gaussian distribution of + * the (X,Y) component of position in the particle filter; default = 100 + */ + public double sigma_xy_mm = DEFAULT_SIGMA_XY_MM;; + + /** + * The standard deviation in degrees of the Gaussian distribution of + * the angular rotation component of position in the particle filter; default = 20 + */ + public double sigma_theta_degrees = DEFAULT_SIGMA_THETA_DEGREES; + + /** + * The maximum number of iterations for particle-filter search; default = 1000 + */ + public int max_search_iter = DEFAULT_MAX_SEARCH_ITER; + + /** + * Returns a new position based on RMHC search from a starting position. Called automatically by + * SinglePositionSLAM::updateMapAndPointcloud() + * @param start_position the starting position + */ + protected Position getNewPosition(Position start_position) + { + Position newpos = + (Position)positionSearch( + start_position, + this.map, + this.scan_for_distance, + this.sigma_xy_mm, + this.sigma_theta_degrees, + this.max_search_iter); + + return newpos; + } + +} // RMHCSLAM diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java new file mode 100644 index 0000000..085064b --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java @@ -0,0 +1,118 @@ +/** +* 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) +* +* 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; + +import edu.wlu.cs.levy.breezyslam.components.Position; +import edu.wlu.cs.levy.breezyslam.components.PoseChange; +import edu.wlu.cs.levy.breezyslam.components.Laser; + + +public abstract class SinglePositionSLAM extends CoreSLAM +{ + /** + * Returns the current position. + * @return the current position as a Position object. + */ + public Position getpos() + { + return this.position; + } + + /** + * Creates a SinglePositionSLAM object. + * @param laser a Laser object containing parameters for your Lidar equipment + * @param map_size_pixels the size of the desired map (map is square) + * @param map_size_meters the size of the area to be mapped, in meters + * @return a new SinglePositionSLAM object + */ + protected SinglePositionSLAM(Laser laser, int map_size_pixels, double map_size_meters) + { + super(laser, map_size_pixels, map_size_meters); + + this.position = new Position(this.init_coord_mm(), this.init_coord_mm(), 0); + } + + + /** + * Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM::update() + * @param poseChange poseChange for odometry + */ + protected void updateMapAndPointcloud(PoseChange poseChange) + { + // Start at current position + Position start_pos = new Position(this.position); + + // Add effect of poseChange + start_pos.x_mm += poseChange.getDxyMm() * this.costheta(); + start_pos.y_mm += poseChange.getDxyMm() * this.sintheta(); + start_pos.theta_degrees += poseChange.getDthetaDegrees(); + + // Add offset from laser + start_pos.x_mm += this.laser.getOffsetMm() * this.costheta(); + start_pos.y_mm += this.laser.getOffsetMm() * this.sintheta(); + + // Get new position from implementing class + Position new_position = this.getNewPosition(start_pos); + + // Update the map with this new position + this.map.update(this.scan_for_mapbuild, new_position, this.map_quality, this.hole_width_mm); + + // Update the current position with this new position, adjusted by laser offset + this.position = new Position(new_position); + this.position.x_mm -= this.laser.getOffsetMm() * this.costheta(); + this.position.y_mm -= this.laser.getOffsetMm() * this.sintheta(); + } + + /** + * Returns a new position based on searching from a starting position. Called automatically by + * SinglePositionSLAM::updateMapAndPointcloud() + * @param start_pos the starting position + */ + + protected abstract Position getNewPosition(Position start_pos); + + private Position position; + + private double theta_radians() + { + return java.lang.Math.toRadians(this.position.theta_degrees); + } + + private double costheta() + { + return java.lang.Math.cos(this.theta_radians()); + } + + private double sintheta() + { + return java.lang.Math.sin(this.theta_radians()); + } + + private double init_coord_mm() + { + // Center of map + return 500 * this.map.sizeMeters(); + } +} diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c b/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c new file mode 100644 index 0000000..e2e8d8f --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c @@ -0,0 +1,67 @@ +/* + * 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" + +#include + + +// RMHC_SLAM methods ----------------------------------------------------------------------------------------- + +JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_algorithms_RMHCSLAM_init (JNIEnv *env, jobject thisobject, jint random_seed) +{ + ptr_to_obj(env, thisobject, random_new(random_seed)); +} + +JNIEXPORT jobject JNICALL Java_edu_wlu_cs_levy_breezyslam_algorithms_RMHCSLAM_positionSearch (JNIEnv *env, jobject thisobject, + jobject startpos_object, + jobject map_object, + jobject scan_object, + jdouble sigma_xy_mm, + jdouble sigma_theta_degrees, + jint max_search_iter) +{ + position_t startpos; + + startpos.x_mm = get_double_field(env, startpos_object, "x_mm"); + startpos.y_mm = get_double_field(env, startpos_object, "y_mm"); + startpos.theta_degrees = get_double_field(env, startpos_object, "theta_degrees"); + + void * random = ptr_from_obj(env, thisobject); + + position_t newpos = + rmhc_position_search( + startpos, + cmap_from_jmap(env, map_object), + cscan_from_jscan(env, scan_object), + sigma_xy_mm, + sigma_theta_degrees, + max_search_iter, + random); + + jclass cls = (*env)->FindClass(env, "edu/wlu/cs/levy/breezyslam/components/Position"); + + jmethodID constructor = (*env)->GetMethodID(env, cls, "", "(DDD)V"); + + jobject newpos_object = (*env)->NewObject(env, cls, constructor, newpos.x_mm, newpos.y_mm, newpos.theta_degrees); + + return newpos_object; +} diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Laser.java b/java/edu/wlu/cs/levy/breezyslam/components/Laser.java new file mode 100644 index 0000000..aa77425 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Laser.java @@ -0,0 +1,108 @@ +/** +* +* 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 +{ + + protected int scan_size; + protected double scan_rate_hz; + protected double detection_angle_degrees; + protected double distance_no_detection_mm; + protected int detection_margin; + 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( + int scan_size, + double scan_rate_hz, + double detection_angle_degrees, + double distance_no_detection_mm, + int detection_margin, + double offset_mm) + { + 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 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; + this.detection_angle_degrees = laser.detection_angle_degrees; + this.distance_no_detection_mm = laser.distance_no_detection_mm; + this.detection_margin = laser.detection_margin; + 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 | " + + "detection_angle=%3.3f deg | " + + "distance_no_detection=%7.4f mm | " + + "detection_margin=%d | offset=%4.4f m"; + + return String.format(format, this.scan_size, this.scan_rate_hz, + this.detection_angle_degrees, + this.distance_no_detection_mm, + this.detection_margin, + this.offset_mm); + + } + + /** + * 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 new file mode 100644 index 0000000..35a37bc --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Makefile @@ -0,0 +1,89 @@ +# 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 +JFLAGS = -Xlint +JDKINC = -I /usr/lib/jvm/java-7-openjdk-amd64/include +#JDKINC = -I /opt/jdk1.7.0_67/include -I /opt/jdk1.7.0_67/include/linux + +# Set library extension based on OS +ifeq ("$(shell uname)","Darwin") + LIBEXT = dylib +else ifeq ("$(shell uname)","Linux") + CFLAGS = -fPIC + LIBEXT = so +else + LIBEXT = dll +endif + +ARCH = $(shell uname -m) + +# Set SIMD compile params based on architecture +ifeq ("$(ARCH)","armv7l") + SIMD_FLAGS = -mfpu=neon +else ifeq ("$(ARCH)","i686") + SIMD_FLAGS = -msse3 +else + ARCH = sisd +endif + +ALL = libjnibreezyslam_components.$(LIBEXT) Laser.class Position.class PoseChange.class URG04LX.class + +all: $(ALL) + +libjnibreezyslam_components.$(LIBEXT): jnibreezyslam_components.o coreslam.o coreslam_$(ARCH).o + gcc -shared -Wl,-soname,libjnibreezyslam_components.so -o libjnibreezyslam_components.so jnibreezyslam_components.o \ + coreslam.o coreslam_$(ARCH).o + +jnibreezyslam_components.o: jnibreezyslam_components.c Map.h Scan.h ../jni_utils.h + gcc $(JDKINC) -fPIC -c jnibreezyslam_components.c + +coreslam.o: $(CDIR)/coreslam.c $(CDIR)/coreslam.h + gcc -O3 -c -Wall $(CFLAGS) $(CDIR)/coreslam.c + +coreslam_$(ARCH).o: $(CDIR)/coreslam_$(ARCH).c $(CDIR)/coreslam.h + gcc -O3 -c -Wall $(CFLAGS) $(SIMD_FLAGS) $(CDIR)/coreslam_$(ARCH).c + +Map.h: Map.class + javah -o Map.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.components.Map + +Map.class: Map.java Scan.class Position.class + javac $(JFLAGS) -classpath $(JAVADIR) Map.java + +Scan.h: Scan.class + javah -o Scan.h -classpath $(JAVADIR) -jni edu.wlu.cs.levy.breezyslam.components.Scan + +Scan.class: Scan.java Laser.class + javac $(JFLAGS) -classpath $(JAVADIR) Scan.java + +Laser.class: Laser.java + javac $(JFLAGS) Laser.java + +URG04LX.class: URG04LX.java Laser.class + javac $(JFLAGS) -classpath $(JAVADIR) URG04LX.java + +PoseChange.class: PoseChange.java + javac $(JFLAGS) PoseChange.java + + +Position.class: Position.java + javac $(JFLAGS) Position.java + +clean: + rm -f *.so *.class *.o *.h *~ diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Map.java b/java/edu/wlu/cs/levy/breezyslam/components/Map.java new file mode 100644 index 0000000..4bfa2f6 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Map.java @@ -0,0 +1,96 @@ +/** +* +* 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 + { + System.loadLibrary("jnibreezyslam_components"); + } + + private native void init(int size_pixels, double size_meters); + + private double size_meters; + + private native void update( + Scan scan, + double position_x_mm, + double position_y_mm, + double position_theta_degrees, + int quality, + double hole_width_mm); + + 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); + + // for public accessor + this.size_meters = size_meters; + } + + /** + * Puts current map values into bytearray, which should of which should be of + * this.size map_size_pixels ^ 2. + * @param bytes byte array that gets the map values + */ + public native void get(byte [] bytes); + + /** + * Updates this map object based on new data. + * @param scan a new scan + * @param position a new postion + * @param quality speed with which scan is integerate into map (0 through 255) + * @param hole_width_mm hole width in millimeters + * + */ + public void update(Scan scan, Position position, int quality, double 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() + { + return this.size_meters; + } + +} diff --git a/java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java b/java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java new file mode 100644 index 0000000..7682724 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java @@ -0,0 +1,115 @@ +/** +* +* BreezySLAM: Simple, efficient SLAM in Java +* +* PoseChange.java - Java code for PoseChange class, encoding triple +* (dxy_mm, dtheta_degrees, dt_seconds) +* +* 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 as determined by odometry. +*/ +public class PoseChange +{ + + /** + * Creates a new PoseChange object with specified velocities. + */ + public PoseChange(double dxy_mm, double dtheta_degrees, double dtSeconds) + { + this.dxy_mm = dxy_mm; + this.dtheta_degrees = dtheta_degrees; + this.dt_seconds = dtSeconds; + } + + /** + * Creates a new PoseChange object with zero velocities. + */ + public PoseChange() + { + this.dxy_mm = 0; + this.dtheta_degrees = 0; + this.dt_seconds = 0; + } + + /** + * Updates this PoseChange object. + * @param dxy_mm new forward distance traveled in millimeters + * @param dtheta_degrees new angular rotation in degrees + * @param dtSeconds time in seconds since last velocities + */ + public void update(double dxy_mm, double dtheta_degrees, double dtSeconds) + { + double velocity_factor = (dtSeconds > 0) ? (1 / dtSeconds) : 0; + + this.dxy_mm = dxy_mm * velocity_factor; + + this.dtheta_degrees = dtheta_degrees * velocity_factor; + } + + /** + * Returns a string representation of this PoseChange object. + */ + public String toString() + { + return String.format(". + */ + +package edu.wlu.cs.levy.breezyslam.components; + +/** + * A class representing the position of a robot. + */ +public class Position +{ + + /** + * 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; + } + + /** + * 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 = ""; + + return String.format(format, this.x_mm, this.y_mm, this.theta_degrees); + + } +} diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Scan.java b/java/edu/wlu/cs/levy/breezyslam/components/Scan.java new file mode 100644 index 0000000..9f898a8 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Scan.java @@ -0,0 +1,97 @@ +/** +* +* 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 + { + System.loadLibrary("jnibreezyslam_components"); + } + + private native void init( + int span, + int scan_size, + double scan_rate_hz, + double detection_angle_degrees, + double distance_no_detection_mm, + int detection_margin, + double offset_mm); + + private long native_ptr; + + public native String toString(); + + /** + * Returns a string representation of this Scan object. + */ + public native void update( + int [] lidar_mm, + double hole_width_mm, + double poseChange_dxy_mm, + double poseChange_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, + laser.scan_size, + laser.scan_rate_hz, + laser.detection_angle_degrees, + laser.distance_no_detection_mm, + laser.detection_margin, + laser.offset_mm); + } + + /** + * Builds a Scan object. + * @param laser laser parameters + * + */ + public Scan(Laser laser) + { + this(laser, 1); + } + + /** + * Updates this Scan object with new values from a Lidar scan. + * @param scanvals_mm scanned Lidar distance values in millimeters + * @param hole_width_millimeters hole width in millimeters + * @param poseChange forward velocity and angular velocity of robot at scan time + * + */ + public void update(int [] scanvals_mm, double hole_width_millimeters, PoseChange poseChange) + { + this.update(scanvals_mm, hole_width_millimeters, poseChange.dxy_mm, poseChange.dtheta_degrees); + } +} + diff --git a/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java b/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java new file mode 100644 index 0000000..053b174 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java @@ -0,0 +1,29 @@ +package edu.wlu.cs.levy.breezyslam.components; + +/** + * A class for the Hokuyo URG-04LX 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) + { + 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/jnibreezyslam_components.c b/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c new file mode 100644 index 0000000..6e4d8fa --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c @@ -0,0 +1,132 @@ +/* + * 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 . + */ + +#include "../jni_utils.h" + +#include "Map.h" +#include "Scan.h" + +#include +#include + +#define MAXSTR 100 + +// Map methods ----------------------------------------------------------------------------------------- + +JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_init (JNIEnv *env, jobject thisobject, jint size_pixels, jdouble size_meters) +{ + map_t * map = (map_t *)malloc(sizeof(map_t)); + map_init(map, (int)size_pixels, (double)size_meters); + ptr_to_obj(env, thisobject, map); +} + +JNIEXPORT jstring JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_toString (JNIEnv *env, jobject thisobject) +{ + map_t * map = cmap_from_jmap(env, thisobject); + + char str[MAXSTR]; + + map_string(*map, str); + + return (*env)->NewStringUTF(env, str); +} + +JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_get (JNIEnv *env, jobject thisobject, jbyteArray bytes) +{ + map_t * map = cmap_from_jmap(env, thisobject); + + jbyte * ptr = (*env)->GetByteArrayElements(env, bytes, NULL); + + map_get(map, ptr); + + (*env)->ReleaseByteArrayElements(env, bytes, ptr, 0); +} + +JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Map_update (JNIEnv *env, jobject thisobject, + jobject scanobject, + jdouble position_x_mm, + jdouble position_y_mm, + jdouble position_theta_degrees, + jint quality, + jdouble hole_width_mm) +{ + map_t * map = cmap_from_jmap(env, thisobject); + + scan_t * scan = cscan_from_jscan(env, scanobject); + + position_t position; + + position.x_mm = position_x_mm; + position.y_mm = position_y_mm; + position.theta_degrees = position_theta_degrees; + + map_update(map, scan, position, quality, hole_width_mm); +} + + +// Scan methods ----------------------------------------------------------------------------------------- + +JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_init (JNIEnv *env, jobject thisobject, + jint span, + jint scan_size, + jdouble scan_rate_hz, + jdouble detection_angle_degrees, + jdouble distance_no_detection_mm, + jint detection_margin, + jdouble offset_mm) + { + scan_t * scan = (scan_t *)malloc(sizeof(scan_t)); + + scan_init(scan, + span, + scan_size, + scan_rate_hz, + detection_angle_degrees, + distance_no_detection_mm, + detection_margin, + offset_mm); + + ptr_to_obj(env, thisobject, scan); +} + +JNIEXPORT jstring JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_toString (JNIEnv *env, jobject thisobject) +{ + scan_t * scan = cscan_from_jscan(env, thisobject); + + char str[MAXSTR]; + + scan_string(*scan, str); + + return (*env)->NewStringUTF(env, str); +} + +JNIEXPORT void JNICALL Java_edu_wlu_cs_levy_breezyslam_components_Scan_update (JNIEnv *env, jobject thisobject, + jintArray lidar_mm, + jdouble hole_width_mm, + jdouble velocities_dxy_mm, + jdouble velocities_dtheta_degrees) +{ + scan_t * scan = cscan_from_jscan(env, thisobject); + + jint * lidar_mm_c = (*env)->GetIntArrayElements(env, lidar_mm, 0); + + scan_update(scan, lidar_mm_c, hole_width_mm, velocities_dxy_mm, velocities_dtheta_degrees); + + (*env)->ReleaseIntArrayElements(env, lidar_mm, lidar_mm_c, 0); +} diff --git a/java/edu/wlu/cs/levy/breezyslam/jni_utils.h b/java/edu/wlu/cs/levy/breezyslam/jni_utils.h new file mode 100644 index 0000000..acaa3a5 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/jni_utils.h @@ -0,0 +1,64 @@ +/* + * 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 +#include + +static jfieldID get_fid(JNIEnv *env, jobject object, const char * fieldname, const char * fieldsig) +{ + jclass cls = (*env)->GetObjectClass(env, object); + return (*env)->GetFieldID(env, cls, fieldname, fieldsig); +} + +static jfieldID get_this_fid(JNIEnv *env, jobject thisobject) +{ + return get_fid(env, thisobject, "native_ptr", "J"); +} + +static void * ptr_from_obj(JNIEnv *env, jobject thisobject) +{ + return (void *)(*env)->GetLongField (env, thisobject, get_this_fid(env, thisobject)); +} + +static void ptr_to_obj(JNIEnv *env, jobject thisobject, void * ptr) +{ + (*env)->SetLongField (env, thisobject, get_this_fid(env, thisobject), (long)ptr); +} + +static scan_t * cscan_from_jscan(JNIEnv *env, jobject thisobject) +{ + return (scan_t *)ptr_from_obj(env, thisobject); +} + +static map_t * cmap_from_jmap(JNIEnv *env, jobject thisobject) +{ + return (map_t *)ptr_from_obj(env, thisobject); +} + +static double get_double_field(JNIEnv *env, jobject object, const char * fieldname) +{ + return (*env)->GetDoubleField (env, object, get_fid(env, object, fieldname, "D")); +} + + diff --git a/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java b/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java new file mode 100644 index 0000000..f2b2822 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java @@ -0,0 +1,130 @@ +/** + * + * 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 + * 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.robots; + +import edu.wlu.cs.levy.breezyslam.components.PoseChange; + + +/** + * An abstract class for wheeled robots. Supports computation of forward and angular + * poseChange based on odometry. Your subclass should should implement the + * extractOdometry method. + */ +public abstract class WheeledRobot +{ + + /** + * Builds a WheeledRobot object. Parameters should be based on the specifications for + * your robot. + * @param wheel_radius_mm radius of each odometry wheel, in meters + * @param half_axle_length_mm half the length of the axle between the odometry wheels, in meters + * @return a new WheeledRobot object + */ + protected WheeledRobot(double wheel_radius_mm, double half_axle_length_mm) + { + this.wheel_radius_mm = wheel_radius_mm; + this.half_axle_length_mm = half_axle_length_mm; + + this.timestamp_seconds_prev = 0; + this.left_wheel_degrees_prev = 0; + this.right_wheel_degrees_prev = 0; + } + + public String toString() + { + + return String.format("", + this.wheel_radius_mm, this.half_axle_length_mm, this.descriptorString()); + } + + /** + * Computes forward and angular poseChange based on odometry. + * @param timestamp time stamp, in whatever units your robot uses + * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses + * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses + * @return poseChange object representing poseChange for these odometry values + */ + + public PoseChange computePoseChange( double timestamp, double left_wheel_odometry, double right_wheel_odometry) + { + WheelOdometry odometry = this.extractOdometry(timestamp, left_wheel_odometry, right_wheel_odometry); + + double dxy_mm = 0; + double dtheta_degrees = 0; + double dt_seconds = 0; + + if (this.timestamp_seconds_prev > 0) + { + double left_diff_degrees = odometry.left_wheel_degrees - this.left_wheel_degrees_prev; + double right_diff_degrees = odometry.right_wheel_degrees - this.right_wheel_degrees_prev; + + dxy_mm = this.wheel_radius_mm * (java.lang.Math.toRadians(left_diff_degrees) + java.lang.Math.toRadians(right_diff_degrees)); + + dtheta_degrees = this.wheel_radius_mm / this.half_axle_length_mm * (right_diff_degrees - left_diff_degrees); + + dt_seconds = odometry.timestamp_seconds - this.timestamp_seconds_prev; + } + + // Store current odometry for next time + this.timestamp_seconds_prev = odometry.timestamp_seconds; + this.left_wheel_degrees_prev = odometry.left_wheel_degrees; + this.right_wheel_degrees_prev = odometry.right_wheel_degrees; + + return new PoseChange(dxy_mm, dtheta_degrees, dt_seconds); + } + + /** + * Extracts usable odometry values from your robot's odometry. + * @param timestamp time stamp, in whatever units your robot uses + * @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses + * @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses + * @return WheelOdometry object containing timestamp in seconds, left wheel degrees, right wheel degrees + */ + protected abstract WheelOdometry extractOdometry(double timestamp, double left_wheel_odometry, double right_wheel_odometry); + + protected abstract String descriptorString(); + + protected class WheelOdometry + { + public WheelOdometry(double timestamp_seconds, double left_wheel_degrees, double right_wheel_degrees) + { + this.timestamp_seconds = timestamp_seconds; + this.left_wheel_degrees = left_wheel_degrees; + this.right_wheel_degrees = right_wheel_degrees; + } + + public double timestamp_seconds; + public double left_wheel_degrees; + public double right_wheel_degrees; + + } + + + private double wheel_radius_mm; + private double half_axle_length_mm; + + private double timestamp_seconds_prev; + private double left_wheel_degrees_prev; + private double right_wheel_degrees_prev; +} diff --git a/matlab/CoreSLAM.m b/matlab/CoreSLAM.m new file mode 100644 index 0000000..ad17c74 --- /dev/null +++ b/matlab/CoreSLAM.m @@ -0,0 +1,136 @@ +classdef CoreSLAM + %CoreSLAM CoreSLAM abstract class for BreezySLAM + % CoreSLAM is an abstract class that uses the classes Position, + % Map, Scan, and Laser to run variants of the simple CoreSLAM + % (tinySLAM) algorithm described in + % + % @inproceedings{coreslam-2010, + % author = {Bruno Steux and Oussama El Hamzaoui}, + % title = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code}, + % booktitle = {11th International Conference on Control, Automation, + % Robotics and Vision, ICARCV 2010, Singapore, 7-10 + % December 2010, Proceedings}, + % pages = {1975-1979}, + % publisher = {IEEE}, + % year = {2010} + % } + % + % + % Implementing classes should provide the method + % + % updateMapAndPointcloud(scan_mm, velocities) + % + % to update the map and point-cloud (particle cloud). + % + % 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 . + + properties (Access = 'public') + map_quality = 50; % The quality of the map (0 through 255); default = 50 + hole_width_mm = 600; % The width in millimeters of each "hole" in the map (essentially, wall width); default = 600 + end + + properties (Access = 'protected') + laser % (internal) + scan_for_distance % (internal) + scan_for_mapbuild % (internal) + map % (internal) + velocities % (internal) + end + + methods (Abstract, Access = 'protected') + + updateMapAndPointcloud(slam, velocities) + + end + + methods (Access = 'private') + + function scan_update(slam, scanobj, scans_mm) + scanobj.update(scans_mm, slam.hole_width_mm, slam.velocities) + end + + end + + methods (Access = 'protected') + + function slam = CoreSLAM(laser, map_size_pixels, map_size_meters) + % Creates a CoreSLAM object suitable for updating with new Lidar and odometry data. + % CoreSLAM(laser, map_size_pixels, map_size_meters) + % laser is a Laser object representing the specifications of your Lidar unit + % map_size_pixels is the size of the square map in pixels + % map_size_meters is the size of the square map in meters + + % Store laser for later + slam.laser = laser; + + % Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry + slam.velocities = [0, 0, 0]; + + % Initialize a scan for computing distance to map, and one for updating map + slam.scan_for_distance = Scan(laser, 1); + slam.scan_for_mapbuild = Scan(laser, 3); + + % Initialize the map + slam.map = Map(map_size_pixels, map_size_meters); + + end + end + + methods (Access = 'public') + + function slam = update(slam, scans_mm, velocities) + % Updates the scan and odometry. + % Calls the the implementing class's updateMapAndPointcloud() + % method with the specified 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 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 + velocity_factor = 1 / velocities(3); + end + new_dxy_mm = velocities(1) * velocity_factor; + new_dtheta_degrees = velocities(2) * velocity_factor; + slam.velocities = [new_dxy_mm, new_dtheta_degrees, 0]; + + % Implementing class updates map and pointcloud + slam = slam.updateMapAndPointcloud(velocities); + + end + + function map = getmap(slam) + % Returns the current map. + % Map is returned as an occupancy grid (matrix of pixels). + map = slam.map.get(); + end + + end + +end + diff --git a/matlab/Deterministic_SLAM.m b/matlab/Deterministic_SLAM.m new file mode 100644 index 0000000..2b7c8e7 --- /dev/null +++ b/matlab/Deterministic_SLAM.m @@ -0,0 +1,44 @@ +classdef Deterministic_SLAM < SinglePositionSLAM + %Deterministic_SLAM SLAM with no Monte Carlo search + % Implements the getNewPosition() method of SinglePositionSLAM + % by copying the start 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 . + + methods + + function slam = Deterministic_SLAM(laser, map_size_pixels, map_size_meters) + %Creates a Deterministic_SLAM object suitable for updating with new Lidar and odometry data. + % slam = Deterministic_SLAM(laser, map_size_pixels, map_size_meters) + % laser is a Laser object representing the specifications of your Lidar unit + % map_size_pixels is the size of the square map in pixels + % map_size_meters is the size of the square map in meters + + slam = slam@SinglePositionSLAM(laser, map_size_pixels, map_size_meters); + + end + + function new_pos = getNewPosition(~, start_pos) + % Implements the _getNewPosition() method of SinglePositionSLAM. + % new_pos = getNewPosition(~, start_pos) simply returns start_pos + + new_pos = start_pos; + end + + end + +end + diff --git a/matlab/Map.m b/matlab/Map.m new file mode 100644 index 0000000..e3f960e --- /dev/null +++ b/matlab/Map.m @@ -0,0 +1,59 @@ +classdef Map + %A class for maps (occupancy grids) used in SLAM + % + % 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 . + + properties (Access = {?RMHC_SLAM}) + + c_map + end + + methods (Access = 'public') + + function map = Map(size_pixels, size_meters) + % Map creates an empty square map + % map = Map(size_pixels, size_meters) + map.c_map = mex_breezyslam('Map_init', size_pixels, size_meters); + + end + + function disp(map) + % Displays data about this map + mex_breezyslam('Map_disp', map.c_map) + + end + + function update(map, scan, new_position, map_quality, hole_width_mm) + % Updates this map with a new scan and position + % + % update(map, scan, new_position, map_quality, hole_width_mm) + mex_breezyslam('Map_update', map.c_map, scan.c_scan, new_position, int32(map_quality), hole_width_mm) + + end + + function bytes = get(map) + % Returns occupancy grid matrix of bytes for this map + % + % bytes = get(map) + + % Transpose for uniformity with Python, C++ versions + bytes = mex_breezyslam('Map_get', map.c_map)'; + + end + + end % methods + +end % classdef diff --git a/matlab/RMHC_SLAM.m b/matlab/RMHC_SLAM.m new file mode 100644 index 0000000..0062574 --- /dev/null +++ b/matlab/RMHC_SLAM.m @@ -0,0 +1,72 @@ +classdef RMHC_SLAM < SinglePositionSLAM + %RMHC_SLAM Random Mutation Hill-Climbing SLAM + % Implements the getNewPosition() method of SinglePositionSLAM + % using Random-Mutation Hill-Climbing search. Uses its own internal + % pseudorandom-number generator for efficiency. + % + % 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 . + + properties (Access = 'public') + sigma_xy_mm = 100; % std. dev. of X/Y component of search + sigma_theta_degrees = 20; % std. dev. of angular component of search + max_search_iter = 1000; % max. # of search iterations per update + end + + properties (Access = 'private') + c_randomizer + end + + methods + + function slam = RMHC_SLAM(laser, map_size_pixels, map_size_meters, random_seed) + %Creates an RMHC_SLAM object suitable for updating with new Lidar and odometry data. + % slam = RMHC_SLAM(laser, map_size_pixels, map_size_meters, random_seed) + % laser is a Laser object representing the specifications of your Lidar unit + % map_size_pixels is the size of the square map in pixels + % map_size_meters is the size of the square map in meters + % random_seed supports reproducible results; defaults to system time if unspecified + + slam = slam@SinglePositionSLAM(laser, map_size_pixels, map_size_meters); + + if nargin < 3 + random_seed = floor(cputime) & hex2dec('FFFF'); + end + + slam.c_randomizer = mex_breezyslam('Randomizer_init', random_seed); + + end + + function new_pos = getNewPosition(slam, start_pos) + % Implements the _getNewPosition() method of SinglePositionSLAM. + % Uses Random-Mutation Hill-Climbing search to look for a + % better position based on a starting position. + + [new_pos.x_mm,new_pos.y_mm,new_pos.theta_degrees] = ... + mex_breezyslam('rmhcPositionSearch', ... + start_pos, ... + slam.map.c_map, ... + slam.scan_for_distance.c_scan, ... + slam.laser,... + slam.sigma_xy_mm,... + slam.sigma_theta_degrees,... + slam.max_search_iter,... + slam.c_randomizer); + end + + end + +end + diff --git a/matlab/Scan.m b/matlab/Scan.m new file mode 100644 index 0000000..4004572 --- /dev/null +++ b/matlab/Scan.m @@ -0,0 +1,66 @@ +classdef Scan + %A class for Lidar scans used in SLAM + % + % 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 . + + properties (Access = {?Map, ?RMHC_SLAM}) + + c_scan + end + + methods + + function scan = Scan(laser, span) + % Creates a new scan object + % scan = Scan(laser, [span]) + % laser is a structure with the fields: + % scan_size + % scan_rate_hz + % detection_angle_degrees + % distance_no_detection_mm + % distance_no_detection_mm + % detection_margin + % 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 = mex_breezyslam('Scan_init', laser, span); + + end + + function disp(scan) + % Displays information about this Scan + + mex_breezyslam('Scan_disp', scan.c_scan) + + end + + function update(scan, scans_mm, hole_width_mm, velocities) + % Updates scan with new lidar and velocity data + % update(scans_mm, hole_width_mm, [velocities]) + % scans_mm is a list of integers representing scanned distances in mm. + % 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, int32(scans_mm), hole_width_mm, velocities) + end + + end + +end diff --git a/matlab/SinglePositionSLAM.m b/matlab/SinglePositionSLAM.m new file mode 100644 index 0000000..787753c --- /dev/null +++ b/matlab/SinglePositionSLAM.m @@ -0,0 +1,101 @@ +classdef SinglePositionSLAM < CoreSLAM + %SinglePositionSLAM Abstract class for CoreSLAM with single-point cloud + % Implementing classes should provide the method + % + % getNewPosition(self, start_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 . + + properties (Access = 'private') + position + end + + methods (Abstract) + getNewPosition(slam, start_pos) + end + + methods (Access = 'private') + + function c = costheta(slam) + c = cosd(slam.position.theta_degrees); + end + + function s = sintheta(slam) + s = sind(slam.position.theta_degrees); + end + + end + + methods (Access = 'public') + + function slam = SinglePositionSLAM(laser, map_size_pixels, map_size_meters) + + slam = slam@CoreSLAM(laser, map_size_pixels, map_size_meters); + + % Initialize the position (x, y, theta) + init_coord_mm = 500 * map_size_meters; % center of map + slam.position.x_mm = init_coord_mm; + slam.position.y_mm = init_coord_mm; + slam.position.theta_degrees = 0; + + end + + function [x_mm, y_mm, theta_degrees] = getpos(slam) + % Returns the current position. + % [x_mm, y_mm, theta_degrees] = getpos(slam) + + x_mm = slam.position.x_mm; + y_mm = slam.position.y_mm; + theta_degrees = slam.position.theta_degrees; + end + + end + + methods (Access = 'protected') + + function slam = updateMapAndPointcloud(slam, velocities) + + % Start at current position + start_pos = slam.position; + + % Add effect of velocities + start_pos.x_mm = start_pos.x_mm + velocities(1) * slam.costheta(); + start_pos.y_mm = start_pos.y_mm + velocities(1) * slam.sintheta(); + start_pos.theta_degrees = start_pos.theta_degrees + velocities(2); + + % Add offset from laser + start_pos.x_mm = start_pos.x_mm + slam.laser.offset_mm * slam.costheta(); + start_pos.y_mm = start_pos.y_mm + slam.laser.offset_mm * slam.sintheta(); + + % Get new position from implementing class + new_position = slam.getNewPosition(start_pos); + + % Update the map with this new position + slam.map.update(slam.scan_for_mapbuild, new_position, slam.map_quality, slam.hole_width_mm) + + % Update the current position with this new position, adjusted by laser offset + slam.position = new_position; + slam.position.x_mm = slam.position.x_mm - slam.laser.offset_mm * slam.costheta(); + slam.position.y_mm = slam.position.y_mm - slam.laser.offset_mm * slam.sintheta(); + + end + + end + +end + diff --git a/matlab/WheeledRobot.m b/matlab/WheeledRobot.m new file mode 100644 index 0000000..ac155c7 --- /dev/null +++ b/matlab/WheeledRobot.m @@ -0,0 +1,112 @@ +classdef WheeledRobot + %WheeledRobot An abstract class supporting ododmetry for wheeled robots. + % Your implementing class should provide the method: + % + % extractOdometry(obj, timestamp, leftWheel, rightWheel) --> + % (timestampSeconds, leftWheelDegrees, rightWheelDegrees) + % + % 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 . + + properties (Access = 'private') + + wheelRadiusMillimeters + halfAxleLengthMillimeters + + timestampSecondsPrev + leftWheelDegreesPrev + rightWheelDegreesPrev + + end + + methods (Access = 'protected') + + function s = str(obj) + % Returns a string representation of this WheeledRobot + s = sprintf('', ... + obj.wheelRadiusMillimeters, obj.halfAxleLengthMillimeters); + end + + function robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters) + % Constructs a WheeledRobot object + % robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters) + robot.wheelRadiusMillimeters = wheelRadiusMillimeters; + robot.halfAxleLengthMillimeters = halfAxleLengthMillimeters; + + end + + function r = deg2rad(~, d) + % Converts degrees to radians + r = d * pi / 180; + end + + end + + methods (Abstract) + + extractOdometry(obj, timestamp, leftWheel, rightWheel) + + end + + methods (Access = 'public') + + function [poseChange, obj] = computePoseChange(obj, timestamp, leftWheelOdometry, rightWheelOdometry) + % Computes forward and angular poseChange based on odometry. + % + % Parameters: + + % timestamp time stamp, in whatever units your robot uses + % leftWheelOdometry odometry for left wheel, in whatever form your robot uses + % rightWheelOdometry odometry for right wheel, in whatever form your robot uses + % + % Returns a tuple (dxyMillimeters, dthetaDegrees, dtSeconds) + + % dxyMillimeters forward distance traveled, in millimeters + % dthetaDegrees change in angular position, in degrees + % dtSeconds elapsed time since previous odometry, in seconds + + dxyMillimeters = 0; + dthetaDegrees = 0; + dtSeconds = 0; + + [timestampSecondsCurr, leftWheelDegreesCurr, rightWheelDegreesCurr] = ... + obj.extractOdometry(timestamp, leftWheelOdometry, rightWheelOdometry); + + if obj.timestampSecondsPrev + + leftDiffDegrees = leftWheelDegreesCurr - obj.leftWheelDegreesPrev; + rightDiffDegrees = rightWheelDegreesCurr - obj.rightWheelDegreesPrev; + + dxyMillimeters = obj.wheelRadiusMillimeters * ... + (obj.deg2rad(leftDiffDegrees) + obj.deg2rad(rightDiffDegrees)); + + dthetaDegrees = (obj.wheelRadiusMillimeters / obj.halfAxleLengthMillimeters) * ... + (rightDiffDegrees - leftDiffDegrees); + + dtSeconds = timestampSecondsCurr - obj.timestampSecondsPrev; + end + + % Store current odometry for next time + obj.timestampSecondsPrev = timestampSecondsCurr; + obj.leftWheelDegreesPrev = leftWheelDegreesCurr; + obj.rightWheelDegreesPrev = rightWheelDegreesCurr; + + % Return linear velocity, angular velocity, time difference + poseChange = [dxyMillimeters, dthetaDegrees, dtSeconds]; + + end + end +end + diff --git a/matlab/make.m b/matlab/make.m new file mode 100644 index 0000000..b00df47 --- /dev/null +++ b/matlab/make.m @@ -0,0 +1,19 @@ +% Script for building BreezySLAM C extensions + +% 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 . + + +mex mex_breezyslam.c ../c/coreslam.c ../c/coreslam_sisd.c ../c/random.c ../c/ziggurat.c diff --git a/matlab/mex_breezyslam.c b/matlab/mex_breezyslam.c new file mode 100644 index 0000000..b74cbf3 --- /dev/null +++ b/matlab/mex_breezyslam.c @@ -0,0 +1,294 @@ +/* + * mex_breezyslam.c : C extensions for BreezySLAM in Matlab + * + * Copyright (C) 2014 Simon D. Levy + * + * This code is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This code is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this code. If not, see . + */ + +#include "mex.h" + +#include "../c/coreslam.h" +#include "../c/random.h" + +#define MAXSTR 100 + +/* Helpers ------------------------------------------------------------- */ + +static int _streq(char * s, const char * t) +{ + return !strcmp(s, t); +} + +static void _insert_obj_lhs(mxArray *plhs[], void * obj, int pos) +{ + long * outptr = NULL; + + plhs[pos] = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL); + + outptr = (long *) mxGetPr(plhs[pos]); + + mexMakeMemoryPersistent(obj); + + *outptr = (long)obj; +} + +static double _get_field(const mxArray * pm, const char * fieldname) +{ + mxArray * field_array_ptr = mxGetField(pm, 0, fieldname); + + return mxGetScalar(field_array_ptr); +} + +static long _rhs2ptr(const mxArray * prhs[], int index) +{ + long * inptr = (long *) mxGetPr(prhs[index]); + + return *inptr; +} + +static scan_t * _rhs2scan(const mxArray * prhs[], int index) +{ + long inptr = _rhs2ptr(prhs, index); + + return (scan_t *)inptr; +} + +static map_t * _rhs2map(const mxArray * prhs[], int index) +{ + long inptr = _rhs2ptr(prhs, index); + + return (map_t *)inptr; +} + +static position_t _rhs2pos(const mxArray * prhs[], int index) +{ + position_t position; + + position.x_mm = _get_field(prhs[index], "x_mm"); + position.y_mm = _get_field(prhs[index], "y_mm"); + position.theta_degrees = _get_field(prhs[index], "theta_degrees"); + + return position; +} + +/* Class methods ------------------------------------------------------- */ + +static void _map_init(mxArray *plhs[], const mxArray * prhs[]) +{ + + int size_pixels = (int)mxGetScalar(prhs[1]); + + double size_meters = mxGetScalar(prhs[2]); + + map_t * map = (map_t *)mxMalloc(sizeof(map_t)); + + map_init(map, size_pixels, size_meters); + + _insert_obj_lhs(plhs, map, 0); +} + +static void _map_disp(const mxArray * prhs[]) +{ + char str[MAXSTR]; + + map_t * map = _rhs2map(prhs, 1); + + map_string(*map, str); + + printf("%s\n", str); +} + +static void _map_update(const mxArray * prhs[]) +{ + map_t * map = _rhs2map(prhs, 1); + + scan_t * scan = _rhs2scan(prhs, 2); + + position_t position = _rhs2pos(prhs, 3); + + int map_quality = (int)mxGetScalar(prhs[4]); + + double hole_width_mm = mxGetScalar(prhs[5]); + + map_update(map, scan, position, map_quality, hole_width_mm); +} + +static void _map_get(mxArray *plhs[], const mxArray * prhs[]) +{ + map_t * map = _rhs2map(prhs, 1); + + unsigned char * pointer = NULL; + + plhs[0] = mxCreateNumericMatrix(map->size_pixels, map->size_pixels, + mxUINT8_CLASS, mxREAL); + + pointer = (unsigned char *)mxGetPr(plhs[0]); + + map_get(map, pointer); +} + + +static void _scan_init(mxArray *plhs[], const mxArray * prhs[]) +{ + + scan_t * scan = (scan_t *)mxMalloc(sizeof(scan_t)); + + int span = (int)mxGetScalar(prhs[2]); + + 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); +} + +static void _scan_disp(const mxArray * prhs[]) +{ + char str[MAXSTR]; + + scan_t * scan = _rhs2scan(prhs, 1); + + scan_string(*scan, str); + + printf("%s\n", str); +} + +static void _scan_update(const mxArray * prhs[]) +{ + scan_t * scan = _rhs2scan(prhs, 1); + + int scansize = (int)mxGetNumberOfElements(prhs[2]); + + int * lidar_mm = (int *)mxGetPr(prhs[2]); + + double hole_width_mm = mxGetScalar(prhs[3]); + + double * velocities = mxGetPr(prhs[4]); + + scan_update(scan, lidar_mm, hole_width_mm, velocities[0], velocities[1]); +} + +static void _randomizer_init(mxArray *plhs[], const mxArray * prhs[]) +{ + int seed = (int)mxGetScalar(prhs[1]); + + void * r = mxMalloc(random_size()); + + random_init(r, seed); + + _insert_obj_lhs(plhs, r, 0); +} + +static void _rmhcPositionSearch(mxArray *plhs[], const mxArray * prhs[]) +{ + position_t start_pos = _rhs2pos(prhs, 1); + + map_t * map = _rhs2map(prhs, 2); + + scan_t * scan = _rhs2scan(prhs, 3); + + position_t new_pos; + + double sigma_xy_mm = mxGetScalar(prhs[5]); + + double sigma_theta_degrees = mxGetScalar(prhs[6]); + + int max_search_iter = (int)mxGetScalar(prhs[7]); + + void * randomizer = (void *)(long)mxGetScalar(prhs[8]); + + new_pos = rmhc_position_search( + start_pos, + map, + scan, + sigma_xy_mm, + sigma_theta_degrees, + max_search_iter, + randomizer); + + plhs[0] = mxCreateDoubleScalar(new_pos.x_mm); + plhs[1] = mxCreateDoubleScalar(new_pos.y_mm); + plhs[2] = mxCreateDoubleScalar(new_pos.theta_degrees); +} + +/* The gateway function ------------------------------------------------ */ +void mexFunction( int nlhs, mxArray *plhs[], + int nrhs, const mxArray * prhs[]) +{ + + char methodname[MAXSTR]; + + mxGetString(prhs[0], methodname, 100); + + if (_streq(methodname, "Map_init")) + { + _map_init(plhs, prhs); + } + + else if (_streq(methodname, "Map_disp")) + { + _map_disp(prhs); + } + + else if (_streq(methodname, "Map_update")) + { + _map_update(prhs); + } + + else if (_streq(methodname, "Map_get")) + { + _map_get(plhs, prhs); + } + + else if (_streq(methodname, "Scan_init")) + { + _scan_init(plhs, prhs); + } + + else if (_streq(methodname, "Scan_disp")) + { + _scan_disp(prhs); + } + + else if (_streq(methodname, "Scan_update")) + { + _scan_update(prhs); + } + + else if (_streq(methodname, "Randomizer_init")) + { + _randomizer_init(plhs, prhs); + } + + else if (_streq(methodname, "rmhcPositionSearch")) + { + _rmhcPositionSearch(plhs, prhs); + } +} + diff --git a/matlab/mex_breezyslam.mexa64 b/matlab/mex_breezyslam.mexa64 new file mode 100755 index 0000000..0e94256 Binary files /dev/null and b/matlab/mex_breezyslam.mexa64 differ diff --git a/matlab/mex_breezyslam.mexmaci64 b/matlab/mex_breezyslam.mexmaci64 new file mode 100644 index 0000000..97fc14b Binary files /dev/null and b/matlab/mex_breezyslam.mexmaci64 differ diff --git a/matlab/mex_breezyslam.mexw64 b/matlab/mex_breezyslam.mexw64 new file mode 100644 index 0000000..28bdc0a Binary files /dev/null and b/matlab/mex_breezyslam.mexw64 differ