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

Velocities => PoseChange

Browse Source
This commit is contained in:
Simon D. Levy
2018-06-10 11:52:58 -04:00
parent 4d1cdb0461
commit 52b582612a
7 changed files with 46 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -22,7 +22,7 @@
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.PoseChange;
import edu.wlu.cs.levy.breezyslam.components.Map;
import edu.wlu.cs.levy.breezyslam.components.Scan;
@@ -43,7 +43,7 @@ import edu.wlu.cs.levy.breezyslam.components.Scan;
* </pre>
* Implementing classes should provide the method
*
* void updateMapAndPointcloud(int * scan_mm, Velocities & velocities)
* void updateMapAndPointcloud(int * scan_mm, PoseChange & poseChange)
*
* to update the map and point-cloud (particle cloud).
*
@@ -62,7 +62,7 @@ public abstract class CoreSLAM {
protected Laser laser;
protected Velocities velocities;
protected PoseChange poseChange;
protected Map map;
@@ -74,8 +74,8 @@ public abstract class CoreSLAM {
// Set default params
this.laser = new Laser(laser);
// Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry
this.velocities = new Velocities();
// 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);
@@ -92,38 +92,38 @@ public abstract class CoreSLAM {
private void scan_update(Scan scan, int [] scan_mm)
{
scan.update(scan_mm, this.hole_width_mm, this.velocities);
scan.update(scan_mm, this.hole_width_mm, this.poseChange);
}
public void update(int [] scan_mm, Velocities velocities)
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 velocities
this.velocities.update(velocities.getDxyMm(), velocities.getDthetaDegrees(), velocities.getDtSeconds());
// Update poseChange
this.poseChange.update(poseChange.getDxyMm(), poseChange.getDthetaDegrees(), poseChange.getDtSeconds());
// Implementing class updates map and pointcloud
this.updateMapAndPointcloud(velocities);
this.updateMapAndPointcloud(poseChange);
}
/**
* Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero velocities
* 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)
{
Velocities zero_velocities = new Velocities();
PoseChange zero_poseChange = new PoseChange();
this.update(scan_mm, zero_velocities);
this.update(scan_mm, zero_poseChange);
}
protected abstract void updateMapAndPointcloud(Velocities velocities);
protected abstract void updateMapAndPointcloud(PoseChange poseChange);
public void getmap(byte [] mapbytes)
{

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

@@ -21,7 +21,7 @@ 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.PoseChange;
import edu.wlu.cs.levy.breezyslam.components.Map;
import edu.wlu.cs.levy.breezyslam.components.Scan;

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

@@ -25,7 +25,7 @@
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.PoseChange;
import edu.wlu.cs.levy.breezyslam.components.Laser;
@@ -57,17 +57,17 @@ public abstract class SinglePositionSLAM extends CoreSLAM
/**
* Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM::update()
* @param velocities velocities for odometry
* @param poseChange poseChange for odometry
*/
protected void updateMapAndPointcloud(Velocities velocities)
protected void updateMapAndPointcloud(PoseChange poseChange)
{
// 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 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();

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

@@ -43,7 +43,7 @@ else
ARCH = sisd
endif
ALL = libjnibreezyslam_components.$(LIBEXT) Laser.class Position.class Velocities.class URG04LX.class
ALL = libjnibreezyslam_components.$(LIBEXT) Laser.class Position.class PoseChange.class URG04LX.class
all: $(ALL)
@@ -78,8 +78,8 @@ Laser.class: Laser.java
URG04LX.class: URG04LX.java Laser.class
javac $(JFLAGS) -classpath $(JAVADIR) URG04LX.java
Velocities.class: Velocities.java
javac $(JFLAGS) Velocities.java
PoseChange.class: PoseChange.java
javac $(JFLAGS) PoseChange.java
Position.class: Position.java

23
java/edu/wlu/cs/levy/breezyslam/components/Velocities.java → java/edu/wlu/cs/levy/breezyslam/components/PoseChange.java
View File

@@ -2,7 +2,8 @@
*
* BreezySLAM: Simple, efficient SLAM in Java
*
* Velocities.java - Java code for Velocities class
* PoseChange.java - Java code for PoseChange class, encoding triple
* (dxy_mm, dtheta_degrees, dt_seconds)
*
* Copyright (C) 2014 Simon D. Levy
*
@@ -25,13 +26,13 @@ package edu.wlu.cs.levy.breezyslam.components;
/**
* A class representing the forward and angular velocities of a robot as determined by odometry.
*/
public class Velocities
public class PoseChange
{
/**
* Creates a new Velocities object with specified velocities.
* Creates a new PoseChange object with specified velocities.
*/
public Velocities(double dxy_mm, double dtheta_degrees, double dtSeconds)
public PoseChange(double dxy_mm, double dtheta_degrees, double dtSeconds)
{
this.dxy_mm = dxy_mm;
this.dtheta_degrees = dtheta_degrees;
@@ -39,9 +40,9 @@ public class Velocities
}
/**
* Creates a new Velocities object with zero velocities.
* Creates a new PoseChange object with zero velocities.
*/
public Velocities()
public PoseChange()
{
this.dxy_mm = 0;
this.dtheta_degrees = 0;
@@ -49,7 +50,7 @@ public class Velocities
}
/**
* Updates this Velocities object.
* 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
@@ -64,7 +65,7 @@ public class Velocities
}
/**
* Returns a string representation of this Velocities object.
* Returns a string representation of this PoseChange object.
*/
public String toString()
{
@@ -73,7 +74,7 @@ public class Velocities
}
/**
* Returns the forward component of this Velocities object.
* Returns the forward component of this PoseChange object.
*/
public double getDxyMm()
{
@@ -81,7 +82,7 @@ public class Velocities
}
/**
* Returns the angular component of this Velocities object.
* Returns the angular component of this PoseChange object.
*/
public double getDthetaDegrees()
{
@@ -89,7 +90,7 @@ public class Velocities
}
/**
* Returns the time component of this Velocities object.
* Returns the time component of this PoseChange object.
*/
public double getDtSeconds()
{

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

@@ -51,8 +51,8 @@ public class Scan
public native void update(
int [] lidar_mm,
double hole_width_mm,
double velocities_dxy_mm,
double velocities_dtheta_degrees);
double poseChange_dxy_mm,
double poseChange_dtheta_degrees);
/**
@@ -86,12 +86,12 @@ public class Scan
* 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
* @param poseChange forward velocity and angular velocity of robot at scan time
*
*/
public void update(int [] scanvals_mm, double hole_width_millimeters, Velocities velocities)
public void update(int [] scanvals_mm, double hole_width_millimeters, PoseChange poseChange)
{
this.update(scanvals_mm, hole_width_millimeters, velocities.dxy_mm, velocities.dtheta_degrees);
this.update(scanvals_mm, hole_width_millimeters, poseChange.dxy_mm, poseChange.dtheta_degrees);
}
}