Restored Matlab, Java

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 <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.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 
+*    <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, 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 <tt>scan_size</tt> 
+    * 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);
+    }
+
+}

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 <http://www.gnu.org/licenses/>.
+*/
+
+
+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 

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 <http://www.gnu.org/licenses/>.
+
+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

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 <http://www.gnu.org/licenses/>.
+*/
+
+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

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 <http://www.gnu.org/licenses/>.
+*/
+
+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();
+    }
+}

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 <http:#www.gnu.org/licenses/>.
+ */
+
+
+#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;
+} 

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 <http://www.gnu.org/licenses/>.
+*/
+
+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;
+     }
+}

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 <http://www.gnu.org/licenses/>.
+
+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 *~

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 <http://www.gnu.org/licenses/>.
+*/
+
+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;
+    }
+
+}

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 <http://www.gnu.org/licenses/>.
+*/
+
+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("<dxy=%7.0f mm dtheta = %+3.3f degrees dt = %f s",
+            this.dxy_mm, this.dtheta_degrees, this.dt_seconds);
+    }
+
+    /**
+      * Returns the forward component of this PoseChange object.
+      */
+    public double getDxyMm()
+    {
+        return this.dxy_mm;
+    }
+
+    /**
+      * Returns the angular component of this PoseChange object.
+      */
+     public double getDthetaDegrees()
+    {
+        return this.dtheta_degrees;
+    }
+
+    /**
+      * Returns the time component of this PoseChange object.
+      */
+    public double getDtSeconds()
+    {
+        return this.dt_seconds;
+    }
+
+    /**
+      * Forward component of velocity, in mm to be divided by time in seconds.
+      */
+    protected double dxy_mm;
+
+    /**
+      * Angular component of velocity, in mm to be divided by time in seconds.
+      */
+
+     protected double dtheta_degrees;
+
+    /**
+      *  Time in seconds between successive velocity measurements.
+      */
+     protected double dt_seconds;
+}

java/edu/wlu/cs/levy/breezyslam/components/Position.java

@@ -0,0 +1,81 @@
+/**
+ * 
+ * BreezySLAM: Simple, efficient SLAM in Java
+ *
+ * Position.java - Java code for Position class
+ *
+ * Copyright (C) 2014 Simon D. Levy
+ *
+ * This code is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as 
+ * published by the Free Software Foundation, either version 3 of the 
+ * License, or (at your option) any later version.
+ * 
+ * This code is distributed in the hope that it will be useful,     
+ * but WITHOUT ANY WARRANTY without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ * 
+ * You should have received a copy of the GNU Lesser General Public License 
+ * along with this code.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+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 = "<x = %7.0f mm  y = %7.0f mm theta = %+3.3f degrees>";
+
+        return String.format(format, this.x_mm, this.y_mm, this.theta_degrees);
+
+    }
+}

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

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

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 <http:#www.gnu.org/licenses/>.
+ */
+
+#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);
+}

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 <http:#www.gnu.org/licenses/>.
+ */
+
+
+#include <stdlib.h>
+#include <stdio.h>
+#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"));
+}
+
+

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 <http://www.gnu.org/licenses/>.
+ */
+
+
+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("<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 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;    
+}