#include ".\robot.h"
Robot::Robot(void)
{
PI = 3.141592653589793;
//setting the origin of the robot
this->x_orig = 0.0;
this->y_orig = 0.0;
this->theta = 0.0;
//setting the origin for the particles
this->x_orig_old = 0.0;
this->y_orig_old = 0.0;
this->theta_old = 0.0;
//setting initial bool
this->show_IR = false;
this->show_Hist = false;
this->show_sonar = false;
this->show_particle_sonar = false;
this->draw_particles = false;
this->is_red = 3;
//setting the IR lengths
this->ir[0] = 25.0;
this->ir[1] = 25.0;
this->ir[2] = 25.0;
//setting the left sensor origin
this->ir_x[0] = -12.0;
this->ir_y[0] = 20.0;
this->ir_th[0] = 90.0;
//setting the center sensor origin
this->ir_x[1] = 15.0;
this->ir_y[1] = 0.0;
this->ir_th[1] = 0.0;
//setting the right sensor origin
this->ir_x[2] = -12.0;
this->ir_y[2] = -20.0;
this->ir_th[2] = -90.0;
//setting the camera origin
this->cam_x = -20.0;
this->cam_y = 0.0;
this->cam_th = -90.0;
this->cam_red = 0.0;
//setting the sonar origin
this->son_x = 2.54*8.0; // convert from inches to cm
this->son_y = 2.54;
this->son_th = 0.0; // this, of course, comes from the client
this->son_dist = 50.0; // this too should be sent from the real robot
this->trans_adj = -0.010415;
this->trans_noise = 0.05;
this->angle_adj = -0.034899;
this->angle_noise = 0.015;
//pushing the origin into list
x_hist.push_back(x_orig);
y_hist.push_back(y_orig);
th_hist.push_back(theta);
//calls upon set particles
this->setupParticles(500,Robot::AT_POSITION);
this->percentNewParticles = 0.02;
}
Robot::~Robot(void)
{
}
void Robot::setSonarStatus(double _dist, double _theta)
{
this->son_dist = _dist;
this->son_th = _theta;
}
void Robot::clearParticleFilter()
{
// need to get each item out and delete it!
while (!Filter.empty())
{
Particle* d = Filter.front(); // get a particle
Filter.pop_front(); // remove it from the list
delete d; // reclaim the memory
}
}
// sets up particles in a variety of ways... also clears the
// particles beforehand...
void Robot::setupParticles(int N, int particleSetupType)
{
while (filterIsInUse != 0) ; // busy waiting is probably not so good...
if (N <= 1) N = 100; // one hunderd particles by default
this->totalParticles = N;
filterIsInUse = 1;
// clear the filter
clearParticleFilter();
// which kind of particle setup do we want?
switch (particleSetupType)
{
// at the Robot's position (not including orientation)
case Robot::AT_POSITION:
for (int i=0; i<N ; ++i) {
int theta = rand() % 360;
Particle* par = new Particle(x_orig, y_orig, theta, 1.0);
Filter.push_back(par);
}
break;
// anywhere within the map
case Robot::ANYWHERE:
{
// Retrieve the current map
Map* map = Program::Get().map;
int i=0;
while (i<N) {
// not very efficient, but it'll do for now
int negative1 = (rand() % 2)*2-1; // -1 or 1
int negative2 = (rand() % 2)*2-1; // -1 or 1
int x = negative1 * rand(); // rand()/13;
int y = negative2 * rand(); // rand()/7;
int numberOfIntersections = 0;
map->getIntersectionNumber(x, y, &numberOfIntersections);
if (numberOfIntersections % 2 == 0)
continue;
int theta = rand() % 360;
Particle* par = new Particle(x, y, theta, 1.0);
Filter.push_back(par);
++i;
if (i%100 == 0) cout << "Placed particle " << i << endl;
}
break;
}
// back some distance from the map...
case Robot::WALL_RELATIVE:
//break; not implemented yet!! instead we simply use the robot's pose
// at the Robot's pose (including orientation)
case Robot::AT_POSE:
default:
for (int i=0; i<N ; ++i) {
Particle* par = new Particle(x_orig, y_orig, theta, 1.0);
Filter.push_back(par);
}
break;
}
filterIsInUse = 0;
}
// Creates a pointer to a new particle somewhere randomish.
// At the moment, we don't do a check to make sure it's in bounds,
// since trying to naively generate a particle that's in bounds
// using the method above is really slow. So they're mostly
// useless.
Robot::Particle* Robot::generateRandomParticle()
{
/*
int negative1 = (rand() % 2)*2-1; // -1 or 1
int negative2 = (rand() % 2)*2-1; // -1 or 1
int x = negative1 * rand(); // rand()/13;
int y = negative2 * rand(); // rand()/7;
*/
/*
* Bounded random generation (to slightly beyond map borders)
*/
int x = (rand() % 4980) - 2570;
int y = (rand() % 4830) - 4740;
int theta = rand() % 360;
Particle* par = new Particle(x, y, theta, 1.0);
return par;
}
//This updates the position and probabilities of the list of
// particles
void Robot::UpdateParticles() // Monte Carlo Localization
{
while (filterIsInUse != 0) ; // semaphore
filterIsInUse = 1;
MoveParticles();
UpdateParticleProbabilities();
ResampleParticles();
filterIsInUse = 0;
}
//Moves the particles based on original position and the new position
// of the robot. Noise is added to the particles movement based on
// where the robot is moved from the last particle movement
void Robot::MoveParticles()
{
cout << "Moving particles!" << endl;
// Find the change in pose
double dx, dy, dth;
dx = x_orig - x_orig_old;
dy = y_orig - y_orig_old;
dth = 6.28*((double)(theta - theta_old))/360;
cout << "dth is " << dth << endl;
// go through each particle and move it
list<Particle*>::iterator startPar = Filter.begin();
list<Particle*>::iterator endPar = Filter.end();
Particle* par;
int pcount = 0;
for ( ; startPar != endPar; ++startPar)
{
par = *startPar;
// Update particle positions. We add some noise as well,
// based on our odometry calculations. These were on carpet,
// so who knows if they'll actually model the odometry right
// at all.
// Victoria's version, minor debugging by steph
double r, alpha, beta, px, py;
r = alpha = beta = px = py = 0;
r = sqrt(dx*dx+dy*dy);
alpha = atan2(dy, dx); // radians
double par_th = par->th; // Degrees
par_th = (par_th/360.0) * 6.28; // Convert to radians
beta = alpha + par_th;
px = r*cos(beta);
py = r*sin(beta);
par->x += applyMotionModel(px, Particle::TRANS_NOISE);
par->y += applyMotionModel(py, Particle::TRANS_NOISE);
par->th += applyMotionModel(dth, Particle::ANGLE_NOISE);
// Make sure th is in [0, 360). Although... I don't think
// we really need to.
//while (par->th >= 360)
// par->th -= 360;
//while (par->th < 0)
// par->th += 360;
pcount++;
if (pcount % 100 == 0)
cout << "Moved " << pcount << " particles..." << endl;
}
// use current pose as new reference point
x_orig_old = x_orig;
y_orig_old = y_orig;
theta_old = theta;
}
/*
* these functions are examples of how to get
* the robot's simulated sensor readings
*
* In other words, I think you use this to "fake" a robot sonar reading. -- Paul
*/
void Robot::computeSonarDistance()
{
// sets son_dist
// first, get global coordinates
double gx,gy,gth;
this->getGlobalCoord(son_x,son_y,son_th,&gx,&gy,>h);
cout << "Coords of the robot " << x_orig << "," << y_orig << "," << theta << endl;
cout << "Global coords of the sonar: " << gx << "," << gy << "," << gth << endl;
// Retrieve the current map
Map* map = Program::Get().map;
// next, find the distance
double ix,iy,dist; int onSegment;
map->obstacleDist(gx,gy,gth, &ix, &iy, &dist, &onSegment);
cout << "int is " << ix << "," << iy << " and onSeg is " << onSegment << endl;
// set the sonar distance...
this->son_dist = dist;
}
/*
* here is one to get the particles' simulated readings
* similar things are possible with the camera...
* the *onseg value will be 2 for red and 1 for white (I think)
*/
void Robot::computeSonarDistance(double x, double y, double th, double* dist, int* onseg)
{
// sets son_dist -- actually, this is a lie, it doesn't. -- Paul
// first, get global coordinates
double gx,gy,gth;
this->getGlobalCoord(x,y,th,son_x,son_y,son_th,&gx,&gy,>h);
//cout << "Coords of the robot " << x_orig << "," << y_orig << "," << theta << endl;
//cout << "Global coords of the particle: " << x << "," << y << "," << th << endl;
//cout << "Global coords of the sonar: " << gx << "," << gy << "," << gth << endl;
// Retrieve the current map
Map* map = Program::Get().map;
// next, find the distance
double ix,iy;
map->obstacleDist(gx,gy,gth, &ix, &iy, dist, onseg);
// is it valid?
if (*onseg < 1)
*dist = 0;
}
/*
* some code for testing if things are inbounds and for handling
* sonar values has been left here, in case you'd like to use it...
*/
void Robot::UpdateParticleProbabilities()
{
cout << "X: " << x_orig << "Y: " << y_orig << endl;
cout << "Sensing!" << endl;
Map* map = Program::Get().map;
// SONAR: get "real" sonar value (where "real" == "fake")
computeSonarDistance(); // now this->son_dist is the robot's sonar value
// go through each particle
list<Particle*>::iterator startPar = Filter.begin();
list<Particle*>::iterator endPar = Filter.end();
Particle* par;
int pcount = 0;
double totalProb = 0;
for ( ; startPar != endPar; ++startPar)
{
//maximizing to find the minimum
par = *startPar;
// particles at the moment are hypotheses for the robot
// pose (including orientation), not the sensor's pose, so we
// need to adjust both pose and orientation...
// Except this currently already happens inside computeSonarDistance,
// which calls getGlobalCoord and uses the set parameters for the
// sonar's location on the robot to calculate it's dist.
// sonar reading for each particle
// the particle represents the robot's position, so we need to
// calculate the estimated sonar reading
int onsegment;
computeSonarDistance(par->x, par->y, par->th, &(par->son_dist), &onsegment);
// adjust for sonar closeness...
if (onsegment > 0) // if it hit a wall at all
{
par->p *= 1/(1 + 0.09*fabs(this->son_dist - par->son_dist));
}
else // out of bounds...
{
par->p *= 0.0001; // out of the map
}
// adjustments for staying in the map
int i;
// obtains the number of intersections
map->getIntersectionNumber(par->x,par->y,&i);
if (i%2 == 0) // decrease prob if it leaves the map
{
par->p*=.000001; // why not
}
// increment particle count, and add particle's final prob to
// total probability of particles
pcount++;
totalProb += par->p;
if (pcount % 100 == 0)
cout << "Analyzed " << pcount << " particles using sensor data..." << endl;
}
cout << "Total prob was " << totalProb << endl;
// Here we go again...
startPar = Filter.begin();
endPar = Filter.end();
double normalProb = 0;
// Normalize the probabilties so they sum to 1.
for ( ; startPar != endPar; ++startPar)
{
par = *startPar;
par->p /= totalProb;
normalProb += par->p;
}
cout << "Total prob after normalizing is " << normalProb << endl;
// BEGIN MCL PATH LOGGING
// We define the mcl location as the weighted average location of
// the particles, weighing based on the particle probability. This
// information then get put on the MCL list so that we draw a pretty
// path later.
startPar = Filter.begin();
endPar = Filter.end();
double mcl_x, mcl_y, mcl_th;
mcl_x = mcl_y = mcl_th = 0;
int foo;
// Iterate over them, getting average x, y, th
for ( ; startPar != endPar; ++startPar)
{
par = *startPar;
foo++;
mcl_x += par->x*par->p;
mcl_y += par->y*par->p;
mcl_th += par->th*par->p;
if (foo%100 == 0)
{
cout<<"Particle "<<foo<<" at "<<par->x<<", "<<par->y<<", "<<par->th<<endl;
}
}
cout << "MCL position at "<<mcl_x<<", "<<mcl_y<<", "<<mcl_th<<endl;
mcl_x_hist.push_back(mcl_x);
mcl_y_hist.push_back(mcl_y);
mcl_th_hist.push_back(mcl_th);
}
// Function that goes through each particle and makes copies of it and pushes
// it into the Filter list. If the probability is too low, it will not get copied.
// After copying the needed number of particles, the old particles (the first elements
// in the list) will be deleted.
void Robot::ResampleParticles()
{
cout << "Resampling particles!" << endl;
// Retrieve the current map
Map* map = Program::Get().map;
// go through each particle
list<Particle*>::iterator startPar = Filter.begin();
// We need a way of generating new particles based on the probability
// of the old particles. We use the following method:
//
// For every particle, add it's probability to an accumulatior (probAccum).
// When we've accumulated more probability than 1 divided by the total
// number of particles we're going to generate, we generate a particle
// based on the current particle (and keep generating them, in the case
// of a high probability particle, subtracting that threshold probNeeded
// until probAccum < probNeeded).
//
// Is this method totally fair? Well, no... look at it this way: for
// all particles with more probability than the threshold, there's
// guaranteed to be a particle generated. Once you subtract all that
// out, well, everything depends on the placement in the array. However,
// it's *fairly* random, and has a couple interesting properties: while
// absolutely guaranteeing that all points with high probability
// stick around, it nonetheless will tend to favor scattering points,
// since points that are near each other in the array should also
// tend towards being near each other in space, being generated by the
// same original high probability point, and unless they're all really
// high, some will be skipped over in favor of others.
//
// That all made sense in my head, anyway. -- Paul
double probAccum = 0;
double probNeeded = 1.0/totalParticles;
Particle* par;
int pcount = 0;
int newpcount = 0;
for ( ; pcount < totalParticles; ++startPar)
{
par = *startPar;
pcount++; // Let's just do this now before I forget.
// Add the probability of the current particle to the accumulator.
probAccum += par->p;
// Generate particles if we've got enough probability.
while (probAccum > probNeeded)
{
// So, just to be on the safe side, we want to scatter
// some random particles too. We occasionally make a random
// particle instead of our regularly scheduled particle
// via sampling.
if (((double)rand())/RAND_MAX < percentNewParticles)
{
Filter.push_back(generateRandomParticle());
}
// Otherwise, make a particle based on the current particle.
else {
// Add some noise
int maxNoise = 20;
int maxThNoise = 10;
double noiseAngle = ((double)(rand() % 360)) * 6.28/360;
int noiseRad = rand() % maxNoise - maxNoise/2;
int xNoise = noiseRad * cos(noiseAngle);
int yNoise = noiseRad * sin(noiseAngle);
int thNoise = rand() % maxThNoise - maxThNoise/2;
double newx = par->x + xNoise; //+ noise(?, Particle::TRANS_NOISE);
double newy = par->y + yNoise; //+ noise(?, Particle::TRANS_NOISE);
double newth = par->th + thNoise; //+ noise(?, Particle::ANGLE_NOISE);
Particle* newp = new Particle(newx, newy, newth, probNeeded);
Filter.push_back(newp);
}
newpcount++;
probAccum -= probNeeded;
if (newpcount % 100 == 0)
cout << "Generated " << pcount << " new particles..." << endl;
}
}
// On the off chance we didn't make enough particles (rounding
// wierdness?), well, make some more!
while (newpcount < totalParticles)
{
Filter.push_back(generateRandomParticle());
newpcount++;
cout << "Generated " << pcount << " new particles..." << endl;
}
// Get rid of all the old particles. On the off chance we made
// too many particles, we'll just delete using the newpcount
// instead, and, uhh, unless something's really crazy, we'll
// end up with totalParticles left at the end, since that's how
// many we started with.
Particle* d;
int delParts = 0;
for(; delParts < newpcount; ++delParts)
{
d = Filter.front(); // get a particle
Filter.pop_front(); // remove it from the list
delete d; // reclaim the memory
}
cout << "Killed " << delParts << " old particles" << endl;
}
//set the coordinates based on the values that were passed through
void Robot::deltaCoordinates(double dx, double dy, double dth)
{
this->x_orig += dx;
this->y_orig += dy;
this->theta += dth;
// stores the values where the origin moves to
x_hist.push_back(x_orig);
y_hist.push_back(y_orig);
th_hist.push_back(theta);
}
void Robot::setCoordinates(double x, double y, double th)
{
this->x_orig = x;
this->y_orig = y;
this->theta = th;
// stores the values where the origin moves to
x_hist.push_back(x_orig);
y_hist.push_back(y_orig);
th_hist.push_back(theta);
}
//sets the values of the IR sensors
void Robot::setIR(double L, double C, double R)
{
this->ir[0] = L;
this->ir[1] = C;
this->ir[2] = R;
}
void Robot::Draw()
{
glPushMatrix();
glTranslatef(x_orig,y_orig,0);
glRotatef(theta,0.0,0.0,1.0);
//draws the camera
glBegin(GL_LINES);
glColor3f(1.0,0.0,0.0);
glVertex2f(cam_x,cam_y);
glVertex2f(cam_x + cam_red*cos((cam_th)*(PI/180)),
cam_red*sin((cam_th)*(PI/180)));
glEnd();
//draws the robot
glBegin(GL_LINE_LOOP);
glColor3f(1.0,1.0,1.0);
/* old
glVertex2f(-35.0,-20.0);
glVertex2f(0.0,-20.0);
glVertex2f(15.0,0.0);
glVertex2f(0.0,20.0);
glVertex2f(-35.0,20.0);
*/
// convert to cm?
double cnv = 2.54;
glVertex2f(-3*cnv, -7.5*cnv);
glVertex2f(8*cnv, -7.5*cnv);
glVertex2f(15*cnv, 0.0*cnv);
glVertex2f(8*cnv, 7.5*cnv);
glVertex2f(-3*cnv, 7.5*cnv);
glVertex2f(-3*cnv, -7.5*cnv);
glEnd();
//draws the sonar
if (show_sonar == true) {
glBegin(GL_LINES);
glColor3f(1.0,1.0,0.0);
glVertex2f(son_x,son_y);
glVertex2f(son_x + son_dist*cos((son_th)*(PI/180)),
son_y + son_dist*sin((son_th)*(PI/180)));
glEnd();
}
//show the IR range sensors
if (show_IR == true) {
//get the IR values from the bot (changing y value)
glBegin(GL_LINES);
glColor3f(0.0,1.0,1.0);
glVertex2f(ir_x[0], ir_y[0]);
glVertex2f(ir_x[0] + ir[0]*cos((PI/180)*ir_th[0]),
ir_y[0] + ir[0]*sin((PI/180)*ir_th[0]));
glVertex2f(ir_x[1],ir_y[1]);
glVertex2f(ir_x[1] + ir[1]*cos((PI/180)*ir_th[1]),
ir_y[1] + ir[1]*sin((PI/180)*ir_th[1]));
glVertex2f(ir_x[2],ir_y[2]);
glVertex2f(ir_x[2] + ir[2]*cos((PI/180)*ir_th[2]),
ir_y[2] + ir[2]*sin((PI/180)*ir_th[2]));
glEnd();
}
glPopMatrix();
//show the path taken by robot
glPushMatrix();
if (show_Hist == true) {
list<double>::iterator x_end = x_hist.end();
list<double>::iterator x_iter = x_hist.begin();
list<double>::iterator y_iter = y_hist.begin();
glBegin(GL_LINE_STRIP);
glColor3f(1.0,1.0,0.5);
for( ; x_iter != x_end; ++x_iter, ++y_iter)
{
glVertex2f(*x_iter,*y_iter);
}
glEnd();
}
glPopMatrix();
//show the path we think the robot took, based on mcl
glPushMatrix();
if (show_Hist == true) {
list<double>::iterator mcl_x_end = mcl_x_hist.end();
list<double>::iterator mcl_x_iter = mcl_x_hist.begin();
list<double>::iterator mcl_y_iter = mcl_y_hist.begin();
glBegin(GL_LINE_STRIP);
glColor3f(1.0,0.5,0.5);
for( ; mcl_x_iter != mcl_x_end; ++mcl_x_iter, ++mcl_y_iter)
{
glVertex2f(*mcl_x_iter,*mcl_y_iter);
}
glEnd();
}
glPopMatrix();
//draws the particles of the robot
float red=1.0, green=0, blue=0;
glPushMatrix();
int count = 0;
if (draw_particles == true && filterIsInUse == 0)
{
filterIsInUse = 1;
Particle* par;
list<Particle*>::iterator par_iter = Filter.begin();
list<Particle*>::iterator par_end = Filter.end();
glPointSize(4); //makes the points the size of n pixels
glEnable(GL_POINT_SMOOTH); //makes the points rounded
glColor3f(0.0,1.0,0.0);
for( ; par_iter != par_end; ++par_iter)
{
par = *par_iter;
count++;
//obtains the color of the particle based on probability
probToColor(par->p,&red,&green,&blue);
glBegin(GL_POINTS);
glColor3f(red,green,blue);
glVertex2f(par->x,par->y);
glEnd();
double gx,gy,gth;
if (show_particle_sonar) { // draw the sonar reading
glBegin(GL_LINES);
glColor3f(0.8,0.8,0.0);
// get global coords!
getGlobalCoord(par->x,par->y,par->th,son_x,son_y,son_th,&gx,&gy,>h);
//cout << "Particle #" << pcount++ << endl;
//cout << "particle pose: " << par->x << "," << par->y << ","<< par->th << endl;
//cout << "sonar pose: " << gx << "," << gy << ","<< gth << endl;
//cout << "particle pose: " << par->x << "," << par->y << ","<< par->th << endl;
glVertex2f(gx,gy);
double S = par->son_dist;
glVertex2f(gx + S*cos((PI/180)*(gth)), gy + S*sin((PI/180)*(gth)));
glEnd();
}
// in either case draw the direction it's facing
glBegin(GL_LINES);
glColor3f(red,green,blue);
glVertex2f(par->x,par->y);
double S = 10.0;
glVertex2f(par->x + S*cos((PI/180)*(par->th)), par->y + S*sin((PI/180)*(par->th)));
glEnd();
}
filterIsInUse = 0;
//cout << "Number of particles drawn is " << count << endl;
}
glPopMatrix();
}
//Changes the color of the particles based on the probability
void Robot::probToColor(double p, float *r, float *g, float *b)
{
if (p > 0.5)
{
*r = 0.0f; *g = 1.0f; *b = 0.0f;
}
else
{
*r = 1.0f; *g = 0.0f; *b = 0.0f;
}
}
//Function used to convert the sensor's robot coordinates to global coordinates
void Robot::getGlobalCoord(double x, double y, double th, double *gx, double *gy, double *gth)
{
// find r/theta of sensor relative to robot's center
double r = sqrt((x*x)+(y*y));
double sensor_th = 180*atan2(y,x)/PI;
// modify robot's current position by offsets to find global pose of sensor
*gx = x_orig + r*cos(PI*(theta+sensor_th)/180);
*gy = y_orig + r*sin(PI*(theta+sensor_th)/180);
*gth = theta + th;
}
// Function used to convert the sensor's coordinates to global coordinates.
//
// Except this one lets you input a value for the robot's current "position",
// so you can use it for making sensor readings from the particles.
void Robot::getGlobalCoord(double x_orig, double y_orig, double theta,
double x, double y, double th,
double *gx, double *gy, double *gth)
{
double r = sqrt((x*x)+(y*y));
double sensor_th = 180*atan2(y,x)/PI;
*gx = x_orig + r*cos(PI*(theta+sensor_th)/180);
*gy = y_orig + r*sin(PI*(theta+sensor_th)/180);
*gth = theta + th;
}
//Function that finds the sensor readings for each sensor on the robot
// as well as the camera which senses only the red walls
void Robot::findSensorReadings()
{
double gx; //variable for the global x coordinate
double gy; //variable for the global y coordinate
double gth; //variable for the global theta coordinate
double ix; //variable for the x coordinate of intersection
double iy; //variable for the y coordinate of intersection
double dist; //variable for the distance from sensor to intersetion
int onSegment; //variable to test if intersection is on the wall
Map* map = Program::Get().map;
for (int i = 0; i < 3; i++)
{
dist = 10000000000000; //setting and resetting distance
Robot::getGlobalCoord(ir_x[i],ir_y[i],ir_th[i],&gx,&gy,>h);
map->obstacleDist(gx, gy, gth, &ix, &iy, &dist, &onSegment);
ir[i] = dist; //setting the sensor reading for distance
}
//finding the distance value for the camera
dist = 10000000000000;
Robot::getGlobalCoord(cam_x, cam_y, cam_th, &gx, &gy, >h);
map->obstacleDist(gx,gy,gth,&ix,&iy,&dist, &onSegment);
//setting the length of the camera
if(onSegment == 2)
{
cam_red = dist;
}
else
{
//no red is detected
cam_red = 0.0;
}
}
syntax highlighted by Code2HTML, v. 0.9.1