From 081d33aedf2dd121c6bb529dbc3353987ac106bd Mon Sep 17 00:00:00 2001 From: "Simon D. Levy" Date: Sun, 3 Jun 2018 14:38:07 -0400 Subject: [PATCH] Use "pose change" instead of "velocities" for (dxy_mm, dtheta_degrees, dt_seconds) --- .gitignore | 5 +-- cpp/Makefile | 4 +-- cpp/{Velocities.hpp => PoseChange.hpp} | 26 +++++++------- cpp/Scan.cpp | 12 +++---- cpp/Scan.hpp | 6 ++-- cpp/WheeledRobot.cpp | 16 ++++----- cpp/WheeledRobot.hpp | 12 +++---- cpp/algorithms.cpp | 36 +++++++++---------- cpp/algorithms.hpp | 22 ++++++------ examples/Log2PGM.java | 6 ++-- examples/MinesRover.m | 4 +-- examples/log2pgm.cpp | 10 +++--- examples/log2pgm.py | 4 +-- examples/log2pkl.py | 2 +- examples/log2png.py | 2 +- examples/logdemo.m | 2 +- examples/logmovie.py | 2 +- examples/mines.py | 4 +-- .../levy/breezyslam/robots/WheeledRobot.java | 12 +++---- matlab/WheeledRobot.m | 6 ++-- python/breezyslam/algorithms.py | 28 +++++++-------- python/breezyslam/vehicles.py | 4 +-- python/pybreezyslam.c | 23 ------------ 23 files changed, 113 insertions(+), 135 deletions(-) rename cpp/{Velocities.hpp => PoseChange.hpp} (73%) diff --git a/.gitignore b/.gitignore index ae43b70..471aab4 100644 --- a/.gitignore +++ b/.gitignore @@ -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 diff --git a/cpp/Makefile b/cpp/Makefile index dee4f6e..4db0608 100644 --- a/cpp/Makefile +++ b/cpp/Makefile @@ -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 diff --git a/cpp/Velocities.hpp b/cpp/PoseChange.hpp similarity index 73% rename from cpp/Velocities.hpp rename to cpp/PoseChange.hpp index 4baf59c..0d8fa61 100644 --- a/cpp/Velocities.hpp +++ b/cpp/PoseChange.hpp @@ -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, "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) diff --git a/cpp/Scan.hpp b/cpp/Scan.hpp index daa5506..d08338c 100644 --- a/cpp/Scan.hpp +++ b/cpp/Scan.hpp @@ -27,7 +27,7 @@ #include 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); diff --git a/cpp/WheeledRobot.cpp b/cpp/WheeledRobot.cpp index 84921cf..4088451 100644 --- a/cpp/WheeledRobot.cpp +++ b/cpp/WheeledRobot.cpp @@ -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; } diff --git a/cpp/WheeledRobot.hpp b/cpp/WheeledRobot.hpp index c352c21..99d1e78 100644 --- a/cpp/WheeledRobot.hpp +++ b/cpp/WheeledRobot.hpp @@ -27,11 +27,11 @@ #include 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); diff --git a/cpp/algorithms.cpp b/cpp/algorithms.cpp index bfd7b09..aa7b942 100644 --- a/cpp/algorithms.cpp +++ b/cpp/algorithms.cpp @@ -24,7 +24,7 @@ along with this code. If not, see . #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(); diff --git a/cpp/algorithms.hpp b/cpp/algorithms.hpp index 09913bd..b69ff72 100644 --- a/cpp/algorithms.hpp +++ b/cpp/algorithms.hpp @@ -47,7 +47,7 @@ class Laser; * * 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 scan_size * 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 scan_size * 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 diff --git a/examples/Log2PGM.java b/examples/Log2PGM.java index 35834d0..47d2100 100644 --- a/examples/Log2PGM.java +++ b/examples/Log2PGM.java @@ -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 { diff --git a/examples/MinesRover.m b/examples/MinesRover.m index 9143552..a0f2378 100755 --- a/examples/MinesRover.m +++ b/examples/MinesRover.m @@ -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 diff --git a/examples/log2pgm.cpp b/examples/log2pgm.cpp index feabd87..a88789d 100644 --- a/examples/log2pgm.cpp +++ b/examples/log2pgm.cpp @@ -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 { diff --git a/examples/log2pgm.py b/examples/log2pgm.py index 524b082..a2fb68e 100755 --- a/examples/log2pgm.py +++ b/examples/log2pgm.py @@ -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) diff --git a/examples/log2pkl.py b/examples/log2pkl.py index bf536b4..df00c02 100755 --- a/examples/log2pkl.py +++ b/examples/log2pkl.py @@ -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) diff --git a/examples/log2png.py b/examples/log2png.py index e7a6e52..882fdda 100755 --- a/examples/log2png.py +++ b/examples/log2png.py @@ -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) diff --git a/examples/logdemo.m b/examples/logdemo.m index b764357..c59e336 100644 --- a/examples/logdemo.m +++ b/examples/logdemo.m @@ -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); diff --git a/examples/logmovie.py b/examples/logmovie.py index 41f09be..d6f7c77 100755 --- a/examples/logmovie.py +++ b/examples/logmovie.py @@ -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) diff --git a/examples/mines.py b/examples/mines.py index 69bc59d..592eecd 100644 --- a/examples/mines.py +++ b/examples/mines.py @@ -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): diff --git a/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java b/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java index a06766e..f2b2822 100644 --- a/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java +++ b/java/edu/wlu/cs/levy/breezyslam/robots/WheeledRobot.java @@ -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); } /** diff --git a/matlab/WheeledRobot.m b/matlab/WheeledRobot.m index 6dd5604..ac155c7 100644 --- a/matlab/WheeledRobot.m +++ b/matlab/WheeledRobot.m @@ -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 diff --git a/python/breezyslam/algorithms.py b/python/breezyslam/algorithms.py index b158eda..0b28746 100644 --- a/python/breezyslam/algorithms.py +++ b/python/breezyslam/algorithms.py @@ -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() diff --git a/python/breezyslam/vehicles.py b/python/breezyslam/vehicles.py index 878c80e..ddc8a1e 100644 --- a/python/breezyslam/vehicles.py +++ b/python/breezyslam/vehicles.py @@ -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: diff --git a/python/pybreezyslam.c b/python/pybreezyslam.c index d20e33a..7f02581 100644 --- a/python/pybreezyslam.c +++ b/python/pybreezyslam.c @@ -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 . - -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