00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00012
00013
00014 #include <sys/time.h>
00015
00016 #include "stage.hh"
00017 #include "worldfile.hh"
00018 using namespace Stg;
00019
00088 static const double WATTS_KGMS = 10.0;
00089 static const double WATTS = 1.0;
00090
00091
00092
00093 static const double INTEGRATION_ERROR_MAX_X = 0.03;
00094 static const double INTEGRATION_ERROR_MAX_Y = 0.03;
00095 static const double INTEGRATION_ERROR_MAX_Z = 0.00;
00096 static const double INTEGRATION_ERROR_MAX_A = 0.05;
00097
00098 ModelPosition::ModelPosition( World* world,
00099 Model* parent,
00100 const std::string& type ) :
00101 Model( world, parent, type ),
00102
00103 velocity(),
00104 goal(0,0,0,0),
00105 control_mode( CONTROL_VELOCITY ),
00106 drive_mode( DRIVE_DIFFERENTIAL ),
00107 localization_mode( LOCALIZATION_GPS ),
00108 integration_error( drand48() * INTEGRATION_ERROR_MAX_X - INTEGRATION_ERROR_MAX_X/2.0,
00109 drand48() * INTEGRATION_ERROR_MAX_Y - INTEGRATION_ERROR_MAX_Y/2.0,
00110 drand48() * INTEGRATION_ERROR_MAX_Z - INTEGRATION_ERROR_MAX_Z/2.0,
00111 drand48() * INTEGRATION_ERROR_MAX_A - INTEGRATION_ERROR_MAX_A/2.0 ),
00112 wheelbase( 1.0 ),
00113 acceleration_bounds(),
00114 velocity_bounds(),
00115
00116 waypoints(),
00117 wpvis(),
00118 posevis()
00119 {
00120 PRINT_DEBUG2( "Constructing ModelPosition %d (%s)\n",
00121 id, typestr );
00122
00123
00124 thread_safe = false;
00125
00126
00127 for( int i=0; i<3; i++ )
00128 {
00129 velocity_bounds[i].min = -1.0;
00130 velocity_bounds[i].max = 1.0;
00131
00132 acceleration_bounds[i].min = -1.0;
00133 acceleration_bounds[i].max = 1.0;
00134 }
00135
00136 velocity_bounds[3].min = -M_PI/2.0;
00137 velocity_bounds[3].max = M_PI/2.0;
00138
00139 acceleration_bounds[3].min = -M_PI/2.0;
00140 acceleration_bounds[3].max = M_PI/2.0;
00141
00142
00143 this->SetBlobReturn( true );
00144
00145 AddVisualizer( &wpvis, true );
00146 AddVisualizer( &posevis, false );
00147 }
00148
00149
00150 ModelPosition::~ModelPosition( void )
00151 {
00152
00153 }
00154
00155 void ModelPosition::SetVelocity( const Velocity& val )
00156 {
00157 velocity = val;
00158 CallCallbacks( CB_VELOCITY );
00159 }
00160
00161
00162 void ModelPosition::Load( void )
00163 {
00164 Model::Load();
00165
00166 if( wf->PropertyExists( wf_entity, "velocity" ))
00167 SetVelocity( GetVelocity().Load( wf, wf_entity, "velocity" ));
00168
00169
00170 if( wf->PropertyExists( wf_entity, "drive" ) )
00171 {
00172 const std::string& mode_str =
00173 wf->ReadString( wf_entity, "drive", "diff" );
00174
00175 if( mode_str == "diff" )
00176 drive_mode = DRIVE_DIFFERENTIAL;
00177 else if( mode_str == "omni" )
00178 drive_mode = DRIVE_OMNI;
00179 else if( mode_str == "car" )
00180 drive_mode = DRIVE_CAR;
00181 else
00182 PRINT_ERR1( "invalid position drive mode specified: \"%s\" - should be one of: \"diff\", \"omni\" or \"car\". Using \"diff\" as default.", mode_str.c_str() );
00183
00184 }
00185
00186
00187 this->wheelbase = wf->ReadLength( wf_entity, "wheelbase", this->wheelbase );
00188
00189
00190 if( wf->PropertyExists( wf_entity, "odom" ) )
00191 {
00192 PRINT_WARN1( "the odom property is specified for model \"%s\","
00193 " but this property is no longer available."
00194 " Use localization_origin instead. See the position"
00195 " entry in the manual or src/model_position.c for details.",
00196 this->Token() );
00197 }
00198
00199
00200
00201
00202 est_origin = this->GetGlobalPose();
00203 est_origin.Load( wf, wf_entity, "localization_origin" );
00204
00205
00206 const Pose gpose = this->GetGlobalPose();
00207
00208 est_pose.a = normalize( gpose.a - est_origin.a );
00209 const double cosa = cos(est_origin.a);
00210 const double sina = sin(est_origin.a);
00211 const double dx = gpose.x - est_origin.x;
00212 const double dy = gpose.y - est_origin.y;
00213 est_pose.x = dx * cosa + dy * sina;
00214 est_pose.y = dy * cosa - dx * sina;
00215
00216
00217 est_pose_error.Zero();
00218
00219
00220
00221 integration_error.Load( wf, wf_entity, "odom_error" );
00222
00223
00224 if( wf->PropertyExists( wf_entity, "localization" ) )
00225 {
00226 const std::string& loc_str =
00227 wf->ReadString( wf_entity, "localization", "gps" );
00228
00229 if( loc_str == "gps" )
00230 localization_mode = LOCALIZATION_GPS;
00231 else if( loc_str == "odom" )
00232 localization_mode = LOCALIZATION_ODOM;
00233 else
00234 PRINT_ERR2( "unrecognized localization mode \"%s\" for model \"%s\"."
00235 " Valid choices are \"gps\" and \"odom\".",
00236 loc_str.c_str(), this->Token() );
00237 }
00238
00239
00240
00241 wf->ReadTuple( wf_entity, "acceleration_bounds", 0, 8, "llllllaa",
00242 &acceleration_bounds[0].min,
00243 &acceleration_bounds[0].max,
00244 &acceleration_bounds[1].min,
00245 &acceleration_bounds[1].max,
00246 &acceleration_bounds[2].min,
00247 &acceleration_bounds[2].max,
00248 &acceleration_bounds[3].min,
00249 &acceleration_bounds[3].max );
00250
00251 wf->ReadTuple( wf_entity, "velocity_bounds", 0, 8, "llllllaa",
00252 &velocity_bounds[0].min,
00253 &velocity_bounds[0].max,
00254 &velocity_bounds[1].min,
00255 &velocity_bounds[1].max,
00256 &velocity_bounds[2].min,
00257 &velocity_bounds[2].max,
00258 &velocity_bounds[3].min,
00259 &velocity_bounds[3].max );
00260 }
00261
00262 void ModelPosition::Update( void )
00263 {
00264 PRINT_DEBUG1( "[%lu] position update", this->world->sim_time );
00265
00266
00267 Velocity vel(0,0,0,0);
00268
00269 if( this->subs )
00270 {
00271 switch( control_mode )
00272 {
00273 case CONTROL_ACCELERATION:
00274 {
00275
00276 goal.x = std::min( goal.x, acceleration_bounds[0].max );
00277 goal.x = std::max( goal.x, acceleration_bounds[0].min );
00278
00279 goal.y = std::min( goal.y, acceleration_bounds[1].max );
00280 goal.y = std::max( goal.y, acceleration_bounds[1].min );
00281
00282 goal.z = std::min( goal.z, acceleration_bounds[2].max );
00283 goal.z = std::max( goal.z, acceleration_bounds[2].min );
00284
00285 goal.a = std::min( goal.a, acceleration_bounds[3].max );
00286 goal.a = std::max( goal.a, acceleration_bounds[3].min );
00287
00288 vel = this->velocity;
00289
00290
00291 const double interval( (double)world->sim_interval / 1e6 );
00292
00293 PRINT_DEBUG( "acceleration control mode" );
00294 PRINT_DEBUG4( "model %s command(%.2f %.2f %.2f)",
00295 this->Token(),
00296 this->goal.x,
00297 this->goal.y,
00298
00299 this->goal.a );
00300
00301 switch( drive_mode )
00302 {
00303 case DRIVE_DIFFERENTIAL:
00304
00305 vel.x += goal.x * interval;
00306 vel.y = 0;
00307 vel.a += goal.a * interval;
00308 break;
00309
00310 case DRIVE_OMNI:
00311
00312 vel.x += goal.x * interval;
00313 vel.y += goal.y * interval;
00314 vel.a += goal.a * interval;
00315 break;
00316
00317 case DRIVE_CAR:
00318 PRINT_ERR( "car drive not supported in accelerartion control [to do]" );
00319
00320
00321
00322
00323 break;
00324
00325 default:
00326 PRINT_ERR1( "unknown steering mode %d", drive_mode );
00327 }
00328
00329
00330
00331
00332
00333
00334 } break;
00335
00336
00337 case CONTROL_VELOCITY :
00338 {
00339 PRINT_DEBUG( "velocity control mode" );
00340 PRINT_DEBUG4( "model %s command(%.2f %.2f %.2f)",
00341 this->Token(),
00342 this->goal.x,
00343 this->goal.y,
00344 this->goal.a );
00345
00346 switch( drive_mode )
00347 {
00348 case DRIVE_DIFFERENTIAL:
00349
00350 vel.x = goal.x;
00351 vel.y = 0;
00352 vel.a = goal.a;
00353 break;
00354
00355 case DRIVE_OMNI:
00356
00357 vel.x = goal.x;
00358 vel.y = goal.y;
00359 vel.a = goal.a;
00360 break;
00361
00362 case DRIVE_CAR:
00363
00364 vel.x = goal.x * cos(goal.a);
00365 vel.y = 0;
00366 vel.a = goal.x * sin(goal.a)/wheelbase;
00367 break;
00368
00369 default:
00370 PRINT_ERR1( "unknown steering mode %d", drive_mode );
00371 }
00372 } break;
00373
00374 case CONTROL_POSITION:
00375 {
00376 PRINT_DEBUG( "position control mode" );
00377
00378 double x_error = goal.x - est_pose.x;
00379 double y_error = goal.y - est_pose.y;
00380 double a_error = normalize( goal.a - est_pose.a );
00381
00382 PRINT_DEBUG3( "errors: %.2f %.2f %.2f\n", x_error, y_error, a_error );
00383
00384
00385
00386 double max_speed_x = 0.4;
00387 double max_speed_y = 0.4;
00388 double max_speed_a = 1.0;
00389
00390 switch( drive_mode )
00391 {
00392 case DRIVE_OMNI:
00393 {
00394
00395
00396
00397 vel.x = std::min( x_error, max_speed_x );
00398 vel.y = std::min( y_error, max_speed_y );
00399 vel.a = std::min( a_error, max_speed_a );
00400 }
00401 break;
00402
00403 case DRIVE_DIFFERENTIAL:
00404 {
00405
00406
00407
00408
00409
00410
00411
00412
00413 Velocity calc;
00414 double close_enough = 0.02;
00415
00416
00417 if( fabs(x_error) < close_enough && fabs(y_error) < close_enough )
00418 {
00419 PRINT_DEBUG( "TURNING ON THE SPOT" );
00420
00421 calc.a = std::min( a_error, max_speed_a );
00422 calc.a = std::max( a_error, -max_speed_a );
00423 }
00424 else
00425 {
00426 PRINT_DEBUG( "TURNING TO FACE THE GOAL POINT" );
00427
00428 double goal_angle = atan2( y_error, x_error );
00429 double goal_distance = hypot( y_error, x_error );
00430
00431 a_error = normalize( goal_angle - est_pose.a );
00432 calc.a = std::min( a_error, max_speed_a );
00433 calc.a = std::max( a_error, -max_speed_a );
00434
00435 PRINT_DEBUG2( "steer errors: %.2f %.2f \n", a_error, goal_distance );
00436
00437
00438
00439 if( fabs(a_error) < M_PI/16 )
00440 {
00441 PRINT_DEBUG( "DRIVING TOWARDS THE GOAL" );
00442 calc.x = std::min( goal_distance, max_speed_x );
00443 }
00444 }
00445
00446
00447
00448 vel.x = calc.x;
00449 vel.y = 0;
00450 vel.a = calc.a;
00451 }
00452 break;
00453
00454 default:
00455 PRINT_ERR1( "unknown steering mode %d", (int)drive_mode );
00456 }
00457 }
00458 break;
00459
00460 default:
00461 PRINT_ERR1( "unrecognized position command mode %d", control_mode );
00462 }
00463
00464
00465 watts = WATTS +
00466 fabs(vel.x) * WATTS_KGMS * mass +
00467 fabs(vel.y) * WATTS_KGMS * mass +
00468 fabs(vel.a) * WATTS_KGMS * mass;
00469
00470
00471
00472
00473
00474
00475
00476
00477 vel.x = velocity_bounds[0].Constrain( vel.x );
00478 vel.y = velocity_bounds[1].Constrain( vel.y );
00479 vel.z = velocity_bounds[2].Constrain( vel.z );
00480 vel.a = velocity_bounds[3].Constrain( vel.a );
00481
00482
00483
00484
00485 this->SetVelocity( vel );
00486 }
00487
00488 switch( localization_mode )
00489 {
00490 case LOCALIZATION_GPS:
00491 {
00492
00493 Pose gpose = this->GetGlobalPose();
00494
00495 est_pose.a = normalize( gpose.a - est_origin.a );
00496 double cosa = cos(est_origin.a);
00497 double sina = sin(est_origin.a);
00498 double dx = gpose.x - est_origin.x;
00499 double dy = gpose.y - est_origin.y;
00500 est_pose.x = dx * cosa + dy * sina;
00501 est_pose.y = dy * cosa - dx * sina;
00502
00503 }
00504 break;
00505
00506 case LOCALIZATION_ODOM:
00507 {
00508
00509 double dt = world->sim_interval / 1e6;
00510
00511 est_pose.a = normalize( est_pose.a + (vel.a * dt) * (1.0 +integration_error.a) );
00512
00513 double cosa = cos(est_pose.a);
00514 double sina = sin(est_pose.a);
00515 double dx = (vel.x * dt) * (1.0 + integration_error.x );
00516 double dy = (vel.y * dt) * (1.0 + integration_error.y );
00517
00518 est_pose.x += dx * cosa + dy * sina;
00519 est_pose.y -= dy * cosa - dx * sina;
00520 }
00521 break;
00522
00523 default:
00524 PRINT_ERR2( "unknown localization mode %d for model %s\n",
00525 localization_mode, Token() );
00526 break;
00527 }
00528
00529 PRINT_DEBUG3( " READING POSITION: [ %.4f %.4f %.4f ]\n",
00530 est_pose.x, est_pose.y, est_pose.a );
00531
00532 Model::Update();
00533 }
00534
00535 void ModelPosition::Move( void )
00536 {
00537 if( velocity.IsZero() )
00538 return;
00539
00540 if( disabled )
00541 return;
00542
00543
00544 const double interval( (double)world->sim_interval / 1e6 );
00545
00546
00547 const Pose p( velocity.x * interval,
00548 velocity.y * interval,
00549 velocity.z * interval,
00550 normalize( velocity.a * interval ));
00551
00552
00553 const Pose newpose( pose + p );
00554
00555
00556 const Pose startpose( pose );
00557
00558 pose = newpose;
00559
00560 const unsigned int layer( world->UpdateCount()%2 );
00561
00562 UnMapWithChildren( layer );
00563 MapWithChildren( layer );
00564
00565 if( TestCollision() )
00566 {
00567
00568
00569 pose = startpose;
00570 UnMapWithChildren( layer );
00571 MapWithChildren( layer );
00572
00573 SetStall(true);
00574 }
00575 else
00576 {
00577
00578 SetStall(false);
00579 }
00580 }
00581
00582
00583 void ModelPosition::Startup( void )
00584 {
00585 world->active_velocity.insert( this );
00586
00587 Model::Startup();
00588
00589 PRINT_DEBUG( "position startup" );
00590 }
00591
00592 void ModelPosition::Shutdown( void )
00593 {
00594 PRINT_DEBUG( "position shutdown" );
00595
00596
00597 goal.Zero();
00598 velocity.Zero();
00599
00600 world->active_velocity.erase( this );
00601
00602 Model::Shutdown();
00603 }
00604
00605 void ModelPosition::Stop()
00606 {
00607 SetSpeed( 0,0,0 );
00608 }
00609
00610 void ModelPosition::SetSpeed( double x, double y, double a )
00611 {
00612 control_mode = CONTROL_VELOCITY;
00613 goal.x = x;
00614 goal.y = y;
00615 goal.z = 0;
00616 goal.a = a;
00617 }
00618
00619 void ModelPosition::SetXSpeed( double x )
00620 {
00621 control_mode = CONTROL_VELOCITY;
00622 goal.x = x;
00623 }
00624
00625 void ModelPosition::SetYSpeed( double y )
00626 {
00627 control_mode = CONTROL_VELOCITY;
00628 goal.y = y;
00629 }
00630
00631 void ModelPosition::SetZSpeed( double z )
00632 {
00633 control_mode = CONTROL_VELOCITY;
00634 goal.z = z;
00635 }
00636
00637 void ModelPosition::SetTurnSpeed( double a )
00638 {
00639 control_mode = CONTROL_VELOCITY;
00640 goal.a = a;
00641 }
00642
00643 void ModelPosition::SetSpeed( Velocity vel )
00644 {
00645 control_mode = CONTROL_VELOCITY;
00646 goal.x = vel.x;
00647 goal.y = vel.y;
00648 goal.z = vel.z;
00649 goal.a = vel.a;
00650 }
00651
00652 void ModelPosition::GoTo( double x, double y, double a )
00653 {
00654 control_mode = CONTROL_POSITION;
00655 goal.x = x;
00656 goal.y = y;
00657 goal.z = 0;
00658 goal.a = a;
00659 }
00660
00661 void ModelPosition::GoTo( Pose pose )
00662 {
00663 control_mode = CONTROL_POSITION;
00664 goal = pose;
00665 }
00666
00667 void ModelPosition::SetAcceleration( double x, double y, double a )
00668 {
00669 control_mode = CONTROL_ACCELERATION;
00670 goal.x = x;
00671 goal.y = y;
00672 goal.z = 0;
00673 goal.a = a;
00674 }
00675
00679 void ModelPosition::SetOdom( Pose odom )
00680 {
00681 est_pose = odom;
00682
00683
00684 const Pose gp = GetGlobalPose();
00685 const double da = normalize( -odom.a + gp.a );
00686 const double dx = -odom.x * cos(da) + odom.y * sin(da);
00687 const double dy = -odom.y * cos(da) - odom.x * sin(da);
00688
00689
00690 est_origin.x = gp.x + dx;
00691 est_origin.y = gp.y + dy;
00692 est_origin.a = da;
00693 }
00694
00695 ModelPosition::PoseVis::PoseVis()
00696 : Visualizer( "Position coordinates", "show_position_coords" )
00697 {}
00698
00699 void ModelPosition::PoseVis::Visualize( Model* mod, Camera* cam )
00700 {
00701 (void)cam;
00702
00703 ModelPosition* pos = dynamic_cast<ModelPosition*>(mod);
00704
00705
00706 glPushMatrix();
00707
00708
00709 Gl::pose_inverse_shift( pos->GetGlobalPose() );
00710
00711 Gl::pose_shift( pos->est_origin );
00712 pos->PushColor( 1,0,0,1 );
00713 Gl::draw_origin( 0.5 );
00714
00715 glEnable (GL_LINE_STIPPLE);
00716 glLineStipple (3, 0xAAAA);
00717
00718 pos->PushColor( 1,0,0,0.5 );
00719 glBegin( GL_LINE_STRIP );
00720 glVertex2f( 0,0 );
00721 glVertex2f( pos->est_pose.x, 0 );
00722 glVertex2f( pos->est_pose.x, pos->est_pose.y );
00723 glEnd();
00724
00725 glDisable(GL_LINE_STIPPLE);
00726
00727 char label[64];
00728 snprintf( label, 64, "x:%.3f", pos->est_pose.x );
00729 Gl::draw_string( pos->est_pose.x / 2.0, -0.5, 0, label );
00730
00731 snprintf( label, 64, "y:%.3f", pos->est_pose.y );
00732 Gl::draw_string( pos->est_pose.x + 0.5 , pos->est_pose.y / 2.0, 0, (const char*)label );
00733
00734 pos->PopColor();
00735
00736 Gl::pose_shift( pos->est_pose );
00737 pos->PushColor( 0,1,0,1 );
00738 Gl::draw_origin( 0.5 );
00739 pos->PopColor();
00740
00741 Gl::pose_shift( pos->geom.pose );
00742 pos->PushColor( 0,0,1,1 );
00743 Gl::draw_origin( 0.5 );
00744 pos->PopColor();
00745
00746 Color c = pos->color;
00747 c.a = 0.5;
00748 pos->PushColor( c );
00749
00750 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
00751 pos->blockgroup.DrawFootPrint( pos->geom );
00752 pos->PopColor();
00753
00754 glPopMatrix();
00755 }
00756
00757
00758 ModelPosition::WaypointVis::WaypointVis()
00759 : Visualizer( "Position waypoints", "show_position_waypoints" )
00760 {}
00761
00762 void ModelPosition::WaypointVis::Visualize( Model* mod, Camera* cam )
00763 {
00764 (void)cam;
00765
00766 ModelPosition* pos = dynamic_cast<ModelPosition*>(mod);
00767 const std::vector<Waypoint>& waypoints = pos->waypoints;
00768
00769 if( waypoints.empty() )
00770 return;
00771
00772 glPointSize( 5 );
00773 glPushMatrix();
00774 pos->PushColor( pos->color );
00775
00776 Gl::pose_inverse_shift( pos->pose );
00777 Gl::pose_shift( pos->est_origin );
00778
00779 glTranslatef( 0,0,0.02 );
00780
00781
00782 glLineWidth( 3 );
00783 FOR_EACH( it, waypoints )
00784 it->Draw();
00785 glLineWidth( 1 );
00786
00787
00788 const size_t num(waypoints.size());
00789 if( num > 1 )
00790 {
00791 pos->PushColor( 1,0,0,0.3 );
00792 glBegin( GL_LINES );
00793
00794 for( size_t i(1); i<num ; i++ )
00795 {
00796 Pose p = waypoints[i].pose;
00797 Pose o = waypoints[i-1].pose;
00798
00799 glVertex2f( p.x, p.y );
00800 glVertex2f( o.x, o.y );
00801 }
00802
00803 glEnd();
00804
00805 pos->PopColor();
00806 }
00807
00808 pos->PopColor();
00809 glPopMatrix();
00810 }
00811
00812 ModelPosition::Waypoint::Waypoint( const Pose& pose, Color color )
00813 : pose(pose), color(color)
00814 {
00815 }
00816
00817 ModelPosition::Waypoint::Waypoint( meters_t x, meters_t y, meters_t z, radians_t a, Color color )
00818 : pose(x,y,z,a), color(color)
00819 {
00820 }
00821
00822
00823 ModelPosition::Waypoint::Waypoint()
00824 : pose(), color()
00825 {
00826 };
00827
00828
00829 void ModelPosition::Waypoint::Draw() const
00830 {
00831 GLdouble d[4];
00832
00833 d[0] = color.r;
00834 d[1] = color.g;
00835 d[2] = color.b;
00836 d[3] = color.a;
00837
00838 glColor4dv( d );
00839
00840 glBegin(GL_POINTS);
00841 glVertex3f( pose.x, pose.y, pose.z );
00842 glEnd();
00843
00844 meters_t quiver_length = 0.15;
00845
00846 double dx = cos(pose.a) * quiver_length;
00847 double dy = sin(pose.a) * quiver_length;
00848
00849 glBegin(GL_LINES);
00850 glVertex3f( pose.x, pose.y, pose.z );
00851 glVertex3f( pose.x+dx, pose.y+dy, pose.z );
00852 glEnd();
00853 }