Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

ported to Stage 4

  • Loading branch information...
commit 2198de7bbd2d83f95934917027251525ca67fc06 1 parent 419a7a6
@rtv authored
Showing with 148 additions and 152 deletions.
  1. +91 −103 pioneer.inc
  2. +57 −49 wheel.cc
View
194 pioneer.inc
@@ -4,115 +4,105 @@
# CVS: $Id: pioneer.inc,v 1.30 2008-01-15 01:25:42 rtv Exp $
# The Pioneer2DX sonar array
-define p2dx_sonar ranger
+
+# prototype for p2dx sonar sensors
+define p2dxsonar sensor
(
- scount 16 # the number of transducers
-
- # define the pose of each transducer [xpos ypos heading]
- spose[0] [ 0.075 0.130 90 ]
- spose[1] [ 0.115 0.115 50 ]
- spose[2] [ 0.150 0.080 30 ]
- spose[3] [ 0.170 0.025 10 ]
- spose[4] [ 0.170 -0.025 -10 ]
- spose[5] [ 0.150 -0.080 -30 ]
- spose[6] [ 0.115 -0.115 -50 ]
- spose[7] [ 0.075 -0.130 -90 ]
- spose[8] [ -0.155 -0.130 -90 ]
- spose[9] [ -0.195 -0.115 -130 ]
- spose[10] [ -0.230 -0.080 -150 ]
- spose[11] [ -0.250 -0.025 -170 ]
- spose[12] [ -0.250 0.025 170 ]
- spose[13] [ -0.230 0.080 150 ]
- spose[14] [ -0.195 0.115 130 ]
- spose[15] [ -0.155 0.130 90 ]
-
- # define the field of view of each transducer [range_min range_max view_angle]
- sview [0 5.0 15]
+ # define the size of each transducer [xsize ysize zsize] in meters
+ size [0.01 0.05 0.01 ]
+ # define the range bounds [min max]
+ range [0 5.0]
+ # define the angular field of view in degrees
+ fov 15
+ # define the number of samples spread over the fov
+ samples 1
+ # define the color that ranges are drawn in the gui
+ color_rgba [ 0 1 0 0.2 ]
+)
- # define the size of each transducer [xsize ysize] in meters
- ssize [0.01 0.05]
+define p2dx_sonar ranger
+(
+ # 16 transducers spread about the robot as follows
+ p2dxsonar( pose [ 0.075 0.130 0 90 ] )
+ p2dxsonar( pose [ 0.115 0.115 0 50 ] )
+ p2dxsonar( pose [ 0.150 0.080 0 30 ] )
+ p2dxsonar( pose [ 0.170 0.025 0 10 ] )
+ p2dxsonar( pose [ 0.170 -0.025 0 -10 ] )
+ p2dxsonar( pose [ 0.150 -0.080 0 -30 ] )
+ p2dxsonar( pose [ 0.115 -0.115 0 -50 ] )
+ p2dxsonar( pose [ 0.075 -0.130 0 -90 ] )
+ p2dxsonar( pose [ -0.155 -0.130 0 -90 ] )
+ p2dxsonar( pose [ -0.195 -0.115 0 -130 ] )
+ p2dxsonar( pose [ -0.230 -0.080 0 -150 ] )
+ p2dxsonar( pose [ -0.250 -0.025 0 -170 ] )
+ p2dxsonar( pose [ -0.250 0.025 0 170 ] )
+ p2dxsonar( pose [ -0.230 0.080 0 150 ] )
+ p2dxsonar( pose [ -0.195 0.115 0 130 ] )
+ p2dxsonar( pose [ -0.155 0.130 0 90 ] )
)
define p2dx_sonar_front ranger
(
- scount 8 # the number of transducers
-
- # define the pose of each transducer [xpos ypos heading]
- spose[0] [ 0.075 0.130 90 ]
- spose[1] [ 0.115 0.115 50 ]
- spose[2] [ 0.150 0.080 30 ]
- spose[3] [ 0.170 0.025 10 ]
- spose[4] [ 0.170 -0.025 -10 ]
- spose[5] [ 0.150 -0.080 -30 ]
- spose[6] [ 0.115 -0.115 -50 ]
- spose[7] [ 0.075 -0.130 -90 ]
-
- # define the field of view of each transducer [range_min range_max view_angle]
- sview [0 5.0 15]
-
- # define the size of each transducer [xsize ysize] in meters
- ssize [0.01 0.05]
+ # 8 transducers spread about the robot as follows
+ p2dxsonar( pose [ 0.075 0.130 0 90 ] )
+ p2dxsonar( pose [ 0.115 0.115 0 50 ] )
+ p2dxsonar( pose [ 0.150 0.080 0 30 ] )
+ p2dxsonar( pose [ 0.170 0.025 0 10 ] )
+ p2dxsonar( pose [ 0.170 -0.025 0 -10 ] )
+ p2dxsonar( pose [ 0.150 -0.080 0 -30 ] )
+ p2dxsonar( pose [ 0.115 -0.115 0 -50 ] )
+ p2dxsonar( pose [ 0.075 -0.130 0 -90 ] )
)
+# prototype for p2dx sonar sensors
+define p3dxsonar sensor
+(
+ # define the size of each transducer [xsize ysize] in meters
+ size [0.01 0.04]
+ # define the field of view of each transducer [range_min range_max view_angle]
+ view [0.1 5.0 30] # min (m), max (m), field of view (deg)
+ )
# The Pioneer3DX sonar array
define p3dx_sonar ranger
(
- scount 16
-
- # define the pose of each transducer [xpos ypos heading]
- spose[0] [ 0.069 0.136 90 ]
- spose[1] [ 0.114 0.119 50 ]
- spose[2] [ 0.148 0.078 30 ]
- spose[3] [ 0.166 0.027 10 ]
- spose[4] [ 0.166 -0.027 -10 ]
- spose[5] [ 0.148 -0.078 -30 ]
- spose[6] [ 0.114 -0.119 -50 ]
- spose[7] [ 0.069 -0.136 -90 ]
- spose[8] [ -0.157 -0.136 -90 ]
- spose[9] [ -0.203 -0.119 -130 ]
- spose[10] [ -0.237 -0.078 -150 ]
- spose[11] [ -0.255 -0.027 -170 ]
- spose[12] [ -0.255 0.027 170 ]
- spose[13] [ -0.237 0.078 150 ]
- spose[14] [ -0.103 0.119 130 ]
- spose[15] [ -0.157 0.136 90 ]
-
- # define the field of view of each transducer [range_min range_max view_angle]
- sview [0.1 5.0 30] # min (m), max (m), field of view (deg)
-
- # define the size of each transducer [xsize ysize] in meters
- ssize [0.01 0.04]
+ p3dxsonar ( pose [ 0.069 0.136 0 90 ] )
+ p3dxsonar ( pose [ 0.114 0.119 0 50 ] )
+ p3dxsonar ( pose [ 0.148 0.078 0 30 ] )
+ p3dxsonar ( pose [ 0.166 0.027 0 10 ] )
+ p3dxsonar ( pose [ 0.166 -0.027 0 -10 ] )
+ p3dxsonar ( pose [ 0.148 -0.078 0 -30 ] )
+ p3dxsonar ( pose [ 0.114 -0.119 0 -50 ] )
+ p3dxsonar ( pose [ 0.069 -0.136 0 -90 ] )
+ p3dxsonar ( pose [ -0.157 -0.136 0 -90 ] )
+ p3dxsonar ( pose [ -0.203 -0.119 0 -130 ] )
+ p3dxsonar ( pose [ -0.237 -0.078 0 -150 ] )
+ p3dxsonar ( pose [ -0.255 -0.027 0 -170 ] )
+ p3dxsonar ( pose [ -0.255 0.027 0 170 ] )
+ p3dxsonar ( pose [ -0.237 0.078 0 150 ] )
+ p3dxsonar ( pose [ -0.103 0.119 0 130 ] )
+ p3dxsonar ( pose [ -0.157 0.136 0 90 ] )
)
# The Pioneer3AT sonar array
define p3at_sonar ranger
(
- scount 16
-
- # define the pose of each transducer [xpos ypos heading]
- spose[0] [0.147 0.136 90]
- spose[1] [0.193 0.119 50]
- spose[2] [0.227 0.079 30]
- spose[3] [0.245 0.027 10]
- spose[4] [0.245 -0.027 -10]
- spose[5] [0.227 -0.079 -30]
- spose[6] [0.193 -0.119 -50]
- spose[7] [0.147 -0.136 -90]
- spose[8] [-0.144 -0.136 -90]
- spose[9] [-0.189 -0.119 -130]
- spose[10] [-0.223 -0.079 -150]
- spose[11] [-0.241 -0.027 -170]
- spose[12] [-0.241 0.027 170]
- spose[13] [-0.223 0.079 150]
- spose[14] [-0.189 0.119 130]
- spose[15] [-0.144 0.136 90]
-
- # define the field of view of each transducer [range_min range_max view_angle]
- sview [0.1 5.0 30] # min (m), max (m), field of view (deg)
-
- # define the size of each transducer [xsize ysize] in meters
- ssize [0.01 0.04]
+ p3dxsonar ( pose [0.147 0.136 0 90] )
+ p3dxsonar ( pose [0.193 0.119 0 50] )
+ p3dxsonar ( pose [0.227 0.079 0 30] )
+ p3dxsonar ( pose [0.245 0.027 0 10] )
+ p3dxsonar ( pose [0.245 -0.027 0 -10] )
+ p3dxsonar ( pose [0.227 -0.079 0 -30] )
+ p3dxsonar ( pose [0.193 -0.119 0 -50] )
+ p3dxsonar ( pose [0.147 -0.136 0 -90] )
+ p3dxsonar ( pose [-0.144 -0.136 0 -90] )
+ p3dxsonar ( pose [-0.189 -0.119 0 -130] )
+ p3dxsonar ( pose [-0.223 -0.079 0 -150] )
+ p3dxsonar ( pose [-0.241 -0.027 0 -170] )
+ p3dxsonar ( pose [-0.241 0.027 0 170] )
+ p3dxsonar ( pose [-0.223 0.079 0 150] )
+ p3dxsonar ( pose [-0.189 0.119 0 130] )
+ p3dxsonar ( pose [-0.144 0.136 0 90] )
)
define pioneer_base position
@@ -121,8 +111,7 @@ define pioneer_base position
drive "diff" # Differential steering model.
gui_nose 1 # Draw a nose on the robot so we can see which way it points
obstacle_return 1 # Can hit things.
- laser_return 1 # reflects laser beams
- ranger_return 1 # reflects sonar beams
+ ranger_return 0.5 # reflects sonar beams
blob_return 1 # Seen by blobfinders
fiducial_return 1 # Seen as "1" fiducial finders
@@ -419,15 +408,14 @@ define pioneer3at pioneer_base
# The AmigoBot sonar array
define amigo_sonar ranger
(
- scount 8
- spose[0] [ 0.073 0.105 90 ]
- spose[1] [ 0.130 0.078 41 ]
- spose[2] [ 0.154 0.030 15 ]
- spose[3] [ 0.154 -0.030 -15 ]
- spose[4] [ 0.130 -0.078 -41 ]
- spose[5] [ 0.073 -0.105 -90 ]
- spose[6] [ -0.146 -0.060 -145 ]
- spose[7] [ -0.146 0.060 145 ]
+ p2dxsonar ( pose [ 0.073 0.105 0 90 ] )
+ p2dxsonar ( pose [ 0.130 0.078 0 41 ] )
+ p2dxsonar ( pose [ 0.154 0.030 0 15 ] )
+ p2dxsonar ( pose [ 0.154 -0.030 0 -15 ] )
+ p2dxsonar ( pose [ 0.130 -0.078 0 -41 ] )
+ p2dxsonar ( pose [ 0.073 -0.105 0 -90 ] )
+ p2dxsonar ( pose [ -0.146 -0.060 0 -145 ] )
+ p2dxsonar ( pose [ -0.146 0.060 0 145 ] )
)
define amigobot position
View
106 wheel.cc
@@ -33,9 +33,12 @@ class Robot
ModelRanger* ranger;
ModelFiducial* fiducial;
- stg_radians_t best_bearing;
- stg_meters_t best_range;
- stg_radians_t best_heading_error;
+ radians_t best_bearing;
+ meters_t best_range;
+ radians_t best_heading_error;
+ ModelFiducial::Fiducial* bestp;
+
+ double resultant_angle;
Robot( ModelPosition* p, ModelRanger* r, ModelFiducial* f)
: position(p),
@@ -44,14 +47,16 @@ class Robot
best_bearing(0.0),
best_range(1.0),
best_heading_error(0.0),
+ bestp(NULL),
+ resultant_angle(0.0),
vis(this)
{
assert(position);
assert(ranger);
assert(fiducial);
- ranger->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)RangerUpdateCb, this );
- fiducial->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)FiducialUpdateCb, this );
+ ranger->AddCallback( Model::CB_UPDATE, (model_callback_t)RangerUpdateCb, this );
+ fiducial->AddCallback( Model::CB_UPDATE, (model_callback_t)FiducialUpdateCb, this );
fiducial->AddVisualizer( &vis, true );
@@ -71,12 +76,6 @@ class Robot
virtual void Visualize( Model* mod, Camera* cam )
{
- // glPushMatrix();
- // Gl::pose_inverse_shift( mod->GetGlobalPose() );
-
- //Color c = mod->GetColor();
- //c.a = 0.4;
-
double x = robot->best_range * cos( robot->best_bearing );
double y = robot->best_range * sin( robot->best_bearing );
@@ -87,23 +86,27 @@ class Robot
double d = 0.2 * sin( robot->best_bearing + 4.0*M_PI/5.0 );
mod->PushColor( Color(0,0,0,1.0) );
- //graph.Draw();
- //mod->PopColor();
-
- GLfloat eps = 0.1;
-
- glBegin( GL_LINES );
- glVertex2f( 0,0 );
- glVertex2f( x, y );
- glEnd();
- glBegin( GL_POLYGON );
- glVertex2f( x+c, y+d );
- glVertex2f( x+a, y+b );
- glVertex2f( x, y );
- glEnd();
-
-
+ glBegin( GL_LINES );
+ glVertex2f( 0,0 );
+ glVertex2f( x, y );
+ glEnd();
+
+
+ glBegin( GL_POLYGON );
+ glVertex2f( x+c, y+d );
+ glVertex2f( x+a, y+b );
+ glVertex2f( x, y );
+ glEnd();
+
+
+ mod->PushColor( Color::green );
+ glBegin( GL_LINES );
+ glVertex2f( 0,0 );
+ glVertex2f( cos(robot->resultant_angle), sin(robot->resultant_angle) );
+ glEnd();
+ mod->PopColor();
+
// a sphere over the robot
// GLUquadric* quadric = gluNewQuadric();
// gluQuadricDrawStyle( quadric, GLU_FILL );
@@ -148,26 +151,30 @@ int Robot::RangerUpdate()
// use the front-facing sensors only
for( unsigned int i=0; i < 8; i++ )
{
- dx += sensors[i].range * cos( sensors[i].pose.a );
- dy += sensors[i].range * sin( sensors[i].pose.a );
+ dx += ( sensors[i].ranges[0]) * cos( sensors[i].pose.a );
+ dy += ( sensors[i].ranges[0]) * sin( sensors[i].pose.a );
}
- double resultant_angle = atan2( dy, dx );
+ resultant_angle = atan2( dy, dx );
double forward_speed = 0.0;
double side_speed = 0.0;
double turn_speed = EXPAND_WGAIN * resultant_angle;
// if the front is clear, drive forwards
- if( (sensors[3].range > SAFE_DIST) && // forwards
- (sensors[4].range > SAFE_DIST) &&
- (sensors[5].range > SAFE_DIST ) && //
- (sensors[6].range > SAFE_DIST/2.0) &&
- (sensors[2].range > SAFE_DIST ) &&
- (sensors[1].range > SAFE_DIST/2.0) &&
- (fabs( resultant_angle ) < SAFE_ANGLE) )
- {
- forward_speed = VSPEED;
+ if( (sensors[3].ranges[0] > SAFE_DIST) && // forwards
+ (sensors[4].ranges[0] > SAFE_DIST) &&
+ (sensors[5].ranges[0] > SAFE_DIST ) && //
+ (sensors[6].ranges[0] > SAFE_DIST/2.0) &&
+ (sensors[2].ranges[0] > SAFE_DIST ) &&
+ (sensors[1].ranges[0] > SAFE_DIST/2.0) &&
+ (fabs( resultant_angle ) < SAFE_ANGLE) )
+ {
+ if( bestp )
+ forward_speed = VSPEED;
+ // if( best_range < 4.0 )
+ //forward_speed *= 0.8;
+
//forward_speed = 0.2 * (best_range - 2.0);
@@ -196,9 +203,8 @@ int Robot::RangerUpdate()
if( fabs(turn_speed) < 0.1 )
turn_speed = drand48();
}
-
-
+
position->SetSpeed( forward_speed, side_speed, turn_speed );
return 0;
@@ -208,24 +214,26 @@ int Robot::FiducialUpdate()
{
double best = 1e9; // big - low scores are good
- ModelFiducial::Fiducial* bestp = NULL;
+ bestp = NULL;
FOR_EACH( it, fiducial->GetFiducials() )
{
ModelFiducial::Fiducial* other = &(*it);
+ // ignore things going the other way
+ if( fabs(other->geom.a) > (1.0 * (M_PI/2.0)) )
+ continue;
+
// find the squared error difference from the last favoured fiducial
- double kdist = 0.5;
- double kbear = 1.0;
- double korient = 0.0;
+ double kd = 0.5;
+ double kb = 1.0;
double dist_score = other->range - best_range;
double bear_score = anglediff( other->bearing, best_bearing );
- double orient_score = anglediff( M_PI, other->bearing );
+ //double orient_score = anglediff( M_PI, other->bearing );
- double score = pow( kdist * dist_score +
- kbear * bear_score +
- korient * orient_score, 2.0 );
+ double score = pow( kd*dist_score + kb*bear_score,
+ 2.0 );
if( score < best ) // best score so far seen
{
Please sign in to comment.
Something went wrong with that request. Please try again.