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

Use "pose change" instead of "velocities" for (dxy_mm, dtheta_degrees, dt_seconds)

Browse Source
This commit is contained in:
Simon D. Levy
2018-06-03 14:38:07 -04:00
parent 8934a3370f
commit 081d33aedf
23 changed files with 113 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored
View File

@@ -3,10 +3,11 @@ examples/*.pyc
examples/*.pgm
examples/*.png
examples/*.map
examples/log2pgm
python/breezyslam/*.pyc
python/breezyslam/__pycache__
python/build
cpp/*.o
cpp/*.so
*.o
*.so
*.swp

4
cpp/Makefile
View File

@@ -50,11 +50,11 @@ libbreezyslam.$(LIBEXT): algorithms.o Scan.o Map.o WheeledRobot.o \
coreslam.o coreslam_$(ARCH).o random.o ziggurat.o \
-o libbreezyslam.$(LIBEXT) -lm
algorithms.o: algorithms.cpp algorithms.hpp Laser.hpp Position.hpp Map.hpp Scan.hpp Velocities.hpp \
algorithms.o: algorithms.cpp algorithms.hpp Laser.hpp Position.hpp Map.hpp Scan.hpp PoseChange.hpp \
WheeledRobot.hpp ../c/coreslam.h
g++ -O3 -I../c -c -Wall $(CFLAGS) algorithms.cpp
Scan.o: Scan.cpp Scan.hpp Velocities.hpp Laser.hpp ../c/coreslam.h
Scan.o: Scan.cpp Scan.hpp PoseChange.hpp Laser.hpp ../c/coreslam.h
g++ -O3 -I../c -c -Wall $(CFLAGS) Scan.cpp
Map.o: Map.cpp Map.hpp Position.hpp Scan.hpp ../c/coreslam.h

26
cpp/Velocities.hpp → cpp/PoseChange.hpp
View File

@@ -1,6 +1,6 @@
/**
*
* Velocities.hpp - C++ header for Velocities class
* PoseChange.hpp - C++ header for PoseChange class
*
* Copyright (C) 2014 Simon D. Levy
@@ -27,18 +27,18 @@ using namespace std;
/**
* A class representing the forward and angular velocities of a robot.
* A class representing the forward and angular poseChange of a robot.
*/
class Velocities
class PoseChange
{
friend class Scan;
public:
/**
* Creates a new Velocities object with specified velocities.
* Creates a new PoseChange object with specified poseChange.
*/
Velocities(double dxy_mm, double dtheta_degrees, double dtSeconds)
PoseChange(double dxy_mm, double dtheta_degrees, double dtSeconds)
{
this->dxy_mm = dxy_mm;
this->dtheta_degrees = dtheta_degrees;
@@ -46,9 +46,9 @@ Velocities(double dxy_mm, double dtheta_degrees, double dtSeconds)
}
/**
* Creates a new Velocities object with zero velocities.
* Creates a new PoseChange object with zero poseChange.
*/
Velocities(void)
PoseChange(void)
{
this->dxy_mm = 0;
this->dtheta_degrees = 0;
@@ -58,10 +58,10 @@ Velocities(void)
/**
* 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
* @param dtSeconds time in seconds since last poseChange
*/
void update(double dxy_mm, double dtheta_degrees, double dtSeconds)
{
@@ -72,14 +72,14 @@ void update(double dxy_mm, double dtheta_degrees, double dtSeconds)
this->dtheta_degrees = dtheta_degrees * velocity_factor;
}
friend ostream& operator<< (ostream & out, Velocities & velocities)
friend ostream& operator<< (ostream & out, PoseChange & poseChange)
{
char str[100];
sprintf(str, "<dxy=%7.0f mm dtheta = %+3.3f degrees dt = %f s",
velocities.dxy_mm,
velocities.dtheta_degrees,
velocities.dt_seconds);
poseChange.dxy_mm,
poseChange.dtheta_degrees,
poseChange.dt_seconds);
out << str;

12
cpp/Scan.cpp
View File

@@ -31,7 +31,7 @@ using namespace std;
#include "coreslam.h"
#include "Velocities.hpp"
#include "PoseChange.hpp"
#include "Laser.hpp"
#include "Scan.hpp"
@@ -76,22 +76,22 @@ void
Scan::update(
int * scanvals_mm,
double hole_width_millimeters,
Velocities & velocities)
PoseChange & poseChange)
{
scan_update(
this->scan,
scanvals_mm,
hole_width_millimeters,
velocities.dxy_mm,
velocities.dtheta_degrees);
poseChange.dxy_mm,
poseChange.dtheta_degrees);
}
void
Scan::update(
int * scanvals_mm,
double hole_width_millimeters)
{
Velocities zeroVelocities;
this->update(scanvals_mm, hole_width_millimeters, zeroVelocities);
PoseChange zeroPoseChange;
this->update(scanvals_mm, hole_width_millimeters, zeroPoseChange);
}
ostream& operator<< (ostream & out, Scan & scan)

