Revision: 7532
http://playerstage.svn.sourceforge.net/playerstage/?rev=7532&view=rev
Author: rtv
Date: 2009-03-19 06:38:54 +0000 (Thu, 19 Mar 2009)
Log Message:
-----------
fixed localization_origin bug and cleaning up
Modified Paths:
--------------
code/stage/trunk/CMakeLists.txt
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_position.cc
code/stage/trunk/libstage/waypoint.cc
code/stage/trunk/libstage/world.cc
code/stage/trunk/stage.pc.in
Modified: code/stage/trunk/CMakeLists.txt
===================================================================
--- code/stage/trunk/CMakeLists.txt 2009-03-19 02:50:10 UTC (rev 7531)
+++ code/stage/trunk/CMakeLists.txt 2009-03-19 06:38:54 UTC (rev 7532)
@@ -14,6 +14,7 @@
OPTION (BUILD_LSPTEST "Build Player plugin tests" OFF)
OPTION (CPACK_CFG "[release building] generate CPack configuration files" OFF)
+add_definitions(-g -Wall)
# todo
# OPTION (BUILD_GUI "Build FLTK-based GUI. If OFF, build a gui-less Stage
useful e.g. for headless compute clusters." ON )
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-03-19 02:50:10 UTC (rev 7531)
+++ code/stage/trunk/libstage/model.cc 2009-03-19 06:38:54 UTC (rev 7532)
@@ -678,28 +678,27 @@
// printf( "[%llu] %s update (%d subs)\n",
// this->world->sim_time, this->token, this->subs );
- // f we're drawing current and a power pack has been installed
+ // if we're drawing current and a power pack has been installed
PowerPack* pp = FindPowerPack();
if( pp && ( watts > 0 ))
- {
- // consume energy stored in the power pack
- stg_joules_t consumed = watts * (world->interval_sim * 1e-6);
- pp->Subtract( consumed );
-
- /*
- printf ( "%s current %.2f consumed %.6f ppack @
%p [ %.2f/%.2f (%.0f)\n",
- token,
- watts,
- consumed,
- power_pack,
- power_pack->stored,
- power_pack->capacity,
- power_pack->stored / power_pack->capacity *
100.0 );
- */
-
- }
-
+ {
+ // consume energy stored in the power pack
+ stg_joules_t consumed = watts * (world->interval_sim * 1e-6);
+ pp->Subtract( consumed );
+
+ /*
+ printf ( "%s current %.2f consumed %.6f ppack @ %p [ %.2f/%.2f
(%.0f)\n",
+ token,
+ watts,
+ consumed,
+ power_pack,
+ power_pack->stored,
+ power_pack->capacity,
+ power_pack->stored / power_pack->capacity * 100.0 );
+ */
+ }
+
CallCallbacks( &hooks.update );
last_update = world->sim_time;
}
@@ -720,9 +719,6 @@
return geom.size.z + m_child;
}
-
-
-
void Model::AddToPose( double dx, double dy, double dz, double da )
{
if( dx || dy || dz || da )
@@ -832,9 +828,9 @@
Model* Model::ConditionalMove( Pose newpose )
{
- if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a
) )
- printf( "ConditionalMove bad newpose %s [%.2f %.2f %.2f %.2f]\n",
- token, newpose.x, newpose.y, newpose.z, newpose.a );
+// if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan(
pose.a ) )
+// printf( "ConditionalMove bad newpose %s [%.2f %.2f %.2f %.2f]\n",
+// token, newpose.x, newpose.y, newpose.z, newpose.a );
Pose startpose = pose;
pose = newpose; // do the move provisionally - we might undo it below
@@ -848,11 +844,10 @@
CommitTestedPose(); // shift anyrecursively commit to blocks to the new
pose
world->dirty = true; // need redraw
}
-
- if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a
) )
- printf( "ConditionalMove bad pose %s [%.2f %.2f %.2f %.2f]\n",
- token, pose.x, pose.y, pose.z, pose.a );
+// if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan(
pose.a ) )
+// printf( "ConditionalMove bad pose %s [%.2f %.2f %.2f %.2f]\n",
+// token, pose.x, pose.y, pose.z, pose.a );
return hitmod;
}
@@ -862,13 +857,13 @@
{
if( disabled )
return;
-
- if( velocity.IsZero() )
- {
- PRINT_WARN1( "model %s has velocity zero but its pose is being
updated", token );
- return;
- }
-
+
+// if( velocity.IsZero() )
+// {
+// PRINT_WARN1( "model %s has velocity zero but its pose is being
updated", token );
+// return;
+// }
+
// TODO - control this properly, and maybe do it faster
//if( 0 )
//if( (world->updates % 10 == 0) )
Modified: code/stage/trunk/libstage/model_position.cc
===================================================================
--- code/stage/trunk/libstage/model_position.cc 2009-03-19 02:50:10 UTC (rev
7531)
+++ code/stage/trunk/libstage/model_position.cc 2009-03-19 06:38:54 UTC (rev
7532)
@@ -24,52 +24,52 @@
using namespace Stg;
/**
-...@ingroup model
-...@defgroup model_position Position model
+ @ingroup model
+ @defgroup model_position Position model
-The position model simulates a
-mobile robot base. It can drive in one of two modes; either
-<i>differential</i>, i.e. able to control its speed and turn rate by
-driving left and roght wheels like a Pioneer robot, or
-<i>omnidirectional</i>, i.e. able to control each of its three axes
-independently.
+ The position model simulates a
+ mobile robot base. It can drive in one of two modes; either
+ <i>differential</i>, i.e. able to control its speed and turn rate by
+ driving left and roght wheels like a Pioneer robot, or
+ <i>omnidirectional</i>, i.e. able to control each of its three axes
+ independently.
-API: Stg::ModelPosition
+ API: Stg::ModelPosition
-<h2>Worldfile properties</h2>
+ <h2>Worldfile properties</h2>
-...@par Summary and default values
+ @par Summary and default values
-...@verbatim
-position
-(
- # position properties
- drive "diff"
- localization "gps"
- localization_origin [ <defaults to model's start pose> ]
+ @verbatim
+ position
+ (
+ # position properties
+ drive "diff"
+ localization "gps"
+ localization_origin [ <defaults to model's start pose> ]
- # odometry error model parameters,
- # only used if localization is set to "odom"
- odom_error [0.03 0.03 0.00 0.05]
+ # odometry error model parameters,
+ # only used if localization is set to "odom"
+ odom_error [0.03 0.03 0.00 0.05]
- # model properties
-)
-...@endverbatim
+ # model properties
+ )
+ @endverbatim
-...@par Note
-Since Stage-1.6.5 the odom property has been removed. Stage will generate a
warning if odom is defined in your worldfile. See localization_origin instead.
+ @par Note
+ Since Stage-1.6.5 the odom property has been removed. Stage will generate
a warning if odom is defined in your worldfile. See localization_origin instead.
-...@par Details
+ @par Details
-- drive "diff", "omni" or "car"\n
- select differential-steer model(like a Pioneer), omnidirectional mode or
carlike (velocity and steering angle).
-- localization "gps" or "odom"\n
- if "gps" the position model reports its position with perfect accuracy. If
"odom", a simple odometry model is used and position data drifts from the
ground truth over time. The odometry model is parameterized by the odom_error
property.
-- localization_origin [x y theta]
-- set the origin of the localization coordinate system. By default, this is
copied from the model's initial pose, so the robot reports its position
relative to the place it started out. Tip: If localization_origin is set to [0
0 0] and localization is "gps", the model will return its true global position.
This is unrealistic, but useful if you want to abstract away the details of
localization. Be prepared to justify the use of this mode in your research!
-- odom_error [x y theta]
-- parameters for the odometry error model used when specifying localization
"odom". Each value is the maximum proportion of error in intergrating x, y, and
theta velocities to compute odometric position estimate. For each axis, if the
the value specified here is E, the actual proportion is chosen at startup at
random in the range -E/2 to +E/2. Note that due to rounding errors, setting
these values to zero does NOT give you perfect localization - for that you need
to choose localization "gps".
- */
+ - drive "diff", "omni" or "car"\n
+ select differential-steer model(like a Pioneer), omnidirectional mode or
carlike (velocity and steering angle).
+ - localization "gps" or "odom"\n
+ if "gps" the position model reports its position with perfect accuracy. If
"odom", a simple odometry model is used and position data drifts from the
ground truth over time. The odometry model is parameterized by the odom_error
property.
+ - localization_origin [x y theta]
+ - set the origin of the localization coordinate system. By default, this
is copied from the model's initial pose, so the robot reports its position
relative to the place it started out. Tip: If localization_origin is set to [0
0 0] and localization is "gps", the model will return its true global position.
This is unrealistic, but useful if you want to abstract away the details of
localization. Be prepared to justify the use of this mode in your research!
+ - odom_error [x y theta]
+ - parameters for the odometry error model used when specifying
localization "odom". Each value is the maximum proportion of error in
intergrating x, y, and theta velocities to compute odometric position estimate.
For each axis, if the the value specified here is E, the actual proportion is
chosen at startup at random in the range -E/2 to +E/2. Note that due to
rounding errors, setting these values to zero does NOT give you perfect
localization - for that you need to choose localization "gps".
+*/
@@ -86,393 +86,389 @@
const stg_position_localization_mode_t POSITION_LOCALIZATION_DEFAULT =
STG_POSITION_LOCALIZATION_GPS;
const stg_position_drive_mode_t POSITION_DRIVE_DEFAULT =
STG_POSITION_DRIVE_DIFFERENTIAL;
-//TODO make instance attempt to register an option (as customvisualizations do)
Option ModelPosition::showCoords( "Position Coordinates", "show_coords", "",
false, NULL );
Option ModelPosition::showWaypoints( "Position Waypoints", "show_waypoints",
"", false, NULL );
-
ModelPosition::ModelPosition( World* world,
-
Model* parent )
+ Model* parent )
: Model( world, parent, MODEL_TYPE_POSITION ),
- waypoints( NULL ),
- waypoint_count( 0 )
+ waypoints( NULL ),
+ waypoint_count( 0 )
{
- PRINT_DEBUG2( "Constructing ModelPosition %d (%s)\n",
- id, typestr );
-
- // assert that Update() is reentrant for this derived model
- thread_safe = true;
+ PRINT_DEBUG2( "Constructing ModelPosition %d (%s)\n",
+ id, typestr );
+
+ // assert that Update() is reentrant for this derived model
+ thread_safe = true;
- // no power consumed until we're subscribed
- this->SetWatts( 0 );
-
- // sensible position defaults
- Velocity vel;
- memset( &vel, 0, sizeof(vel));
- this->SetVelocity( vel );
-
- this->SetBlobReturn( TRUE );
-
- // configure the position-specific stuff
-
- // control
- memset( &goal, 0, sizeof(goal));
- drive_mode = POSITION_DRIVE_DEFAULT;
- control_mode = POSITION_CONTROL_DEFAULT;
-
- // localization
- localization_mode = POSITION_LOCALIZATION_DEFAULT;
-
- integration_error.x =
- drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_X -
- STG_POSITION_INTEGRATION_ERROR_MAX_X/2.0;
-
- integration_error.y =
- drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_Y -
- STG_POSITION_INTEGRATION_ERROR_MAX_Y/2.0;
-
- integration_error.a =
- drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_A -
- STG_POSITION_INTEGRATION_ERROR_MAX_A/2.0;
-
- RegisterOption( &showCoords );
- RegisterOption( &showWaypoints );
+ // no power consumed until we're subscribed
+ this->SetWatts( 0 );
+
+ // sensible position defaults
+ Velocity vel;
+ memset( &vel, 0, sizeof(vel));
+ this->SetVelocity( vel );
+
+ this->SetBlobReturn( TRUE );
+
+ // configure the position-specific stuff
+
+ // control
+ memset( &goal, 0, sizeof(goal));
+ drive_mode = POSITION_DRIVE_DEFAULT;
+ control_mode = POSITION_CONTROL_DEFAULT;
+
+ // localization
+ localization_mode = POSITION_LOCALIZATION_DEFAULT;
+
+ integration_error.x =
+ drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_X -
+ STG_POSITION_INTEGRATION_ERROR_MAX_X/2.0;
+
+ integration_error.y =
+ drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_Y -
+ STG_POSITION_INTEGRATION_ERROR_MAX_Y/2.0;
+
+ integration_error.a =
+ drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_A -
+ STG_POSITION_INTEGRATION_ERROR_MAX_A/2.0;
+
+ RegisterOption( &showCoords );
+ RegisterOption( &showWaypoints );
}
ModelPosition::~ModelPosition( void )
{
- // nothing to do
+ // nothing to do
}
void ModelPosition::Load( void )
{
- Model::Load();
+ Model::Load();
- char* keyword = NULL;
+ // load steering mode
+ if( wf->PropertyExists( wf_entity, "drive" ) )
+ {
+ const char* mode_str =
+ wf->ReadString( wf_entity, "drive", NULL );
- // load steering mode
- if( wf->PropertyExists( wf_entity, "drive" ) )
+ if( mode_str )
{
- const char* mode_str =
- wf->ReadString( wf_entity, "drive", NULL );
+ if( strcmp( mode_str, "diff" ) == 0 )
+ drive_mode = STG_POSITION_DRIVE_DIFFERENTIAL;
+ else if( strcmp( mode_str, "omni" ) == 0 )
+ drive_mode = STG_POSITION_DRIVE_OMNI;
+ else if( strcmp( mode_str, "car" ) == 0 )
+ drive_mode = STG_POSITION_DRIVE_CAR;
+ else
+ {
+ PRINT_ERR1( "invalid position drive mode specified: \"%s\" -
should be one of: \"diff\", \"omni\" or \"car\". Using \"diff\" as default.",
mode_str );
+ }
+ }
+ }
- if( mode_str )
- {
- if( strcmp( mode_str, "diff" ) == 0 )
- drive_mode = STG_POSITION_DRIVE_DIFFERENTIAL;
- else if( strcmp( mode_str, "omni" ) == 0 )
- drive_mode = STG_POSITION_DRIVE_OMNI;
- else if( strcmp( mode_str, "car" ) == 0 )
- drive_mode = STG_POSITION_DRIVE_CAR;
- else
- {
- PRINT_ERR1( "invalid position drive mode
specified: \"%s\" - should be one of: \"diff\", \"omni\" or \"car\". Using
\"diff\" as default.", mode_str );
- }
- }
- }
+ // load odometry if specified
+ if( wf->PropertyExists( wf_entity, "odom" ) )
+ {
+ PRINT_WARN1( "the odom property is specified for model \"%s\","
+ " but this property is no longer available."
+ " Use localization_origin instead. See the position"
+ " entry in the manual or src/model_position.c for details.",
+ this->token );
+ }
- // load odometry if specified
- if( wf->PropertyExists( wf_entity, "odom" ) )
- {
- PRINT_WARN1( "the odom property is specified for model \"%s\","
- " but this property is no longer available."
- " Use localization_origin instead. See the
position"
- " entry in the manual or src/model_position.c
for details.",
- this->token );
- }
+ // set the starting pose as my initial odom position. This could be
+ // overwritten below if the localization_origin property is
+ // specified
+ est_origin = this->GetGlobalPose();
- // set the starting pose as my initial odom position. This could be
- // overwritten below if the localization_origin property is
- // specified
- est_origin = this->GetGlobalPose();
+ if( wf->PropertyExists( wf_entity, "localization_origin" ) )
+ {
+ est_origin.x = wf->ReadTupleLength( wf_entity, "localization_origin", 0,
est_origin.x );
+ est_origin.y = wf->ReadTupleLength( wf_entity, "localization_origin", 1,
est_origin.y );
+ est_origin.z = wf->ReadTupleLength( wf_entity, "localization_origin", 2,
est_origin.z );
+ est_origin.a = wf->ReadTupleAngle( wf_entity, "localization_origin", 3,
est_origin.a );
- if( wf->PropertyExists( wf_entity, keyword ) )
- {
- est_origin.x = wf->ReadTupleLength( wf_entity,
"localization_origin", 0, est_origin.x );
- est_origin.y = wf->ReadTupleLength( wf_entity,
"localization_origin", 1, est_origin.y );
- est_origin.z = wf->ReadTupleLength( wf_entity,
"localization_origin", 2, est_origin.z );
- est_origin.a = wf->ReadTupleAngle( wf_entity,
"localization_origin", 3, est_origin.a );
+ // compute our localization pose based on the origin and true pose
+ Pose gpose = this->GetGlobalPose();
- // compute our localization pose based on the origin and true
pose
- Pose gpose = this->GetGlobalPose();
+ est_pose.a = normalize( gpose.a - est_origin.a );
+ double cosa = cos(est_origin.a);
+ double sina = sin(est_origin.a);
+ double dx = gpose.x - est_origin.x;
+ double dy = gpose.y - est_origin.y;
+ est_pose.x = dx * cosa + dy * sina;
+ est_pose.y = dy * cosa - dx * sina;
- est_pose.a = normalize( gpose.a - est_origin.a );
- double cosa = cos(est_origin.a);
- double sina = sin(est_origin.a);
- double dx = gpose.x - est_origin.x;
- double dy = gpose.y - est_origin.y;
- est_pose.x = dx * cosa + dy * sina;
- est_pose.y = dy * cosa - dx * sina;
+ // zero position error: assume we know exactly where we are on startup
+ memset( &est_pose_error, 0, sizeof(est_pose_error));
+ }
- // zero position error: assume we know exactly where we are on
startup
- memset( &est_pose_error, 0, sizeof(est_pose_error));
- }
+ // odometry model parameters
+ if( wf->PropertyExists( wf_entity, "odom_error" ) )
+ {
+ integration_error.x =
+ wf->ReadTupleLength( wf_entity, "odom_error", 0, integration_error.x );
+ integration_error.y =
+ wf->ReadTupleLength( wf_entity, "odom_error", 1, integration_error.y );
+ integration_error.z =
+ wf->ReadTupleLength( wf_entity, "odom_error", 2, integration_error.z );
+ integration_error.a
+ = wf->ReadTupleAngle( wf_entity, "odom_error", 3, integration_error.a );
+ }
- // odometry model parameters
- if( wf->PropertyExists( wf_entity, "odom_error" ) )
- {
- integration_error.x =
- wf->ReadTupleLength( wf_entity, "odom_error", 0,
integration_error.x );
- integration_error.y =
- wf->ReadTupleLength( wf_entity, "odom_error", 1,
integration_error.y );
- integration_error.z =
- wf->ReadTupleLength( wf_entity, "odom_error", 2,
integration_error.z );
- integration_error.a
- = wf->ReadTupleAngle( wf_entity, "odom_error", 3,
integration_error.a );
- }
+ // choose a localization model
+ if( wf->PropertyExists( wf_entity, "localization" ) )
+ {
+ const char* loc_str =
+ wf->ReadString( wf_entity, "localization", NULL );
- // choose a localization model
- if( wf->PropertyExists( wf_entity, "localization" ) )
+ if( loc_str )
{
- const char* loc_str =
- wf->ReadString( wf_entity, "localization", NULL );
-
- if( loc_str )
- {
- if( strcmp( loc_str, "gps" ) == 0 )
- localization_mode =
STG_POSITION_LOCALIZATION_GPS;
- else if( strcmp( loc_str, "odom" ) == 0 )
- localization_mode =
STG_POSITION_LOCALIZATION_ODOM;
- else
- PRINT_ERR2( "unrecognized localization mode
\"%s\" for model \"%s\"."
- " Valid choices are \"gps\" and
\"odom\".",
- loc_str, this->token );
- }
- else
- PRINT_ERR1( "no localization mode string specified for
model \"%s\"",
- this->token );
+ if( strcmp( loc_str, "gps" ) == 0 )
+ localization_mode = STG_POSITION_LOCALIZATION_GPS;
+ else if( strcmp( loc_str, "odom" ) == 0 )
+ localization_mode = STG_POSITION_LOCALIZATION_ODOM;
+ else
+ PRINT_ERR2( "unrecognized localization mode \"%s\" for model
\"%s\"."
+ " Valid choices are \"gps\" and \"odom\".",
+ loc_str, this->token );
}
+ else
+ PRINT_ERR1( "no localization mode string specified for model \"%s\"",
+ this->token );
+ }
- // TODO
- // we've probably poked the localization data, so we must let people
know
- //model_change( mod, &mod->data );
+ // TODO
+ // we've probably poked the localization data, so we must let people know
+ //model_change( mod, &mod->data );
}
void ModelPosition::Update( void )
{
- PRINT_DEBUG1( "[%lu] position update", this->world->sim_time );
+ PRINT_DEBUG1( "[%lu] position update", this->world->sim_time );
- // stop by default
- Velocity vel;
- memset( &vel, 0, sizeof(Velocity) );
+ // stop by default
+ Velocity vel;
+ memset( &vel, 0, sizeof(Velocity) );
- if( this->subs ) // no driving if noone is subscribed
- {
- switch( control_mode )
- {
- case STG_POSITION_CONTROL_VELOCITY :
- {
- PRINT_DEBUG( "velocity control mode" );
- PRINT_DEBUG4( "model %s command(%.2f
%.2f %.2f)",
- this->token,
- this->goal.x,
- this->goal.y,
- this->goal.a );
+ if( this->subs ) // no driving if noone is subscribed
+ {
+ switch( control_mode )
+ {
+ case STG_POSITION_CONTROL_VELOCITY :
+ {
+ PRINT_DEBUG( "velocity control mode" );
+ PRINT_DEBUG4( "model %s command(%.2f %.2f %.2f)",
+ this->token,
+ this->goal.x,
+ this->goal.y,
+ this->goal.a );
- switch( drive_mode )
- {
- case
STG_POSITION_DRIVE_DIFFERENTIAL:
- //
differential-steering model, like a Pioneer
- vel.x = goal.x;
- vel.y = 0;
- vel.a = goal.a;
- break;
+ switch( drive_mode )
+ {
+ case STG_POSITION_DRIVE_DIFFERENTIAL:
+ // differential-steering model, like a Pioneer
+ vel.x = goal.x;
+ vel.y = 0;
+ vel.a = goal.a;
+ break;
- case STG_POSITION_DRIVE_OMNI:
- // direct steering
model, like an omnidirectional robot
- vel.x = goal.x;
- vel.y = goal.y;
- vel.a = goal.a;
- break;
+ case STG_POSITION_DRIVE_OMNI:
+ // direct steering model, like an omnidirectional robot
+ vel.x = goal.x;
+ vel.y = goal.y;
+ vel.a = goal.a;
+ break;
- case STG_POSITION_DRIVE_CAR:
- // car like steering
model based on speed and turning angle
- vel.x = goal.x *
cos(goal.a);
- vel.y = 0;
- vel.a = goal.x *
sin(goal.a)/1.0; // here 1.0 is the wheel base, this should be a config option
- break;
+ case STG_POSITION_DRIVE_CAR:
+ // car like steering model based on speed and turning angle
+ vel.x = goal.x * cos(goal.a);
+ vel.y = 0;
+ vel.a = goal.x * sin(goal.a)/1.0; // here 1.0 is the wheel
base, this should be a config option
+ break;
- default:
- PRINT_ERR1( "unknown
steering mode %d", drive_mode );
- }
- } break;
+ default:
+ PRINT_ERR1( "unknown steering mode %d", drive_mode );
+ }
+ } break;
- case STG_POSITION_CONTROL_POSITION:
- {
- PRINT_DEBUG( "position control mode" );
+ case STG_POSITION_CONTROL_POSITION:
+ {
+ PRINT_DEBUG( "position control mode" );
- double x_error = goal.x - est_pose.x;
- double y_error = goal.y - est_pose.y;
- double a_error = normalize( goal.a -
est_pose.a );
+ double x_error = goal.x - est_pose.x;
+ double y_error = goal.y - est_pose.y;
+ double a_error = normalize( goal.a - est_pose.a );
- PRINT_DEBUG3( "errors: %.2f %.2f
%.2f\n", x_error, y_error, a_error );
+ PRINT_DEBUG3( "errors: %.2f %.2f %.2f\n", x_error, y_error, a_error
);
- // speed limits for controllers
- // TODO - have these configurable
- double max_speed_x = 0.4;
- double max_speed_y = 0.4;
- double max_speed_a = 1.0;
+ // speed limits for controllers
+ // TODO - have these configurable
+ double max_speed_x = 0.4;
+ double max_speed_y = 0.4;
+ double max_speed_a = 1.0;
- switch( drive_mode )
- {
- case STG_POSITION_DRIVE_OMNI:
- {
- // this is easy
- we just reduce the errors in each axis
- //
independently with a proportional controller, speed
- // limited
- vel.x = MIN(
x_error, max_speed_x );
- vel.y = MIN(
y_error, max_speed_y );
- vel.a = MIN(
a_error, max_speed_a );
- }
- break;
+ switch( drive_mode )
+ {
+ case STG_POSITION_DRIVE_OMNI:
+ {
+ // this is easy - we just reduce the errors in each axis
+ // independently with a proportional controller, speed
+ // limited
+ vel.x = MIN( x_error, max_speed_x );
+ vel.y = MIN( y_error, max_speed_y );
+ vel.a = MIN( a_error, max_speed_a );
+ }
+ break;
- case
STG_POSITION_DRIVE_DIFFERENTIAL:
- {
- // axes can not
be controlled independently. We have to
- // turn towards
the desired x,y position, drive there,
- // then turn to
face the desired angle. this is a
- // simple
controller that works ok. Could easily be
- // improved if
anyone needs it better. Who really does
- // position
control anyhoo?
+ case STG_POSITION_DRIVE_DIFFERENTIAL:
+ {
+ // axes can not be controlled independently. We have to
+ // turn towards the desired x,y position, drive there,
+ // then turn to face the desired angle. this is a
+ // simple controller that works ok. Could easily be
+ // improved if anyone needs it better. Who really does
+ // position control anyhoo?
- // start out
with no velocity
- Velocity calc;
- memset( &calc,
0, sizeof(calc));
+ // start out with no velocity
+ Velocity calc;
+ memset( &calc, 0, sizeof(calc));
- double
close_enough = 0.02; // fudge factor
+ double close_enough = 0.02; // fudge factor
- // if we're at
the right spot
- if(
fabs(x_error) < close_enough && fabs(y_error) < close_enough )
- {
-
PRINT_DEBUG( "TURNING ON THE SPOT" );
- // turn
on the spot to minimize the error
- calc.a
= MIN( a_error, max_speed_a );
- calc.a
= MAX( a_error, -max_speed_a );
- }
- else
- {
-
PRINT_DEBUG( "TURNING TO FACE THE GOAL POINT" );
- // turn
to face the goal point
- double
goal_angle = atan2( y_error, x_error );
- double
goal_distance = hypot( y_error, x_error );
+ // if we're at the right spot
+ if( fabs(x_error) < close_enough && fabs(y_error) <
close_enough )
+ {
+ PRINT_DEBUG( "TURNING ON THE SPOT" );
+ // turn on the spot to minimize the error
+ calc.a = MIN( a_error, max_speed_a );
+ calc.a = MAX( a_error, -max_speed_a );
+ }
+ else
+ {
+ PRINT_DEBUG( "TURNING TO FACE THE GOAL POINT" );
+ // turn to face the goal point
+ double goal_angle = atan2( y_error, x_error );
+ double goal_distance = hypot( y_error, x_error );
- a_error
= normalize( goal_angle - est_pose.a );
- calc.a
= MIN( a_error, max_speed_a );
- calc.a
= MAX( a_error, -max_speed_a );
+ a_error = normalize( goal_angle - est_pose.a );
+ calc.a = MIN( a_error, max_speed_a );
+ calc.a = MAX( a_error, -max_speed_a );
-
PRINT_DEBUG2( "steer errors: %.2f %.2f \n", a_error, goal_distance );
+ PRINT_DEBUG2( "steer errors: %.2f %.2f \n", a_error,
goal_distance );
- // if
we're pointing about the right direction, move
- //
forward
- if(
fabs(a_error) < M_PI/16 )
- {
-
PRINT_DEBUG( "DRIVING TOWARDS THE GOAL" );
-
calc.x = MIN( goal_distance, max_speed_x );
- }
- }
+ // if we're pointing about the right direction, move
+ // forward
+ if( fabs(a_error) < M_PI/16 )
+ {
+ PRINT_DEBUG( "DRIVING TOWARDS THE GOAL" );
+ calc.x = MIN( goal_distance, max_speed_x );
+ }
+ }
- // now set the
underlying velocities using the normal
- // diff-steer
model
- vel.x = calc.x;
- vel.y = 0;
- vel.a = calc.a;
- }
- break;
+ // now set the underlying velocities using the normal
+ // diff-steer model
+ vel.x = calc.x;
+ vel.y = 0;
+ vel.a = calc.a;
+ }
+ break;
- default:
- PRINT_ERR1( "unknown
steering mode %d", (int)drive_mode );
- }
- }
- break;
+ default:
+ PRINT_ERR1( "unknown steering mode %d", (int)drive_mode );
+ }
+ }
+ break;
- default:
- PRINT_ERR1( "unrecognized position command mode
%d", control_mode );
- }
+ default:
+ PRINT_ERR1( "unrecognized position command mode %d", control_mode );
+ }
- // simple model of power consumption
- watts = STG_POSITION_WATTS +
- fabs(vel.x) * STG_POSITION_WATTS_KGMS * mass +
- fabs(vel.y) * STG_POSITION_WATTS_KGMS * mass +
- fabs(vel.a) * STG_POSITION_WATTS_KGMS * mass;
+ // simple model of power consumption
+ watts = STG_POSITION_WATTS +
+ fabs(vel.x) * STG_POSITION_WATTS_KGMS * mass +
+ fabs(vel.y) * STG_POSITION_WATTS_KGMS * mass +
+ fabs(vel.a) * STG_POSITION_WATTS_KGMS * mass;
- //PRINT_DEBUG4( "model %s velocity (%.2f %.2f %.2f)",
- // this->token,
- // this->velocity.x,
- // this->velocity.y,
- // this->velocity.a );
+ //PRINT_DEBUG4( "model %s velocity (%.2f %.2f %.2f)",
+ // this->token,
+ // this->velocity.x,
+ // this->velocity.y,
+ // this->velocity.a );
- this->SetVelocity( vel );
- }
+ this->SetVelocity( vel );
+ }
- switch( localization_mode )
- {
- case STG_POSITION_LOCALIZATION_GPS:
- {
- // compute our localization pose based on the
origin and true pose
- Pose gpose = this->GetGlobalPose();
+ switch( localization_mode )
+ {
+ case STG_POSITION_LOCALIZATION_GPS:
+ {
+ // compute our localization pose based on the origin and true pose
+ Pose gpose = this->GetGlobalPose();
- est_pose.a = normalize( gpose.a - est_origin.a
);
- double cosa = cos(est_origin.a);
- double sina = sin(est_origin.a);
- double dx = gpose.x - est_origin.x;
- double dy = gpose.y - est_origin.y;
- est_pose.x = dx * cosa + dy * sina;
- est_pose.y = dy * cosa - dx * sina;
+ est_pose.a = normalize( gpose.a - est_origin.a );
+ double cosa = cos(est_origin.a);
+ double sina = sin(est_origin.a);
+ double dx = gpose.x - est_origin.x;
+ double dy = gpose.y - est_origin.y;
+ est_pose.x = dx * cosa + dy * sina;
+ est_pose.y = dy * cosa - dx * sina;
- }
- break;
+ }
+ break;
- case STG_POSITION_LOCALIZATION_ODOM:
- {
- // integrate our velocities to get an
'odometry' position estimate.
- double dt = this->world->GetSimInterval()/1e6;
+ case STG_POSITION_LOCALIZATION_ODOM:
+ {
+ // integrate our velocities to get an 'odometry' position estimate.
+ double dt = this->world->GetSimInterval()/1e6;
- est_pose.a = normalize( est_pose.a + (vel.a *
dt) * (1.0 +integration_error.a) );
+ est_pose.a = normalize( est_pose.a + (vel.a * dt) * (1.0
+integration_error.a) );
- double cosa = cos(est_pose.a);
- double sina = sin(est_pose.a);
- double dx = (vel.x * dt) * (1.0 +
integration_error.x );
- double dy = (vel.y * dt) * (1.0 +
integration_error.y );
+ double cosa = cos(est_pose.a);
+ double sina = sin(est_pose.a);
+ double dx = (vel.x * dt) * (1.0 + integration_error.x );
+ double dy = (vel.y * dt) * (1.0 + integration_error.y );
- est_pose.x += dx * cosa + dy * sina;
- est_pose.y -= dy * cosa - dx * sina;
- }
- break;
+ est_pose.x += dx * cosa + dy * sina;
+ est_pose.y -= dy * cosa - dx * sina;
+ }
+ break;
- default:
- PRINT_ERR2( "unknown localization mode %d for model
%s\n",
- localization_mode, this->token );
- break;
- }
+ default:
+ PRINT_ERR2( "unknown localization mode %d for model %s\n",
+ localization_mode, this->token );
+ break;
+ }
- PRINT_DEBUG3( " READING POSITION: [ %.4f %.4f %.4f ]\n",
- est_pose.x, est_pose.y, est_pose.a );
+ PRINT_DEBUG3( " READING POSITION: [ %.4f %.4f %.4f ]\n",
+ est_pose.x, est_pose.y, est_pose.a );
- Model::Update();
+ Model::Update();
}
void ModelPosition::Startup( void )
{
- Model::Startup();
+ Model::Startup();
- PRINT_DEBUG( "position startup" );
+ PRINT_DEBUG( "position startup" );
- this->SetWatts( STG_POSITION_WATTS );
+ this->SetWatts( STG_POSITION_WATTS );
}
void ModelPosition::Shutdown( void )
{
- PRINT_DEBUG( "position shutdown" );
+ PRINT_DEBUG( "position shutdown" );
- // safety features!
- bzero( &goal, sizeof(goal) );
- bzero( &velocity, sizeof(velocity) );
+ // safety features!
+ bzero( &goal, sizeof(goal) );
+ bzero( &velocity, sizeof(velocity) );
- this->SetWatts( 0 );
+ this->SetWatts( 0 );
- Model::Shutdown();
+ Model::Shutdown();
}
void ModelPosition::Stop()
@@ -530,11 +526,11 @@
assert( ! isnan(vel.z) );
assert( ! isnan(vel.a) );
- control_mode = STG_POSITION_CONTROL_VELOCITY;
- goal.x = vel.x;
- goal.y = vel.y;
- goal.z = vel.z;
- goal.a = vel.a;
+ control_mode = STG_POSITION_CONTROL_VELOCITY;
+ goal.x = vel.x;
+ goal.y = vel.y;
+ goal.z = vel.z;
+ goal.a = vel.a;
}
void ModelPosition::GoTo( double x, double y, double a )
@@ -543,37 +539,37 @@
assert( ! isnan(y) );
assert( ! isnan(a) );
- control_mode = STG_POSITION_CONTROL_POSITION;
- goal.x = x;
- goal.y = y;
- goal.z = 0;
- goal.a = a;
+ control_mode = STG_POSITION_CONTROL_POSITION;
+ goal.x = x;
+ goal.y = y;
+ goal.z = 0;
+ goal.a = a;
}
void ModelPosition::GoTo( Pose pose )
{
- control_mode = STG_POSITION_CONTROL_POSITION;
- goal = pose;
+ control_mode = STG_POSITION_CONTROL_POSITION;
+ goal = pose;
}
/**
- Set the current odometry estimate
- */
+ Set the current odometry estimate
+*/
void ModelPosition::SetOdom( Pose odom )
{
- est_pose = odom;
+ est_pose = odom;
- // figure out where the implied origin is in global coords
- Pose gp = GetGlobalPose();
+ // figure out where the implied origin is in global coords
+ Pose gp = GetGlobalPose();
- double da = -odom.a + gp.a;
- double dx = -odom.x * cos(da) + odom.y * sin(da);
- double dy = -odom.y * cos(da) - odom.x * sin(da);
+ double da = -odom.a + gp.a;
+ double dx = -odom.x * cos(da) + odom.y * sin(da);
+ double dy = -odom.y * cos(da) - odom.x * sin(da);
- // origin of our estimated pose
- est_origin.x = gp.x + dx;
- est_origin.y = gp.y + dy;
- est_origin.a = da;
+ // origin of our estimated pose
+ est_origin.x = gp.x + dx;
+ est_origin.y = gp.y + dy;
+ est_origin.a = da;
}
/** Set the waypoint array pointer. Returns the old pointer, in case you need
to free/delete[] it */
@@ -590,62 +586,61 @@
void ModelPosition::DataVisualize( Camera* cam )
{
if( showWaypoints && waypoints && waypoint_count )
- DrawWaypoints();
+ DrawWaypoints();
if( showCoords )
- {
- // vizualize my estimated pose
- glPushMatrix();
+ {
+ // vizualize my estimated pose
+ glPushMatrix();
- // back into global coords
- Gl::pose_inverse_shift( GetGlobalPose() );
+ // back into global coords
+ Gl::pose_inverse_shift( GetGlobalPose() );
- Gl::pose_shift( est_origin );
- PushColor( 1,0,0,1 ); // origin in red
- Gl::draw_origin( 0.5 );
+ Gl::pose_shift( est_origin );
+ PushColor( 1,0,0,1 ); // origin in red
+ Gl::draw_origin( 0.5 );
- glEnable (GL_LINE_STIPPLE);
- glLineStipple (3, 0xAAAA);
+ glEnable (GL_LINE_STIPPLE);
+ glLineStipple (3, 0xAAAA);
- PushColor( 1,0,0,0.5 );
- glBegin( GL_LINE_STRIP );
- glVertex2f( 0,0 );
- glVertex2f( est_pose.x, 0 );
- glVertex2f( est_pose.x, est_pose.y );
- glEnd();
+ PushColor( 1,0,0,0.5 );
+ glBegin( GL_LINE_STRIP );
+ glVertex2f( 0,0 );
+ glVertex2f( est_pose.x, 0 );
+ glVertex2f( est_pose.x, est_pose.y );
+ glEnd();
- glDisable(GL_LINE_STIPPLE);
+ glDisable(GL_LINE_STIPPLE);
- char label[64];
- snprintf( label, 64, "x:%.3f", est_pose.x );
- Gl::draw_string( est_pose.x / 2.0, -0.5, 0, label );
+ char label[64];
+ snprintf( label, 64, "x:%.3f", est_pose.x );
+ Gl::draw_string( est_pose.x / 2.0, -0.5, 0, label );
- snprintf( label, 64, "y:%.3f", est_pose.y );
- Gl::draw_string( est_pose.x + 0.5 , est_pose.y / 2.0, 0, (const
char*)label );
+ snprintf( label, 64, "y:%.3f", est_pose.y );
+ Gl::draw_string( est_pose.x + 0.5 , est_pose.y / 2.0, 0, (const
char*)label );
+
+ PopColor();
+ Gl::pose_shift( est_pose );
+ PushColor( 0,1,0,1 ); // pose in green
+ Gl::draw_origin( 0.5 );
+ PopColor();
- PopColor();
-
- Gl::pose_shift( est_pose );
- PushColor( 0,1,0,1 ); // pose in green
- Gl::draw_origin( 0.5 );
- PopColor();
-
- Gl::pose_shift( geom.pose );
- PushColor( 0,0,1,1 ); // offset in blue
- Gl::draw_origin( 0.5 );
- PopColor();
+ Gl::pose_shift( geom.pose );
+ PushColor( 0,0,1,1 ); // offset in blue
+ Gl::draw_origin( 0.5 );
+ PopColor();
- double r,g,b,a;
- stg_color_unpack( color, &r, &g, &b, &a );
- PushColor( r, g, b, 0.5 );
+ double r,g,b,a;
+ stg_color_unpack( color, &r, &g, &b, &a );
+ PushColor( r, g, b, 0.5 );
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- blockgroup.DrawFootPrint( geom );
- PopColor();
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+ blockgroup.DrawFootPrint( geom );
+ PopColor();
- glPopMatrix();
- }
+ glPopMatrix();
+ }
// inherit more viz
Model::DataVisualize( cam );
@@ -653,24 +648,31 @@
void ModelPosition::DrawWaypoints()
{
- glPointSize( 3 );
-
- // TRUE
+ glPointSize( 5 );
glPushMatrix();
PushColor( color );
-
+
Gl::pose_inverse_shift( pose );
Gl::pose_shift( est_origin );
+ // draw waypoints
for( unsigned int i=0; i < waypoint_count; i++ )
- waypoints[i].Draw();
+ waypoints[i].Draw();
+ // draw lines connecting the waypoints
+ if( waypoint_count > 1 )
+ {
+ glBegin( GL_LINE_STRIP );
+
+ for( unsigned int i=1; i < waypoint_count; i++ )
+ {
+ glVertex2f( waypoints[i].pose.x, waypoints[i].pose.y );
+ glVertex2f( waypoints[i-1].pose.x, waypoints[i-1].pose.y );
+ }
+
+ glEnd();
+ }
+
PopColor();
glPopMatrix();
-
- /* printf( "pose [%.2f %.2f %.2f %.2f] est_pose [%.2f %.2f %.2f %.2f]
est_origin [%.2f %.2f %.2f %.2f]\n",
- pose.x, pose.y, pose.z, pose.a,
- est_pose.x, est_pose.y, est_pose.z, est_pose.a,
- est_origin.x, est_origin.y, est_origin.z, est_origin.a
);
- */
}
Modified: code/stage/trunk/libstage/waypoint.cc
===================================================================
--- code/stage/trunk/libstage/waypoint.cc 2009-03-19 02:50:10 UTC (rev
7531)
+++ code/stage/trunk/libstage/waypoint.cc 2009-03-19 06:38:54 UTC (rev
7532)
@@ -20,15 +20,15 @@
void Waypoint::Draw()
{
- GLdouble d[4];
-
- d[0] = ((color & 0x00FF0000) >> 16) / 256.0;
- d[1] = ((color & 0x0000FF00) >> 8) / 256.0;
- d[2] = ((color & 0x000000FF) >> 0) / 256.0;
- d[3] = (((color & 0xFF000000) >> 24) / 256.0);
-
- glColor4dv( d );
-
+ GLdouble d[4];
+
+ d[0] = ((color & 0x00FF0000) >> 16) / 256.0;
+ d[1] = ((color & 0x0000FF00) >> 8) / 256.0;
+ d[2] = ((color & 0x000000FF) >> 0) / 256.0;
+ d[3] = (((color & 0xFF000000) >> 24) / 256.0);
+
+ glColor4dv( d );
+
glBegin(GL_POINTS);
glVertex3f( pose.x, pose.y, pose.z );
glEnd();
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-03-19 02:50:10 UTC (rev 7531)
+++ code/stage/trunk/libstage/world.cc 2009-03-19 06:38:54 UTC (rev 7532)
@@ -78,7 +78,6 @@
destroy( false ),
dirty( true ),
models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ),
- powerpack_list( NULL ),
ppm( ppm ), // raytrace resolution
quit( false ),
quit_time( 0 ),
@@ -97,12 +96,12 @@
graphics( false ),
interval_sim( (stg_usec_t)thousand * interval_sim ),
option_table( g_hash_table_new( g_str_hash, g_str_equal ) ),
+ powerpack_list( NULL ),
ray_list( NULL ),
sim_time( 0 ),
superregions( g_hash_table_new( (GHashFunc)PointIntHash,
(GEqualFunc)PointIntEqual ) ),
sr_cached(NULL),
- // update_list( NULL ),
reentrant_update_list( NULL ),
nonreentrant_update_list( NULL ),
updates( 0 ),
@@ -437,6 +436,8 @@
return true;
}
+ dirty = true; // need redraw
+
// upate all positions first
LISTMETHOD( velocity_list, Model*, UpdatePose );
@@ -999,5 +1000,6 @@
void World::StopUpdatingModelPose( Model* mod )
{
- velocity_list = g_list_remove( velocity_list, mod );
+ // TODO XX figure out how to handle velcoties a bit better
+ //velocity_list = g_list_remove( velocity_list, mod );
}
Modified: code/stage/trunk/stage.pc.in
===================================================================
--- code/stage/trunk/stage.pc.in 2009-03-19 02:50:10 UTC (rev 7531)
+++ code/stage/trunk/stage.pc.in 2009-03-19 06:38:54 UTC (rev 7532)
@@ -1,8 +1,8 @@
pref...@cmake_install_prefix@
Name: stage
-Description: Stage robot simulation C library and Player plugin - part of the
Player/Stage Project
+Description: Stage robot simulation program, C++ library and Player plugin -
part of the Player/Stage Project
Version: @VERSION@
Requires: glib-2.0 >= 2.4
Libs: -L${prefix}/lib -lstage
-Cflags: -I${prefix}/include/Stage-3.0
+Cflags: -I${prefix}/include/sta...@apiversion@
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Apps built with the Adobe(R) Flex(R) framework and Flex Builder(TM) are
powering Web 2.0 with engaging, cross-platform capabilities. Quickly and
easily build your RIAs with Flex Builder, the Eclipse(TM)based development
software that enables intelligent coding and step-through debugging.
Download the free 60 day trial. http://p.sf.net/sfu/www-adobe-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
