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

Added Java support.

Browse Source
This commit is contained in:
Simon D. Levy
2014-10-26 17:46:47 -04:00
parent 721f75e2af
commit 08ab4a55bb
36 changed files with 630 additions and 311 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
cpp/Laser.hpp
View File

@@ -35,6 +35,15 @@ class Laser
friend class SinglePositionSLAM;
friend class RMHC_SLAM;
friend class Scan;
protected:
int scan_size; /* number of points per scan */
double scan_rate_hz; /* scans per second */
double detection_angle_degrees; /* e.g. 240, 360 */
double distance_no_detection_mm; /* default value when the laser returns 0 */
int detection_margin; /* first scan element to consider */
double offset_mm; /* position of the laser wrt center of rotation */
public:
@@ -56,34 +65,53 @@ public:
float distance_no_detection_mm,
int detection_margin = 0,
float offset_mm = 0.
);
)
{
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 an empty Laser object (all parameters zero).
*/
Laser(void);
/**
* Dealloates memory for this Laser object.
*/
~Laser(void);
friend ostream& operator<< (ostream & out, Laser & laser);
friend ostream& operator<< (ostream & out, Laser & laser)
{
char str[512];
private:
struct laser_t * laser;
sprintf(str,
"<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 mm>",
laser.scan_size, laser.scan_rate_hz,
laser.detection_angle_degrees,
laser.distance_no_detection_mm,
laser.detection_margin,
laser.offset_mm);
out << str;
return out;
}
};
/**
* A class for the Hokuyo URG-04LX laser.
*/
class URG04LX : public Laser
{
public:
/**
* Builds a Laser object from parameters based on the specifications for your
* 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
@@ -94,7 +122,7 @@ public:
Laser(682, 10, 240, 4000, detection_margin, offset_mm) { }
/**
* Builds an empty Laser object (all parameters zero).
* Builds an empty URG04LX object (all parameters zero).
*/
URG04LX(void) : Laser() {}

7
cpp/Makefile
View File

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

7
cpp/Position.hpp
View File

@@ -28,7 +28,9 @@ using namespace std;
/**
* A class representing the position of a robot.
*/class Position
*/
class Position
{
friend class CoreSLAM;
@@ -64,7 +66,8 @@ public:
{
char str[100];
sprintf(str, "<x = %7.0f mm y = %7.0f mm theta = %+3.3f degrees>",
//sprintf(str, "<x = %7.0f mm y = %7.0f mm theta = %+3.3f degrees>",
sprintf(str, "<x = %f mm y = %f mm theta = %f degrees>",
position.x_mm, position.y_mm, position.theta_degrees);
out << str;

23
cpp/Scan.cpp
View File

@@ -54,10 +54,15 @@ void Scan::init(Laser * laser, int span)
{
this->scan = new scan_t;
scan_init(this->scan, laser->laser->scan_size, span);
this->laser = new laser_t;
memcpy(this->laser, laser->laser, sizeof(laser_t));
scan_init(
this->scan,
span,
laser->scan_size,
laser->scan_rate_hz,
laser->detection_angle_degrees,
laser->distance_no_detection_mm,
laser->detection_margin,
laser->offset_mm);
}
Scan::~Scan(void)
@@ -76,7 +81,6 @@ Scan::update(
scan_update(
this->scan,
scanvals_mm,
*this->laser,
hole_width_millimeters,
velocities.dxy_mm,
velocities.dtheta_degrees);
@@ -86,13 +90,8 @@ Scan::update(
int * scanvals_mm,
double hole_width_millimeters)
{
scan_update(
this->scan,
scanvals_mm,
*this->laser,
hole_width_millimeters,
0,
0);
Velocities zeroVelocities;
this->update(scanvals_mm, hole_width_millimeters, zeroVelocities);
}
ostream& operator<< (ostream & out, Scan & scan)

2
cpp/Scan.hpp
View File

@@ -96,8 +96,6 @@ private:
struct scan_t * scan;
struct laser_t * laser;
void init(Laser * laser, int span);
};

25
cpp/algorithms.cpp
View File

@@ -99,6 +99,13 @@ void CoreSLAM::update(int * scan_mm, Velocities & velocities)
this->updateMapAndPointcloud(velocities);
}
void CoreSLAM::update(int * scan_mm)
{
Velocities zero_velocities;
this->update(scan_mm, zero_velocities);
}
void CoreSLAM::getmap(unsigned char * mapbytes)
{
@@ -130,15 +137,15 @@ void SinglePositionSLAM::updateMapAndPointcloud(Velocities & velocities)
{
// 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 offset from laser
start_pos.x_mm += this->laser->laser->offset_mm * this->costheta();
start_pos.y_mm += this->laser->laser->offset_mm * this->sintheta();
start_pos.x_mm += this->laser->offset_mm * this->costheta();
start_pos.y_mm += this->laser->offset_mm * this->sintheta();
// Get new position from implementing class
Position new_position = this->getNewPosition(start_pos);
@@ -148,8 +155,8 @@ void SinglePositionSLAM::updateMapAndPointcloud(Velocities & velocities)
// Update the current position with this new position, adjusted by laser offset
this->position = Position(new_position);
this->position.x_mm -= this->laser->laser->offset_mm * this->costheta();
this->position.y_mm -= this->laser->laser->offset_mm * this->sintheta();
this->position.x_mm -= this->laser->offset_mm * this->costheta();
this->position.y_mm -= this->laser->offset_mm * this->sintheta();
}
Position & SinglePositionSLAM::getpos(void)
@@ -198,13 +205,6 @@ RMHC_SLAM::~RMHC_SLAM(void)
free(this->randomizer);
}
void RMHC_SLAM::update(int * scan_mm)
{
Velocities zero_velocities;
CoreSLAM::update(scan_mm, zero_velocities);
}
Position RMHC_SLAM::getNewPosition(Position & start_pos)
{
// Search for a new position if indicated
@@ -219,7 +219,6 @@ Position RMHC_SLAM::getNewPosition(Position & start_pos)
start_pos_c,
this->map->map,
this->scan_for_distance->scan,
*this->laser->laser,
this->sigma_xy_mm,
this->sigma_theta_degrees,
this->max_search_iter,

17
cpp/algorithms.hpp
View File

@@ -83,6 +83,14 @@ public:
void update(int * scan_mm, Velocities & velocities);
/**
* Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero velocities
* (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
*/
void update(int * scan_mm);
/**
* The quality of the map (0 through 255); default = 50
*/
@@ -232,14 +240,7 @@ public:
~RMHC_SLAM(void);
/**
* Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero velocities
* (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
*/
void update(int * scan_mm);
/**
* The standard deviation in millimeters of the Gaussian distribution of
* the (X,Y) component of position in the particle filter; default = 100