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

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