6
cpp/Scan.hpp
View File

@@ -27,7 +27,7 @@
#include <iostream>
using namespace std;
class Velocities;
class PoseChange;
class Laser;
@@ -81,14 +81,14 @@ update(
* 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
*
*/
void
update(
int * scanvals_mm,
double hole_width_millimeters,
Velocities & velocities);
PoseChange & poseChange);
friend ostream& operator<< (ostream & out, Scan & scan);

16
cpp/WheeledRobot.cpp
View File

@@ -28,15 +28,15 @@
using namespace std;
#include "WheeledRobot.hpp"
#include "Velocities.hpp"
#include "PoseChange.hpp"
Velocities
WheeledRobot::computeVelocities(
PoseChange
WheeledRobot::computePoseChange(
double timestamp,
double left_wheel_odometry,
double right_wheel_odometry)
{
Velocities velocities;
PoseChange poseChange;
double timestamp_seconds_curr, left_wheel_degrees_curr, right_wheel_degrees_curr;
@@ -48,12 +48,12 @@ WheeledRobot::computeVelocities(
double left_diff_degrees = left_wheel_degrees_curr - this->left_wheel_degrees_prev;
double right_diff_degrees = right_wheel_degrees_curr - this->right_wheel_degrees_prev;
velocities.dxy_mm = this->wheel_radius_mm * (radians(left_diff_degrees) + radians(right_diff_degrees));
poseChange.dxy_mm = this->wheel_radius_mm * (radians(left_diff_degrees) + radians(right_diff_degrees));
velocities.dtheta_degrees = this->wheel_radius_mm / this->half_axle_length_mm *
poseChange.dtheta_degrees = this->wheel_radius_mm / this->half_axle_length_mm *
(right_diff_degrees - left_diff_degrees);
velocities.dt_seconds = timestamp_seconds_curr - timestamp_seconds_prev;
poseChange.dt_seconds = timestamp_seconds_curr - timestamp_seconds_prev;
}
// Store current odometry for next time
@@ -61,5 +61,5 @@ WheeledRobot::computeVelocities(
this->left_wheel_degrees_prev = left_wheel_degrees_curr;
this->right_wheel_degrees_prev = right_wheel_degrees_curr;
return velocities;
return poseChange;
}

12
cpp/WheeledRobot.hpp
View File

@@ -27,11 +27,11 @@
#include <vector>
using namespace std;
class Velocities;
class PoseChange;
/**
* An abstract class for wheeled robots. Supports computation of forward and angular
* velocities based on odometry. Your subclass should should implement the
* poseChange based on odometry. Your subclass should should implement the
* extractOdometry method.
*/
class WheeledRobot
@@ -57,15 +57,15 @@ WheeledRobot(double wheel_radius_mm, double half_axle_length_mm)
}
/**
* Computes forward and angular velocities based on odometry.
* 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 velocities object representing velocities for these odometry values
* @return poseChange object representing poseChange for these odometry values
*/
Velocities
computeVelocities(
PoseChange
computePoseChange(
double timestamp,
double left_wheel_odometry,
double right_wheel_odometry);

36
cpp/algorithms.cpp
View File

@@ -24,7 +24,7 @@ along with this code. If not, see <http://www.gnu.org/licenses/>.
#include "Map.hpp"
#include "Laser.hpp"
#include "Scan.hpp"
#include "Velocities.hpp"
#include "PoseChange.hpp"
#include "WheeledRobot.hpp"
#include "algorithms.hpp"
@@ -64,8 +64,8 @@ CoreSLAM::CoreSLAM(
// Store laser for later
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);
@@ -80,30 +80,30 @@ CoreSLAM::~CoreSLAM(void)
delete this->map;
delete this->scan_for_distance;
delete this->scan_for_mapbuild;
delete this->velocities;
delete this->poseChange;
}
void CoreSLAM::update(int * scan_mm, Velocities & velocities)
void CoreSLAM::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.dxy_mm,
velocities.dtheta_degrees,
velocities.dt_seconds);
// Update poseChange
this->poseChange->update(poseChange.dxy_mm,
poseChange.dtheta_degrees,
poseChange.dt_seconds);
// Implementing class updates map and pointcloud
this->updateMapAndPointcloud(velocities);
this->updateMapAndPointcloud(poseChange);
}
void CoreSLAM::update(int * scan_mm)
{
Velocities zero_velocities;
PoseChange zero_poseChange;
this->update(scan_mm, zero_velocities);
this->update(scan_mm, zero_poseChange);
}
@@ -121,7 +121,7 @@ Scan * CoreSLAM::scan_create(int span)
void
CoreSLAM::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);
}
SinglePositionSLAM::SinglePositionSLAM(Laser & laser, int map_size_pixels, double map_size_meters) :
@@ -133,15 +133,15 @@ CoreSLAM(laser, map_size_pixels, map_size_meters)
// SinglePositionSLAM class -------------------------------------------------------------------------------------------
void SinglePositionSLAM::updateMapAndPointcloud(Velocities & velocities)
void SinglePositionSLAM::updateMapAndPointcloud(PoseChange & poseChange)
{
// Start at current position
Position start_pos = this->position;
// Add effect of velocities
start_pos.x_mm += velocities.dxy_mm * this->costheta(),
start_pos.y_mm += velocities.dxy_mm * this->sintheta(),
start_pos.theta_degrees += velocities.dtheta_degrees;
// Add effect of poseChange
start_pos.x_mm += poseChange.dxy_mm * this->costheta(),
start_pos.y_mm += poseChange.dxy_mm * this->sintheta(),
start_pos.theta_degrees += poseChange.dtheta_degrees;
// Add offset from laser
start_pos.x_mm += this->laser->offset_mm * this->costheta();

22
cpp/algorithms.hpp
View File

@@ -47,7 +47,7 @@ class Laser;
* </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).
*
@@ -74,17 +74,17 @@ public:
/**
* Updates the scan and odometry, and calls the the implementing class's updateMapAndPointcloud method with
* the specified velocities.
* the specified poseChange.
*
* @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
* @param velocities velocities for odometry
* @param poseChange poseChange for odometry
*/
void update(int * scan_mm, Velocities & velocities);
void update(int * scan_mm, PoseChange & 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
@@ -139,15 +139,15 @@ protected:
Scan * scan_for_distance;
/**
* The current velocities from odometry
* The current poseChange from odometry
*/
class Velocities * velocities;
class PoseChange * poseChange;
/**
* Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM::update()
* @param velocities velocities for odometry
* @param poseChange poseChange for odometry
*/
virtual void updateMapAndPointcloud(Velocities & velocities) = 0;
virtual void updateMapAndPointcloud(PoseChange & poseChange) = 0;
private:
@@ -191,9 +191,9 @@ protected:
/**
* Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM::update()
* @param velocities velocities for odometry
* @param poseChange poseChange for odometry
*/
void updateMapAndPointcloud(Velocities & velocities);
void updateMapAndPointcloud(PoseChange & poseChange);
/**
* Returns a new position based on searching from a starting position. Called automatically by

6
examples/Log2PGM.java
View File

@@ -35,7 +35,7 @@ import edu.wlu.cs.levy.breezyslam.components.Map;
import edu.wlu.cs.levy.breezyslam.components.Scan;
import edu.wlu.cs.levy.breezyslam.components.Position;
import edu.wlu.cs.levy.breezyslam.components.URG04LX;
import edu.wlu.cs.levy.breezyslam.components.Velocities;
import edu.wlu.cs.levy.breezyslam.components.PoseChange;
import edu.wlu.cs.levy.breezyslam.robots.WheeledRobot;
@@ -226,8 +226,8 @@ public class Log2PGM
if (this.use_odometry)
{
long [] odometry = odometries.elementAt(scanno);
Velocities velocities = this.robot.computeVelocities(odometry[0], odometry[1], odometry[2]);
slam.update(scan, velocities);
PoseChange poseChange = this.robot.computePoseChange(odometry[0], odometry[1], odometry[2]);
slam.update(scan, poseChange);
}
else
{

4
examples/MinesRover.m
View File

@@ -45,9 +45,9 @@ classdef MinesRover < WheeledRobot
end
function [velocities, obj] = computeVelocities(obj, odometry)
function [poseChange, obj] = computePoseChange(obj, odometry)
[velocities, obj] = computeVelocities@WheeledRobot(obj, odometry(1), odometry(2), odometry(3));
[poseChange, obj] = computePoseChange@WheeledRobot(obj, odometry(1), odometry(2), odometry(3));
end

10
examples/log2pgm.cpp
View File

@@ -58,7 +58,7 @@ using namespace std;
#include "Position.hpp"
#include "Laser.hpp"
#include "WheeledRobot.hpp"
#include "Velocities.hpp"
#include "PoseChange.hpp"
#include "algorithms.hpp"
@@ -161,9 +161,9 @@ public:
{
}
Velocities computeVelocities(long * odometry, Velocities & velocities)
PoseChange computePoseChange(long * odometry, PoseChange & poseChange)
{
return WheeledRobot::computeVelocities(
return WheeledRobot::computePoseChange(
odometry[0],
odometry[1],
odometry[2]);
@@ -350,8 +350,8 @@ int main( int argc, const char** argv )
// Update with/out odometry
if (use_odometry)
{
Velocities velocities = robot.computeVelocities(odometries[scanno], velocities);
slam->update(lidar, velocities);
PoseChange poseChange = robot.computePoseChange(odometries[scanno], poseChange);
slam->update(lidar, poseChange);
}
else
{

4
examples/log2pgm.py
View File

@@ -94,8 +94,8 @@ def main():
if use_odometry:
# Convert odometry to velocities
velocities = robot.computeVelocities(odometries[scanno])
# Convert odometry to pose change (dxy_mm, dtheta_degrees, dt_seconds)
velocities = robot.computePoseChange(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)

2
examples/log2pkl.py
View File

@@ -77,7 +77,7 @@ def main():
if use_odometry:
# Convert odometry to velocities
velocities = robot.computeVelocities(odometries[scanno])
velocities = robot.computePoseChange(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)

2
examples/log2png.py
View File

@@ -96,7 +96,7 @@ def main():
if use_odometry:
# Convert odometry to velocities
velocities = robot.computeVelocities(odometries[scanno])
velocities = robot.computePoseChange(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)

2
examples/logdemo.m
View File

@@ -82,7 +82,7 @@ for scanno = 1:size(scans, 1)
if use_odometry
% Convert odometry to velocities
[velocities,robot] = robot.computeVelocities(odometries(scanno, :));
[velocities,robot] = robot.computePoseChange(odometries(scanno, :));
% Update SLAM with lidar and velocities
slam = slam.update(scans(scanno,:), velocities);

2
examples/logmovie.py
View File

@@ -67,7 +67,7 @@ def threadfunc(robot, slam, timestamps, lidars, odometries, mapbytes, pose):
else:
# Convert odometry to velocities
velocities = robot.computeVelocities(odometries[scanno])
velocities = robot.computePoseChange(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)

4
examples/mines.py
View File

@@ -99,9 +99,9 @@ class Rover(WheeledVehicle):
return '<%s ticks_per_cycle=%d>' % (WheeledVehicle.__str__(self), self.ticks_per_cycle)
def computeVelocities(self, odometry):
def computePoseChange(self, odometry):
return WheeledVehicle.computeVelocities(self, odometry[0], odometry[1], odometry[2])
return WheeledVehicle.computePoseChange(self, odometry[0], odometry[1], odometry[2])
def extractOdometry(self, timestamp, leftWheel, rightWheel):

12
java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java
View File

@@ -23,12 +23,12 @@
package edu.wlu.cs.levy.breezyslam.robots;
import edu.wlu.cs.levy.breezyslam.components.Velocities;
import edu.wlu.cs.levy.breezyslam.components.PoseChange;
/**
* An abstract class for wheeled robots. Supports computation of forward and angular
* velocities based on odometry. Your subclass should should implement the
* poseChange based on odometry. Your subclass should should implement the
* extractOdometry method.
*/
public abstract class WheeledRobot
@@ -59,14 +59,14 @@ public abstract class WheeledRobot
}
/**
* Computes forward and angular velocities based on odometry.
* 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 velocities object representing velocities for these odometry values
* @return poseChange object representing poseChange for these odometry values
*/
public Velocities computeVelocities( double timestamp, double left_wheel_odometry, double right_wheel_odometry)
public PoseChange computePoseChange( double timestamp, double left_wheel_odometry, double right_wheel_odometry)
{
WheelOdometry odometry = this.extractOdometry(timestamp, left_wheel_odometry, right_wheel_odometry);
@@ -91,7 +91,7 @@ public abstract class WheeledRobot
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);
return new PoseChange(dxy_mm, dtheta_degrees, dt_seconds);
}
/**

6
matlab/WheeledRobot.m
View File

@@ -62,8 +62,8 @@ classdef WheeledRobot
methods (Access = 'public')
function [velocities, obj] = computeVelocities(obj, timestamp, leftWheelOdometry, rightWheelOdometry)
% Computes forward and angular velocities based on odometry.
function [poseChange, obj] = computePoseChange(obj, timestamp, leftWheelOdometry, rightWheelOdometry)
% Computes forward and angular poseChange based on odometry.
%
% Parameters:
@@ -104,7 +104,7 @@ classdef WheeledRobot
obj.rightWheelDegreesPrev = rightWheelDegreesCurr;
% Return linear velocity, angular velocity, time difference
velocities = [dxyMillimeters, dthetaDegrees, dtSeconds];
poseChange = [dxyMillimeters, dthetaDegrees, dtSeconds];
end
end

28
python/breezyslam/algorithms.py
View File

@@ -57,7 +57,7 @@ class CoreSLAM(object):
Implementing classes should provide the method
_updateMapAndPointcloud(scan_mm, velocities, should_update_map)
_updateMapAndPointcloud(scan_mm, dxy_mm, dtheta_degrees, should_update_map)
to update the point-cloud (particle cloud) and map (if should_update_map true)
'''
@@ -80,7 +80,7 @@ class CoreSLAM(object):
# Store laser for later
self.laser = laser
# Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry
# Initialize velocities (dxyMillimeters/dt, dthetaDegrees/dt) for scan update
self.velocities = (0, 0)
# Initialize a scan for computing distance to map, and one for updating map
@@ -97,22 +97,22 @@ class CoreSLAM(object):
scan_mm is a list of Lidar scan values, whose count is specified in the scan_size
attribute of the Laser object passed to the CoreSlam constructor
velocities is a tuple of velocities (dxy_mm, dtheta_degrees, dt_seconds) for odometry
velocities is a tuple of velocities (dxy_mm, dtheta_degrees, dt_seconds) computed from odometry
should_update_map flags for whether you want to update the map
'''
# Convert (dxy,dtheta,dt) to (dxy/dt, dtheta/dt) for scan update
velocity_factor = (1 / velocities[2]) if (velocities[2] > 0) else 0 # units => units/sec
dxy_mm_dt = velocities[0] * velocity_factor
dtheta_degrees_dt = velocities[1] * velocity_factor
self.velocities = (dxy_mm_dt, dtheta_degrees_dt)
# Build a scan for computing distance to map, and one for updating map
self._scan_update(self.scan_for_mapbuild, scans_mm)
self._scan_update(self.scan_for_distance, scans_mm)
# Convert (dxy,dtheta,dt) to (dxy/dt, dtheta/dt) for scan update
velocity_factor = (1 / velocities[2]) if (velocities[2] > 0) else 0 # units => units/sec
new_dxy_mm = velocities[0] * velocity_factor
new_dtheta_degrees = velocities[1] * velocity_factor
self.velocities = (new_dxy_mm, new_dtheta_degrees)
# Implementing class updates map and pointcloud
self._updateMapAndPointcloud(self.velocities, should_update_map)
self._updateMapAndPointcloud(velocities[0], velocities[1], should_update_map)
def getmap(self, mapbytes):
'''
@@ -166,7 +166,7 @@ class SinglePositionSLAM(CoreSLAM):
init_coord_mm = 500 * map_size_meters # center of map
self.position = pybreezyslam.Position(init_coord_mm, init_coord_mm, 0)
def _updateMapAndPointcloud(self, velocities, should_update_map):
def _updateMapAndPointcloud(self, dxy_mm, dtheta_degrees, should_update_map):
'''
Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM.update()
velocities is a tuple of the form (dxy_mm, dtheta_degrees, dt_seconds).
@@ -176,9 +176,9 @@ class SinglePositionSLAM(CoreSLAM):
start_pos = self.position.copy()
# Add effect of velocities
start_pos.x_mm += velocities[0] * self._costheta()
start_pos.y_mm += velocities[0] * self._sintheta()
start_pos.theta_degrees += velocities[1]
start_pos.x_mm += dxy_mm * self._costheta()
start_pos.y_mm += dxy_mm * self._sintheta()
start_pos.theta_degrees += dtheta_degrees
# Add offset from laser
start_pos.x_mm += self.laser.offset_mm * self._costheta()

4
python/breezyslam/vehicles.py
View File

@@ -52,9 +52,9 @@ class WheeledVehicle(object):
return self.__str__()
def computeVelocities(self, timestamp, leftWheelOdometry, rightWheelOdometry):
def computePoseChange(self, timestamp, leftWheelOdometry, rightWheelOdometry):
'''
Computes forward and angular velocities based on odometry.
Computes pose change based on odometry.
Parameters:

23
python/pybreezyslam.c
View File

@@ -15,29 +15,6 @@ 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/>.
Change log:
07-FEB-2014 : Simon D. Levy - Initial release
17-FEB-2014 : SDL - Prohibit non-positive random seed
28-FEB-2014 : SDL - Check for null return in Robot.computeVelocities()
01-MAR-2014 : SDL - Moved module code to in __init__.py
14-MAR_2014 : SDL - Changed mm_per_pixel to pixels_per_meter
- No more robot object passed to __init__(); velocities
sent directly into update()
16-MAR_2014 : SDL - Changed #include to ../pybreezyslam
23-MAR-2014 : SDL - Changed millimeters to meters
31-MAR-2014 : SDL - Improved documetnation for Laser class
- Made all units explicit
30-APR-2014 : SDL - Migrated CoreSLAM algorithm to pure Python
- Added Position, Map, Scan classes
- Added distanceScanToMap method
04-MAY-2014 : SDL - Changed back from meters to mm
03-JUN-2014 : SDL - Made distanceScanToMap() return -1 for infinity
23-JUL-2014 : SDL - Simplified API for Laser
08-AUG-2014: SDL - Moved Laser to pure Python
13-AUG-2014: SDL - Fixed Scan.update() bug in Python3
*/
#include <Python.h>