From 721f75e2afa494af875ece496b33cfafed6d0db2 Mon Sep 17 00:00:00 2001 From: "Simon D. Levy" Date: Sun, 26 Oct 2014 17:46:28 -0400 Subject: [PATCH] Added Java support. --- .../levy/breezyslam/algorithms/CoreSLAM.java | 133 ++++++++++++++++++ .../algorithms/DeterministicSLAM.java | 36 +++++ .../cs/levy/breezyslam/algorithms/Makefile | 73 ++++++++++ .../levy/breezyslam/algorithms/RMHCSLAM.java | 88 ++++++++++++ .../algorithms/SinglePositionSLAM.java | 103 ++++++++++++++ .../breezyslam/algorithms/bak/CoreSLAM.java | 66 +++++++++ .../levy/breezyslam/algorithms/bak/Makefile | 15 ++ .../algorithms/jnibreezyslam_algorithms.c | 47 +++++++ .../cs/levy/breezyslam/components/Laser.java | 58 ++++++++ .../cs/levy/breezyslam/components/Makefile | 72 ++++++++++ .../cs/levy/breezyslam/components/Map.java | 58 ++++++++ .../levy/breezyslam/components/Position.java | 38 +++++ .../cs/levy/breezyslam/components/Scan.java | 58 ++++++++ .../levy/breezyslam/components/URG04LX.java | 15 ++ .../breezyslam/components/Velocities.java | 68 +++++++++ .../components/jnibreezyslam_components.c | 113 +++++++++++++++ java/edu/wlu/cs/levy/breezyslam/jni_utils.h | 42 ++++++ .../wlu/cs/levy/breezyslam/robots/Makefile | 13 ++ .../levy/breezyslam/robots/WheeledRobot.java | 130 +++++++++++++++++ 19 files changed, 1226 insertions(+) create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/bak/CoreSLAM.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/bak/Makefile create mode 100644 java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Laser.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Makefile create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Map.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Position.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Scan.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/Velocities.java create mode 100644 java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c create mode 100644 java/edu/wlu/cs/levy/breezyslam/jni_utils.h create mode 100644 java/edu/wlu/cs/levy/breezyslam/robots/Makefile create mode 100644 java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java 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..0f58bfb --- /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.Velocities; +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, Velocities & velocities) +* +* 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 Velocities velocities; + + 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 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); + } + + 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.velocities); + } + + + 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.getDxyMm(), velocities.getDthetaDegrees(), velocities.getDtSeconds()); + + // Implementing class updates map and pointcloud + this.updateMapAndPointcloud(velocities); + } + + /** + * Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero velocities + * (no odometry). + * @param scan_mm Lidar scan values, whose count is specified in the scan_size + * attribute of the Laser object passed to the CoreSlam constructor + */ + public void update(int [] scan_mm) + { + Velocities zero_velocities = new Velocities(); + + this.update(scan_mm, zero_velocities); + } + + + protected abstract void updateMapAndPointcloud(Velocities velocities); + + 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..d0eca8a --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java @@ -0,0 +1,36 @@ +/** +* DeterministicSLAM implements SinglePositionSLAM using by returning the starting position instead of searching +* on it; i.e., using odometry alone. +*/ + +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..745d115 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile @@ -0,0 +1,73 @@ +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..62b5815 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java @@ -0,0 +1,88 @@ +/** +* RMHCSLAM implements SinglePositionSLAM using random-mutation hill-climbing search on the starting position. +*/ + +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.Velocities; +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..80b0924 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java @@ -0,0 +1,103 @@ +/** +* 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. +*/ + +package edu.wlu.cs.levy.breezyslam.algorithms; + +import edu.wlu.cs.levy.breezyslam.components.Position; +import edu.wlu.cs.levy.breezyslam.components.Velocities; +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 velocities velocities for odometry + */ + protected void updateMapAndPointcloud(Velocities velocities) + { + // Start at current position + Position start_pos = new Position(this.position); + + // Add effect of velocities + start_pos.x_mm += velocities.getDxyMm() * this.costheta(); + start_pos.y_mm += velocities.getDxyMm() * this.sintheta(); + start_pos.theta_degrees += velocities.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/bak/CoreSLAM.java b/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/CoreSLAM.java new file mode 100644 index 0000000..b8481d4 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/CoreSLAM.java @@ -0,0 +1,66 @@ +package edu.wlu.cs.levy.breezyslam.algorithms; + +import edu.wlu.cs.levy.breezyslam.components.Laser; +import edu.wlu.cs.levy.breezyslam.components.Velocities; + +public class CoreSLAM { + + public CoreSLAM(Laser laser, int map_size_pixels, double map_size_meters) + { + /* + // Set default params + this->map_quality = DEFAULT_MAP_QUALITY; + this->hole_width_mm = DEFAULT_HOLE_WIDTH_MM; + + // Store laser for later + this->laser = new Laser(laser); + + // Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry + this->velocities = new Velocities(); + + // Initialize a scan for computing distance to map, and one for updating map + this->scan_for_mapbuild = this->scan_create(3); + this->scan_for_distance = this->scan_create(1); + + // Initialize the map + this->map = new Map(map_size_pixels, map_size_meters); + */ + } + + + public void update(int [] scan_mm, Velocities velocities) + { + /* + // Build a scan for computing distance to map, and one for updating map + this->scan_update(this->scan_for_mapbuild, scan_mm); + this->scan_update(this->scan_for_distance, scan_mm); + + // Update velocities + this->velocities->update(velocities.dxy_mm, + velocities.dtheta_degrees, + velocities.dt_seconds); + + // Implementing class updates map and pointcloud + this->updateMapAndPointcloud(velocities); + */ + } + + + public void getmap(byte [] mapbytes) + { + //this->map->get((char *)mapbytes); + } + + /* + Scan * CoreSLAM::scan_create(int span) + { + //return new Scan(this->laser, span); + } + + + void scan_update(Scan scan, int [] scan_mm) + { + //scan->update(scan_mm, this->hole_width_mm, *this->velocities); + } + */ +} diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/Makefile b/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/Makefile new file mode 100644 index 0000000..8918ed4 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/bak/Makefile @@ -0,0 +1,15 @@ +JAVADIR = ../../../../../.. + +ALL = CoreSLAM.class + +all: $(ALL) + +CoreSLAM.class: CoreSLAM.java + javac -classpath $(JAVADIR):. CoreSLAM.java + +clean: + rm -f *.class *~ + +backup: + cp *.java bak + cp Makefile bak diff --git a/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c b/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c new file mode 100644 index 0000000..db97d3d --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c @@ -0,0 +1,47 @@ +#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..d1f4b23 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Laser.java @@ -0,0 +1,58 @@ +package edu.wlu.cs.levy.breezyslam.components; + +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; + + 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; + } + + 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; + } + + 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); + + } + + 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..d037b4c --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Makefile @@ -0,0 +1,72 @@ +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 Velocities.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 + +Velocities.class: Velocities.java + javac $(JFLAGS) Velocities.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..22e1da8 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Map.java @@ -0,0 +1,58 @@ +package edu.wlu.cs.levy.breezyslam.components; + +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; + + public native String toString(); + + 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. + */ + 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); + } + + public double sizeMeters() + { + return this.size_meters; + } + +} diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Position.java b/java/edu/wlu/cs/levy/breezyslam/components/Position.java new file mode 100644 index 0000000..862a55e --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Position.java @@ -0,0 +1,38 @@ +package edu.wlu.cs.levy.breezyslam.components; + +public class Position +{ + + public double x_mm; + public double y_mm; + public double theta_degrees; + + public Position(double x_mm, double y_mm, double theta_degrees) + { + this.x_mm = x_mm; + this.y_mm = y_mm; + this.theta_degrees = theta_degrees; + } + + public Position(Position position) + { + this.x_mm = position.x_mm; + this.y_mm = position.y_mm; + this.theta_degrees = position.theta_degrees; + } + + public String toString() + { + //String format = ""; + String format = ""; + + return String.format(format, this.x_mm, this.y_mm, this.theta_degrees); + + } + + public static void main(String[] argv) + { + Position position = new Position(300, 400, 120); + System.out.println(position); + } +} diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Scan.java b/java/edu/wlu/cs/levy/breezyslam/components/Scan.java new file mode 100644 index 0000000..b14a3dc --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Scan.java @@ -0,0 +1,58 @@ +package edu.wlu.cs.levy.breezyslam.components; + +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(); + + public native void update( + int [] lidar_mm, + double hole_width_mm, + double velocities_dxy_mm, + double velocities_dtheta_degrees); + + + 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); + } + + 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 velocities forward velocity and angular velocity of robot at scan time + * + */ + public void update(int [] scanvals_mm, double hole_width_millimeters, Velocities velocities) + { + this.update(scanvals_mm, hole_width_millimeters, velocities.dxy_mm, velocities.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..4696d47 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java @@ -0,0 +1,15 @@ +package edu.wlu.cs.levy.breezyslam.components; + +public class URG04LX extends Laser +{ + + public URG04LX(int detection_margin, double offset_mm) + { + super(682, 10, 240, 4000, detection_margin, offset_mm); + } + + public URG04LX() + { + this(0, 0); + } +} diff --git a/java/edu/wlu/cs/levy/breezyslam/components/Velocities.java b/java/edu/wlu/cs/levy/breezyslam/components/Velocities.java new file mode 100644 index 0000000..76e2855 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/components/Velocities.java @@ -0,0 +1,68 @@ +package edu.wlu.cs.levy.breezyslam.components; + +/** +* A class representing the forward and angular velocities of a robot. +*/ +public class Velocities +{ + + /** + * Creates a new Velocities object with specified velocities. + */ + public Velocities(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 Velocities object with zero velocities. + */ + public Velocities() + { + this.dxy_mm = 0; + this.dtheta_degrees = 0; + this.dt_seconds = 0; + } + + /** + * Updates this Velocities 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; + } + + public String toString() + { + return String.format(" +#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..079875a --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/jni_utils.h @@ -0,0 +1,42 @@ +#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/Makefile b/java/edu/wlu/cs/levy/breezyslam/robots/Makefile new file mode 100644 index 0000000..f97b0d9 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/robots/Makefile @@ -0,0 +1,13 @@ +BASEDIR = ../../../../../../.. +JAVADIR = $(BASEDIR)/java +JFLAGS = -Xlint + +ALL = WheeledRobot.class + +all: $(ALL) + +WheeledRobot.class: WheeledRobot.java + javac $(JFLAGS) -classpath $(JAVADIR) WheeledRobot.java + +clean: + rm -f *.class *~ 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..8ccc871 --- /dev/null +++ b/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java @@ -0,0 +1,130 @@ +/** + * + * BreezySLAM: Simple, efficient SLAM in C++ + * + * 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.Velocities; + + +/** + * An abstract class for wheeled robots. Supports computation of forward and angular + * velocities 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 velocities 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 velocities object representing velocities for these odometry values + */ + + public Velocities computeVelocities( 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 Velocities(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; +}