Permalink
Browse files

kinda works.

  • Loading branch information...
1 parent 5db3f7e commit 86bb24641d9d3e591646cf91ca444e18aa1cf802 @alecain committed May 22, 2012
Showing with 110 additions and 57 deletions.
  1. +2 −2 CMakeLists.txt
  2. +4 −1 Particle.cpp
  3. +0 −1 Particle.h
  4. +1 −1 PathPlanner.cpp
  5. +1 −1 Scan.cpp
  6. +9 −0 map.cpp
  7. +2 −0 map.h
  8. +46 −7 navigator.cpp
  9. +37 −39 safeGoto.cpp
  10. +8 −5 safeGoto.h
View
@@ -5,8 +5,8 @@ add_library(Vector2d Vector2d.cpp)
add_library(Scan Scan.cpp)
target_link_libraries(Vector2d m)
-#SET (CMAKE_MODULE_PATH "/usr/local/share/cmake/Modules")
-SET (CMAKE_MODULE_PATH "/usr/local/dcs/share/cmake/Modules")
+SET (CMAKE_MODULE_PATH "/usr/local/share/cmake/Modules")
+#SET (CMAKE_MODULE_PATH "/usr/local/dcs/share/cmake/Modules")
#SET (CMAKE_MODULE_PATH "/usr/local/dcs/share/cmake/Modules:/usr/share/cmake-2.8/Modules")
SET(CMAKE_BUILD_TYPE debug)
INCLUDE (UsePlayerC++)
View
@@ -13,6 +13,8 @@
#include <GL/glut.h>
#include "util.h"
+#define SCANS_TO_PROCESS 80
+
double Particle::thetaThetaCov=0.01;
double Particle::thetaLinearCov=0.2;
double Particle::linearLinearCov=0.4;
@@ -71,6 +73,7 @@ double Particle::score(Map *map, Scan *scan){
}
double cost=0;
int toSkip = scan->scans.size()/SCANS_TO_PROCESS;
+ if(!toSkip) toSkip = 1;
for (vector<ScanNode>::iterator it= scan->scans.begin(); it < scan->scans.end(); it+= toSkip){
double mapRange= 0xFFFF;
@@ -91,7 +94,7 @@ double Particle::score(Map *map, Scan *scan){
temp = clock();
double age = (temp-creation)/CLOCKS_PER_SEC; //now minus then
- if ( age > 5 ) age = 5;
+ if ( age > 2 ) age = 2;
scoreVal -=age;
return scoreVal;
View
@@ -7,7 +7,6 @@
#ifndef _PARTICLE_H
#define _PARTICLE_H
-#define SCANS_TO_PROCESS 50
#include "Vector2d.h"
View
@@ -23,7 +23,7 @@ void PathPlanner::generatePaths(int count, double destX, double destY) {
for (int i = 0; i < count; i++) {
while (x = _map->getPixelWidth() * rand() / RAND_MAX,
y = _map->getPixelHeight() * rand() / RAND_MAX,
- _map->getPixel(x, y) != 255);
+ _map->getPixel(x, y) < 100);
_points.push_back(new PathPoint(x, y));
}
View
@@ -65,7 +65,7 @@ void Scan::addScan(double angle, double len){
double width;
if (type== SONAR){
width=toRad(20);
- if (len < .3){ //forgot this about sonars...
+ if (len == 0){ //forgot this about sonars...
len = MAX_RANGE;
}
}else{
View
@@ -26,6 +26,14 @@ int Map::yToMap(double y){
return y/this->resolution +y0;
}
+double Map::xToMeters(int x){
+ return (x-x0)*this->resolution;
+}
+
+double Map::yToMeters(int y){
+ return (y-y0)*this->resolution;
+}
+
Map::Map(string filename, string copyto, double resolution){
this->filename=copyto;
@@ -235,6 +243,7 @@ double Map::raytrace(double x0, double y0, double angle, double mrange){
return range;
}
+
double Map::getPixelWidth() {
return x;
}
View
@@ -32,6 +32,8 @@ class Map{
double raytrace(double x, double y, double angle, double maxRange);
int xToMap(double x);
int yToMap(double y);
+ double xToMeters(int x);
+ double yToMeters(int y);
double getPixelWidth();
double getPixelHeight();
double getPixelLeft();
View
@@ -30,6 +30,8 @@ using namespace std;
* Definitions
****************************************************************************
*/
+#define MIN_COV 1.0
+#define GOAL_TOLERANCE 2.0
/**
@@ -66,9 +68,9 @@ void Particle::draw(){
double x3=(x1-cos(this->theta-toRad(30))*.6);
double y3=(y1-sin(this->theta-toRad(30))*.6);
- glColor3ub(255,0,0);
+ glColor3ub(255,0,0);
glLineWidth(1);
- glBegin(GL_LINE_STRIP);
+ glBegin(GL_LINE_STRIP);
glVertex2d(windowX(x0),windowY(y0));
glVertex2d(windowX(x1),windowY(y1));
glVertex2d(windowX(x2),windowY(y2));
@@ -180,10 +182,12 @@ double getRange(int index){
// 0 will plot as white and 1 will plot as black.
Map localMap("test.pgm", "out.pgm", X_RES);
-Ploc localizer(800,3000,&localMap);
+Map confMap("conf.pgm", "cout.pgm", X_RES);
+Ploc localizer(800,2000,&localMap);
Pose estimate;
-PathPlanner planner(&localMap);
+PathPlanner planner(&confMap);
list<PathPoint *> route;
+list<PathPoint *>::const_iterator current_waypoint;
SafeGoTo nav;
static void display() {
@@ -263,8 +267,11 @@ void* robotLoop(void* args) {
double lastx=0,lasty=0,lasttheta=0;
double x,y,theta;
double dx,dy,dtheta;
+ bool position_unknown = true;
+
+ //set current nav goal to random to get us moving
+ nav.goTo(Vector2d(rand(),rand()));
- nav.goTo(Vector2d(-50,-10));
while(true) {
@@ -318,16 +325,49 @@ void* robotLoop(void* args) {
localizer.pruneParticles();
estimate = localizer.getPose(10);
- cout<<"pose: "<<estimate.origin<<endl;
+ //cout<<"pose: "<<estimate.origin<<endl;
//score each particle.
//pPosition->SetOdometry(averagex,averagey,wrap(averagetheta));
//lastx=averagex;
//lasty=averagey;
//lasttheta=wrap(averagetheta);
+
+ if (estimate.sigx < MIN_COV && estimate.sigy < MIN_COV){
+ if (position_unknown){
+ position_unknown=false;
+
+ route = planner.findRoute(localMap.xToMap(estimate.origin.x), localMap.yToMap(estimate.origin.y));
+ current_waypoint = route.begin();
+ //set current nav goal
+
+ double goalx = localMap.xToMeters((*current_waypoint)->getX());
+ double goaly = localMap.yToMeters((*current_waypoint)->getY());
+ nav.goTo(Vector2d(goalx,goaly));
+ } else {
+ position_unknown=true;
+ }
+
+ if (estimate.origin.distance(nav.goal) < GOAL_TOLERANCE ){
+ cout<<"waypoint reached"<<endl;
+ current_waypoint++;
+ if (current_waypoint == route.end()){
+ cout<<"goal reached"<<endl;
+ exit(0);
+ }
+ double goalx = localMap.xToMeters((*current_waypoint)->getX());
+ double goaly = localMap.yToMeters((*current_waypoint)->getY());
+ nav.goTo(Vector2d(goalx,goaly));
+ }
+ }
+
pthread_mutex_unlock(&particles_mut);
+
}
+
+ cout<<"Pose: "<< estimate.origin.x<< "," <<estimate.origin.y<<"->"<< estimate.origin.distance(nav.goal)<<endl;
+ //recalculate velocity
nav.update(&scans,&estimate);
nav.applyVelocity(pPosition);
@@ -363,7 +403,6 @@ int main(int argc, char *argv[]) {
pthread_mutex_init(&particles_mut, NULL);
planner.generatePaths(500, 200, 200);
- route = planner.findRoute(2000, 2000);
glutInit( &argc, argv );
glutInitDisplayMode( GLUT_RGB | GLUT_DOUBLE );
View
@@ -35,24 +35,26 @@ using namespace PlayerCc;
*/
const double SafeGoTo::dt = 0.1;//time quantum
-const double SafeGoTo::maxSpeed = .75;// m/s
+const double SafeGoTo::maxSpeed = .5;// m/s
const double SafeGoTo::maxTwist = .6;// radians per second (.157 per tick)
const double SafeGoTo::laserRes=0.00613593; //Is this right?
const double SafeGoTo::laserMin = -1.932817852;
const double SafeGoTo::laserMax = 1.932817852;
SafeGoTo::SafeGoTo(void){
- Velocity= Vector2d(0,0);
- goal= Vector2d(0,0);
+ Velocity= Vector2d(0,0);
+ goal= Vector2d(0,0);
}
//Produces a Vector2d away from the given obstacle
Vector2d SafeGoTo::avoid(Vector2d position, Vector2d toAvoid){
Vector2d ret=position.minus(toAvoid);
ret.norm();
double dist = position.distance(toAvoid);
- ret=ret.divide(dist*dist);
+ if (dist != 0){
+ ret=ret.divide(dist*dist);
+ }
return ret;
}
@@ -63,65 +65,61 @@ void SafeGoTo::goTo(Vector2d g){
int SafeGoTo::update(Scan *scan, Pose *pose){//updates the scan to be used by the navigator
- Velocity = Velocity.minus(Velocity);
double distance;
- if( (distance = goal.distance(pose->origin)) > GOAL_TOLERANCE){
-
- Vector2d goalForce;
- goalForce = goal.minus(pose->origin);
- goalForce = goalForce.norm().times(SEEK_FORCE); //normalize the force, then scale by seek weight
- goalForce = goalForce.rotate(-pose->theta); //rotate the force to be relative to the robot
+ Vector2d goalForce;
+ goalForce = goal.minus(pose->origin);
+ goalForce = goalForce.norm().times(SEEK_FORCE); //normalize the force, then scale by seek weight
+ goalForce = goalForce.rotate(-pose->theta); //rotate the force to be relative to the robot
- int toSkip = scan->scans.size()/SCANS_TO_PROCESS;
+ int toSkip = scan->scans.size()/SCANS_TO_PROCESS;
+ if(!toSkip) toSkip = 1;
- if(distance > .5){ //only avoid obstacles if we're far from our goal.. not ideal, but easier than changing weights.
- //for each scan in scan
- for (vector<ScanNode>::iterator it = scan->scans.begin(); it < scan->scans.end(); it+=toSkip){
- double dist =it->range;
+ if(distance > .5){ //only avoid obstacles if we're far from our goal.. not ideal, but easier than changing weights.
+ //for each scan in scan
+ for (vector<ScanNode>::iterator it = scan->scans.begin(); it < scan->scans.end(); it+=toSkip){
+ double dist =it->range;
- if (it->angle < toRad(90) && it->angle > toRad(-90) ){
- Vector2d position(0,0); //our position
- //generate a Vector2d pointing towards the object (from us as origin)
- Vector2d obs = it->getObstacle();
+ if (it->angle <= toRad(120) && it->angle >= toRad(-120) ){
+ Vector2d position(0,0); //our position
+ //generate a Vector2d pointing towards the object (from us as origin)
+ Vector2d obs = it->getObstacle();
- //produce a Vector2d pointing away inversely proportional to ditance
- Vector2d force = avoid(position, obs);
+ //produce a Vector2d pointing away inversely proportional to distance
+ Vector2d force = avoid(position, obs);
- //scale vector so it isn't a huge sum
- force = force.divide(SCANS_TO_PROCESS);
+ //scale vector so it isn't a huge sum
+ force = force.divide(SCANS_TO_PROCESS);
- //pay more attention to the vectors near our current heading
- force = force.times(cos(it->angle));
- Velocity = Velocity.plus(force);
- }
+ //pay more attention to the vectors near our current heading
+ force = force.times(cos(it->angle)*2);
+ Velocity = Velocity.plus(force);
}
- } else{
- //goalForce = goalForce.divide(10);
}
+ } else{
+ }
- //add a constant attractive force towards our goal
- Velocity = Velocity.plus(goalForce);
-
- //finally, divide by two to average with the previous run.
- }else{
+ //add a constant attractive force towards our goal
+ Velocity = Velocity.plus(goalForce);
+ if (Velocity.len() > maxSpeed){
+ Velocity = Velocity.norm().times(maxSpeed);
}
+ //finally, divide by two to average with the previous run.
}
void SafeGoTo::applyVelocity(Position2dProxy* pp ){
double turnRate = (Velocity.getAngle() ) *.9;
if (turnRate > this->maxTwist) turnRate=this->maxTwist;
if (turnRate < -this->maxTwist) turnRate=-this->maxTwist;
- double speed = Velocity.len() * .9;
+ double speed = Velocity.len();
if (speed>maxSpeed) speed = maxSpeed;
if(Velocity.getAngle() > PI/4 || Velocity.getAngle() < -PI/4){
pp->SetSpeed(0,turnRate);
-
}else if(Velocity.getAngle() > .1 || Velocity.getAngle() < -.1){
//angle is not small. turn and drive slowly
- pp->SetSpeed(speed * .9,turnRate);
+ pp->SetSpeed(speed * .9,turnRate*.7);
}else{
- pp->SetSpeed(speed , turnRate);
+ pp->SetSpeed(speed , turnRate*.5);
}
}
View
@@ -3,10 +3,13 @@
#ifndef _SAFEGOTO_H
#define _SAFEGOTO_H
-#define SCANS_TO_PROCESS 50
-#define GOAL_TOLERANCE .1
-#define SEEK_FORCE .5
-#define MAX_OBS_FORCE SEEK_FORCE
+#ifdef SCAN_SONAR
+ #define SCANS_TO_PROCESS 8
+#else
+ #define SCANS_TO_PROCESS 80
+#endif
+#define SEEK_FORCE 1
+#define MAX_OBS_FORCE SEEK_FORCE
#include "util.h"
#include "Scan.h"
@@ -36,10 +39,10 @@ class SafeGoTo{
static const double laserMin;
static const double laserMax;
+ Vector2d goal;
private:
Vector2d avoid(Vector2d position, Vector2d toAvoid);
Vector2d Velocity;
- Vector2d goal;

0 comments on commit 86bb246

Please sign in to comment.