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

Added Java support.

Browse Source
This commit is contained in:
Simon D. Levy
2014-10-26 17:46:28 -04:00
parent 345b3e75d4
commit 721f75e2af
19 changed files with 1226 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

133
java/edu/wlu/cs/levy/breezyslam/algorithms/CoreSLAM.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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
* <pre>
* @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}
* }
* </pre>
* 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 <tt>scan_size</tt>
* 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);
}
}

36
java/edu/wlu/cs/levy/breezyslam/algorithms/DeterministicSLAM.java Normal file
View File

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

73
java/edu/wlu/cs/levy/breezyslam/algorithms/Makefile Normal file
View File

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

88
java/edu/wlu/cs/levy/breezyslam/algorithms/RMHCSLAM.java Normal file
View File

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

103
java/edu/wlu/cs/levy/breezyslam/algorithms/SinglePositionSLAM.java Normal file
View File

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

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

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

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

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

47
java/edu/wlu/cs/levy/breezyslam/algorithms/jnibreezyslam_algorithms.c Normal file
View File

@@ -0,0 +1,47 @@
#include "../../../../../../../c/random.h"
#include "../jni_utils.h"
#include <jni.h>
// 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, "<init>", "(DDD)V");
jobject newpos_object = (*env)->NewObject(env, cls, constructor, newpos.x_mm, newpos.y_mm, newpos.theta_degrees);
return newpos_object;
}

58
java/edu/wlu/cs/levy/breezyslam/components/Laser.java Normal file
View File

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

72
java/edu/wlu/cs/levy/breezyslam/components/Makefile Normal file
View File

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

58
java/edu/wlu/cs/levy/breezyslam/components/Map.java Normal file
View File

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

38
java/edu/wlu/cs/levy/breezyslam/components/Position.java Normal file
View File

@@ -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 = "<x = %7.0f mm y = %7.0f mm theta = %+3.3f degrees>";
String format = "<x = %f mm y = %f mm theta = %f degrees>";
return String.format(format, this.x_mm, this.y_mm, this.theta_degrees);
}
public static void main(String[] argv)
{
Position position = new Position(300, 400, 120);
System.out.println(position);
}
}

58
java/edu/wlu/cs/levy/breezyslam/components/Scan.java Normal file
View File

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

15
java/edu/wlu/cs/levy/breezyslam/components/URG04LX.java Normal file
View File

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

68
java/edu/wlu/cs/levy/breezyslam/components/Velocities.java Normal file
View File

@@ -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("<dxy=%7.0f mm dtheta = %+3.3f degrees dt = %f s",
this.dxy_mm, this.dtheta_degrees, this.dt_seconds);
}
public double getDxyMm()
{
return this.dxy_mm;
}
public double getDthetaDegrees()
{
return this.dtheta_degrees;
}
public double getDtSeconds()
{
return this.dt_seconds;
}
protected double dxy_mm;
protected double dtheta_degrees;
protected double dt_seconds;
}

113
java/edu/wlu/cs/levy/breezyslam/components/jnibreezyslam_components.c Normal file
View File

@@ -0,0 +1,113 @@
#include "../jni_utils.h"
#include "Map.h"
#include "Scan.h"
#include <jni.h>
#include <stdlib.h>
#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);
}

42
java/edu/wlu/cs/levy/breezyslam/jni_utils.h Normal file
View File

@@ -0,0 +1,42 @@
#include "../../../../../../c/coreslam.h"
#include <jni.h>
#include <stdlib.h>
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"));
}

13
java/edu/wlu/cs/levy/breezyslam/robots/Makefile Normal file
View File

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

130
java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java Normal file
View File

@@ -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 <http://www.gnu.org/licenses/>.
*/
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("<Wheel radius=%f m Half axle Length=%f m | %s>",
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;
}