Permalink
Browse files

DW work

  • Loading branch information...
1 parent 0c0b143 commit cd27a447f8e40de6ff1e59733f6e5569c818584e @rtv committed Dec 5, 2011
Showing with 34 additions and 21 deletions.
  1. +19 −13 examples/ctrl/dw.cc
  2. +15 −8 worlds/dw.world
View
@@ -8,12 +8,12 @@ template<typename T> int sgn(T val)
}
// number of different velocities and turn rates to consider
-const double vres = 16.0;
-const double wres = 16.0;
+const double vres = 32.0;
+const double wres = 32.0;
const double safety = 0.7;
-const double obstacle_dist_max = 10.0; // a bit more than the max sensor range
+const double obstacle_dist_max = 1.0; // a bit more than the max sensor range
// time interval between updates
double interval = 0.1;
@@ -180,8 +180,8 @@ bool PointInPolygon( int polySides, float polyX[], float polyY[], float x, float
bool oddNodes=false;
for (i=0; i<polySides; i++) {
- if ((polyY[i]< y && polyY[j]>=y
- || polyY[j]< y && polyY[i]>=y)
+ if (((polyY[i]< y && polyY[j]>=y)
+ || (polyY[j]< y && polyY[i]>=y))
&& (polyX[i]<=x || polyX[j]<=x)) {
oddNodes^=(polyX[i]+(y-polyY[i])/(polyY[j]-polyY[i])*(polyX[j]-polyX[i])<x); }
j=i; }
@@ -339,7 +339,7 @@ class Vel
// default value will be overridden if this trajectory hits an obstacle
obstacle_dist = obstacle_dist_max;
- if( fabs(w) > 0.01 )
+ // if( fabs(w) > 0.01 )
{
if( cloud_hits.size() )
@@ -473,12 +473,19 @@ class Robot
const double wrange = wmax - wmin;
const double wincr = wrange / wres;
+ int a=0;
+
for( double v=vmin; v<vmax; v += vincr )
for( double w=wmin; w<wmax; w += wincr )
{
- if( w == 0 )
- w = 0.01;
-
+ if( fabs(w) < 0.001)
+ if( a > 0 )
+ w = 0.001;
+ else
+ w = -0.01;
+
+ a = (a++ % 2 ); // a alternates between 0 and 1
+
vspace.push_back( Vel( v, w ) );
}
}
@@ -497,7 +504,6 @@ class Robot
gdx = goal.x - gpose.x;
gdy = goal.y - gpose.y;
-
const double goal_dist = hypot( gdx, gdy );
const double goal_angle = normalize( atan2( gdy, gdx ) - gpose.a );
@@ -640,7 +646,7 @@ void Mesh( const std::vector<double>& arr, unsigned int w, unsigned int h, doubl
}
-void DrawMesh( double x, double y, double z, const std::vector<Vel>& vels )
+void DrawMesh( double x, double y, double z, const std::vector<Vel>& vels, const Vel& best_vel )
{
glPushMatrix();
glTranslatef( x,y,z );
@@ -649,7 +655,7 @@ void DrawMesh( double x, double y, double z, const std::vector<Vel>& vels )
double sx = 1.0 / vres;;
double sy = 1.0 / wres;
- double sz = 0.03;
+ //double sz = 0.03;
int side = sqrt( vels.size() );
@@ -864,7 +870,7 @@ class DWVis : public Visualizer
// glEnd();
glColor3f( 0,0,1 );
- DrawMesh( 0,0,1, rob.vspace );
+ DrawMesh( 0,0,1, rob.vspace, *rob.best_vel );
glPopMatrix();
View
@@ -17,8 +17,8 @@ resolution 0.02
window
(
size [ 635 666 ] # in pixels
- scale 79.890 # pixels per meter
- center [ 5.590 5.246 ]
+ scale 64.907 # pixels per meter
+ center [ 4.743 4.205 ]
rotate [ 0.500 0.500 ]
show_data 1 # 1=on 0=off
@@ -34,12 +34,8 @@ floorplan
)
-pioneer2dx
+define rob pioneer2dx
(
- # can refer to the robot by this name
- name "r0"
- pose [ 5.978 6.139 0.000 41.349 ]
-
# pioneer2dx's sonars will be ranger:0 and the laser will be ranger:1
sicklaser( pose [ 0.000 0.000 0.000 0.000 ] )
@@ -51,6 +47,17 @@ pioneer2dx
localization "gps"
localization_origin [ 0 0 0 0 ]
- velocity_bounds [ 0.0 1.0 0 0 0 0 -45 45 ]
+ velocity_bounds [ 0.1 1.0 0 0 0 0 -45 45 ]
acceleration_bounds [ -1 1 0 0 0 0 -90 90 ]
)
+
+
+rob( pose [ 2.372 6.622 0.000 41.349 ] )
+rob( pose [ 2.097 3.369 0.000 41.349 ] )
+rob( pose [ 2.738 4.965 0.000 41.349 ] )
+rob( pose [ 4.239 7.083 0.000 41.349 ] )
+rob( pose [ 5.946 7.074 0.000 41.349 ] )
+rob( pose [ 4.319 6.008 0.000 41.349 ] )
+rob( pose [ 3.679 4.342 0.000 41.349 ] )
+rob( pose [ 5.144 5.220 0.000 41.349 ] )
+

0 comments on commit cd27a44

Please sign in to comment.