Menu
▾
▴
playerstage-commit — CVS / SVN log messages
You can subscribe to this list here.
| 2006 |
Jan
|
Feb
|
Mar
|
Apr
(85) |
May
(154) |
Jun
(108) |
Jul
(90) |
Aug
(150) |
Sep
(147) |
Oct
(57) |
Nov
(90) |
Dec
(160) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2007 |
Jan
(40) |
Feb
(54) |
Mar
(90) |
Apr
(86) |
May
(60) |
Jun
(80) |
Jul
(114) |
Aug
(198) |
Sep
(265) |
Oct
(298) |
Nov
(191) |
Dec
(161) |
| 2008 |
Jan
(77) |
Feb
(155) |
Mar
(64) |
Apr
(148) |
May
(95) |
Jun
(265) |
Jul
(259) |
Aug
(73) |
Sep
(68) |
Oct
(96) |
Nov
(129) |
Dec
(82) |
| 2009 |
Jan
(135) |
Feb
(69) |
Mar
(233) |
Apr
(96) |
May
(263) |
Jun
(295) |
Jul
(294) |
Aug
(106) |
Sep
|
Oct
|
Nov
|
Dec
|
|
From: <gb...@us...> - 2009-08-30 01:56:26
|
Revision: 8240
http://playerstage.svn.sourceforge.net/playerstage/?rev=8240&view=rev
Author: gbiggs
Date: 2009-08-30 01:56:15 +0000 (Sun, 30 Aug 2009)
Log Message:
-----------
Changed default velocity to 0.7m/s.
Modified Paths:
--------------
code/player/trunk/server/drivers/position/goto/goto.cc
Modified: code/player/trunk/server/drivers/position/goto/goto.cc
===================================================================
--- code/player/trunk/server/drivers/position/goto/goto.cc 2009-08-28 01:58:11 UTC (rev 8239)
+++ code/player/trunk/server/drivers/position/goto/goto.cc 2009-08-30 01:56:15 UTC (rev 8240)
@@ -117,7 +117,7 @@
#define AVMAX 45.0
#define MAXD 2.0
#define TVMIN 0.1
-#define TVMAX 3
+#define TVMAX 0.7
#ifndef M_PI
#define M_PI 3.14159265358979323846
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: Feehan <wo...@aw...> - 2009-08-28 07:28:20
|
Elow May now retire to rest: Whilst in those countries far beyond, The day begins to break, A many a child, and many a bird, Doth now begin to wake. [Illustration] And when the morning dawns again, The Sun comes to our east, Then evening will begin with them, And _they_ to bed will haste. How very good of God it is, To make the Sun to go About this great round world of ours, To light each country so. [Illustration] GOOD MAMA. Love, come and sit upon my knee, And give me kisses, one, two, three, And tell me whether you love me, My baby. For this I'm sure, that I love you, And many, many things I do, And all day long I sit and sew For baby. And then at night I lay awake, Thinking of things that I can make, And trouble that I mean to take For baby. And when you're good and do not cry Nor into wicked passion fly, You can't think how papa and I Love b |
|
From: <rt...@us...> - 2009-08-28 01:58:19
|
Revision: 8239
http://playerstage.svn.sourceforge.net/playerstage/?rev=8239&view=rev
Author: rtv
Date: 2009-08-28 01:58:11 +0000 (Fri, 28 Aug 2009)
Log Message:
-----------
added vis of current draw from powerpacks
Modified Paths:
--------------
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_actuator.cc
code/stage/trunk/libstage/model_getset.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/model_load.cc
code/stage/trunk/libstage/model_position.cc
code/stage/trunk/libstage/powerpack.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/worldgui.cc
code/stage/trunk/worlds/simple.world
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-08-27 23:41:43 UTC (rev 8238)
+++ code/stage/trunk/libstage/model.cc 2009-08-28 01:58:11 UTC (rev 8239)
@@ -110,7 +110,6 @@
// static members
uint32_t Model::count = 0;
std::map<stg_id_t,Model*> Model::modelsbyid;
-
std::map<std::string, creator_t> Model::name_map;
void Size::Load( Worldfile* wf, int section, const char* keyword )
@@ -957,7 +956,7 @@
{
if( power_pack )
return power_pack;
-
+
if( parent )
return parent->FindPowerPack();
@@ -1120,4 +1119,3 @@
{
pts.clear();
}
-
Modified: code/stage/trunk/libstage/model_actuator.cc
===================================================================
--- code/stage/trunk/libstage/model_actuator.cc 2009-08-27 23:41:43 UTC (rev 8238)
+++ code/stage/trunk/libstage/model_actuator.cc 2009-08-28 01:58:11 UTC (rev 8239)
@@ -56,8 +56,8 @@
if a linear actuator the axis that the actuator will move along
*/
-static const double STG_ACTUATOR_WATTS_KGMS = 5.0; // cost per kg per meter per second
-static const double STG_ACTUATOR_WATTS = 2.0; // base cost of position device
+static const double WATTS_KGMS = 5.0; // cost per kg per meter per second
+static const double WATTS_BASE = 2.0; // base cost of position device
ModelActuator::ModelActuator( World* world,
Model* parent,
@@ -68,12 +68,11 @@
max_speed(1),
min_position(0),
max_position(1),
- control_mode( STG_ACTUATOR_CONTROL_VELOCITY ),
- actuator_type( STG_ACTUATOR_TYPE_LINEAR ),
+ control_mode( CONTROL_VELOCITY ),
+ actuator_type( TYPE_LINEAR ),
axis(0,0,0)
{
- // no power consumed until we're subscribed
- this->SetWatts( 0 );
+ this->SetWatts( WATTS_BASE );
// sensible position defaults
this->SetVelocity( Velocity(0,0,0,0) );
@@ -100,9 +99,9 @@
if( type_str )
{
if( strcmp( type_str, "linear" ) == 0 )
- actuator_type = STG_ACTUATOR_TYPE_LINEAR;
+ actuator_type = TYPE_LINEAR;
else if( strcmp( type_str, "rotational" ) == 0 )
- actuator_type = STG_ACTUATOR_TYPE_ROTATIONAL;
+ actuator_type = TYPE_ROTATIONAL;
else
{
PRINT_ERR1( "invalid actuator type specified: \"%s\" - should be one of: \"linear\" or \"rotational\". Using \"linear\" as default.", type_str );
@@ -110,7 +109,7 @@
}
}
- if (actuator_type == STG_ACTUATOR_TYPE_LINEAR)
+ if (actuator_type == TYPE_LINEAR)
{
// if we are a linear actuator find the axis we operate in
if( wf->PropertyExists( wf_entity, "axis" ) )
@@ -169,11 +168,11 @@
switch (actuator_type)
{
- case STG_ACTUATOR_TYPE_LINEAR:
+ case TYPE_LINEAR:
{
pos = PoseDiff.x * axis.x + PoseDiff.y * axis.y + PoseDiff.z * axis.z; // Dot product to find distance along axis
} break;
- case STG_ACTUATOR_TYPE_ROTATIONAL:
+ case TYPE_ROTATIONAL:
{
pos = PoseDiff.a;
} break;
@@ -186,7 +185,7 @@
{
switch( control_mode )
{
- case STG_ACTUATOR_CONTROL_VELOCITY :
+ case CONTROL_VELOCITY :
{
PRINT_DEBUG( "actuator velocity control mode" );
PRINT_DEBUG2( "model %s command(%.2f)",
@@ -198,7 +197,7 @@
velocity = goal;
} break;
- case STG_ACTUATOR_CONTROL_POSITION:
+ case CONTROL_POSITION:
{
PRINT_DEBUG( "actuator position control mode" );
@@ -233,14 +232,14 @@
Velocity outvel;
switch (actuator_type)
{
- case STG_ACTUATOR_TYPE_LINEAR:
+ case TYPE_LINEAR:
{
outvel.x = axis.x * velocity;
outvel.y = axis.y * velocity;
outvel.z = axis.z * velocity;
outvel.a = 0;
} break;
- case STG_ACTUATOR_TYPE_ROTATIONAL:
+ case TYPE_ROTATIONAL:
{
outvel.x = outvel.y = outvel.z = 0;
outvel.a = velocity;
@@ -263,9 +262,6 @@
Model::Startup();
PRINT_DEBUG( "position startup" );
-
- this->SetWatts( STG_ACTUATOR_WATTS );
-
}
void ModelActuator::Shutdown( void )
@@ -277,20 +273,18 @@
velocity.Zero();
- this->SetWatts( 0 );
-
Model::Shutdown();
}
void ModelActuator::SetSpeed( double speed)
{
- control_mode = STG_ACTUATOR_CONTROL_VELOCITY;
+ control_mode = CONTROL_VELOCITY;
goal = speed;
}
void ModelActuator::GoTo( double pos)
{
- control_mode = STG_ACTUATOR_CONTROL_POSITION;
+ control_mode = CONTROL_POSITION;
goal = pos;
}
Modified: code/stage/trunk/libstage/model_getset.cc
===================================================================
--- code/stage/trunk/libstage/model_getset.cc 2009-08-27 23:41:43 UTC (rev 8238)
+++ code/stage/trunk/libstage/model_getset.cc 2009-08-28 01:58:11 UTC (rev 8239)
@@ -116,7 +116,7 @@
CallCallbacks( &gui.outline );
}
-void Model::SetWatts( stg_watts_t val )
+void Model::SetWatts( stg_watts_t val )
{
watts = val;
CallCallbacks( &watts );
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2009-08-27 23:41:43 UTC (rev 8238)
+++ code/stage/trunk/libstage/model_laser.cc 2009-08-28 01:58:11 UTC (rev 8239)
@@ -96,6 +96,9 @@
// set up our data buffers and raytracing
SampleConfig();
+ // start consuming power
+ watts = DEFAULT_WATTS;
+
AddVisualizer( &vis, true );
}
@@ -111,9 +114,6 @@
fov = wf->ReadAngle( wf_entity, "fov", fov );
resolution = wf->ReadInt( wf_entity, "resolution", resolution );
- //showLaserData.Load( wf, wf_entity );
- //showLaserStrikes.Load( wf, wf_entity );
-
if( resolution < 1 )
{
PRINT_WARN( "laser resolution set < 1. Forcing to 1" );
@@ -228,17 +228,12 @@
void ModelLaser::Startup( void )
{
Model::Startup();
-
PRINT_DEBUG( "laser startup" );
-
- // start consuming power
- SetWatts( DEFAULT_WATTS );
}
void ModelLaser::Shutdown( void )
{
PRINT_DEBUG( "laser shutdown" );
- SetWatts( 0 ); // stop consuming power
Model::Shutdown();
}
Modified: code/stage/trunk/libstage/model_load.cc
===================================================================
--- code/stage/trunk/libstage/model_load.cc 2009-08-27 23:41:43 UTC (rev 8238)
+++ code/stage/trunk/libstage/model_load.cc 2009-08-28 01:58:11 UTC (rev 8239)
@@ -47,40 +47,28 @@
power_pack->GetCapacity() ) );
}
- // use my own pack or an ancestor's for the other energy properties
- PowerPack* pp = FindPowerPack();
-
- watts = wf->ReadFloat( wf_entity, "watts", watts );
- if( (watts > 0) && !pp )
- PRINT_WARN1( "Model %s: Setting \"watts\" has no effect unless \"joules\" is specified for this model or a parent", token );
-
- watts_give = wf->ReadFloat( wf_entity, "give_watts", watts_give );
- if( (watts_give > 0.0) && !pp)
- PRINT_WARN1( "Model %s: Setting \"watts_give\" has no effect unless \"joules\" is specified for this model or a parent", token );
-
+ watts = wf->ReadFloat( wf_entity, "watts", watts );
+ watts_give = wf->ReadFloat( wf_entity, "give_watts", watts_give );
watts_take = wf->ReadFloat( wf_entity, "take_watts", watts_take );
- if( (watts_take > 0.0) & !pp )
- PRINT_WARN1( "Model %s: Setting \"watts_take\" has no effect unless \"joules\" is specified for this model or a parent", token );
-
if( wf->PropertyExists( wf_entity, "debug" ) )
{
PRINT_WARN2( "debug property specified for model %d %s\n",
- wf_entity, this->token );
+ wf_entity, this->token );
this->debug = wf->ReadInt( wf_entity, "debug", this->debug );
}
-
+
if( wf->PropertyExists( wf_entity, "name" ) )
{
char *name = (char*)wf->ReadString(wf_entity, "name", NULL );
if( name )
- {
- //printf( "adding name %s to %s\n", name, this->token );
- this->token = strdup( name );
- world->AddModel( this ); // add this name to the world's table
- }
+ {
+ //printf( "adding name %s to %s\n", name, this->token );
+ this->token = strdup( name );
+ world->AddModel( this ); // add this name to the world's table
+ }
else
- PRINT_ERR1( "Name blank for model %s. Check your worldfile\n", this->token );
+ PRINT_ERR1( "Name blank for model %s. Check your worldfile\n", this->token );
}
//PRINT_WARN1( "%s::Load", token );
Modified: code/stage/trunk/libstage/model_position.cc
===================================================================
--- code/stage/trunk/libstage/model_position.cc 2009-08-27 23:41:43 UTC (rev 8238)
+++ code/stage/trunk/libstage/model_position.cc 2009-08-28 01:58:11 UTC (rev 8239)
@@ -83,9 +83,9 @@
const std::string& type ) :
Model( world, parent, type ),
goal(0,0,0,0),
- control_mode( STG_POSITION_CONTROL_VELOCITY ),
- drive_mode( STG_POSITION_DRIVE_DIFFERENTIAL ),
- localization_mode( STG_POSITION_LOCALIZATION_GPS ),
+ control_mode( CONTROL_VELOCITY ),
+ drive_mode( DRIVE_DIFFERENTIAL ),
+ localization_mode( LOCALIZATION_GPS ),
integration_error( drand48() * INTEGRATION_ERROR_MAX_X - INTEGRATION_ERROR_MAX_X/2.0,
drand48() * INTEGRATION_ERROR_MAX_Y - INTEGRATION_ERROR_MAX_Y/2.0,
drand48() * INTEGRATION_ERROR_MAX_Z - INTEGRATION_ERROR_MAX_Z/2.0,
@@ -101,11 +101,6 @@
// assert that Update() is reentrant for this derived model
thread_safe = false;
- // no power consumed until we're subscribed
- this->SetWatts( 0 );
-
- this->SetVelocity( Velocity(0,0,0,0) );
-
this->SetBlobReturn( true );
AddVisualizer( &wpvis, true );
@@ -131,11 +126,11 @@
if( mode_str )
{
if( strcmp( mode_str, "diff" ) == 0 )
- drive_mode = STG_POSITION_DRIVE_DIFFERENTIAL;
+ drive_mode = DRIVE_DIFFERENTIAL;
else if( strcmp( mode_str, "omni" ) == 0 )
- drive_mode = STG_POSITION_DRIVE_OMNI;
+ drive_mode = DRIVE_OMNI;
else if( strcmp( mode_str, "car" ) == 0 )
- drive_mode = STG_POSITION_DRIVE_CAR;
+ drive_mode = 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 );
@@ -202,9 +197,9 @@
if( loc_str )
{
if( strcmp( loc_str, "gps" ) == 0 )
- localization_mode = STG_POSITION_LOCALIZATION_GPS;
+ localization_mode = LOCALIZATION_GPS;
else if( strcmp( loc_str, "odom" ) == 0 )
- localization_mode = STG_POSITION_LOCALIZATION_ODOM;
+ localization_mode = LOCALIZATION_ODOM;
else
PRINT_ERR2( "unrecognized localization mode \"%s\" for model \"%s\"."
" Valid choices are \"gps\" and \"odom\".",
@@ -227,7 +222,7 @@
{
switch( control_mode )
{
- case STG_POSITION_CONTROL_VELOCITY :
+ case CONTROL_VELOCITY :
{
PRINT_DEBUG( "velocity control mode" );
PRINT_DEBUG4( "model %s command(%.2f %.2f %.2f)",
@@ -238,21 +233,21 @@
switch( drive_mode )
{
- case STG_POSITION_DRIVE_DIFFERENTIAL:
+ case 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:
+ case 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:
+ case DRIVE_CAR:
// car like steering model based on speed and turning angle
vel.x = goal.x * cos(goal.a);
vel.y = 0;
@@ -264,7 +259,7 @@
}
} break;
- case STG_POSITION_CONTROL_POSITION:
+ case CONTROL_POSITION:
{
PRINT_DEBUG( "position control mode" );
@@ -282,7 +277,7 @@
switch( drive_mode )
{
- case STG_POSITION_DRIVE_OMNI:
+ case DRIVE_OMNI:
{
// this is easy - we just reduce the errors in each axis
// independently with a proportional controller, speed
@@ -293,7 +288,7 @@
}
break;
- case STG_POSITION_DRIVE_DIFFERENTIAL:
+ case DRIVE_DIFFERENTIAL:
{
// axes can not be controlled independently. We have to
// turn towards the desired x,y position, drive there,
@@ -371,7 +366,7 @@
switch( localization_mode )
{
- case STG_POSITION_LOCALIZATION_GPS:
+ case LOCALIZATION_GPS:
{
// compute our localization pose based on the origin and true pose
Pose gpose = this->GetGlobalPose();
@@ -387,7 +382,7 @@
}
break;
- case STG_POSITION_LOCALIZATION_ODOM:
+ case LOCALIZATION_ODOM:
{
// integrate our velocities to get an 'odometry' position estimate.
double dt = interval / 1e6; // update interval convert to seconds
@@ -419,10 +414,8 @@
void ModelPosition::Startup( void )
{
Model::Startup();
-
+
PRINT_DEBUG( "position startup" );
-
- this->SetWatts( WATTS );
}
void ModelPosition::Shutdown( void )
@@ -430,11 +423,9 @@
PRINT_DEBUG( "position shutdown" );
// safety features!
- bzero( &goal, sizeof(goal) );
- bzero( &velocity, sizeof(velocity) );
+ goal.Zero();
+ velocity.Zero();
- this->SetWatts( 0 );
-
Model::Shutdown();
}
@@ -449,7 +440,7 @@
//assert( ! isnan(y) );
//assert( ! isnan(a) );
- control_mode = STG_POSITION_CONTROL_VELOCITY;
+ control_mode = CONTROL_VELOCITY;
goal.x = x;
goal.y = y;
goal.z = 0;
@@ -459,7 +450,7 @@
void ModelPosition::SetXSpeed( double x )
{
//assert( ! isnan(x) );
- control_mode = STG_POSITION_CONTROL_VELOCITY;
+ control_mode = CONTROL_VELOCITY;
goal.x = x;
}
@@ -467,21 +458,21 @@
void ModelPosition::SetYSpeed( double y )
{
//assert( ! isnan(y) );
- control_mode = STG_POSITION_CONTROL_VELOCITY;
+ control_mode = CONTROL_VELOCITY;
goal.y = y;
}
void ModelPosition::SetZSpeed( double z )
{
//assert( ! isnan(z) );
- control_mode = STG_POSITION_CONTROL_VELOCITY;
+ control_mode = CONTROL_VELOCITY;
goal.z = z;
}
void ModelPosition::SetTurnSpeed( double a )
{
//assert( ! isnan(a) );
- control_mode = STG_POSITION_CONTROL_VELOCITY;
+ control_mode = CONTROL_VELOCITY;
goal.a = a;
}
@@ -493,7 +484,7 @@
//assert( ! isnan(vel.z) );
//assert( ! isnan(vel.a) );
- control_mode = STG_POSITION_CONTROL_VELOCITY;
+ control_mode = CONTROL_VELOCITY;
goal.x = vel.x;
goal.y = vel.y;
goal.z = vel.z;
@@ -506,7 +497,7 @@
//assert( ! isnan(y) );
//assert( ! isnan(a) );
- control_mode = STG_POSITION_CONTROL_POSITION;
+ control_mode = CONTROL_POSITION;
goal.x = x;
goal.y = y;
goal.z = 0;
@@ -515,7 +506,7 @@
void ModelPosition::GoTo( Pose pose )
{
- control_mode = STG_POSITION_CONTROL_POSITION;
+ control_mode = CONTROL_POSITION;
goal = pose;
}
Modified: code/stage/trunk/libstage/powerpack.cc
===================================================================
--- code/stage/trunk/libstage/powerpack.cc 2009-08-27 23:41:43 UTC (rev 8238)
+++ code/stage/trunk/libstage/powerpack.cc 2009-08-28 01:58:11 UTC (rev 8239)
@@ -23,7 +23,10 @@
stored( 0.0 ),
capacity( 0.0 ),
charging( false ),
- dissipated( 0.0 )
+ dissipated( 0.0 ),
+ last_time(0),
+ last_joules(0.0),
+ last_watts(0.0)
{
// tell the world about this new pp
mod->world->AddPowerPack( this );
@@ -48,7 +51,7 @@
}
/** OpenGL visualization of the powerpack state */
-void PowerPack::Visualize( Camera* cam ) const
+void PowerPack::Visualize( Camera* cam )
{
const double height = 0.5;
const double width = 0.2;
@@ -64,8 +67,7 @@
else
glColor4f( 1,0,0, alpha ); // red
- static char buf[6];
- snprintf( buf, 6, "%.0f", percent );
+ // snprintf( buf, 32, "%.0f", percent );
glTranslatef( -width, 0.0, 0.0 );
@@ -126,9 +128,29 @@
glLineWidth( 1.0 );
}
-
- // draw the percentage
- //gl_draw_string( -0.2, 0, 0, buf );
+
+ // compute the instantaneous power output
+ stg_usec_t time_now = mod->world->SimTimeNow();
+ stg_usec_t delta_t = time_now - last_time;
+ stg_watts_t watts = last_watts;
+
+ if( delta_t > 0 ) // some sim time elapsed
+ {
+ stg_joules_t delta_j = stored - last_joules;
+ stg_watts_t watts = (-1e6 * delta_j) / (double)delta_t;
+
+ last_joules = stored;
+ last_time = time_now;
+ last_watts = watts;
+ }
+
+ if( fabs(watts) > 1e-5 ) // any current
+ {
+ glColor4f( 1,0,0,0.8 ); // red
+ char buf[32];
+ snprintf( buf, 32, "%.1fW", watts );
+ Gl::draw_string( -0.05,height+0.05,0, buf );
+ }
}
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-08-27 23:41:43 UTC (rev 8238)
+++ code/stage/trunk/libstage/stage.hh 2009-08-28 01:58:11 UTC (rev 8239)
@@ -146,9 +146,6 @@
"The text of the license may also be available online at\n" \
"http://www.gnu.org/licenses/old-licenses/gpl-2.0.html\n";
- /** The maximum length of a Stage model identifier string */
- const uint32_t TOKEN_MAX = 64;
-
/** Convenient constant */
const double thousand = 1e3;
@@ -695,8 +692,10 @@
void Load( Worldfile* wf, int section );
void Save( Worldfile* wf, int section );
- public:
-
+ public:
+ /** The maximum length of a Stage model identifier string */
+ static const uint32_t TOKEN_MAX = 64;
+
/** get the children of the this element */
ModelPtrVec& GetChildren(){ return children;}
@@ -704,7 +703,6 @@
void ForEachDescendant( stg_model_callback_t func, void* arg );
/** array contains the number of each type of child model */
- //unsigned int child_type_counts[MODEL_TYPE_COUNT];
std::map<std::string,unsigned int> child_type_counts;
Ancestor();
@@ -1223,16 +1221,17 @@
uint32_t GetCount(){ return blocks.size(); };
const Size& GetSize(){ return size; };
const stg_point3_t& GetOffset(){ return offset; };
-
- /** establish the min and max of all the blocks, so we can scale this
- group later */
+
+ /** Establish the min and max of all the blocks, so we can scale this
+ group later. */
void CalcSize();
+
void AppendBlock( Block* block );
void CallDisplayList( Model* mod );
void Clear() ; /** deletes all blocks from the group */
void AppendTouchingModels( ModelPtrSet& touchers );
-
+
/** Returns a pointer to the first model detected to be colliding
with a block in this group, or NULL, if none are detected. */
Model* TestCollision();
@@ -1242,12 +1241,12 @@
void Map();
void UnMap();
- void DrawSolid( const Geom &geom); // draw the block in OpenGL as a solid single color
- void DrawFootPrint( const Geom &geom); // draw the
- // projection of the
- // block onto the z=0
- // plane
+ /** Draw the block in OpenGL as a solid single color. */
+ void DrawSolid( const Geom &geom);
+ /** Draw the projection of the block onto the z=0 plane. */
+ void DrawFootPrint( const Geom &geom);
+
void LoadBitmap( Model* mod, const char* bitmapfile, Worldfile *wf );
void LoadBlock( Model* mod, Worldfile* wf, int entity );
@@ -1263,12 +1262,6 @@
};
-
- //typedef int ctrlinit_t( Model* mod );
- //typedef void ctrlupdate_t( Model* mod );
-
- // BLOCKS
-
class Camera
{
protected:
@@ -1419,8 +1412,8 @@
timesteps. */
stg_usec_t real_time_interval;
- stg_usec_t real_time_now; ///< The current real time in microseconds
- //stg_usec_t real_time_start; ///< the real time at which this world was created
+ /** The current real time in microseconds. */
+ stg_usec_t real_time_now;
/** The last recorded real time, sampled every $timing_interval
updates. */
@@ -1547,6 +1540,7 @@
void Accumulate( stg_meters_t x, stg_meters_t y, stg_joules_t amount );
} event_vis;
+
StripPlotVis output_vis;
StripPlotVis stored_vis;
@@ -1565,6 +1559,11 @@
/** Energy dissipated */
stg_joules_t dissipated;
+
+ // these are used to visualize the power draw
+ stg_usec_t last_time;
+ stg_joules_t last_joules;
+ stg_watts_t last_watts;
static stg_joules_t global_stored;
static stg_joules_t global_capacity;
@@ -1576,7 +1575,7 @@
~PowerPack();
/** OpenGL visualization of the powerpack state */
- void Visualize( Camera* cam ) const;
+ void Visualize( Camera* cam );
/** Print human-readable status on stdout, prefixed with the
argument string */
@@ -1816,7 +1815,7 @@
void ClearPts();
} rastervis;
-
+
bool rebuild_displaylist; ///< iff true, regenerate block display list before redraw
char* say_string; ///< if non-null, this string is displayed in the GUI
@@ -2849,21 +2848,21 @@
public:
/** Define a position control method */
typedef enum
- { STG_POSITION_CONTROL_VELOCITY,
- STG_POSITION_CONTROL_POSITION
+ { CONTROL_VELOCITY,
+ CONTROL_POSITION
} ControlMode;
/** Define a localization method */
typedef enum
- { STG_POSITION_LOCALIZATION_GPS,
- STG_POSITION_LOCALIZATION_ODOM
+ { LOCALIZATION_GPS,
+ LOCALIZATION_ODOM
} LocalizationMode;
/** Define a driving method */
typedef enum
- { STG_POSITION_DRIVE_DIFFERENTIAL,
- STG_POSITION_DRIVE_OMNI,
- STG_POSITION_DRIVE_CAR
+ { DRIVE_DIFFERENTIAL,
+ DRIVE_OMNI,
+ DRIVE_CAR
} DriveMode;
private:
@@ -2942,14 +2941,14 @@
public:
/** Define a actuator control method */
typedef enum
- { STG_ACTUATOR_CONTROL_VELOCITY,
- STG_ACTUATOR_CONTROL_POSITION
+ { CONTROL_VELOCITY,
+ CONTROL_POSITION
} ControlMode;
/** Define an actuator type */
typedef enum
- { STG_ACTUATOR_TYPE_LINEAR,
- STG_ACTUATOR_TYPE_ROTATIONAL
+ { TYPE_LINEAR,
+ TYPE_ROTATIONAL
} ActuatorType;
private:
@@ -2976,13 +2975,13 @@
virtual void Update();
virtual void Load();
- /** Sets the control_mode to STG_ACTUATOR_CONTROL_VELOCITY and sets
+ /** Sets the control_mode to CONTROL_VELOCITY and sets
the goal velocity. */
void SetSpeed( double speed );
double GetSpeed() const {return goal;}
- /** Sets the control mode to STG_ACTUATOR_CONTROL_POSITION and sets
+ /** Sets the control mode to CONTROL_POSITION and sets
the goal pose */
void GoTo( double pose );
Modified: code/stage/trunk/libstage/worldgui.cc
===================================================================
--- code/stage/trunk/libstage/worldgui.cc 2009-08-27 23:41:43 UTC (rev 8238)
+++ code/stage/trunk/libstage/worldgui.cc 2009-08-28 01:58:11 UTC (rev 8239)
@@ -607,13 +607,6 @@
void WorldGui::viewOptionsCb( OptionsDlg* oDlg, WorldGui* wg )
{
- // the options dialog expects a std::vector of options (annoyingly)
- // std::vector<Option*> optvec;
- // adds each option to the vector
- //g_hash_table_foreach( wg->option_table,
- // (GHFunc)append_option,
- // (void*)&optvec );
-
// sort the vector by option label alphabetically
//std::sort();// wg->option_table.begin(), wg->option_table.end() );//, sort_option_pointer );
//std::sort();// wg->option_table.begin(), wg->option_table.end() );//, sort_option_pointer );
Modified: code/stage/trunk/worlds/simple.world
===================================================================
--- code/stage/trunk/worlds/simple.world 2009-08-27 23:41:43 UTC (rev 8238)
+++ code/stage/trunk/worlds/simple.world 2009-08-28 01:58:11 UTC (rev 8239)
@@ -48,7 +48,6 @@
# ctrl "lasernoise" # uncomment this line to run a laser noise generator
)
-
ctrl "wander"
# report error-free position in world coordinates
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2009-08-27 23:41:52
|
Revision: 8238
http://playerstage.svn.sourceforge.net/playerstage/?rev=8238&view=rev
Author: gbiggs
Date: 2009-08-27 23:41:43 +0000 (Thu, 27 Aug 2009)
Log Message:
-----------
Fixed potential segfault in dev_map.c
Modified Paths:
--------------
code/player/trunk/client_libs/libplayerc/dev_map.c
Modified: code/player/trunk/client_libs/libplayerc/dev_map.c
===================================================================
--- code/player/trunk/client_libs/libplayerc/dev_map.c 2009-08-27 19:46:25 UTC (rev 8237)
+++ code/player/trunk/client_libs/libplayerc/dev_map.c 2009-08-27 23:41:43 UTC (rev 8238)
@@ -99,7 +99,7 @@
int playerc_map_get_map(playerc_map_t* device)
{
player_map_info_t *info_req;
- player_map_data_t *data_req, *data_resp;
+ player_map_data_t *data_req, *data_resp = NULL;
int i,j;
int oi,oj;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rt...@us...> - 2009-08-27 19:46:32
|
Revision: 8237
http://playerstage.svn.sourceforge.net/playerstage/?rev=8237&view=rev
Author: rtv
Date: 2009-08-27 19:46:25 +0000 (Thu, 27 Aug 2009)
Log Message:
-----------
GUI clean ups
Modified Paths:
--------------
code/stage/trunk/libstage/canvas.cc
code/stage/trunk/libstage/canvas.hh
code/stage/trunk/libstage/region.cc
code/stage/trunk/libstage/region.hh
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/worldgui.cc
Modified: code/stage/trunk/libstage/canvas.cc
===================================================================
--- code/stage/trunk/libstage/canvas.cc 2009-08-26 23:43:56 UTC (rev 8236)
+++ code/stage/trunk/libstage/canvas.cc 2009-08-27 19:46:25 UTC (rev 8237)
@@ -52,8 +52,8 @@
}
Canvas::Canvas( WorldGui* world,
- int x, int y,
- int width, int height) :
+ int x, int y,
+ int width, int height) :
Fl_Gl_Window( x, y, width, height ),
colorstack(),
models_sorted(),
@@ -69,7 +69,9 @@
interval( 20 ), // msec between redraws
// initialize Option objects
// showBlinken( "Blinkenlights", "show_blinkenlights", "", true, world ),
+ showBBoxes( "Debug/Bounding boxes", "show_boundingboxes", "^b", false, world ),
showBlocks( "Blocks", "show_blocks", "b", true, world ),
+ showBlur( "Trails/Blur", "show_trailblur", "^d", false, world ),
showClock( "Clock", "show_clock", "c", true, world ),
showData( "Data", "show_data", "d", false, world ),
showFlags( "Flags", "show_flags", "l", true, world ),
@@ -82,11 +84,9 @@
showTrailArrows( "Trails/Rising Arrows", "show_trailarrows", "^a", false, world ),
showTrailRise( "Trails/Rising blocks", "show_trailrise", "^r", false, world ),
showTrails( "Trails/Fast", "show_trailfast", "^f", false, world ),
- showTree( "Debug/Tree", "show_tree", "^t", false, world ),
- showBBoxes( "Debug/Bounding boxes", "show_boundingboxes", "^b", false, world ),
- showBlur( "Trails/Blur", "show_trailblur", "^d", false, world ),
+ showVoxels( "Debug/Voxels", "show_voxels", "^v", false, world ),
pCamOn( "Perspective camera", "pcam_on", "r", false, world ),
- visualizeAll( "Selected only", "vis_all", "^v", false, world ),
+ visualizeAll( "Selected only", "vis_all", "v", false, world ),
// and the rest
graphics( true ),
world( world ),
@@ -151,27 +151,27 @@
GLuint mains_id = TextureManager::getInstance().loadTexture( fullpath.c_str() );
TextureManager::getInstance()._mains_texture_id = mains_id;
-// // generate a small glow texture
-// GLubyte* pixels = new GLubyte[ 4 * 128 * 128 ];
+ // // generate a small glow texture
+ // GLubyte* pixels = new GLubyte[ 4 * 128 * 128 ];
-// for( int x=0; x<128; x++ )
-// for( int y=0; y<128; y++ )
-// {
-// GLubyte* p = &pixels[ 4 * (128*y + x)];
-// p[0] = (GLubyte)255; // red
-// p[1] = (GLubyte)0; // green
-// p[2] = (GLubyte)0; // blue
-// p[3] = (GLubyte)128; // alpha
-// }
+ // for( int x=0; x<128; x++ )
+ // for( int y=0; y<128; y++ )
+ // {
+ // GLubyte* p = &pixels[ 4 * (128*y + x)];
+ // p[0] = (GLubyte)255; // red
+ // p[1] = (GLubyte)0; // green
+ // p[2] = (GLubyte)0; // blue
+ // p[3] = (GLubyte)128; // alpha
+ // }
-// glGenTextures(1, &glowTex );
-// glBindTexture( GL_TEXTURE_2D, glowTex );
+ // glGenTextures(1, &glowTex );
+ // glBindTexture( GL_TEXTURE_2D, glowTex );
-// glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, 128, 128, 0,
-// GL_RGBA, GL_UNSIGNED_BYTE, pixels );
+ // glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, 128, 128, 0,
+ // GL_RGBA, GL_UNSIGNED_BYTE, pixels );
-// delete[] pixels;
+ // delete[] pixels;
// draw a check into a bitmap, then load that into a texture
int i, j;
@@ -229,19 +229,19 @@
Model* mod = (*it);
if( mod->gui.move )
- {
- uint8_t rByte, gByte, bByte, aByte;
- uint32_t modelId = mod->id;
- rByte = modelId;
- gByte = modelId >> 8;
- bByte = modelId >> 16;
- aByte = modelId >> 24;
+ {
+ uint8_t rByte, gByte, bByte, aByte;
+ uint32_t modelId = mod->id;
+ rByte = modelId;
+ gByte = modelId >> 8;
+ bByte = modelId >> 16;
+ aByte = modelId >> 24;
- //printf("mod->Id(): 0x%X, rByte: 0x%X, gByte: 0x%X, bByte: 0x%X, aByte: 0x%X\n", modelId, rByte, gByte, bByte, aByte);
+ //printf("mod->Id(): 0x%X, rByte: 0x%X, gByte: 0x%X, bByte: 0x%X, aByte: 0x%X\n", modelId, rByte, gByte, bByte, aByte);
- glColor4ub( rByte, gByte, bByte, aByte );
- mod->DrawPicker();
- }
+ glColor4ub( rByte, gByte, bByte, aByte );
+ mod->DrawPicker();
+ }
}
// read the color of the pixel in the back buffer under the mouse
@@ -253,13 +253,13 @@
uint32_t modelId;
glReadPixels( x,viewport[3]-y,1,1,
- GL_RED,GL_UNSIGNED_BYTE,(void*)&rByte );
+ GL_RED,GL_UNSIGNED_BYTE,(void*)&rByte );
glReadPixels( x,viewport[3]-y,1,1,
- GL_GREEN,GL_UNSIGNED_BYTE,(void*)&gByte );
+ GL_GREEN,GL_UNSIGNED_BYTE,(void*)&gByte );
glReadPixels( x,viewport[3]-y,1,1,
- GL_BLUE,GL_UNSIGNED_BYTE,(void*)&bByte );
+ GL_BLUE,GL_UNSIGNED_BYTE,(void*)&bByte );
glReadPixels( x,viewport[3]-y,1,1,
- GL_ALPHA,GL_UNSIGNED_BYTE,(void*)&aByte );
+ GL_ALPHA,GL_UNSIGNED_BYTE,(void*)&aByte );
modelId = rByte;
modelId |= gByte << 8;
@@ -316,7 +316,7 @@
// convert from 2d window pixel to 3d world coordinates
void Canvas::CanvasToWorld( int px, int py,
- double *wx, double *wy, double* wz )
+ double *wx, double *wy, double* wz )
{
if( px <= 0 )
px = 1;
@@ -359,10 +359,10 @@
{
case FL_MOUSEWHEEL:
if( pCamOn == true ) {
- perspective_camera.scroll( Fl::event_dy() / 10.0 );
+ perspective_camera.scroll( Fl::event_dy() / 10.0 );
}
else {
- camera.scale( Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h() );
+ camera.scale( Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h() );
}
invalidate();
redraw();
@@ -370,154 +370,156 @@
case FL_MOVE: // moused moved while no button was pressed
if( Fl::event_state( FL_META ) )
- {
- puts( "TODO: HANDLE HISTORY" );
- //world->paused = ! world->paused;
- return 1;
- }
+ {
+ puts( "TODO: HANDLE HISTORY" );
+ //world->paused = ! world->paused;
+ return 1;
+ }
if ( startx >=0 )
- {
- // mouse pointing to valid value
+ {
+ // mouse pointing to valid value
- if( Fl::event_state( FL_CTRL ) )
- {
- int dx = Fl::event_x() - startx;
- int dy = Fl::event_y() - starty;
+ if( Fl::event_state( FL_CTRL ) )
+ {
+ int dx = Fl::event_x() - startx;
+ int dy = Fl::event_y() - starty;
- if( pCamOn == true ) {
- perspective_camera.addYaw( -dx );
- perspective_camera.addPitch( -dy );
- }
- else {
- camera.addPitch( - 0.5 * static_cast<double>( dy ) );
- camera.addYaw( - 0.5 * static_cast<double>( dx ) );
- }
- invalidate();
- redraw();
- }
- else if( Fl::event_state( FL_ALT ) )
- {
- int dx = Fl::event_x() - startx;
- int dy = Fl::event_y() - starty;
+ if( pCamOn == true ) {
+ perspective_camera.addYaw( -dx );
+ perspective_camera.addPitch( -dy );
+ }
+ else {
+ camera.addPitch( - 0.5 * static_cast<double>( dy ) );
+ camera.addYaw( - 0.5 * static_cast<double>( dx ) );
+ }
+ invalidate();
+ redraw();
+ }
+ else if( Fl::event_state( FL_ALT ) )
+ {
+ int dx = Fl::event_x() - startx;
+ int dy = Fl::event_y() - starty;
- if( pCamOn == true ) {
- perspective_camera.move( -dx, dy, 0.0 );
- }
- else {
- camera.move( -dx, dy );
- }
- invalidate();
- }
- }
+ if( pCamOn == true ) {
+ perspective_camera.move( -dx, dy, 0.0 );
+ }
+ else {
+ camera.move( -dx, dy );
+ }
+ invalidate();
+ }
+ }
startx = Fl::event_x();
starty = Fl::event_y();
return 1;
case FL_PUSH: // button pressed
{
- //else
- {
- Model* mod = getModel( startx, starty );
- startx = Fl::event_x();
- starty = Fl::event_y();
- selectedModel = false;
- switch( Fl::event_button() )
- {
- case 1:
- clicked_empty_space = ( mod == NULL );
- empty_space_startx = startx;
- empty_space_starty = starty;
- if( mod ) {
- // clicked a model
- if ( Fl::event_state( FL_SHIFT ) ) {
- // holding shift, toggle selection
- if ( selected( mod ) )
- unSelect( mod );
- else {
- select( mod );
- selectedModel = true; // selected a model
+ //else
+ {
+ Model* mod = getModel( startx, starty );
+ startx = Fl::event_x();
+ starty = Fl::event_y();
+ selectedModel = false;
+ switch( Fl::event_button() )
+ {
+ case 1:
+ clicked_empty_space = ( mod == NULL );
+ empty_space_startx = startx;
+ empty_space_starty = starty;
+ if( mod ) {
+ // clicked a model
+ if ( Fl::event_state( FL_SHIFT ) ) {
+ // holding shift, toggle selection
+ if ( selected( mod ) )
+ unSelect( mod );
+ else {
+ select( mod );
+ selectedModel = true; // selected a model
+ }
+ }
+ else {
+ if ( !selected( mod ) ) {
+ // clicked on an unselected model while
+ // not holding shift, this is the new
+ // selection
+ unSelectAll();
+ select( mod );
+ }
+ selectedModel = true; // selected a model
+ }
+ }
+
+ redraw(); // probably required
+ return 1;
+ case 3:
+ {
+ // leave selections alone
+ // rotating handled within FL_DRAG
+ return 1;
+ }
+ default:
+ return 0;
+ }
}
- }
- else {
- if ( !selected( mod ) ) {
- // clicked on an unselected model while
- // not holding shift, this is the new
- // selection
- unSelectAll();
- select( mod );
- }
- selectedModel = true; // selected a model
- }
- }
-
- return 1;
- case 3:
- {
- // leave selections alone
- // rotating handled within FL_DRAG
- return 1;
- }
- default:
- return 0;
- }
- }
}
case FL_DRAG: // mouse moved while button was pressed
{
- int dx = Fl::event_x() - startx;
- int dy = Fl::event_y() - starty;
+ int dx = Fl::event_x() - startx;
+ int dy = Fl::event_y() - starty;
- if ( Fl::event_state( FL_BUTTON1 ) && Fl::event_state( FL_CTRL ) == false ) {
- // Left mouse button drag
- if ( selectedModel ) {
- // started dragging on a selected model
+ if ( Fl::event_state( FL_BUTTON1 ) && Fl::event_state( FL_CTRL ) == false ) {
+ // Left mouse button drag
+ if ( selectedModel ) {
+ // started dragging on a selected model
- double sx,sy,sz;
- CanvasToWorld( startx, starty,
- &sx, &sy, &sz );
- double x,y,z;
- CanvasToWorld( Fl::event_x(), Fl::event_y(),
- &x, &y, &z );
- // move all selected models to the mouse pointer
- FOR_EACH( it, selected_models )
- {
- Model* mod = *it;
- mod->AddToPose( x-sx, y-sy, 0, 0 );
- }
- }
- else {
- // started dragging on empty space or an
- // unselected model, move the canvas
- if( pCamOn == true ) {
- perspective_camera.move( -dx, dy, 0.0 );
- }
- else {
- camera.move( -dx, dy );
- }
- invalidate(); // so the projection gets updated
- }
- }
- else if ( Fl::event_state( FL_BUTTON3 ) || ( Fl::event_state( FL_BUTTON1 ) && Fl::event_state( FL_CTRL ) ) ) {
- // rotate all selected models
- FOR_EACH( it, selected_models )
- {
- Model* mod = *it;
- mod->AddToPose( 0,0,0, 0.05*(dx+dy) );
- }
- }
+ double sx,sy,sz;
+ CanvasToWorld( startx, starty,
+ &sx, &sy, &sz );
+ double x,y,z;
+ CanvasToWorld( Fl::event_x(), Fl::event_y(),
+ &x, &y, &z );
+ // move all selected models to the mouse pointer
+ FOR_EACH( it, selected_models )
+ {
+ Model* mod = *it;
+ mod->AddToPose( x-sx, y-sy, 0, 0 );
+ }
+ }
+ else {
+ // started dragging on empty space or an
+ // unselected model, move the canvas
+ if( pCamOn == true ) {
+ perspective_camera.move( -dx, dy, 0.0 );
+ }
+ else {
+ camera.move( -dx, dy );
+ }
+ invalidate(); // so the projection gets updated
+ }
+ }
+ else if ( Fl::event_state( FL_BUTTON3 ) || ( Fl::event_state( FL_BUTTON1 ) && Fl::event_state( FL_CTRL ) ) ) {
+ // rotate all selected models
+ FOR_EACH( it, selected_models )
+ {
+ Model* mod = *it;
+ mod->AddToPose( 0,0,0, 0.05*(dx+dy) );
+ }
+ }
- startx = Fl::event_x();
- starty = Fl::event_y();
+ startx = Fl::event_x();
+ starty = Fl::event_y();
- redraw();
- return 1;
+ redraw();
+ return 1;
} // end case FL_DRAG
case FL_RELEASE: // mouse button released
if( empty_space_startx == Fl::event_x() && empty_space_starty == Fl::event_y() && clicked_empty_space == true ) {
- // clicked on empty space, unselect all
- unSelectAll();
+ // clicked on empty space, unselect all
+ unSelectAll();
+ redraw();
}
return 1;
@@ -528,23 +530,23 @@
case FL_KEYBOARD:
switch( Fl::event_key() )
- {
- case FL_Left:
- if( pCamOn == false ) { camera.move( -10, 0 ); }
- else { perspective_camera.strafe( -0.5 ); } break;
- case FL_Right:
- if( pCamOn == false ) {camera.move( 10, 0 ); }
- else { perspective_camera.strafe( 0.5 ); } break;
- case FL_Down:
- if( pCamOn == false ) {camera.move( 0, -10 ); }
- else { perspective_camera.forward( -0.5 ); } break;
- case FL_Up:
- if( pCamOn == false ) {camera.move( 0, 10 ); }
- else { perspective_camera.forward( 0.5 ); } break;
- default:
- redraw(); // we probably set a display config - so need this
- return 0; // keypress unhandled
- }
+ {
+ case FL_Left:
+ if( pCamOn == false ) { camera.move( -10, 0 ); }
+ else { perspective_camera.strafe( -0.5 ); } break;
+ case FL_Right:
+ if( pCamOn == false ) {camera.move( 10, 0 ); }
+ else { perspective_camera.strafe( 0.5 ); } break;
+ case FL_Down:
+ if( pCamOn == false ) {camera.move( 0, -10 ); }
+ else { perspective_camera.forward( -0.5 ); } break;
+ case FL_Up:
+ if( pCamOn == false ) {camera.move( 0, 10 ); }
+ else { perspective_camera.forward( 0.5 ); } break;
+ default:
+ redraw(); // we probably set a display config - so need this
+ return 0; // keypress unhandled
+ }
invalidate(); // update projection
return 1;
@@ -681,14 +683,6 @@
{
FOR_EACH( it, models_sorted )
(*it)->DrawBlocksTree();
-
- // some models may be carried by others - this prevents them being drawn twice
- // for( GList* it = models_sorted; it; it=it->next )
- // {
- // Model* mod = (Model*)it->data;
- // if( mod->parent == NULL )
- // mod->DrawBlocksTree();
- // }
}
void Canvas::DrawBoundingBoxes()
@@ -751,10 +745,10 @@
Pose b_pose = b->GetGlobalPose();
stg_meters_t a_dist = hypot( y - a_pose.y,
- x - a_pose.x );
+ x - a_pose.x );
stg_meters_t b_dist = hypot( y - b_pose.y,
- x - b_pose.x );
+ x - b_pose.x );
return ( a_dist < b_dist );
}
@@ -790,23 +784,13 @@
if( ! showTrails )
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
- if( showTree || showOccupancy )
- {
- glPushMatrix();
-
- GLfloat scale = 1.0/world->Resolution();
- glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly
- // out for Z. look into it.
-
- if( showOccupancy )
- ((WorldGui*)world)->DrawTree( false );
-
- if( showTree )
- ((WorldGui*)world)->DrawTree( true );
-
- glPopMatrix();
- }
+ if( showOccupancy )
+ ((WorldGui*)world)->DrawOccupancy();
+ if( showVoxels )
+ ((WorldGui*)world)->DrawVoxels();
+
+
if( ! world->rt_cells.empty() )
{
glPushMatrix();
@@ -821,24 +805,24 @@
glBegin( GL_POINTS );
for( unsigned int i=0;
- i < world->rt_cells.size();
- i++ )
- {
- char str[128];
- snprintf( str, 128, "(%d,%d)",
- world->rt_cells[i].x,
- world->rt_cells[i].y );
+ i < world->rt_cells.size();
+ i++ )
+ {
+ char str[128];
+ snprintf( str, 128, "(%d,%d)",
+ world->rt_cells[i].x,
+ world->rt_cells[i].y );
- Gl::draw_string( world->rt_cells[i].x+1,
- world->rt_cells[i].y+1, 0.1, str );
+ Gl::draw_string( world->rt_cells[i].x+1,
+ world->rt_cells[i].y+1, 0.1, str );
- //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y );
- //glRectf( world->rt_cells[i].x+0.3, world->rt_cells[i].y+0.3,
- // world->rt_cells[i].x+0.7, world->rt_cells[i].y+0.7 );
+ //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y );
+ //glRectf( world->rt_cells[i].x+0.3, world->rt_cells[i].y+0.3,
+ // world->rt_cells[i].x+0.7, world->rt_cells[i].y+0.7 );
- glVertex2f( world->rt_cells[i].x, world->rt_cells[i].y );
+ glVertex2f( world->rt_cells[i].x, world->rt_cells[i].y );
- }
+ }
glEnd();
@@ -846,11 +830,11 @@
world->PushColor( Color( 0,1,0,0.2) );
glBegin( GL_LINE_STRIP );
for( unsigned int i=0;
- i < world->rt_cells.size();
- i++ )
- {
- glVertex2f( world->rt_cells[i].x+0.5, world->rt_cells[i].y+0.5 );
- }
+ i < world->rt_cells.size();
+ i++ )
+ {
+ glVertex2f( world->rt_cells[i].x+0.5, world->rt_cells[i].y+0.5 );
+ }
glEnd();
world->PopColor();
#endif
@@ -870,30 +854,30 @@
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
for( unsigned int i=0;
- i < world->rt_candidate_cells.size();
- i++ )
- {
- // char str[128];
- // snprintf( str, 128, "(%d,%d)",
- // world->rt_candidate_cells[i].x,
- // world->rt_candidate_cells[i].y );
+ i < world->rt_candidate_cells.size();
+ i++ )
+ {
+ // char str[128];
+ // snprintf( str, 128, "(%d,%d)",
+ // world->rt_candidate_cells[i].x,
+ // world->rt_candidate_cells[i].y );
- // Gl::draw_string( world->rt_candidate_cells[i].x+1,
- // world->rt_candidate_cells[i].y+1, 0.1, str );
+ // Gl::draw_string( world->rt_candidate_cells[i].x+1,
+ // world->rt_candidate_cells[i].y+1, 0.1, str );
- //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y );
- glRectf( world->rt_candidate_cells[i].x, world->rt_candidate_cells[i].y,
- world->rt_candidate_cells[i].x+1, world->rt_candidate_cells[i].y+1 );
- }
+ //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y );
+ glRectf( world->rt_candidate_cells[i].x, world->rt_candidate_cells[i].y,
+ world->rt_candidate_cells[i].x+1, world->rt_candidate_cells[i].y+1 );
+ }
world->PushColor( Color( 0,1,0,0.2) );
glBegin( GL_LINE_STRIP );
for( unsigned int i=0;
- i < world->rt_candidate_cells.size();
- i++ )
- {
- glVertex2f( world->rt_candidate_cells[i].x+0.5, world->rt_candidate_cells[i].y+0.5 );
- }
+ i < world->rt_candidate_cells.size();
+ i++ )
+ {
+ glVertex2f( world->rt_candidate_cells[i].x+0.5, world->rt_candidate_cells[i].y+0.5 );
+ }
glEnd();
world->PopColor();
@@ -903,8 +887,6 @@
//world->rt_cells.clear();
}
-
-
if( showGrid )
DrawGlobalGrid();
else
@@ -915,7 +897,7 @@
glDisable( GL_DEPTH_TEST ); // using alpha blending
FOR_EACH( it, models_sorted )
- (*it)->DrawTrailFootprint();
+ (*it)->DrawTrailFootprint();
glEnable( GL_DEPTH_TEST );
}
@@ -952,11 +934,11 @@
if( showData ) {
if ( ! visualizeAll ) {
FOR_EACH( it, world->World::children )
- (*it)->DataVisualizeTree( current_camera );
+ (*it)->DataVisualizeTree( current_camera );
}
else if ( selected_models.size() > 0 ) {
FOR_EACH( it, world->World::children )
- (*it)->DataVisualizeTree( current_camera );
+ (*it)->DataVisualizeTree( current_camera );
}
else if ( last_selection ) {
last_selection->DataVisualizeTree( current_camera );
@@ -972,10 +954,10 @@
glPushMatrix();
//ensure two icons can't be in the exact same plane
if( camera.pitch() == 0 && !pCamOn )
- glTranslatef( 0, 0, 0.1 );
+ glTranslatef( 0, 0, 0.1 );
FOR_EACH( it, models_sorted )
- (*it)->DrawStatusTree( &camera );
+ (*it)->DrawStatusTree( &camera );
glPopMatrix();
}
@@ -985,13 +967,13 @@
glDisable( GL_DEPTH_TEST );
PushColor( 0,0,0,0.5 );
FOR_EACH( it, world->ray_list )
- {
- float* pts = *it;
- glBegin( GL_LINES );
- glVertex2f( pts[0], pts[1] );
- glVertex2f( pts[2], pts[3] );
- glEnd();
- }
+ {
+ float* pts = *it;
+ glBegin( GL_LINES );
+ glVertex2f( pts[0], pts[1] );
+ glVertex2f( pts[2], pts[3] );
+ glEnd();
+ }
PopColor();
glEnable( GL_DEPTH_TEST );
@@ -1015,7 +997,7 @@
std::string clockstr = world->ClockString();
if( showFollow == true && last_selection )
- clockstr.append( " [FOLLOW MODE]" );
+ clockstr.append( " [FOLLOW MODE]" );
float txtWidth = gl_width( clockstr.c_str());
if( txtWidth < 200 ) txtWidth = 200;
@@ -1036,16 +1018,16 @@
// ENERGY BOX
if( PowerPack::global_capacity > 0 )
- {
- colorstack.Push( 0.8,1.0,0.8,0.85 ); // pale green
- glRectf( 0, height, width, 90 );
- colorstack.Push( 0,0,0 ); // black
- Gl::draw_string_multiline( margin, height + margin, width, 50,
- world->EnergyString().c_str(),
- (Fl_Align)( FL_ALIGN_LEFT | FL_ALIGN_BOTTOM) );
- colorstack.Pop();
- colorstack.Pop();
- }
+ {
+ colorstack.Push( 0.8,1.0,0.8,0.85 ); // pale green
+ glRectf( 0, height, width, 90 );
+ colorstack.Push( 0,0,0 ); // black
+ Gl::draw_string_multiline( margin, height + margin, width, 50,
+ world->EnergyString().c_str(),
+ (Fl_Align)( FL_ALIGN_LEFT | FL_ALIGN_BOTTOM) );
+ colorstack.Pop();
+ colorstack.Pop();
+ }
glEnable( GL_DEPTH_TEST );
glPopMatrix();
@@ -1133,11 +1115,11 @@
png_set_rows( pp, info, rowpointers );
png_set_IHDR( pp, info,
- width, height, 8,
- PNG_COLOR_TYPE_RGBA,
- PNG_INTERLACE_NONE,
- PNG_COMPRESSION_TYPE_DEFAULT,
- PNG_FILTER_TYPE_DEFAULT);
+ width, height, 8,
+ PNG_COLOR_TYPE_RGBA,
+ PNG_INTERLACE_NONE,
+ PNG_COMPRESSION_TYPE_DEFAULT,
+ PNG_FILTER_TYPE_DEFAULT);
png_write_png( pp, info, PNG_TRANSFORM_IDENTITY, NULL );
@@ -1183,7 +1165,7 @@
showTrails.createMenuItem( menu, path );
showTrailRise.createMenuItem( menu, path ); // broken
showBBoxes.createMenuItem( menu, path );
- showTree.createMenuItem( menu, path );
+ showVoxels.createMenuItem( menu, path );
showScreenshots.createMenuItem( menu, path );
}
@@ -1213,15 +1195,15 @@
showTrailArrows.Load( wf, sec );
showTrailRise.Load( wf, sec );
showTrails.Load( wf, sec );
- showTree.Load( wf, sec );
+ showVoxels.Load( wf, sec );
showScreenshots.Load( wf, sec );
pCamOn.Load( wf, sec );
if( ! world->paused )
// // start the timer that causes regular redraws
Fl::add_timeout( ((double)interval/1000),
- (Fl_Timeout_Handler)Canvas::TimerCallback,
- this);
+ (Fl_Timeout_Handler)Canvas::TimerCallback,
+ this);
invalidate(); // we probably changed something
}
@@ -1246,7 +1228,7 @@
showTrailArrows.Save( wf, sec );
showTrailRise.Save( wf, sec );
showTrails.Save( wf, sec );
- showTree.Save( wf, sec );
+ showVoxels.Save( wf, sec );
showScreenshots.Save( wf, sec );
pCamOn.Save( wf, sec );
}
@@ -1254,31 +1236,27 @@
void Canvas::draw()
{
- // static unsigned long calls=0;
- // printf( "Draw calls %lu\n", ++calls );
-
-
//Enable the following to debug camera model
// if( loaded_texture == true && pCamOn == true )
// return;
-
+
if (!valid() )
{
if( ! init_done )
- InitGl();
+ InitGl();
if( pCamOn == true )
- {
- perspective_camera.setAspect( static_cast< float >( w() ) / static_cast< float >( h() ) );
- perspective_camera.SetProjection();
- current_camera = &perspective_camera;
- }
+ {
+ perspective_camera.setAspect( static_cast< float >( w() ) / static_cast< float >( h() ) );
+ perspective_camera.SetProjection();
+ current_camera = &perspective_camera;
+ }
else
- {
- stg_bounds3d_t extent = world->GetExtent();
- camera.SetProjection( w(), h(), extent.y.min, extent.y.max );
- current_camera = &camera;
- }
+ {
+ stg_bounds3d_t extent = world->GetExtent();
+ camera.SetProjection( w(), h(), extent.y.min, extent.y.max );
+ current_camera = &camera;
+ }
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
}
@@ -1288,14 +1266,14 @@
{
Pose gpose = last_selection->GetGlobalPose();
if( pCamOn == true )
- {
- perspective_camera.setPose( gpose.x, gpose.y, 0.2 );
- perspective_camera.setYaw( rtod( gpose.a ) - 90.0 );
- }
+ {
+ perspective_camera.setPose( gpose.x, gpose.y, 0.2 );
+ perspective_camera.setYaw( rtod( gpose.a ) - 90.0 );
+ }
else
- {
- camera.setPose( gpose.x, gpose.y );
- }
+ {
+ camera.setPose( gpose.x, gpose.y );
+ }
}
current_camera->Draw();
Modified: code/stage/trunk/libstage/canvas.hh
===================================================================
--- code/stage/trunk/libstage/canvas.hh 2009-08-26 23:43:56 UTC (rev 8236)
+++ code/stage/trunk/libstage/canvas.hh 2009-08-27 19:46:25 UTC (rev 8237)
@@ -78,7 +78,9 @@
void RemoveModel( Model* mod );
Option //showBlinken,
+ showBBoxes,
showBlocks,
+ showBlur,
showClock,
showData,
showFlags,
@@ -91,9 +93,7 @@
showTrailArrows,
showTrailRise,
showTrails,
- showTree,
- showBBoxes,
- showBlur,
+ showVoxels,
pCamOn,
visualizeAll;
Modified: code/stage/trunk/libstage/region.cc
===================================================================
--- code/stage/trunk/libstage/region.cc 2009-08-26 23:43:56 UTC (rev 8236)
+++ code/stage/trunk/libstage/region.cc 2009-08-27 19:46:25 UTC (rev 8237)
@@ -7,10 +7,6 @@
#include "region.hh"
using namespace Stg;
-// static member for accumulating empty regions for occasional garbage
-// collection
-// std::set<Region*> Region::empty_regions;
-
Region::Region( SuperRegion* sr) :
cells(),
superregion(sr),
@@ -29,25 +25,21 @@
count(0)
{
// populate the regions
- regions.insert( regions.begin(), SUPERREGIONSIZE, Region( this ) );
-
- //static int srcount=0;
- //printf( "created SR number %d\n", ++srcount );
- // printf( "superregion at %d %d\n", origin.x, origin.y );
+ regions.insert( regions.begin(), SUPERREGIONSIZE, Region( this ) );
}
SuperRegion::~SuperRegion()
{
- //printf( "deleting SR %p at [%d,%d]\n", this, origin.x, origin.y );
}
-void SuperRegion::Draw( bool drawall )
+void SuperRegion::DrawOccupancy()
{
- glEnable( GL_DEPTH_TEST );
-
- glPushMatrix();
+ glPushMatrix();
+ GLfloat scale = 1.0/world->Resolution();
+ glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly
glTranslatef( origin.x<<SRBITS, origin.y<<SRBITS,0);
-
+
+ glEnable( GL_DEPTH_TEST );
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
// outline superregion
@@ -62,27 +54,27 @@
for( int y=0; y<SUPERREGIONWIDTH; y++ )
for( int x=0; x<SUPERREGIONWIDTH; x++ )
{
- if( r->count )
+ if( r->count ) // region contains some occupied cells
{
- // outline regions with contents
+ // outline the region
glRecti( x<<RBITS, y<<RBITS,
(x+1)<<RBITS, (y+1)<<RBITS );
-
+
+ // show how many cells are occupied
snprintf( buf, 15, "%lu", r->count );
Gl::draw_string( x<<RBITS, y<<RBITS, 0, buf );
+ // draw a rectangle around each occupied cell
for( int p=0; p<REGIONWIDTH; p++ )
for( int q=0; q<REGIONWIDTH; q++ )
if( r->cells[p+(q*REGIONWIDTH)].blocks.size() )
{
GLfloat xx = p+(x<<RBITS);
- GLfloat yy = q+(y<<RBITS);
-
- glRecti( xx, yy,
- xx+1, yy+1);
+ GLfloat yy = q+(y<<RBITS);
+ glRecti( xx, yy, xx+1, yy+1);
}
}
- else if( ! r->cells.empty() )
+ else if( ! r->cells.empty() ) // empty but used previously
{
double left = x << RBITS;
double right = (x+1) << RBITS;
@@ -90,7 +82,7 @@
double top = (y+1) << RBITS;
double d = 3.0;
-
+
// draw little corner markers for regions with memory
// allocated but no contents
glBegin( GL_LINES );
@@ -122,144 +114,92 @@
glPopMatrix();
}
-// TODO
-#if 0
-void SuperRegion::Draw( bool drawall )
+
+static void DrawBlock( GLfloat x, GLfloat y, GLfloat zmin, GLfloat zmax )
{
- glEnable( GL_DEPTH_TEST );
+ glBegin( GL_QUADS );
+
+ // TOP
+ glVertex3f( x, y, zmax );
+ glVertex3f( 1+x, y, zmax );
+ glVertex3f( 1+x, 1+y, zmax );
+ glVertex3f( x, 1+y, zmax );
+
+ // sides
+ glVertex3f( x, y, zmax );
+ glVertex3f( x, 1+y, zmax );
+ glVertex3f( x, 1+y, zmin );
+ glVertex3f( x, y, zmin );
+
+ glVertex3f( 1+x, y, zmax );
+ glVertex3f( x, y, zmax );
+ glVertex3f( x, y, zmin );
+ glVertex3f( 1+x, y, zmin );
+
+ glVertex3f( 1+x, 1+y, zmax );
+ glVertex3f( 1+x, y, zmax );
+ glVertex3f( 1+x, y, zmin );
+ glVertex3f( 1+x, 1+y, zmin );
+
+ glVertex3f( x, 1+y, zmax );
+ glVertex3f( 1+x, 1+y, zmax );
+ glVertex3f( 1+x, 1+y, zmin );
+ glVertex3f( x, 1+y, zmin );
- glPushMatrix();
+ glEnd();
+}
+
+void SuperRegion::DrawVoxels()
+{
+ glPushMatrix();
+ GLfloat scale = 1.0/world->Resolution();
+ glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly
glTranslatef( origin.x<<SRBITS, origin.y<<SRBITS,0);
+
+ glEnable( GL_DEPTH_TEST );
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- r = GetRegion( 0, 0);
-
+ Region* r = GetRegion( 0, 0);
+
for( int y=0; y<SUPERREGIONWIDTH; y++ )
for( int x=0; x<SUPERREGIONWIDTH; x++ )
{
- if( r->count < 1 )
- {
- r++;
- continue;
- }
-
- snprintf( buf, 15, "%lu", r->count );
- Gl::draw_string( x<<RBITS, y<<RBITS, 0, buf );
-
- for( int p=0; p<REGIONWIDTH; p++ )
- for( int q=0; q<REGIONWIDTH; q++ )
- if( r->cells[p+(q*REGIONWIDTH)].blocks.size() )
- {
- GLfloat xx = p+(x<<RBITS);
- GLfloat yy = q+(y<<RBITS);
+ if( r->count )
+ for( int p=0; p<REGIONWIDTH; p++ )
+ for( int q=0; q<REGIONWIDTH; q++ )
+ {
+ Cell* c = (Cell*)&r->cells[p+(q*REGIONWIDTH)];
- if( ! drawall ) // draw a rectangle on the floor
- {
- glRecti( xx, yy,
- xx+1, yy+1);
- }
- else // draw a rectangular solid
- {
- Cell* c = (Cell*)&r->cells[p+(q*REGIONWIDTH)];
-
+ if( c->blocks.size() )
+ {
+ GLfloat xx = p+(x<<RBITS);
+ GLfloat yy = q+(y<<RBITS);
+
FOR_EACH( it, c->blocks )
{
Block* block = *it;
-
- //printf( "zb %.2f %.2f\n", ent->zbounds.min, ent->zbounds.max );
-
+ // first draw filled polygons
Color c = block->GetColor();
glColor4f( c.r, c.g, c.b, 1.0 );
-
+
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
glEnable(GL_POLYGON_OFFSET_FILL);
// TODO - these numbers need tweaking for
// better-looking rendering
- glPolygonOffset(0.01, 0.1);
-
- // // TOP
- glBegin( GL_POLYGON );
- glVertex3f( xx, yy, block->global_z.max );
- glVertex3f( 1+xx, yy, block->global_z.max );
- glVertex3f( 1+xx, 1+yy, block->global_z.max );
- glVertex3f( xx, 1+yy, block->global_z.max );
- glEnd();
-
- // sides
- glBegin( GL_QUADS );
- glVertex3f( xx, yy, block->global_z.max );
- glVertex3f( xx, 1+yy, block->global_z.max );
- glVertex3f( xx, 1+yy, block->global_z.min );
- glVertex3f( xx, yy, block->global_z.min );
-
- glVertex3f( 1+xx, yy, block->global_z.max );
- glVertex3f( xx, yy, block->global_z.max );
- glVertex3f( xx, yy, block->global_z.min );
- glVertex3f( 1+xx, yy, block->global_z.min );
-
- glVertex3f( 1+xx, 1+yy, block->global_z.max );
- glVertex3f( 1+xx, yy, block->global_z.max );
- glVertex3f( 1+xx, yy, block->global_z.min );
- glVertex3f( 1+xx, 1+yy, block->global_z.min );
-
- glVertex3f( xx, 1+yy, block->global_z.max );
- glVertex3f( 1+xx, 1+yy, block->global_z.max );
- glVertex3f( 1+xx, 1+yy, block->global_z.min );
- glVertex3f( xx, 1+yy, block->global_z.min );
- glEnd();
-
- glDisable(GL_POLYGON_OFFSET_FILL);
-
+ glPolygonOffset(0.01, 0.1);
+ DrawBlock( xx, yy, block->global_z.min, block->global_z.max );
+
+ // draw again in outline
+ glDisable(GL_POLYGON_OFFSET_FILL);
glColor4f( c.r/2.0, c.g/2.0, c.b/2.0, c.a );
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
-
- // TOP
- glBegin( GL_POLYGON );
- glVertex3f( xx, yy, block->global_z.max );
- glVertex3f( 1+xx, yy, block->global_z.max );
- glVertex3f( 1+xx, 1+yy, block->global_z.max );
- glVertex3f( xx, 1+yy, block->global_z.max );
- glEnd();
-
- // sides
- glBegin( GL_QUADS );
- glVertex3f( xx, yy, block->global_z.max );
- glVertex3f( xx, 1+yy, block->global_z.max );
- glVertex3f( xx, 1+yy, block->global_z.min );
- glVertex3f( xx, yy, block->global_z.min );
-
- glVertex3f( 1+xx, yy, block->global_z.max );
- glVertex3f( xx, yy, block->global_z.max );
- glVertex3f( xx, yy, block->global_z.min );
- glVertex3f( 1+xx, yy, block->global_z.min );
-
- glVertex3f( 1+xx, 1+yy, block->global_z.max );
- glVertex3f( 1+xx, yy, block->global_z.max );
- glVertex3f( 1+xx, yy, block->global_z.min );
- glVertex3f( 1+xx, 1+yy, block->global_z.min );
-
- glVertex3f( xx, 1+yy, block->global_z.max );
- glVertex3f( 1+xx, 1+yy, block->global_z.max );
- glVertex3f( 1+xx, 1+yy, block->global_z.min );
- glVertex3f( xx, 1+yy, block->global_z.min );
- glEnd();
- }
- }
- }
-
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+ DrawBlock( xx, yy, block->global_z.min, block->global_z.max );
+ }
+ }
+ }
+
++r;
}
- glPopMatrix();
-}
-#endif
-
-//inline
-void SuperRegion::Floor()
-{
- glPushMatrix();
- glTranslatef( origin.x<<SRBITS, origin.y<<SRBITS, 0 );
- glRecti( 0,0, 1<<SRBITS, 1<<SRBITS );
glPopMatrix();
}
-
Modified: code/stage/trunk/libstage/region.hh
===================================================================
--- code/stage/trunk/libstage/region.hh 2009-08-26 23:43:56 UTC (rev 8236)
+++ code/stage/trunk/libstage/region.hh 2009-08-27 19:46:25 UTC (rev 8237)
@@ -94,8 +94,8 @@
Region* GetRegion( int32_t x, int32_t y )
{ return( ®ions[ x + y * SUPERREGIONWIDTH ] ); }
- void Draw( bool drawall );
- void Floor();
+ void DrawOccupancy();
+ void DrawVoxels();
unsigned long count; // number of blocks rendered into this superregion
}; // class SuperRegion;
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-08-26 23:43:56 UTC (rev 8236)
+++ code/stage/trunk/libstage/stage.hh 2009-08-27 19:46:25 UTC (rev 8237)
@@ -1462,9 +1462,9 @@
virtual void PushColor( double r, double g, double b, double a );
virtual void PopColor();
- void DrawTree( bool leaves );
- void DrawFloor();
-
+ void DrawOccupancy();
+ void DrawVoxels();
+
public:
WorldGui(int W,int H,const char*L=0);
Modified: code/stage/trunk/libstage/worldgui.cc
===================================================================
--- code/stage/trunk/libstage/worldgui.cc 2009-08-26 23:43:56 UTC (rev 8236)
+++ code/stage/trunk/libstage/worldgui.cc 2009-08-27 19:46:25 UTC (rev 8237)
@@ -417,20 +417,16 @@
return std::string( str );
}
-void WorldGui::DrawTree( bool drawall )
+void WorldGui::DrawOccupancy()
{
FOR_EACH( it, superregions )
- (*it).second->Draw( drawall );
+ (*it).second->DrawOccupancy();
}
-void WorldGui::DrawFloor()
-{
- PushColor( 1,1,1,1 );
-
+void WorldGui::DrawVoxels()
+{
FOR_EACH( it, superregions )
- (*it).second->Floor();
-
- PopColor();
+ (*it).second->DrawVoxels();
}
void WorldGui::windowCb( Fl_Widget* w, WorldGui* wg )
@@ -523,7 +519,13 @@
void WorldGui::slowerCb( Fl_Widget* w, WorldGui* wg )
{
- wg->speedup *= 0.8;
+ if( wg->speedup <= 0 )
+ {
+ wg->speedup = 100.0;
+ wg->SetTimeouts();
+ }
+ else
+ wg->speedup *= 0.8;
}
void WorldGui::fasterCb( Fl_Widget* w, WorldGui* wg )
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2009-08-26 23:44:03
|
Revision: 8236
http://playerstage.svn.sourceforge.net/playerstage/?rev=8236&view=rev
Author: gbiggs
Date: 2009-08-26 23:43:56 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Applied patch #2845149: Fix to playerv
Modified Paths:
--------------
code/player/trunk/utils/playerv/pv_dev_camera.c
Modified: code/player/trunk/utils/playerv/pv_dev_camera.c
===================================================================
--- code/player/trunk/utils/playerv/pv_dev_camera.c 2009-08-26 23:42:03 UTC (rev 8235)
+++ code/player/trunk/utils/playerv/pv_dev_camera.c 2009-08-26 23:43:56 UTC (rev 8236)
@@ -51,6 +51,7 @@
camera->datatime = 0;
camera->drivername = strdup(drivername);
camera->proxy = playerc_camera_create(client, index);
+ camera->img_buffer = NULL;
// Construct the menu
snprintf(label, sizeof(label), "camera:%d (%s)", index, camera->drivername);
@@ -95,6 +96,7 @@
playerc_camera_destroy(camera->proxy);
free(camera->drivername);
+ free(camera->img_buffer);
free(camera);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2009-08-26 23:42:19
|
Revision: 8235
http://playerstage.svn.sourceforge.net/playerstage/?rev=8235&view=rev
Author: gbiggs
Date: 2009-08-26 23:42:03 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Changed fmin() to <
Modified Paths:
--------------
code/player/trunk/server/drivers/shell/readlog.cc
Modified: code/player/trunk/server/drivers/shell/readlog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/readlog.cc 2009-08-26 23:41:05 UTC (rev 8234)
+++ code/player/trunk/server/drivers/shell/readlog.cc 2009-08-26 23:42:03 UTC (rev 8235)
@@ -1909,7 +1909,9 @@
data.data.ranges = new double[ data.data.ranges_count ];
count = 0;
- int loop_size=fmin(token_count, total_count+data.data.ranges_count);
+ int loop_size = token_count;
+ if (total_count + (int)data.data.ranges_count < loop_size)
+ loop_size = total_count + (int)data.data.ranges_count;
for (i = total_count; i < loop_size; i += 2)
{
data.data.ranges[count] = static_cast<double> (atof(tokens[i]));
@@ -1964,7 +1966,9 @@
data.geom.element_poses = new player_pose3d_t [ data.geom.element_poses_count ];
count = 0;
- loop_size=fmin(token_count, total_count+data.geom.element_poses_count*6);
+ loop_size = token_count;
+ if (total_count + (int)data.geom.element_poses_count*6 < loop_size)
+ loop_size = total_count + (int)data.geom.element_poses_count*6;
for (i = total_count; i < loop_size; i += 6)
{
data.geom.element_poses[count].px = static_cast<double> (atof(tokens[i]));
@@ -1998,7 +2002,9 @@
data.geom.element_sizes = new player_bbox3d_t [ data.geom.element_sizes_count ];
count = 0;
- loop_size=fmin(token_count, total_count+data.geom.element_sizes_count*3);
+ loop_size = token_count;
+ if (total_count + (int)data.geom.element_sizes_count*3 < loop_size)
+ loop_size = total_count + (int)data.geom.element_sizes_count*3;
for (i = total_count; i < loop_size; i += 3)
{
data.geom.element_sizes[count].sw = static_cast<double> (atof(tokens[i]));
@@ -2135,7 +2141,9 @@
data.data.intensities = new double[ data.data.intensities_count ];
count = 0;
- int loop_size=fmin(token_count, total_count+data.data.intensities_count);
+ int loop_size = token_count;
+ if (total_count + (int)data.data.intensities_count < loop_size)
+ loop_size = total_count + (int)data.data.intensities_count;
for (i = total_count; i < loop_size; i += 2)
{
data.data.intensities[count] = static_cast<double> (atof(tokens[i]));
@@ -2190,7 +2198,9 @@
data.geom.element_poses = new player_pose3d_t [ data.geom.element_poses_count ];
count = 0;
- loop_size=fmin(token_count, total_count+data.geom.element_poses_count*6);
+ loop_size=token_count;
+ if (total_count + (int)data.geom.element_poses_count*6 < loop_size)
+ loop_size = total_count + (int)data.geom.element_poses_count*6;
for (i = total_count; i < loop_size; i += 6)
{
data.geom.element_poses[count].px = static_cast<double> (atof(tokens[i]));
@@ -2224,7 +2234,9 @@
data.geom.element_sizes = new player_bbox3d_t [ data.geom.element_sizes_count ];
count = 0;
- loop_size=fmin(token_count, total_count+data.geom.element_sizes_count*3);
+ loop_size=token_count;
+ if (total_count + (int)data.geom.element_sizes_count*3 < loop_size)
+ loop_size = total_count + (int)data.geom.element_sizes_count*3;
for (i = total_count; i < loop_size; i += 3)
{
data.geom.element_sizes[count].sw = static_cast<double> (atof(tokens[i]));
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rt...@us...> - 2009-08-26 23:41:16
|
Revision: 8234
http://playerstage.svn.sourceforge.net/playerstage/?rev=8234&view=rev
Author: rtv
Date: 2009-08-26 23:41:05 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
revised FLTK timer and idle callbacks to better suit Linux
Modified Paths:
--------------
code/stage/trunk/libstage/canvas.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/region.cc
code/stage/trunk/libstage/region.hh
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/worldgui.cc
code/stage/trunk/worlds/fasr.world
Modified: code/stage/trunk/libstage/canvas.cc
===================================================================
--- code/stage/trunk/libstage/canvas.cc 2009-08-26 19:30:17 UTC (rev 8233)
+++ code/stage/trunk/libstage/canvas.cc 2009-08-26 23:41:05 UTC (rev 8234)
@@ -46,9 +46,9 @@
c->world->dirty = false;
}
- Fl::repeat_timeout(((double)c->interval/1000),
- (Fl_Timeout_Handler)Canvas::TimerCallback,
- c);
+ Fl::repeat_timeout( c->interval/1000.0,
+ (Fl_Timeout_Handler)Canvas::TimerCallback,
+ c);
}
Canvas::Canvas( WorldGui* world,
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-08-26 19:30:17 UTC (rev 8233)
+++ code/stage/trunk/libstage/model.cc 2009-08-26 23:41:05 UTC (rev 8234)
@@ -296,9 +296,6 @@
UnMap(); // remove any old cruft rendered during startup
Map();
- if( FindPowerPack() )
- world->Enqueue( 0, World::Event::ENERGY, interval_energy, this );
-
// find the queue for update events: zero if thread safe, else we
// ask the world to assign us to a queue
if( event_queue_num < 1 )
@@ -649,7 +646,10 @@
// put my first events in the world's queue
world->Enqueue( event_queue_num, World::Event::UPDATE, interval, this );
world->Enqueue( 0, World::Event::POSE, interval_pose, this );
+ if( FindPowerPack() )
+ world->Enqueue( 0, World::Event::ENERGY, interval_energy, this );
+
CallCallbacks( &hooks.startup );
}
Modified: code/stage/trunk/libstage/region.cc
===================================================================
--- code/stage/trunk/libstage/region.cc 2009-08-26 19:30:17 UTC (rev 8233)
+++ code/stage/trunk/libstage/region.cc 2009-08-26 23:41:05 UTC (rev 8234)
@@ -52,19 +52,36 @@
// outline superregion
glColor3f( 0,0,1 );
- glRecti( 0,0, 1<<SRBITS, 1<<SRBITS );
+ glRecti( 0,0, 1<<SRBITS, 1<<SRBITS );
+
+ // outline regions
+ const Region* r = GetRegion(0,0);
+ char buf[32];
- // outline regions
glColor3f( 0,1,0 );
- for( int x=0; x<SUPERREGIONWIDTH; x++ )
- for( int y=0; y<SUPERREGIONWIDTH; y++ )
+ for( int y=0; y<SUPERREGIONWIDTH; y++ )
+ for( int x=0; x<SUPERREGIONWIDTH; x++ )
{
- const Region* r = GetRegion(x,y);
-
if( r->count )
- // outline regions with contents
- glRecti( x<<RBITS, y<<RBITS,
- (x+1)<<RBITS, (y+1)<<RBITS );
+ {
+ // outline regions with contents
+ glRecti( x<<RBITS, y<<RBITS,
+ (x+1)<<RBITS, (y+1)<<RBITS );
+
+ snprintf( buf, 15, "%lu", r->count );
+ Gl::draw_string( x<<RBITS, y<<RBITS, 0, buf );
+
+ for( int p=0; p<REGIONWIDTH; p++ )
+ for( int q=0; q<REGIONWIDTH; q++ )
+ if( r->cells[p+(q*REGIONWIDTH)].blocks.size() )
+ {
+ GLfloat xx = p+(x<<RBITS);
+ GLfloat yy = q+(y<<RBITS);
+
+ glRecti( xx, yy,
+ xx+1, yy+1);
+ }
+ }
else if( ! r->cells.empty() )
{
double left = x << RBITS;
@@ -94,22 +111,38 @@
glVertex2f( right, bottom );
glVertex2f( right, bottom+d );
glEnd();
- }
+ }
+
+ r++; // next region quickly
}
-
- char buf[32];
+
snprintf( buf, 15, "%lu", count );
Gl::draw_string( 1<<SBITS, 1<<SBITS, 0, buf );
- glColor3f( 1.0,0,0 );
+ glPopMatrix();
+}
- for( int x=0; x<SUPERREGIONWIDTH; x++ )
- for( int y=0; y<SUPERREGIONWIDTH; y++ )
- {
- const Region* r = GetRegion( x, y);
-
+// TODO
+#if 0
+void SuperRegion::Draw( bool drawall )
+{
+ glEnable( GL_DEPTH_TEST );
+
+ glPushMatrix();
+ glTranslatef( origin.x<<SRBITS, origin.y<<SRBITS,0);
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+
+ r = GetRegion( 0, 0);
+
+ for( int y=0; y<SUPERREGIONWIDTH; y++ )
+ for( int x=0; x<SUPERREGIONWIDTH; x++ )
+ {
if( r->count < 1 )
- continue;
+ {
+ r++;
+ continue;
+ }
snprintf( buf, 15, "%lu", r->count );
Gl::draw_string( x<<RBITS, y<<RBITS, 0, buf );
@@ -131,14 +164,11 @@
Cell* c = (Cell*)&r->cells[p+(q*REGIONWIDTH)];
FOR_EACH( it, c->blocks )
- // for( std::vector<Block*>::iterator it = c->blocks.begin();
-// it != c->blocks.end();
-// ++it )
{
Block* block = *it;
-
+
//printf( "zb %.2f %.2f\n", ent->zbounds.min, ent->zbounds.max );
-
+
Color c = block->GetColor();
glColor4f( c.r, c.g, c.b, 1.0 );
@@ -217,11 +247,13 @@
}
}
}
+
+ ++r;
}
glPopMatrix();
}
+#endif
-
//inline
void SuperRegion::Floor()
{
Modified: code/stage/trunk/libstage/region.hh
===================================================================
--- code/stage/trunk/libstage/region.hh 2009-08-26 19:30:17 UTC (rev 8233)
+++ code/stage/trunk/libstage/region.hh 2009-08-26 23:41:05 UTC (rev 8234)
@@ -7,9 +7,6 @@
#include "stage.hh"
-#include <algorithm>
-#include <vector>
-
namespace Stg
{
@@ -34,7 +31,6 @@
// this is slightly faster than the inline method above, but not as safe
//#define GETREG(X) (( (static_cast<int32_t>(X)) & REGIONMASK ) >> RBITS)
-
class Cell
{
friend class Region;
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-08-26 19:30:17 UTC (rev 8233)
+++ code/stage/trunk/libstage/stage.hh 2009-08-26 23:41:05 UTC (rev 8234)
@@ -1454,6 +1454,8 @@
virtual void AddModel( Model* mod );
+ void SetTimeouts();
+
protected:
virtual void PushColor( Color col );
Modified: code/stage/trunk/libstage/worldgui.cc
===================================================================
--- code/stage/trunk/libstage/worldgui.cc 2009-08-26 19:30:17 UTC (rev 8233)
+++ code/stage/trunk/libstage/worldgui.cc 2009-08-26 23:41:05 UTC (rev 8234)
@@ -342,9 +342,10 @@
}
bool WorldGui::Update()
-{
- double timeout = speedup > 0 ? (sim_interval/1e6) / speedup : 0;
- Fl::repeat_timeout( timeout, (Fl_Timeout_Handler)UpdateCallback, this );
+{
+ if( speedup > 0 )
+ Fl::repeat_timeout( (sim_interval/1e6) / speedup, (Fl_Timeout_Handler)UpdateCallback, this );
+ // else we're called by an idle callback
//printf( "speedup %.2f timeout %.6f\n", speedup, timeout );
@@ -535,12 +536,16 @@
void WorldGui::realtimeCb( Fl_Widget* w, WorldGui* wg )
{
+ //puts( "real time" );
wg->speedup = 1.0;
+ wg->SetTimeouts();
}
void WorldGui::fasttimeCb( Fl_Widget* w, WorldGui* wg )
{
+ //puts( "fast time" );
wg->speedup = -1;
+ wg->SetTimeouts();
}
void WorldGui::Start()
@@ -551,8 +556,27 @@
Fl::add_timeout( ((double)canvas->interval/1000),
(Fl_Timeout_Handler)Canvas::TimerCallback,
canvas );
+
+ SetTimeouts();
+}
- Fl::add_timeout( 0.01, (Fl_Timeout_Handler)UpdateCallback, this );
+
+void WorldGui::SetTimeouts()
+{
+ if( speedup > 0.0 )
+ {
+ //puts( "removing idle" );
+ Fl::remove_idle( (Fl_Timeout_Handler)WorldGui::UpdateCallback, this );
+ Fl::remove_timeout( (Fl_Timeout_Handler)WorldGui::UpdateCallback, this );
+ Fl::add_timeout( (sim_interval/1e6) / speedup, (Fl_Timeout_Handler)UpdateCallback, this );
+ }
+ else
+ {
+ //puts( "removing timeout" );
+ Fl::remove_timeout( (Fl_Timeout_Handler)WorldGui::UpdateCallback, this );
+ Fl::remove_idle( (Fl_Timeout_Handler)WorldGui::UpdateCallback, this );
+ Fl::add_idle( (Fl_Timeout_Handler)UpdateCallback, this );
+ }
}
void WorldGui::Stop()
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2009-08-26 19:30:17 UTC (rev 8233)
+++ code/stage/trunk/worlds/fasr.world 2009-08-26 23:41:05 UTC (rev 8234)
@@ -83,6 +83,8 @@
joules -1 # provides infinite energy
give_watts 1000
fiducial_return 2 # look for this in the fiducial sensor
+
+ alwayson 1 # so we give charge without any explicit subscriber
)
)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rt...@us...> - 2009-08-26 19:30:26
|
Revision: 8233
http://playerstage.svn.sourceforge.net/playerstage/?rev=8233&view=rev
Author: rtv
Date: 2009-08-26 19:30:17 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
more STLing and type safety to hopefully reduce bugs
Modified Paths:
--------------
code/stage/trunk/libstage/ancestor.cc
code/stage/trunk/libstage/block.cc
code/stage/trunk/libstage/canvas.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_draw.cc
code/stage/trunk/libstage/model_getset.cc
code/stage/trunk/libstage/region.cc
code/stage/trunk/libstage/region.hh
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/webstage/webstage.cc
Modified: code/stage/trunk/libstage/ancestor.cc
===================================================================
--- code/stage/trunk/libstage/ancestor.cc 2009-08-26 17:30:15 UTC (rev 8232)
+++ code/stage/trunk/libstage/ancestor.cc 2009-08-26 19:30:17 UTC (rev 8233)
@@ -20,8 +20,16 @@
void Ancestor::AddChild( Model* mod )
{
+ // if the child is already there, this is a serious error
+ if( std::find( children.begin(), children.end(), mod ) != children.end() )
+ {
+ PRINT_ERR2( "Attempting to add child %s to %s - child already exists",
+ mod->Token(), this->Token() );
+ exit( -1 );
+ }
+
// poke a name into the child
- char* buf = new char[TOKEN_MAX];
+ static char* buf = new char[TOKEN_MAX]; // allocated once
// printf( "adding child of type %d token %s\n", mod->type, mod->Token() );
@@ -41,18 +49,17 @@
mod->SetToken( buf );
- children.insert( mod );
+ children.push_back( mod );
child_type_counts[mod->type]++;
- delete[] buf;
+ //delete[] buf; // no need to free the statically allocated buffer
}
void Ancestor::RemoveChild( Model* mod )
{
child_type_counts[mod->type]--;
-
- children.erase( mod );//std::remove( children.begin(), children.end(), mod ) );
+ EraseAll( mod, children );
}
Pose Ancestor::GetGlobalPose()
Modified: code/stage/trunk/libstage/block.cc
===================================================================
--- code/stage/trunk/libstage/block.cc 2009-08-26 17:30:15 UTC (rev 8232)
+++ code/stage/trunk/libstage/block.cc 2009-08-26 19:30:17 UTC (rev 8233)
@@ -206,13 +206,30 @@
void Block::RemoveFromCellArray( CellPtrVec *cells )
{
FOR_EACH( it, *cells )
- (*it)->RemoveBlock( this);
+ {
+ Cell* cell = *it;
+
+ // remove me from the cell
+ EraseAll( this, cell->blocks );
+ --cell->region->count;
+ --cell->region->superregion->count;
+ }
}
void Block::AddToCellArray( CellPtrVec *cells )
{
FOR_EACH( it, *cells )
- (*it)->AddBlock( this);
+ {
+ Cell* cell = *it;
+
+ // record that I am rendered in this cell
+ rendered_cells->push_back( cell );
+
+ // store me in the cell
+ cell->blocks.push_back( this );
+ ++cell->region->count;
+ ++cell->region->superregion->count;
+ }
}
void Block::SwitchToTestedCells()
Modified: code/stage/trunk/libstage/canvas.cc
===================================================================
--- code/stage/trunk/libstage/canvas.cc 2009-08-26 17:30:15 UTC (rev 8232)
+++ code/stage/trunk/libstage/canvas.cc 2009-08-26 19:30:17 UTC (rev 8233)
@@ -304,7 +304,7 @@
{
if( mod )
{
- selected_models.erase( std::remove( selected_models.begin(), selected_models.end(), mod ));
+ EraseAll( mod, selected_models );
redraw();
}
}
@@ -577,7 +577,7 @@
void Canvas::RemoveModel( Model* mod )
{
printf( "removing model %s from canvas list\n", mod->Token() );
- models_sorted.erase( std::remove( models_sorted.begin(), models_sorted.end(), mod ));
+ EraseAll( mod, models_sorted );
}
void Canvas::DrawGlobalGrid()
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-08-26 17:30:15 UTC (rev 8232)
+++ code/stage/trunk/libstage/model.cc 2009-08-26 19:30:17 UTC (rev 8233)
@@ -277,8 +277,8 @@
// remove myself from my parent's child list, or the world's child
// list if I have no parent
- ModelPtrSet& vec = parent ? parent->children : world->children;
- vec.erase( this );//std::remove( vec.begin(), vec.end(), this ));
+ ModelPtrVec& vec = parent ? parent->children : world->children;
+ EraseAll( this, vec );
modelsbyid.erase(id);
@@ -332,7 +332,7 @@
{
if( flag )
{
- flag_list.erase( remove( flag_list.begin(), flag_list.end(), flag ));
+ EraseAll( flag, flag_list );
CallCallbacks( &hooks.flag_decr );
}
}
Modified: code/stage/trunk/libstage/model_draw.cc
===================================================================
--- code/stage/trunk/libstage/model_draw.cc 2009-08-26 17:30:15 UTC (rev 8232)
+++ code/stage/trunk/libstage/model_draw.cc 2009-08-26 19:30:17 UTC (rev 8233)
@@ -246,28 +246,27 @@
if( ! world_gui )
return;
- //save visual instance
- //cv_list = g_list_append(cv_list, cv );
+ //save visual instance
cv_list.push_back( cv );
-
- //register option for all instances which share the same name
- Canvas* canvas = world_gui->GetCanvas();
- std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( cv->GetMenuName() );
- if( i == canvas->_custom_options.end() ) {
- Option* op = new Option( cv->GetMenuName(),
- cv->GetWorldfileName(),
- "",
- on_by_default,
- world_gui );
- canvas->_custom_options[ cv->GetMenuName() ] = op;
- RegisterOption( op );
- }
+
+ //register option for all instances which share the same name
+ Canvas* canvas = world_gui->GetCanvas();
+ std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( cv->GetMenuName() );
+ if( i == canvas->_custom_options.end() ) {
+ Option* op = new Option( cv->GetMenuName(),
+ cv->GetWorldfileName(),
+ "",
+ on_by_default,
+ world_gui );
+ canvas->_custom_options[ cv->GetMenuName() ] = op;
+ RegisterOption( op );
+ }
}
void Model::RemoveVisualizer( Visualizer* cv )
{
if( cv )
- cv_list.erase( remove( cv_list.begin(), cv_list.end(), cv ));
+ EraseAll( cv, cv_list );
//TODO unregister option - tricky because there might still be instances attached to different models which have the same name
}
Modified: code/stage/trunk/libstage/model_getset.cc
===================================================================
--- code/stage/trunk/libstage/model_getset.cc 2009-08-26 17:30:15 UTC (rev 8232)
+++ code/stage/trunk/libstage/model_getset.cc 2009-08-26 19:30:17 UTC (rev 8233)
@@ -49,9 +49,9 @@
// non-zero values mean we need to be in the world's set of
// detectable models
if( val == 0 )
- world->models_with_fiducials.erase( this );
+ world->FiducialErase( this );
else
- world->models_with_fiducials.insert( this );
+ world->FiducialInsert( this );
CallCallbacks( &vis.fiducial_return );
}
@@ -135,17 +135,13 @@
}
int Model::SetParent( Model* newparent)
-{
-
+{
// remove the model from its old parent (if it has one)
if( parent )
- //this->parent->children = g_list_remove( this->parent->children, this );
- //parent->children.erase( remove( parent->children.begin(), parent->children.end(), this ) );
- parent->children.erase( this );
-
+ EraseAll( this, parent->children );
+
if( newparent )
- //newparent->children = g_list_append( newparent->children, this );
- newparent->children.insert( this );
+ newparent->children.push_back( this );
// link from the model to its new parent
this->parent = newparent;
Modified: code/stage/trunk/libstage/region.cc
===================================================================
--- code/stage/trunk/libstage/region.cc 2009-08-26 17:30:15 UTC (rev 8232)
+++ code/stage/trunk/libstage/region.cc 2009-08-26 19:30:17 UTC (rev 8233)
@@ -11,26 +11,25 @@
// collection
// std::set<Region*> Region::empty_regions;
-Region::Region() :
- cells( NULL ),
+Region::Region( SuperRegion* sr) :
+ cells(),
+ superregion(sr),
count(0)
{
}
Region::~Region()
{
- if( cells )
- delete[] cells;
}
SuperRegion::SuperRegion( World* world, stg_point_int_t origin )
- : regions( new Region[ SUPERREGIONSIZE ] ),
+ : regions(),
origin(origin),
world(world),
count(0)
{
- for( int i=0; i<SUPERREGIONSIZE; i++ )
- regions[i].superregion = this;
+ // populate the regions
+ regions.insert( regions.begin(), SUPERREGIONSIZE, Region( this ) );
//static int srcount=0;
//printf( "created SR number %d\n", ++srcount );
@@ -39,9 +38,6 @@
SuperRegion::~SuperRegion()
{
- if( regions )
- delete[] regions;
-
//printf( "deleting SR %p at [%d,%d]\n", this, origin.x, origin.y );
}
@@ -64,12 +60,12 @@
for( int y=0; y<SUPERREGIONWIDTH; y++ )
{
const Region* r = GetRegion(x,y);
-
+
if( r->count )
// outline regions with contents
glRecti( x<<RBITS, y<<RBITS,
(x+1)<<RBITS, (y+1)<<RBITS );
- else if( r->cells )
+ else if( ! r->cells.empty() )
{
double left = x << RBITS;
double right = (x+1) << RBITS;
@@ -81,35 +77,22 @@
// draw little corner markers for regions with memory
// allocated but no contents
glBegin( GL_LINES );
-
glVertex2f( left, bottom );
glVertex2f( left+d, bottom );
-
glVertex2f( left, bottom );
glVertex2f( left, bottom+d );
-
-
glVertex2f( left, top );
glVertex2f( left+d, top );
-
glVertex2f( left, top );
glVertex2f( left, top-d );
-
-
-
glVertex2f( right, top );
glVertex2f( right-d, top );
-
glVertex2f( right, top );
glVertex2f( right, top-d );
-
-
glVertex2f( right, bottom );
glVertex2f( right-d, bottom );
-
glVertex2f( right, bottom );
glVertex2f( right, bottom+d );
-
glEnd();
}
}
Modified: code/stage/trunk/libstage/region.hh
===================================================================
--- code/stage/trunk/libstage/region.hh 2009-08-26 17:30:15 UTC (rev 8232)
+++ code/stage/trunk/libstage/region.hh 2009-08-26 19:30:17 UTC (rev 8233)
@@ -45,12 +45,10 @@
private:
Region* region;
std::vector<Block*> blocks;
- //std::set<Block*> blocks;
- bool boundary;
public:
- Cell()
- : region( NULL),
+ Cell( Region* reg )
+ : region( reg ),
blocks()
{
}
@@ -63,26 +61,22 @@
class Region
{
public:
-
- Cell* cells;
+ std::vector<Cell> cells;
+
SuperRegion* superregion;
unsigned long count; // number of blocks rendered into this region
- Region();
+ Region( SuperRegion* sr );
~Region();
Cell* GetCell( int32_t x, int32_t y )
{
- if( ! cells )
- {
- cells = new Cell[REGIONSIZE];
-
- for( int i=0; i<REGIONSIZE; ++i )
- cells[i].region = this;
- }
-
+ if( cells.empty() ) // lazy population of cells
+ cells.insert( cells.begin(), REGIONSIZE, Cell( this ) );
+
return( (Cell*)&cells[ x + y * REGIONWIDTH ] );
}
+
}; // end class Region
class SuperRegion
@@ -92,7 +86,7 @@
private:
- Region* regions;
+ std::vector<Region> regions;
stg_point_int_t origin;
World* world;
@@ -110,24 +104,4 @@
unsigned long count; // number of blocks rendered into this superregion
}; // class SuperRegion;
- void Cell::RemoveBlock( Block* b )
- {
- // linear time removal, but these vectors are very short, usually 1
- // or 2 elements.
- blocks.erase( std::remove( blocks.begin(), blocks.end(), b ), blocks.end() );
-
- --region->count;
- --region->superregion->count;
- }
-
- void Cell::AddBlock( Block* b )
- {
- blocks.push_back( b );
- b->RecordRendering( this );
-
- ++region->count;
- ++region->superregion->count;
- }
-
-
}; // namespace Stg
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-08-26 17:30:15 UTC (rev 8232)
+++ code/stage/trunk/libstage/stage.hh 2009-08-26 19:30:17 UTC (rev 8233)
@@ -89,6 +89,9 @@
/** Set of pointers to Models. */
typedef std::set<Model*> ModelPtrSet;
+ /** Set of pointers to Models. */
+ typedef std::vector<Model*> ModelPtrVec;
+
/** Set of pointers to Blocks. */
typedef std::set<Block*> BlockPtrSet;
@@ -615,6 +618,11 @@
#define VAR(V,init) __typeof(init) V=(init)
#define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++)
+/** wrapper for Erase-Remove method of removing all instances of thing from container */
+ template <class T, class C>
+ void EraseAll( T thing, C& cont )
+ { cont.erase( std::remove( cont.begin(), cont.end(), thing ), cont.end() ); }
+
// Error macros - output goes to stderr
#define PRINT_ERR(m) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", __FILE__, __FUNCTION__)
#define PRINT_ERR1(m,a) fprintf( stderr, "\033[41merr\033[0m: "m" (%s %s)\n", a, __FILE__, __FUNCTION__)
@@ -679,7 +687,7 @@
friend class Canvas; // allow Canvas access to our private members
protected:
- ModelPtrSet children;
+ ModelPtrVec children;
bool debug;
char* token;
pthread_mutex_t access_mutex; ///< Used by Lock() and Unlock() to prevent parallel access to this model
@@ -690,7 +698,7 @@
public:
/** get the children of the this element */
- ModelPtrSet& GetChildren(){ return children;}
+ ModelPtrVec& GetChildren(){ return children;}
/** recursively call func( model, arg ) for each descendant */
void ForEachDescendant( stg_model_callback_t func, void* arg );
@@ -812,8 +820,22 @@
/** Keep a list of all models with detectable fiducials. This
avoids searching the whole world for fiducials. */
- ModelPtrSet models_with_fiducials;
-
+ ModelPtrVec models_with_fiducials;
+
+ /** Add a model to the set of models with non-zero fiducials, if not already there. */
+ void FiducialInsert( Model* mod )
+ {
+ FiducialErase( mod ); // make sure it's not there already
+ models_with_fiducials.push_back( mod );
+ }
+
+ /** Remove a model from the set of models with non-zero fiducials, if it exists. */
+ void FiducialErase( Model* mod )
+ {
+ //EraseAll<Model*,ModelPtrVec&>( mod, models_with_fiducials );
+ EraseAll( mod, models_with_fiducials );
+ }
+
double ppm; ///< the resolution of the world model in pixels per meter
bool quit; ///< quit this world ASAP
@@ -842,7 +864,7 @@
std::map<stg_point_int_t,SuperRegion*> superregions;
SuperRegion* sr_cached; ///< The last superregion looked up by this world
- std::vector<ModelPtrSet> update_lists;
+ std::vector<ModelPtrVec> update_lists;
uint64_t updates; ///< the number of simulated time steps executed so far
Worldfile* wf; ///< If set, points to the worldfile used to create this world
@@ -1111,9 +1133,6 @@
/** Set the extent in Z of the block */
void SetZ( double min, double max );
- void RecordRendering( Cell* cell )
- { rendered_cells->push_back( cell ); }
-
stg_point_t* Points( unsigned int *count )
{ if( count ) *count = pt_count; return &pts[0]; };
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-08-26 17:30:15 UTC (rev 8232)
+++ code/stage/trunk/libstage/world.cc 2009-08-26 19:30:17 UTC (rev 8233)
@@ -1024,7 +1024,7 @@
void World::RemovePowerPack( PowerPack* pp )
{
- powerpack_list.erase( remove( powerpack_list.begin(), powerpack_list.end(), pp ));
+ EraseAll( pp, powerpack_list );
}
/// Register an Option for pickup by the GUI
Modified: code/stage/trunk/webstage/webstage.cc
===================================================================
--- code/stage/trunk/webstage/webstage.cc 2009-08-26 17:30:15 UTC (rev 8232)
+++ code/stage/trunk/webstage/webstage.cc 2009-08-26 19:30:17 UTC (rev 8233)
@@ -137,7 +137,7 @@
virtual bool GetModelChildren(const std::string& model,
std::vector<std::string>& children)
{
- std::set<Model*> c;
+ std::vector<Model*> c;
if(model == "")
{
@@ -157,9 +157,7 @@
}
- for( std::set<Model*>::iterator it = c.begin();
- it != c.end();
- it++ )
+ FOR_EACH( it, c )
{
children.push_back(std::string((*it)->Token()));
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-08-26 17:30:22
|
Revision: 8232
http://playerstage.svn.sourceforge.net/playerstage/?rev=8232&view=rev
Author: hsujohnhsu
Date: 2009-08-26 17:30:15 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
set invalid quaternion to identity on construction.
Modified Paths:
--------------
code/gazebo/trunk/server/Quatern.cc
Modified: code/gazebo/trunk/server/Quatern.cc
===================================================================
--- code/gazebo/trunk/server/Quatern.cc 2009-08-26 17:27:57 UTC (rev 8231)
+++ code/gazebo/trunk/server/Quatern.cc 2009-08-26 17:30:15 UTC (rev 8232)
@@ -33,7 +33,12 @@
Quatern::Quatern()
: u(1), x(0), y(0), z(0)
{
-
+ if ( u == 0 && x == 0 && y == 0 && z == 0 )
+ {
+ /// @todo: give user warning
+ this->SetToIdentity();
+ }
+ this->Normalize();
}
////////////////////////////////////////////////////////////////////////////////
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rt...@us...> - 2009-08-26 17:28:06
|
Revision: 8231
http://playerstage.svn.sourceforge.net/playerstage/?rev=8231&view=rev
Author: rtv
Date: 2009-08-26 17:27:57 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
fixed occupancy grid bug - but a problem with powerpacks remains
Modified Paths:
--------------
code/stage/trunk/libstage/ancestor.cc
code/stage/trunk/libstage/block.cc
code/stage/trunk/libstage/blockgroup.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_getset.cc
code/stage/trunk/libstage/region.cc
code/stage/trunk/libstage/region.hh
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/webstage/webstage.cc
Modified: code/stage/trunk/libstage/ancestor.cc
===================================================================
--- code/stage/trunk/libstage/ancestor.cc 2009-08-26 08:36:23 UTC (rev 8230)
+++ code/stage/trunk/libstage/ancestor.cc 2009-08-26 17:27:57 UTC (rev 8231)
@@ -41,7 +41,7 @@
mod->SetToken( buf );
- children.push_back( mod );
+ children.insert( mod );
child_type_counts[mod->type]++;
@@ -52,7 +52,7 @@
{
child_type_counts[mod->type]--;
- children.erase( std::remove( children.begin(), children.end(), mod ) );
+ children.erase( mod );//std::remove( children.begin(), children.end(), mod ) );
}
Pose Ancestor::GetGlobalPose()
Modified: code/stage/trunk/libstage/block.cc
===================================================================
--- code/stage/trunk/libstage/block.cc 2009-08-26 08:36:23 UTC (rev 8230)
+++ code/stage/trunk/libstage/block.cc 2009-08-26 17:27:57 UTC (rev 8231)
@@ -193,8 +193,6 @@
// TODO - if called often, we may not need to generate each time
GenerateCandidateCells();
SwitchToTestedCells();
- return;
-
mapped = true;
}
@@ -227,6 +225,8 @@
rendered_cells = candidate_cells;
candidate_cells = tmp;
+ // XXXX todo
+
mapped = true;
}
Modified: code/stage/trunk/libstage/blockgroup.cc
===================================================================
--- code/stage/trunk/libstage/blockgroup.cc 2009-08-26 08:36:23 UTC (rev 8230)
+++ code/stage/trunk/libstage/blockgroup.cc 2009-08-26 17:27:57 UTC (rev 8231)
@@ -26,7 +26,7 @@
void BlockGroup::AppendBlock( Block* block )
{
- blocks.push_back( block );
+ blocks.insert( block );
}
void BlockGroup::Clear()
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-08-26 08:36:23 UTC (rev 8230)
+++ code/stage/trunk/libstage/model.cc 2009-08-26 17:27:57 UTC (rev 8231)
@@ -277,8 +277,8 @@
// remove myself from my parent's child list, or the world's child
// list if I have no parent
- ModelPtrVec& vec = parent ? parent->children : world->children;
- vec.erase( std::remove( vec.begin(), vec.end(), this ));
+ ModelPtrSet& vec = parent ? parent->children : world->children;
+ vec.erase( this );//std::remove( vec.begin(), vec.end(), this ));
modelsbyid.erase(id);
@@ -809,7 +809,9 @@
Model* hitmod( TestCollisionTree() );
if( hitmod )
- pose = startpose; // move failed - put me back where I started
+ {
+ pose = startpose; // move failed - put me back where I started
+ }
else
{
CommitTestedPose(); // shift anyrecursively commit to blocks to the new pose
Modified: code/stage/trunk/libstage/model_getset.cc
===================================================================
--- code/stage/trunk/libstage/model_getset.cc 2009-08-26 08:36:23 UTC (rev 8230)
+++ code/stage/trunk/libstage/model_getset.cc 2009-08-26 17:27:57 UTC (rev 8231)
@@ -136,14 +136,16 @@
int Model::SetParent( Model* newparent)
{
+
// remove the model from its old parent (if it has one)
if( parent )
//this->parent->children = g_list_remove( this->parent->children, this );
- parent->children.erase( remove( parent->children.begin(), parent->children.end(), this ) );
+ //parent->children.erase( remove( parent->children.begin(), parent->children.end(), this ) );
+ parent->children.erase( this );
if( newparent )
//newparent->children = g_list_append( newparent->children, this );
- newparent->children.push_back( this );
+ newparent->children.insert( this );
// link from the model to its new parent
this->parent = newparent;
Modified: code/stage/trunk/libstage/region.cc
===================================================================
--- code/stage/trunk/libstage/region.cc 2009-08-26 08:36:23 UTC (rev 8230)
+++ code/stage/trunk/libstage/region.cc 2009-08-26 17:27:57 UTC (rev 8231)
@@ -146,9 +146,11 @@
else // draw a rectangular solid
{
Cell* c = (Cell*)&r->cells[p+(q*REGIONWIDTH)];
- for( std::vector<Block*>::iterator it = c->blocks.begin();
- it != c->blocks.end();
- ++it )
+
+ FOR_EACH( it, c->blocks )
+ // for( std::vector<Block*>::iterator it = c->blocks.begin();
+// it != c->blocks.end();
+// ++it )
{
Block* block = *it;
Modified: code/stage/trunk/libstage/region.hh
===================================================================
--- code/stage/trunk/libstage/region.hh 2009-08-26 08:36:23 UTC (rev 8230)
+++ code/stage/trunk/libstage/region.hh 2009-08-26 17:27:57 UTC (rev 8231)
@@ -45,6 +45,7 @@
private:
Region* region;
std::vector<Block*> blocks;
+ //std::set<Block*> blocks;
bool boundary;
public:
@@ -82,30 +83,8 @@
return( (Cell*)&cells[ x + y * REGIONWIDTH ] );
}
+ }; // end class Region
-
- /** Returns an initialized region, either from the top of the
- stack, or if the stack is empty, a newly constructed Region */
- //static Region* GetRegion( SuperRegion* superregion );
- // static std::stack<Region*> recycled_regions;
-
-// static std::set<Region*> empty_regions;
-
-// static void GarbageCollect()
-// {
-// FOR_EACH( it, empty_regions )
-// {
-// Region* reg = *it;
-// //delete reg;
-
-// printf( "Garbage collecting region %p\n", reg );
-// }
-
-// empty_regions.clear();
-// }
-
- };
-
class SuperRegion
{
friend class World;
@@ -129,50 +108,26 @@
void Floor();
unsigned long count; // number of blocks rendered into this superregion
- };
-
-
- // inline void printvec( std::vector<Block*>& vec )
- // {
- // printf( "Vec: ");
- // for( size_t i=0; i<vec.size(); i++ )
- // printf( "%p ", vec[i] );
- // puts( "" );
- // }
-
+ }; // class SuperRegion;
+
void Cell::RemoveBlock( Block* b )
{
// linear time removal, but these vectors are very short, usually 1
- // or 2 elements. Fast removal - our strategy is to copy the last
- // item in the vector over the item we want to remove, then pop off
- // the tail. This avoids moving the other items in the vector. Saves
- // maybe 1 or 2% run time in my tests.
-
- // find the value in the vector
- // printf( "\nremoving %p\n", b );
- // puts( "before" );
- // printvec( blocks );
-
- // copy the last item in the vector to overwrite this one
- copy_backward( blocks.end(), blocks.end(), std::find( blocks.begin(), blocks.end(), b ));
- blocks.pop_back(); // and remove the redundant copy at the end of
- // the vector
-
- --region->count;
- --region->superregion->count;
+ // or 2 elements.
+ blocks.erase( std::remove( blocks.begin(), blocks.end(), b ), blocks.end() );
-// if( region->count == 0 && region->candidate_count == 0 )
-// Region::empty_regions.insert( region );
+ --region->count;
+ --region->superregion->count;
}
-
+
void Cell::AddBlock( Block* b )
{
blocks.push_back( b );
b->RecordRendering( this );
-
+
++region->count;
++region->superregion->count;
- }
+ }
}; // namespace Stg
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-08-26 08:36:23 UTC (rev 8230)
+++ code/stage/trunk/libstage/stage.hh 2009-08-26 17:27:57 UTC (rev 8231)
@@ -88,10 +88,10 @@
/** Set of pointers to Models. */
typedef std::set<Model*> ModelPtrSet;
- /** Vector of pointers to Models. */
- typedef std::vector<Model*> ModelPtrVec;
- /** Vector of pointers to Blocks. */
- typedef std::vector<Block*> BlockPtrVec;
+
+ /** Set of pointers to Blocks. */
+ typedef std::set<Block*> BlockPtrSet;
+
/** Vector of pointers to Cells.*/
typedef std::vector<Cell*> CellPtrVec;
@@ -679,7 +679,7 @@
friend class Canvas; // allow Canvas access to our private members
protected:
- ModelPtrVec children;
+ ModelPtrSet children;
bool debug;
char* token;
pthread_mutex_t access_mutex; ///< Used by Lock() and Unlock() to prevent parallel access to this model
@@ -690,7 +690,7 @@
public:
/** get the children of the this element */
- ModelPtrVec& GetChildren(){ return children;}
+ ModelPtrSet& GetChildren(){ return children;}
/** recursively call func( model, arg ) for each descendant */
void ForEachDescendant( stg_model_callback_t func, void* arg );
@@ -842,8 +842,7 @@
std::map<stg_point_int_t,SuperRegion*> superregions;
SuperRegion* sr_cached; ///< The last superregion looked up by this world
- // todo - test performance of std::set
- std::vector<ModelPtrVec > update_lists;
+ std::vector<ModelPtrSet> update_lists;
uint64_t updates; ///< the number of simulated time steps executed so far
Worldfile* wf; ///< If set, points to the worldfile used to create this world
@@ -1193,7 +1192,7 @@
void BuildDisplayList( Model* mod );
- BlockPtrVec blocks;
+ BlockPtrSet blocks;
Size size;
stg_point3_t offset;
stg_meters_t minx, maxx, miny, maxy;
@@ -1239,9 +1238,7 @@
void InvalidateModelPointCache()
{
- for( BlockPtrVec::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
+ FOR_EACH( it, blocks )
(*it)->InvalidateModelPointCache();
}
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-08-26 08:36:23 UTC (rev 8230)
+++ code/stage/trunk/libstage/world.cc 2009-08-26 17:27:57 UTC (rev 8231)
@@ -716,9 +716,7 @@
(cy>=0) && (cy<REGIONWIDTH) &&
n > 0 )
{
- for( BlockPtrVec::iterator it( c->blocks.begin() );
- it != c->blocks.end();
- ++it )
+ FOR_EACH( it, c->blocks )
{
Block* block( *it );
assert( block );
@@ -946,7 +944,7 @@
void World::ForEachCellInLine( const stg_point_int_t& start,
const stg_point_int_t& end,
- std::vector<Cell*>& cells )
+ CellPtrVec& cells )
{
// line rasterization adapted from Cohen's 3D version in
// Graphics Gems II. Should be very fast.
Modified: code/stage/trunk/webstage/webstage.cc
===================================================================
--- code/stage/trunk/webstage/webstage.cc 2009-08-26 08:36:23 UTC (rev 8230)
+++ code/stage/trunk/webstage/webstage.cc 2009-08-26 17:27:57 UTC (rev 8231)
@@ -54,7 +54,7 @@
puts( "[WebStage] Clock tick" );
world->paused = true;
- world->sim_interval = msec * 1e3; // usec
+ world->sim_interval = msec * 1e-3; // usec
world->Update();
return true;
@@ -137,7 +137,7 @@
virtual bool GetModelChildren(const std::string& model,
std::vector<std::string>& children)
{
- std::vector<Model*> c;
+ std::set<Model*> c;
if(model == "")
{
@@ -157,7 +157,7 @@
}
- for( std::vector<Model*>::iterator it = c.begin();
+ for( std::set<Model*>::iterator it = c.begin();
it != c.end();
it++ )
{
@@ -572,21 +572,18 @@
// add an FLTK event loop update to WebSim's implementation
virtual void Wait()
{
- Fl::check();
-
- while( unacknowledged_ticks || unacknowledged_pushes || ticks_remaining )
- {
- printf( "event loop in wait (%d %d %d)\n",
+ do
+ {
+ printf( " event loop in wait (%d %d %d)\r",
unacknowledged_ticks, unacknowledged_pushes, ticks_remaining );
-
- event_loop( EVLOOP_NONBLOCK );
- puts( "fl::check\n" );
- Fl::check();
- }
+ event_loop( EVLOOP_NONBLOCK );
+ Fl::check();
+ }
+ while( unacknowledged_ticks || unacknowledged_pushes || ticks_remaining );
}
-};
+}; // close WebStage class
int main( int argc, char** argv )
@@ -652,7 +649,10 @@
if( usefedfile )
ws.LoadFederationFile( fedfilename );
- ws.Startup( true );
+ ws.Startup( true ); // start http server
+
+ if( ! world->paused )
+ world->Start(); // start sumulation running
puts( "entering main loop" );
@@ -671,14 +671,12 @@
//puts( "tick done" );
// update Stage
- world->Update();
+ //world->Update();
//puts( "update done" );
- Fl::check();
-
// wait until everyone report simulation step done
ws.Wait();
- puts( "wait done" );
+ //puts( "wait done" );
}
printf( "Webstage done.\n" );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2009-08-26 08:36:31
|
Revision: 8230
http://playerstage.svn.sourceforge.net/playerstage/?rev=8230&view=rev
Author: gbiggs
Date: 2009-08-26 08:36:23 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Applied patch #2842767: some win32 issues
Modified Paths:
--------------
code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h
code/player/trunk/server/drivers/blobfinder/shapetracker/shapetracker.cc
code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc
code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc
code/player/trunk/server/drivers/camera/compress/cameracompress.cc
code/player/trunk/server/drivers/camera/compress/camerauncompress.cc
code/player/trunk/server/drivers/camera/cvcam/cvcam.cc
code/player/trunk/server/drivers/camera/imageseq/imageseq.cc
Modified: code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h
===================================================================
--- code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h 2009-08-26 08:30:24 UTC (rev 8229)
+++ code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h 2009-08-26 08:36:23 UTC (rev 8230)
@@ -38,7 +38,7 @@
#include <stdio.h>
#include <math.h>
-#include <opencv/cv.h>
+#include <cv.h>
//Estimate the length between two points (cvpoint)
Modified: code/player/trunk/server/drivers/blobfinder/shapetracker/shapetracker.cc
===================================================================
--- code/player/trunk/server/drivers/blobfinder/shapetracker/shapetracker.cc 2009-08-26 08:30:24 UTC (rev 8229)
+++ code/player/trunk/server/drivers/blobfinder/shapetracker/shapetracker.cc 2009-08-26 08:36:23 UTC (rev 8230)
@@ -50,8 +50,8 @@
#include <libplayercore/playercore.h>
#include <base/imagebase.h>
-#include <opencv/cv.h>
-//#include <opencv/highgui.h>
+#include <cv.h>
+//#include <highgui.h>
#define winName "ShapeTracker"
Modified: code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc
===================================================================
--- code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc 2009-08-26 08:30:24 UTC (rev 8229)
+++ code/player/trunk/server/drivers/blobfinder/simpleshape/simpleshape.cc 2009-08-26 08:36:23 UTC (rev 8230)
@@ -106,8 +106,8 @@
#include <string.h>
#include "../../base/imagebase.h"
-#include <opencv/cv.h>
-#include <opencv/highgui.h>
+#include <cv.h>
+#include <highgui.h>
// Invariant feature set for a contour
class FeatureSet
Modified: code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc
===================================================================
--- code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc 2009-08-26 08:30:24 UTC (rev 8229)
+++ code/player/trunk/server/drivers/blobfinder/upcbarcode/upcbarcode.cc 2009-08-26 08:36:23 UTC (rev 8230)
@@ -108,8 +108,8 @@
#include "../../base/imagebase.h"
-#include <opencv/cv.h>
-#include <opencv/highgui.h>
+#include <cv.h>
+#include <highgui.h>
// Info on potential blobs.
struct blob_t
Modified: code/player/trunk/server/drivers/camera/compress/cameracompress.cc
===================================================================
--- code/player/trunk/server/drivers/camera/compress/cameracompress.cc 2009-08-26 08:30:24 UTC (rev 8229)
+++ code/player/trunk/server/drivers/camera/compress/cameracompress.cc 2009-08-26 08:36:23 UTC (rev 8230)
@@ -75,10 +75,11 @@
#include <string.h>
#include <stdlib.h>
+#ifndef WIN32
#include <unistd.h>
+#endif
#include <stddef.h>
#include <stdlib.h> // for atoi(3)
-#include <netinet/in.h> // for htons(3)
#include <math.h>
#include <libplayercore/playercore.h>
@@ -306,7 +307,11 @@
if (this->save)
{
+#ifdef WIN32
+ _snprintf(filename, sizeof(filename), "click-%04d.jpeg",this->frameno++);
+#else
snprintf(filename, sizeof(filename), "click-%04d.jpeg",this->frameno++);
+#endif
FILE *fp = fopen(filename, "w+");
int ret = fwrite(this->data.image, 1, this->data.image_count, fp);
if (ret < 0)
Modified: code/player/trunk/server/drivers/camera/compress/camerauncompress.cc
===================================================================
--- code/player/trunk/server/drivers/camera/compress/camerauncompress.cc 2009-08-26 08:30:24 UTC (rev 8229)
+++ code/player/trunk/server/drivers/camera/compress/camerauncompress.cc 2009-08-26 08:36:23 UTC (rev 8230)
@@ -77,9 +77,10 @@
#include <string.h>
#include <stdlib.h>
+#ifndef WIN32
#include <unistd.h>
+#endif
#include <stdlib.h> // for atoi(3)
-#include <netinet/in.h> // for htons(3)
#include <math.h>
#include <libplayercore/playercore.h>
@@ -247,7 +248,11 @@
if (this->save)
{
+#ifdef WIN32
+ _snprintf(filename, sizeof(filename), "click-%04d.ppm",this->frameno++);
+#else
snprintf(filename, sizeof(filename), "click-%04d.ppm",this->frameno++);
+#endif
FILE *fp = fopen(filename, "w+");
int ret = fwrite (this->data.image, 1, this->data.image_count, fp);
if (ret < 0)
Modified: code/player/trunk/server/drivers/camera/cvcam/cvcam.cc
===================================================================
--- code/player/trunk/server/drivers/camera/cvcam/cvcam.cc 2009-08-26 08:30:24 UTC (rev 8229)
+++ code/player/trunk/server/drivers/camera/cvcam/cvcam.cc 2009-08-26 08:36:23 UTC (rev 8230)
@@ -87,8 +87,8 @@
#include <pthread.h>
#include <libplayercore/playercore.h>
-#include <opencv/cv.h>
-#include <opencv/highgui.h>
+#include <cv.h>
+#include <highgui.h>
//---------------------------------
Modified: code/player/trunk/server/drivers/camera/imageseq/imageseq.cc
===================================================================
--- code/player/trunk/server/drivers/camera/imageseq/imageseq.cc 2009-08-26 08:30:24 UTC (rev 8229)
+++ code/player/trunk/server/drivers/camera/imageseq/imageseq.cc 2009-08-26 08:36:23 UTC (rev 8230)
@@ -87,17 +87,19 @@
#include <string.h>
#include <stdlib.h>
+#include <stddef.h>
+#ifndef WIN32
#include <unistd.h>
+#endif
+#include <time.h> // for nanosleep() and struct timespec
#include <stdlib.h> // for atoi(3)
-#include <netinet/in.h> // for htons(3)
#include <math.h>
-#include <opencv/cv.h>
-#include <opencv/highgui.h>
+#include <cv.h>
+#include <highgui.h>
#include <libplayercore/playercore.h>
-
class ImageSeq : public ThreadedDriver
{
// Constructor
@@ -175,7 +177,11 @@
pthread_testcancel();
// Compose filename
+#ifdef WIN32
+ _snprintf(filename, sizeof(filename), this->pattern, this->frame);
+#else
snprintf(filename, sizeof(filename), this->pattern, this->frame);
+#endif
// Load the image
if (this->LoadImage(filename) != 0)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2009-08-26 08:30:31
|
Revision: 8229
http://playerstage.svn.sourceforge.net/playerstage/?rev=8229&view=rev
Author: gbiggs
Date: 2009-08-26 08:30:24 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Applied patch #2844288: fixed playerv display on ranger data
Modified Paths:
--------------
code/player/trunk/client_libs/libplayerc/dev_ranger.c
code/player/trunk/utils/playerv/pv_dev_ranger.c
Modified: code/player/trunk/client_libs/libplayerc/dev_ranger.c
===================================================================
--- code/player/trunk/client_libs/libplayerc/dev_ranger.c 2009-08-26 08:23:52 UTC (rev 8228)
+++ code/player/trunk/client_libs/libplayerc/dev_ranger.c 2009-08-26 08:30:24 UTC (rev 8229)
@@ -276,6 +276,7 @@
device->min_angle = config->min_angle;
device->max_angle = config->max_angle;
device->angular_res = config->angular_res;
+ device->min_range = config->min_range;
device->max_range = config->max_range;
device->range_res = config->range_res;
device->frequency = config->frequency;
Modified: code/player/trunk/utils/playerv/pv_dev_ranger.c
===================================================================
--- code/player/trunk/utils/playerv/pv_dev_ranger.c 2009-08-26 08:23:52 UTC (rev 8228)
+++ code/player/trunk/utils/playerv/pv_dev_ranger.c 2009-08-26 08:30:24 UTC (rev 8229)
@@ -221,10 +221,13 @@
// Assume the range is straight ahead (ignore min_angle and angular_res properties)
points[0][0] = 0.0f;
points[0][1] = 0.0f;
- points[1][0] = ranger->proxy->ranges[ii] * cos(-temp);
- points[1][1] = ranger->proxy->ranges[ii] * sin(-temp);
- points[2][0] = ranger->proxy->ranges[ii] * cos(temp);
- points[2][1] = ranger->proxy->ranges[ii] * sin(temp);
+ double range = ranger->proxy->ranges[ii];
+ if (range < ranger->proxy->min_range)
+ range = ranger->proxy->max_range;
+ points[1][0] = range * cos(-temp);
+ points[1][1] = range * sin(-temp);
+ points[2][0] = range * cos(temp);
+ points[2][1] = range * sin(temp);
rtk_fig_polygon(ranger->scan_fig[ii], 0, 0, 0, 3, points, 1);
// Draw the sensor itself
@@ -249,27 +252,37 @@
// Draw empty space
points[0][0] = 0.0;
points[0][1] = 0.0;
+ double b = ranger->proxy->min_angle;
for (ii = 0; ii < ranger->proxy->ranges_count; ii++)
{
- points[ii + 1][0] = ranger->proxy->points[ii].px;
- points[ii + 1][1] = ranger->proxy->points[ii].py;
+ double range = ranger->proxy->ranges[ii];
+ if (range < ranger->proxy->min_range) {
+ points[ii + 1][0] = ranger->proxy->max_range * cos(b);
+ points[ii + 1][1] = ranger->proxy->max_range * sin(b);
+ }
+ else {
+ points[ii + 1][0] = ranger->proxy->points[ii].px;
+ points[ii + 1][1] = ranger->proxy->points[ii].py;
+ }
+ b += ranger->proxy->angular_res;
}
rtk_fig_color_rgb32(ranger->scan_fig[0], COLOR_LASER_EMP);
rtk_fig_polygon(ranger->scan_fig[0], 0, 0, 0,
ranger->proxy->ranges_count + 1, points, 1);
- free(points);
- points = NULL;
// Draw occupied space
rtk_fig_color_rgb32(ranger->scan_fig[0], COLOR_LASER_OCC);
for (ii = 1; ii < ranger->proxy->ranges_count; ii++)
{
- point1[0] = ranger->proxy->points[ii - 1].px;
- point1[1] = ranger->proxy->points[ii - 1].py;
- point2[0] = ranger->proxy->points[ii].px;
- point2[1] = ranger->proxy->points[ii].py;
+ point1[0] = points[ii][0];
+ point1[1] = points[ii][1];
+ point2[0] = points[ii+1][0];
+ point2[1] = points[ii+1][1];
rtk_fig_line(ranger->scan_fig[0], point1[0], point1[1], point2[0], point2[1]);
}
+ free(points);
+ points = NULL;
+
}
else
{
@@ -278,13 +291,31 @@
rtk_fig_color_rgb32(ranger->scan_fig[0], COLOR_LASER_OCC);
// Get the first point
- point1[0] = ranger->proxy->points[0].px;
- point1[1] = ranger->proxy->points[0].py;
+
+ double b = ranger->proxy->min_angle;
+
+ double range = ranger->proxy->ranges[0];
+ if (range < ranger->proxy->min_range) {
+ point1[0] = ranger->proxy->max_range * cos(b);
+ point1[1] = ranger->proxy->max_range * sin(b);
+ }
+ else {
+ point1[0] = ranger->proxy->points[0].px;
+ point1[1] = ranger->proxy->points[0].py;
+ }
// Loop over the rest of the ranges
for (ii = 1; ii < ranger->proxy->ranges_count; ii++)
{
- point2[0] = ranger->proxy->points[ii].px;
- point2[1] = ranger->proxy->points[ii].py;
+ b += ranger->proxy->angular_res;
+ range = ranger->proxy->ranges[ii];
+ if (range < ranger->proxy->min_range) {
+ point2[0] = ranger->proxy->max_range * cos(b);
+ point2[1] = ranger->proxy->max_range * sin(b);
+ }
+ else {
+ point2[0] = ranger->proxy->points[ii].px;
+ point2[1] = ranger->proxy->points[ii].py;
+ }
// Draw a line from point 1 (previous point) to point 2 (current point)
rtk_fig_line(ranger->scan_fig[0], point1[0], point1[1], point2[0], point2[1]);
point1[0] = point2[0];
@@ -295,16 +326,25 @@
if (rtk_menuitem_ischecked(ranger->intns_item))
{
// Draw an intensity scan
+ double b = ranger->proxy->min_angle;
if (ranger->proxy->intensities_count > 0)
{
for (ii = 0; ii < ranger->proxy->intensities_count; ii++)
{
if (ranger->proxy->intensities[ii] != 0)
{
- point1[0] = ranger->proxy->points[ii].px;
- point1[1] = ranger->proxy->points[ii].py;
+ double range = ranger->proxy->ranges[ii];
+ if (range < ranger->proxy->min_range) {
+ point1[0] = ranger->proxy->max_range * cos(b);
+ point1[1] = ranger->proxy->max_range * sin(b);
+ }
+ else {
+ point1[0] = ranger->proxy->points[ii].px;
+ point1[1] = ranger->proxy->points[ii].py;
+ }
rtk_fig_rectangle(ranger->scan_fig[0], point1[0], point1[1], 0, 0.05, 0.05, 1);
}
+ b += ranger->proxy->angular_res;
}
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2009-08-26 08:24:05
|
Revision: 8228
http://playerstage.svn.sourceforge.net/playerstage/?rev=8228&view=rev
Author: gbiggs
Date: 2009-08-26 08:23:52 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Applied patch #2843865: added ranger to writelog/readlog
Modified Paths:
--------------
code/player/trunk/server/drivers/shell/readlog.cc
code/player/trunk/server/drivers/shell/writelog.cc
Modified: code/player/trunk/server/drivers/shell/readlog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/readlog.cc 2009-08-26 08:19:56 UTC (rev 8227)
+++ code/player/trunk/server/drivers/shell/readlog.cc 2009-08-26 08:23:52 UTC (rev 8228)
@@ -57,6 +57,7 @@
The readlog driver can provide the following device interfaces.
- @ref interface_laser
+- @ref interface_ranger
- @ref interface_position2d
- @ref interface_sonar
- @ref interface_wifi
@@ -158,6 +159,7 @@
#define strdup _strdup
#endif
+
#if 0
// we use this pointer to reset timestamps in the client objects when the
// log gets rewound
@@ -206,6 +208,11 @@
player_msghdr_t * hdr,
void * data);
+ // Process ranger interface configuration requests
+ private: int ProcessRangerConfig(QueuePointer & resp_queue,
+ player_msghdr_t * hdr,
+ void * data);
+
// Process sonar interface configuration requests
private: int ProcessSonarConfig(QueuePointer & resp_queue,
player_msghdr_t * hdr,
@@ -269,6 +276,12 @@
int linenum,
int token_count, char **tokens, double time);
+ // Parse ranger data
+ private: int ParseRanger(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum,
+ int token_count, char **tokens, double time);
+
// Parse localize data
private: int ParseLocalize(player_devaddr_t id, unsigned short type,
unsigned short subtype,
@@ -395,6 +408,12 @@
// Should we auto-rewind? This is set in the log devie in the .cfg
// file, and defaults to false
public: bool autorewind;
+
+ private: typedef struct {
+ player_ranger_geom_t* geom;
+ player_ranger_config_t* config;
+ } ranger_meta_t;
+
};
@@ -1024,6 +1043,76 @@
}
int
+ReadLog::ProcessRangerConfig(QueuePointer & resp_queue,
+ player_msghdr_t * hdr,
+ void * data)
+{
+ switch(hdr->subtype)
+ {
+ case PLAYER_RANGER_REQ_GET_GEOM:
+ {
+ // Find the right place from which to retrieve it
+ int j;
+ for(j=0;j<this->provide_count;j++)
+ {
+ if(Device::MatchDeviceAddress(this->provide_ids[j], hdr->addr))
+ break;
+ }
+ if(j>=this->provide_count)
+ {
+ puts("no matching device");
+ return(-1);
+ }
+
+ if(!this->provide_metadata[j])
+ {
+ puts("no metadata");
+ return(-1);
+ }
+
+ this->Publish(this->provide_ids[j], resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK, hdr->subtype,
+ ((ranger_meta_t*)this->provide_metadata[j])->geom,
+ sizeof(player_ranger_geom_t),
+ NULL);
+ return(0);
+ }
+
+ case PLAYER_RANGER_REQ_GET_CONFIG:
+ {
+ // Find the right place from which to retrieve it
+ int j;
+ for(j=0;j<this->provide_count;j++)
+ {
+ if(Device::MatchDeviceAddress(this->provide_ids[j], hdr->addr))
+ break;
+ }
+ if(j>=this->provide_count)
+ {
+ puts("no matching device");
+ return(-1);
+ }
+
+ if(!this->provide_metadata[j])
+ {
+ puts("no metadata");
+ return(-1);
+ }
+
+ this->Publish(this->provide_ids[j], resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK, hdr->subtype,
+ ((ranger_meta_t*)this->provide_metadata[j])->config,
+ sizeof(player_ranger_config_t),
+ NULL);
+ return(0);
+ }
+
+ default:
+ return(-1);
+ }
+}
+
+int
ReadLog::ProcessSonarConfig(QueuePointer & resp_queue,
player_msghdr_t * hdr,
void * data)
@@ -1147,6 +1236,11 @@
return(this->ProcessLaserConfig(resp_queue, hdr, data));
}
else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
+ (hdr->addr.interf == PLAYER_RANGER_CODE))
+ {
+ return(this->ProcessRangerConfig(resp_queue, hdr, data));
+ }
+ else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
(hdr->addr.interf == PLAYER_SONAR_CODE))
{
return(this->ProcessSonarConfig(resp_queue, hdr, data));
@@ -1258,6 +1352,9 @@
if (id.interf == PLAYER_LASER_CODE)
return this->ParseLaser(id, type, subtype, linenum,
token_count, tokens, time);
+ if (id.interf == PLAYER_RANGER_CODE)
+ return this->ParseRanger(id, type, subtype, linenum,
+ token_count, tokens, time);
else if (id.interf == PLAYER_FIDUCIAL_CODE)
return this->ParseFiducial(id, type, subtype, linenum,
token_count, tokens, time);
@@ -1681,55 +1778,7 @@
return ret;
}
- case PLAYER_LASER_DATA_SCANANGLE:
- {
- player_laser_data_scanangle_t data;
- if (token_count < 13)
- {
- PLAYER_ERROR2("incomplete line at %s:%d",
- this->filename, linenum);
- return -1;
- }
-
- data.id = atoi(tokens[7]);
- data.max_range = static_cast<float> (atof(tokens[8]));
- data.ranges_count = atoi(tokens[9]);
- data.intensity_count = data.ranges_count;
- data.angles_count = data.ranges_count;
-
- data.ranges = new float[ data.ranges_count ];
- data.intensity = new uint8_t[ data.ranges_count ];
- data.angles = new float[ data.ranges_count ];
-
- count = 0;
- for (i = 10; i < token_count; i += 3)
- {
- data.ranges[count] = static_cast<float> (atof(tokens[i + 0]));
- data.angles[count] = static_cast<float> (atof(tokens[i + 1]));
- data.intensity[count] = atoi(tokens[i + 2]);
- count += 1;
- }
-
- if (count != (int)data.ranges_count)
- {
- PLAYER_ERROR2("range count mismatch at %s:%d",
- this->filename, linenum);
- ret = -1;
- }
- else
- {
- this->Publish(id, static_cast<uint8_t> (type), static_cast<uint8_t> (subtype),
- (void*)&data, sizeof(data), &time);
- }
- delete [] data.ranges;
- delete [] data.intensity;
- delete [] data.angles;
-
- return ret;
- }
-
-
default:
PLAYER_ERROR1("unknown laser data subtype %d\n", subtype);
return(-1);
@@ -1790,7 +1839,631 @@
}
+////////////////////////////////////////////////////////////////////////////
+// Parse ranger data
+int ReadLog::ParseRanger(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum,
+ int token_count, char **tokens, double time)
+{
+ int i, count, ret;
+ ret = 0;
+ switch(type)
+ {
+ case PLAYER_MSGTYPE_DATA:
+ switch(subtype)
+ {
+ case PLAYER_RANGER_DATA_RANGE:
+ {
+ player_ranger_data_range_t data;
+ if (token_count < 8)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ data.ranges_count = atoi(tokens[7]);
+
+ data.ranges = new double[ data.ranges_count ];
+
+ count = 0;
+ for (i = 8; i < token_count; i++)
+ {
+ data.ranges[count] = static_cast<double> (atof(tokens[i + 0]));
+ count++;
+ }
+
+ if (count != (int)data.ranges_count)
+ {
+ PLAYER_ERROR2("range count mismatch at %s:%d",
+ this->filename, linenum);
+ ret = -1;
+ }
+ else
+ {
+ this->Publish(id, static_cast<uint8_t> (type), static_cast<uint8_t> (subtype),
+ (void*)&data, sizeof(data), &time);
+ }
+ delete [] data.ranges;
+
+ return ret;
+ }
+
+ case PLAYER_RANGER_DATA_RANGESTAMPED:
+ {
+ player_ranger_data_rangestamped_t data;
+
+ if (token_count < 10)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ int total_count=7;
+ data.data.ranges_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.data.ranges = new double[ data.data.ranges_count ];
+
+ count = 0;
+ int loop_size=fmin(token_count, total_count+data.data.ranges_count);
+ for (i = total_count; i < loop_size; i += 2)
+ {
+ data.data.ranges[count] = static_cast<double> (atof(tokens[i]));
+ count++;
+ total_count++;
+ }
+
+ if (count != (int)data.data.ranges_count)
+ {
+ PLAYER_ERROR2("range count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.ranges;
+ return -1;
+ }
+
+ data.have_geom = atoi(tokens[total_count]);
+ total_count++;
+
+ if (data.have_geom)
+ {
+ if (token_count < total_count+11)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.ranges;
+ return -1;
+ }
+
+ data.geom.pose.px = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.py = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.pz = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.proll = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.ppitch = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.pyaw = static_cast<double> (atof(tokens[total_count]));
+
+ total_count++;
+ data.geom.size.sw = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.size.sl = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.size.sh = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+
+ data.geom.element_poses_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.geom.element_poses = new player_pose3d_t [ data.geom.element_poses_count ];
+
+ count = 0;
+ loop_size=fmin(token_count, total_count+data.geom.element_poses_count*6);
+ for (i = total_count; i < loop_size; i += 6)
+ {
+ data.geom.element_poses[count].px = static_cast<double> (atof(tokens[i]));
+ total_count++;
+ data.geom.element_poses[count].py = static_cast<double> (atof(tokens[i+1]));
+ total_count++;
+ data.geom.element_poses[count].pz = static_cast<double> (atof(tokens[i+2]));
+ total_count++;
+ data.geom.element_poses[count].proll = static_cast<double> (atof(tokens[i+3]));
+ total_count++;
+ data.geom.element_poses[count].ppitch = static_cast<double> (atof(tokens[i+4]));
+ total_count++;
+ data.geom.element_poses[count].pyaw = static_cast<double> (atof(tokens[i+5]));
+ total_count++;
+ count++;
+ }
+
+ if (count != (int)data.geom.element_poses_count || total_count > token_count)
+ {
+ PLAYER_ERROR2("poses count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.ranges;
+ delete [] data.geom.element_poses;
+ return -1;
+ }
+
+
+ data.geom.element_sizes_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.geom.element_sizes = new player_bbox3d_t [ data.geom.element_sizes_count ];
+
+ count = 0;
+ loop_size=fmin(token_count, total_count+data.geom.element_sizes_count*3);
+ for (i = total_count; i < loop_size; i += 3)
+ {
+ data.geom.element_sizes[count].sw = static_cast<double> (atof(tokens[i]));
+ total_count++;
+ data.geom.element_sizes[count].sl = static_cast<double> (atof(tokens[i+1]));
+ total_count++;
+ data.geom.element_sizes[count].sh = static_cast<double> (atof(tokens[i+2]));
+ total_count++;
+ count++;
+ }
+
+ if (count != (int)data.geom.element_sizes_count || total_count > token_count)
+ {
+ PLAYER_ERROR2("sizes count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.ranges;
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ return -1;
+ }
+ }
+
+ data.have_config = atoi(tokens[total_count]);
+
+ if (data.have_config)
+ {
+
+ if (token_count < total_count+7)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.ranges;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+ return -1;
+ }
+
+ data.config.min_angle = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.max_angle = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.angular_res = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.min_range = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.max_range = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.range_res = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.frequency = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ }
+
+ if (total_count != token_count)
+ {
+ PLAYER_ERROR2("invalid line at %s:%d: number of tokens does not "
+ "match count", filename, linenum);
+ delete [] data.data.ranges;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+ return -1;
+ }
+
+ this->Publish(id, static_cast<uint8_t> (type), static_cast<uint8_t> (subtype),
+ (void*)&data, sizeof(data), &time);
+ delete [] data.data.ranges;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+
+ return ret;
+ }
+
+ case PLAYER_RANGER_DATA_INTNS:
+ {
+ player_ranger_data_intns_t data;
+
+ if (token_count < 8)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ data.intensities_count = atoi(tokens[7]);
+ data.intensities = new double[ data.intensities_count ];
+
+ count = 0;
+ for (i = 8; i < token_count; i++)
+ {
+ data.intensities[count] = static_cast<double> (atof(tokens[i + 0]));
+ count++;
+ }
+
+ if (count != (int)data.intensities_count)
+ {
+ PLAYER_ERROR2("range count mismatch at %s:%d",
+ this->filename, linenum);
+ ret = -1;
+ }
+ else
+ {
+ this->Publish(id, static_cast<uint8_t> (type), static_cast<uint8_t> (subtype),
+ (void*)&data, sizeof(data), &time);
+ }
+ delete [] data.intensities;
+
+ return ret;
+ }
+
+ case PLAYER_RANGER_DATA_INTNSSTAMPED:
+ {
+ player_ranger_data_intnsstamped_t data;
+
+ if (token_count < 10)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ int total_count=7;
+ data.data.intensities_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.data.intensities = new double[ data.data.intensities_count ];
+
+ count = 0;
+ int loop_size=fmin(token_count, total_count+data.data.intensities_count);
+ for (i = total_count; i < loop_size; i += 2)
+ {
+ data.data.intensities[count] = static_cast<double> (atof(tokens[i]));
+ count++;
+ total_count++;
+ }
+
+ if (count != (int)data.data.intensities_count)
+ {
+ PLAYER_ERROR2("range count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.intensities;
+ return -1;
+ }
+
+ data.have_geom = atoi(tokens[total_count]);
+ total_count++;
+
+ if (data.have_geom)
+ {
+ if (token_count < total_count+11)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.intensities;
+ return -1;
+ }
+
+ data.geom.pose.px = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.py = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.pz = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.proll = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.ppitch = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.pyaw = static_cast<double> (atof(tokens[total_count]));
+
+ total_count++;
+ data.geom.size.sw = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.size.sl = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.geom.size.sh = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+
+ data.geom.element_poses_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.geom.element_poses = new player_pose3d_t [ data.geom.element_poses_count ];
+
+ count = 0;
+ loop_size=fmin(token_count, total_count+data.geom.element_poses_count*6);
+ for (i = total_count; i < loop_size; i += 6)
+ {
+ data.geom.element_poses[count].px = static_cast<double> (atof(tokens[i]));
+ total_count++;
+ data.geom.element_poses[count].py = static_cast<double> (atof(tokens[i+1]));
+ total_count++;
+ data.geom.element_poses[count].pz = static_cast<double> (atof(tokens[i+2]));
+ total_count++;
+ data.geom.element_poses[count].proll = static_cast<double> (atof(tokens[i+3]));
+ total_count++;
+ data.geom.element_poses[count].ppitch = static_cast<double> (atof(tokens[i+4]));
+ total_count++;
+ data.geom.element_poses[count].pyaw = static_cast<double> (atof(tokens[i+5]));
+ total_count++;
+ count++;
+ }
+
+ if (count != (int)data.geom.element_poses_count || total_count > token_count)
+ {
+ PLAYER_ERROR2("poses count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.intensities;
+ delete [] data.geom.element_poses;
+ return -1;
+ }
+
+
+ data.geom.element_sizes_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.geom.element_sizes = new player_bbox3d_t [ data.geom.element_sizes_count ];
+
+ count = 0;
+ loop_size=fmin(token_count, total_count+data.geom.element_sizes_count*3);
+ for (i = total_count; i < loop_size; i += 3)
+ {
+ data.geom.element_sizes[count].sw = static_cast<double> (atof(tokens[i]));
+ total_count++;
+ data.geom.element_sizes[count].sl = static_cast<double> (atof(tokens[i+1]));
+ total_count++;
+ data.geom.element_sizes[count].sh = static_cast<double> (atof(tokens[i+2]));
+ total_count++;
+ count++;
+ }
+
+ if (count != (int)data.geom.element_sizes_count || total_count > token_count)
+ {
+ PLAYER_ERROR2("sizes count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.intensities;
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ return -1;
+ }
+ }
+
+ data.have_config = atoi(tokens[total_count]);
+
+ if (data.have_config)
+ {
+
+ if (token_count < total_count+7)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.intensities;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+ return -1;
+ }
+
+ data.config.min_angle = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.max_angle = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.angular_res = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.min_range = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.max_range = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.range_res = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ data.config.frequency = static_cast<double> (atof(tokens[total_count]));
+ total_count++;
+ }
+
+ if (total_count != token_count)
+ {
+ PLAYER_ERROR2("invalid line at %s:%d: number of tokens does not "
+ "match count", filename, linenum);
+ delete [] data.data.intensities;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+ return -1;
+ }
+
+ this->Publish(id, static_cast<uint8_t> (type), static_cast<uint8_t> (subtype),
+ (void*)&data, sizeof(data), &time);
+ delete [] data.data.intensities;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+ return ret;
+ }
+
+ default:
+ PLAYER_ERROR1("unknown ranger data subtype %d\n", subtype);
+ return(-1);
+ }
+ break;
+
+ case PLAYER_MSGTYPE_RESP_ACK:
+ switch(subtype)
+ {
+ case PLAYER_RANGER_REQ_GET_GEOM:
+ {
+ if(token_count < 18)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ int num_poses=atoi(tokens[16]);
+
+ if(token_count < 18+num_poses*6)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ int num_sizes=atoi(tokens[17+num_poses*6]);
+
+ if(token_count < 18+num_poses*6+num_sizes*3)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+
+ // cache it
+ player_ranger_geom_t* geom =
+ (player_ranger_geom_t*)calloc(1,sizeof(player_ranger_geom_t)+sizeof(player_pose3d_t)*num_poses+sizeof(player_bbox3d_t)*num_sizes);
+ assert(geom);
+
+ geom->pose.px = atof(tokens[7]);
+ geom->pose.py = atof(tokens[8]);
+ geom->pose.pz = atof(tokens[9]);
+ geom->pose.proll = atof(tokens[10]);
+ geom->pose.ppitch = atof(tokens[11]);
+ geom->pose.pyaw = atof(tokens[12]);
+ geom->size.sw = atof(tokens[13]);
+ geom->size.sl = atof(tokens[14]);
+ geom->size.sh = atof(tokens[15]);
+ geom->element_poses_count = num_poses;
+ geom->element_poses = new player_pose3d_t [ num_poses ];
+ geom->element_sizes_count = num_sizes;
+ geom->element_sizes = new player_bbox3d_t [ num_sizes ];
+
+ for (int i=0; i < num_poses; i++)
+ {
+ geom->element_poses[i].px=atof(tokens[17+i*6]);
+ geom->element_poses[i].py=atof(tokens[17+i*6+1]);
+ geom->element_poses[i].pz=atof(tokens[17+i*6+2]);
+ geom->element_poses[i].proll=atof(tokens[17+i*6+3]);
+ geom->element_poses[i].ppitch=atof(tokens[17+i*6+4]);
+ geom->element_poses[i].pyaw=atof(tokens[17+i*6+5]);
+ }
+
+ for (int i=0; i < num_sizes; i++)
+ {
+ geom->element_sizes[i].sw=atof(tokens[17+num_poses*6+1+i*3]);
+ geom->element_sizes[i].sl=atof(tokens[17+num_poses*6+1+i*3+1]);
+ geom->element_sizes[i].sh=atof(tokens[17+num_poses*6+1+i*3+2]);
+ }
+
+
+ // Find the right place to put it
+ int j;
+ for(j=0;j<this->provide_count;j++)
+ {
+ if(Device::MatchDeviceAddress(this->provide_ids[j], id))
+ break;
+ }
+ assert(j<this->provide_count);
+
+ // if something is already here, use it
+ if(this->provide_metadata[j])
+ ((ranger_meta_t*)this->provide_metadata[j])->geom = geom;
+ else
+ {
+ ranger_meta_t* meta =
+ (ranger_meta_t*)calloc(1,sizeof(ranger_meta_t));
+ meta->geom=geom;
+ this->provide_metadata[j] = (void*)meta;
+ }
+
+ // nothing to publish
+ return(0);
+ }
+
+ case PLAYER_RANGER_REQ_GET_CONFIG:
+ {
+ if(token_count < 14)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ // cache it
+ player_ranger_config_t* config =
+ (player_ranger_config_t*)calloc(1,sizeof(player_ranger_config_t));
+ assert(config);
+
+ config->min_angle = atof(tokens[7]);
+ config->max_angle = atof(tokens[8]);
+ config->angular_res = atof(tokens[9]);
+ config->min_range = atof(tokens[10]);
+ config->max_range = atof(tokens[11]);
+ config->range_res = atof(tokens[12]);
+ config->frequency = atof(tokens[13]);
+
+ // Find the right place to put it
+ int j;
+ for(j=0;j<this->provide_count;j++)
+ {
+ if(Device::MatchDeviceAddress(this->provide_ids[j], id))
+ break;
+ }
+ assert(j<this->provide_count);
+
+ // if something is already here, use it
+ if(this->provide_metadata[j]) {
+ ((ranger_meta_t*)this->provide_metadata[j])->config = config;
+ }
+ else
+ {
+ ranger_meta_t* meta =
+ (ranger_meta_t*)calloc(1,sizeof(ranger_meta_t));
+ meta->config=config;
+ this->provide_metadata[j] = (void*)meta;
+ }
+
+ // nothing to publish
+ return(0);
+ }
+
+ default:
+ PLAYER_ERROR1("unknown ranger reply subtype %d\n", subtype);
+ return(-1);
+ }
+ break;
+
+ default:
+ PLAYER_ERROR1("unknown ranger msg type %d\n", type);
+ return(-1);
+ }
+}
+
+
+
////////////////////////////////////////////////////////////////////////////
// Parse localize data
int ReadLog::ParseLocalize(player_devaddr_t id,
Modified: code/player/trunk/server/drivers/shell/writelog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/writelog.cc 2009-08-26 08:19:56 UTC (rev 8227)
+++ code/player/trunk/server/drivers/shell/writelog.cc 2009-08-26 08:23:52 UTC (rev 8228)
@@ -70,6 +70,7 @@
The writelog driver can will log data from the following interfaces:
- @ref interface_laser
+- @ref interface_ranger
- @ref interface_sonar
- @ref interface_position2d
- @ref interface_ptz
@@ -225,6 +226,9 @@
// Write laser data to file
private: int WriteLaser(player_msghdr_t* hdr, void *data);
+ // Write ranger data to file
+ private: int WriteRanger(player_msghdr_t* hdr, void *data);
+
// Write localize data to file
private: int WriteLocalize(player_msghdr_t* hdr, void *data);
@@ -585,9 +589,42 @@
delete msg;
}
}
+ else if (device->addr.interf == PLAYER_RANGER_CODE)
+ {
+ // Get the ranger geometry
+ Message* msg;
+ if(!(msg = device->device->Request(this->InQueue,
+ PLAYER_MSGTYPE_REQ,
+ PLAYER_RANGER_REQ_GET_GEOM,
+ NULL, 0, NULL, true)))
+ {
+ // oh well.
+ PLAYER_WARN("unable to get ranger geometry");
+ }
+ else
+ {
+ // log it
+ this->Write(device, msg->GetHeader(), msg->GetPayload());
+ delete msg;
+ }
+ if(!(msg = device->device->Request(this->InQueue,
+ PLAYER_MSGTYPE_REQ,
+ PLAYER_RANGER_REQ_GET_CONFIG,
+ NULL, 0, NULL, true)))
+ {
+ // oh well.
+ PLAYER_WARN("unable to get ranger config");
+ }
+ else
+ {
+ // log it
+ this->Write(device, msg->GetHeader(), msg->GetPayload());
+ delete msg;
+ }
+ }
else if (device->addr.interf == PLAYER_POSITION2D_CODE)
{
- // Get the laser geometry
+ // Get the position geometry
Message* msg;
if(!(msg = device->device->Request(this->InQueue,
PLAYER_MSGTYPE_REQ,
@@ -607,7 +644,7 @@
/* HHAA 15-02-2007 */
else if (device->addr.interf == PLAYER_BUMPER_CODE)
{
- // Get the laser geometry
+ // Get the bumper geometry
Message* msg;
if(!(msg = device->device->Request(this->InQueue,
PLAYER_MSGTYPE_REQ,
@@ -627,7 +664,7 @@
/* HHAA 15-02-2007 */
else if (device->addr.interf == PLAYER_IR_CODE)
{
- // Get the laser geometry
+ // Get the IR geometry
Message* msg;
if(!(msg = device->device->Request(this->InQueue,
PLAYER_MSGTYPE_REQ,
@@ -827,6 +864,9 @@
case PLAYER_LASER_CODE:
retval = this->WriteLaser(hdr, data);
break;
+ case PLAYER_RANGER_CODE:
+ retval = this->WriteRanger(hdr, data);
+ break;
case PLAYER_LOCALIZE_CODE:
retval = this->WriteLocalize(hdr, data);
break;
@@ -1039,22 +1079,22 @@
}
return(0);
- case PLAYER_LASER_DATA_SCANANGLE:
- scanangle = (player_laser_data_scanangle_t*)data;
- fprintf(this->file, "%04d %+07.4f %04d ",
- scanangle->id, scanangle->max_range, scanangle->ranges_count);
-
- for (i = 0; i < scanangle->ranges_count; i++)
- {
- fprintf(this->file, "%.3f ", scanangle->ranges[i]);
- fprintf(this->file, "%.3f ", scanangle->angles[i]);
- if(i < scanangle->intensity_count)
- fprintf(this->file, "%2d ", scanangle->intensity[i]);
- else
- fprintf(this->file, "%2d ", 0);
- }
- return(0);
-
+ case PLAYER_LASER_DATA_SCANANGLE:
+ scanangle = (player_laser_data_scanangle_t*)data;
+ fprintf(this->file, "%04d %+07.4f %04d ",
+ scanangle->id, scanangle->max_range, scanangle->ranges_count);
+
+ for (i = 0; i < scanangle->ranges_count; i++)
+ {
+ fprintf(this->file, "%.3f ", scanangle->ranges[i]);
+ fprintf(this->file, "%.3f ", scanangle->angles[i]);
+ if(i < scanangle->intensity_count)
+ fprintf(this->file, "%2d ", scanangle->intensity[i]);
+ else
+ fprintf(this->file, "%2d ", 0);
+ }
+ return(0);
+
default:
return(-1);
}
@@ -1078,12 +1118,334 @@
}
}
+
/** @ingroup tutorial_datalog
+ * @defgroup player_driver_writelog_ranger ranger format
+
+@brief ranger log format
+
+The following type:subtype ranger messages can be logged:
+- 1:1 (PLAYER_RANGER_DATA_RANGE) - A range scan. The format is:
+ - ranges_count (uint): number of ranges
+ - list of ranges_count ranges:
+ - range (double): distance
+
+- 1:2 (PLAYER_RANGER_DATA_RANGEPOSE) - A range scan optionally with
+the (possibly estimated) geometry of the device when the scan was
+acquired and optional sensor configuration. The format is:
+ - ranges_count (uint): number of ranges
+ - list of ranges_count ranges:
+ - range (double): distance
+ - have_geom (uint8): If non-zero, the geometry data has been filled
+ - geometry of device at the time of range data:
+ - pose of device:
+ - px (float): X coordinate of the pose, in meters
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - size of device:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - sh (float): height of the device, in meters
+ - element_poses_count (uint): pose of each individual range sensor that makes up the device
+ - list of element_poses_count poses:
+ - px (float): X coordinate of the pose, in meters
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - element_sizes_count (uint): size of each individual range sensor that makes up the device
+ - list of element_sizes_count sizes:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - sh (float): height of the device, in meters
+ - have_config(uint8): If non-zero, the config data has been filled
+ - config of device:
+ - min_angle (float): start angle of scans, in radians
+ - max_angle (float): end angle of scans, in radians
+ - angular_res (float): scan resolution, in radians
+ - min_range (float): minimum range, in meters
+ - max_range (float): maximum range, in meters
+ - range_res (float): range resolution, in meters
+ - frequency (float): scanning frequency, in Hz
+
+- 1:3 (PLAYER_RANGER_DATA_INTNS) - An intensity scan. The format is:
+ - intensities_count (uint): number of intensities
+ - list of intensities_count intensities:
+ - intensity (double)
+
+- 1:4 (PLAYER_RANGER_DATA_ITNSPOSE) - An intensity scan with an attached pose (estimated from the time of the scan). The format is:
+ - intensities_count (uint): number of intensities
+ - list of intensities_count intensities:
+ - intensity (double)
+ - have_geom (uint8): If non-zero, the geometry data has been filled
+ - geometry of device at the time of intensity data:
+ - pose of device:
+ - px (float): X coordinate of the pose, in meters
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - size of device:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - sh (float): height of the device, in meters
+ - element_poses_count (uint): pose of each individual range sensor that makes up the device
+ - list of element_poses_count poses:
+ - px (float): X coordinate of the pose, in meters
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - element_sizes_count (uint): size of each individual range sensor that makes up the device
+ - list of element_sizes_count sizes:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - sh (float): height of the device, in meters
+ - have_config(uint8): If non-zero, the config data has been filled
+ - config of device:
+
+- 4:1 (PLAYER_RANGER_REQ_GET_GEOM) - Ranger pose information. The format is:
+ - pose of device:
+ - px (float): X coordinate of the pose, in meters
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - size of device:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - sh (float): height of the device, in meters
+ - element_poses_count (uint): pose of each individual range sensor that makes up the device
+ - list of element_poses_count poses:
+ - px (float): X coordinate of the pose, in meters
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - element_sizes_count (uint): size of each individual range sensor that makes up the device
+ - list of element_sizes_count sizes:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - sh (float): height of the device, in meters
+*/
+int
+WriteLog::WriteRanger(player_msghdr_t* hdr, void *data)
+{
+ size_t i;
+ player_ranger_data_range_t* rscan;
+ player_ranger_data_rangestamped_t* rscanpose;
+ player_ranger_data_intns_t* iscan;
+ player_ranger_data_intnsstamped_t* iscanpose;
+ player_ranger_geom_t* geom;
+ player_ranger_config_t* config;
+
+ // Check the type
+ switch(hdr->type)
+ {
+ case PLAYER_MSGTYPE_DATA:
+ // Check the subtype
+ switch(hdr->subtype)
+ {
+ case PLAYER_RANGER_DATA_RANGE:
+ rscan = (player_ranger_data_range_t*)data;
+ // Note that, in this format, we need a lot of precision in the
+ // resolution field.
+
+ fprintf(this->file, "%04d ", rscan->ranges_count);
+
+ for (i = 0; i < rscan->ranges_count; i++)
+ {
+ fprintf(this->file, "%.3f ", rscan->ranges[i]);
+ }
+ return(0);
+
+ case PLAYER_RANGER_DATA_RANGESTAMPED:
+ rscanpose = (player_ranger_data_rangestamped_t*)data;
+ // Note that, in this format, we need a lot of precision in the
+ // resolution field.
+
+ fprintf(this->file, "%04d ", rscanpose->data.ranges_count);
+
+ for (i = 0; i < rscanpose->data.ranges_count; i++)
+ {
+ fprintf(this->file, "%.3f ", rscanpose->data.ranges[i]);
+ }
+
+ fprintf(this->file, "%d ", rscanpose->have_geom);
+
+ if (rscanpose->have_geom)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f ",
+ rscanpose->geom.pose.px, rscanpose->geom.pose.py, rscanpose->geom.pose.pz,
+ rscanpose->geom.pose.proll, rscanpose->geom.pose.ppitch, rscanpose->geom.pose.pyaw,
+ rscanpose->geom.size.sw, rscanpose->geom.size.sl, rscanpose->geom.size.sh);
+
+ fprintf(this->file, "%04d ", rscanpose->geom.element_poses_count);
+
+ for (i = 0; i < rscanpose->geom.element_poses_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f ",
+ rscanpose->geom.element_poses[i].px, rscanpose->geom.element_poses[i].py, rscanpose->geom.element_poses[i].pz,
+ rscanpose->geom.element_poses[i].proll, rscanpose->geom.element_poses[i].ppitch, rscanpose->geom.element_poses[i].pyaw);
+ }
+
+ fprintf(this->file, "%04d ", rscanpose->geom.element_sizes_count);
+
+ for (i = 0; i < rscanpose->geom.element_sizes_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f ",
+ rscanpose->geom.element_sizes[i].sw, rscanpose->geom.element_sizes[i].sl, rscanpose->geom.element_sizes[i].sh);
+ }
+ }
+
+ if (rscanpose->have_config)
+ {
+ fprintf(this->file, "%.4f %.4f %.4f %.4f %.4f %.4f %.4f ",
+ rscanpose->config.min_angle, rscanpose->config.max_angle,
+ rscanpose->config.angular_res, rscanpose->config.min_range,
+ rscanpose->config.max_range, rscanpose->config.range_res,
+ rscanpose->config.frequency);
+ }
+
+ return(0);
+
+
+ case PLAYER_RANGER_DATA_INTNS:
+ iscan = (player_ranger_data_intns_t*)data;
+ // Note that, in this format, we need a lot of precision in the
+ // resolution field.
+
+ fprintf(this->file, "%04d ", iscan->intensities_count);
+
+ for (i = 0; i < iscan->intensities_count; i++)
+ {
+ fprintf(this->file, "%.3f ", iscan->intensities[i]);
+ }
+ return(0);
+
+
+ case PLAYER_RANGER_DATA_INTNSSTAMPED:
+ iscanpose = (player_ranger_data_intnsstamped_t*)data;
+ // Note that, in this format, we need a lot of precision in the
+ // resolution field.
+
+ fprintf(this->file, "%04d ", iscanpose->data.intensities_count);
+
+ for (i = 0; i < iscanpose->data.intensities_count; i++)
+ {
+ fprintf(this->file, "%.3f ", iscanpose->data.intensities[i]);
+ }
+
+ fprintf(this->file, "%d ", iscanpose->have_geom);
+
+ if (iscanpose->have_geom)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f ",
+ iscanpose->geom.pose.px, iscanpose->geom.pose.py, iscanpose->geom.pose.pz,
+ iscanpose->geom.pose.proll, iscanpose->geom.pose.ppitch, iscanpose->geom.pose.pyaw,
+ iscanpose->geom.size.sw, iscanpose->geom.size.sl, iscanpose->geom.size.sh);
+
+ fprintf(this->file, "%04d ", iscanpose->geom.element_poses_count);
+
+ for (i = 0; i < iscanpose->geom.element_poses_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f ",
+ iscanpose->geom.element_poses[i].px, iscanpose->geom.element_poses[i].py, iscanpose->geom.element_poses[i].pz,
+ iscanpose->geom.element_poses[i].proll, iscanpose->geom.element_poses[i].ppitch, iscanpose->geom.element_poses[i].pyaw);
+ }
+
+ fprintf(this->file, "%04d ", iscanpose->geom.element_sizes_count);
+
+ for (i = 0; i < iscanpose->geom.element_sizes_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f ",
+ iscanpose->geom.element_sizes[i].sw, iscanpose->geom.element_sizes[i].sl, iscanpose->geom.element_sizes[i].sh);
+ }
+ }
+
+ if (iscanpose->have_config)
+ {
+ fprintf(this->file, "%.4f %.4f %.4f %.4f %.4f %.4f %.4f ",
+ iscanpose->config.min_angle, iscanpose->config.max_angle,
+ iscanpose->config.angular_res, iscanpose->config.min_range,
+ iscanpose->config.max_range, iscanpose->config.range_res,
+ iscanpose->config.frequency);
+ }
+
+ return(0);
+
+ default:
+ return(-1);
+ }
+ case PLAYER_MSGTYPE_RESP_ACK:
+ switch(hdr->subtype)
+ {
+ case PLAYER_RANGER_REQ_GET_GEOM:
+ geom = (player_ranger_geom_t*)data;
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f ",
+ geom->pose.px,
+ geom->pose.py,
+ geom->pose.pz,
+ geom->pose.proll,
+ geom->pose.ppitch,
+ geom->pose.pyaw,
+ geom->size.sw,
+ geom->size.sl,
+ geom->size.sh);
+
+ fprintf(this->file, "%04d ", geom->element_poses_count);
+
+ for (i = 0; i < geom->element_poses_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f ",
+ geom->element_poses[i].px, geom->element_poses[i].py, geom->element_poses[i].pz,
+ geom->element_poses[i].proll, geom->element_poses[i].ppitch, geom->element_poses[i].pyaw);
+ }
+
+ fprintf(this->file, "%04d ", geom->element_sizes_count);
+
+ for (i = 0; i < geom->element_sizes_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f ",
+ geom->element_sizes[i].sw, geom->element_sizes[i].sl, geom->element_sizes[i].sh);
+ }
+
+ return(0);
+
+ case PLAYER_RANGER_REQ_GET_CONFIG:
+ config = (player_ranger_config_t*)data;
+
+ fprintf(this->file, "%lf %lf %lf %lf %lf %lf %lf ",
+ config->min_angle, config->max_angle,
+ config->angular_res, config->min_range,
+ config->max_range, config->range_res,
+ config->frequency);
+
+ return(0);
+
+ default:
+ return(-1);
+ }
+ default:
+ return(-1);
+ }
+}
+
+
+/** @ingroup tutorial_datalog
* @defgroup player_driver_writelog_localize localize format
-@brief laser log format
+@brief localize log format
-The following type:subtype laser messages can be logged:
+The following type:subtype localize messages can be logged:
- 1:1 (PLAYER_LOCALIZE_DATA_HYPOTHS) - A set of pose hypotheses. The format is:
- pending_count (int): number of pending (unprocessed observations)
- pending time (float): time stamp of the last observation processed
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <gb...@us...> - 2009-08-26 08:20:10
|
Revision: 8227
http://playerstage.svn.sourceforge.net/playerstage/?rev=8227&view=rev
Author: gbiggs
Date: 2009-08-26 08:19:56 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Fixed missing dependencies for Python and Ruby bindings. Applied patch #2839013: sonyevid30 RTOD/DTOR patch
Modified Paths:
--------------
code/player/trunk/client_libs/libplayerc/bindings/python/CMakeLists.txt
code/player/trunk/client_libs/libplayerc/bindings/ruby/CMakeLists.txt
code/player/trunk/server/drivers/ptz/sonyevid30.cc
Modified: code/player/trunk/client_libs/libplayerc/bindings/python/CMakeLists.txt
===================================================================
--- code/player/trunk/client_libs/libplayerc/bindings/python/CMakeLists.txt 2009-08-26 06:09:46 UTC (rev 8226)
+++ code/player/trunk/client_libs/libplayerc/bindings/python/CMakeLists.txt 2009-08-26 08:19:56 UTC (rev 8227)
@@ -36,6 +36,8 @@
# Copy playerc.i to the build directory
SET (playerc_i "${CMAKE_CURRENT_SOURCE_DIR}/playerc.i")
+ SET (SWIG_MODULE_playerc_EXTRA_DEPS ${PROJECT_SOURCE_DIR}/client_libs/libplayerc/playerc.h
+ ${PROJECT_BINARY_DIR}/libplayerinterface/player_interfaces.h)
SWIG_ADD_MODULE (playerc python ${playerc_i})
SWIG_LINK_LIBRARIES (playerc ${PYTHON_LIBRARIES})
ADD_DEPENDENCIES (${SWIG_MODULE_playerc_REAL_NAME} playerc_wrap_i_target)
Modified: code/player/trunk/client_libs/libplayerc/bindings/ruby/CMakeLists.txt
===================================================================
--- code/player/trunk/client_libs/libplayerc/bindings/ruby/CMakeLists.txt 2009-08-26 06:09:46 UTC (rev 8226)
+++ code/player/trunk/client_libs/libplayerc/bindings/ruby/CMakeLists.txt 2009-08-26 08:19:56 UTC (rev 8227)
@@ -42,6 +42,8 @@
#If playercr is called simply playerc, all sorts of errors happen
#I guess is namespace conflict with Python bindings' playerc
#currently we create playercr locally but install it as playerc
+ SET (SWIG_MODULE_playercr_EXTRA_DEPS ${PROJECT_SOURCE_DIR}/client_libs/libplayerc/playerc.h
+ ${PROJECT_BINARY_DIR}/libplayerinterface/player_interfaces.h)
SWIG_ADD_MODULE (playercr ruby ${playerc_i})
SWIG_LINK_LIBRARIES (playercr ${RUBY_LIBRARY})
ADD_DEPENDENCIES (${SWIG_MODULE_playercr_REAL_NAME} playerc_wrap_i_target)
Modified: code/player/trunk/server/drivers/ptz/sonyevid30.cc
===================================================================
--- code/player/trunk/server/drivers/ptz/sonyevid30.cc 2009-08-26 06:09:46 UTC (rev 8226)
+++ code/player/trunk/server/drivers/ptz/sonyevid30.cc 2009-08-26 08:19:56 UTC (rev 8227)
@@ -108,6 +108,7 @@
#include "config.h"
#include <fcntl.h>
+#include <stddef.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
@@ -116,6 +117,7 @@
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
+#include <time.h>
#include <libplayercore/playercore.h>
#include <replace/replace.h>
@@ -143,6 +145,22 @@
#define VISCA_COMMAND_CODE 0x01
#define VISCA_INQUIRY_CODE 0x09
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+// Convert degrees to radians
+#ifndef DTOR
+#define DTOR(d) ((static_cast<double>(d)) * (M_PI) / 180.0)
+#endif
+
+// Convert radians to degrees
+#ifndef RTOD
+#define RTOD(r) ((static_cast<double>(r)) * 180.0 / M_PI)
+#endif
+
+#define EPS 0.0000001
+
/*
*
*
@@ -198,14 +216,22 @@
float evid_cam_pan_radians(evid_cam_t cam, short pan_cu)
{
+/*
float conv_factor = -cam.pan_max_rad / cam.pan_max_cu;
return pan_cu * conv_factor;
+*/
+ cam = cam;
+ return DTOR(-pan_cu);
}
float evid_cam_tilt_radians(evid_cam_t cam, short tilt_cu)
{
+/*
float conv_factor = cam.tilt_max_rad / cam.tilt_max_cu;
return tilt_cu * conv_factor;
+*/
+ cam = cam;
+ return DTOR(tilt_cu);
}
float evid_cam_panspeed_radians(evid_cam_t cam, short panspeed_cu)
@@ -232,24 +258,35 @@
float evid_cam_zoom_radians(evid_cam_t cam, short zoom_cu)
{
+/*
float conv_factor = (cam.fov_max_rad - cam.fov_min_rad)
/ (cam.fov_max_cu - cam.fov_min_cu);
return (zoom_cu - cam.fov_min_cu) * conv_factor + cam.fov_min_rad;
+*/
+ return DTOR(RTOD(cam.fov_max_rad) + (static_cast<double>(zoom_cu) * (RTOD(cam.fov_min_rad) - RTOD(cam.fov_max_rad))) / 1024.0);
}
short evid_cam_pan_cu(evid_cam_t cam, float pan_radians)
{
+/*
float conv_factor = -cam.pan_max_cu / cam.pan_max_rad;
pan_radians = clip_float(pan_radians, -cam.pan_max_rad, cam.pan_max_rad);
return (short) rint(pan_radians * conv_factor);
+*/
+ cam = cam;
+ return static_cast<short>(-(RTOD(pan_radians)));
}
short evid_cam_tilt_cu(evid_cam_t cam, float tilt_radians)
{
+/*
float conv_factor = cam.tilt_max_cu / cam.tilt_max_rad;
tilt_radians = clip_float(tilt_radians, cam.tilt_min_rad, cam.tilt_max_rad);
return (short) rint(tilt_radians * conv_factor);
+*/
+ cam = cam;
+ return static_cast<short>(RTOD(tilt_radians));
}
/*
@@ -314,6 +351,7 @@
*/
short evid_cam_zoom_cu(evid_cam_t cam, float zoom_radians)
{
+/*
float conv_factor = (cam.fov_max_cu - cam.fov_min_cu)
/ (cam.fov_max_rad - cam.fov_min_rad);
if (zoom_radians == 0) {
@@ -323,6 +361,9 @@
return (short) ((zoom_radians - cam.fov_min_rad) * conv_factor
+ cam.fov_min_cu);
}
+*/
+ if (fabs(zoom_radians) < EPS) zoom_radians = cam.fov_max_rad;
+ return (1024 * (static_cast<short>(RTOD(zoom_radians)) - static_cast<short>(RTOD(cam.fov_max_rad)))) / (static_cast<short>(RTOD(cam.fov_min_rad)) - static_cast<short>(RTOD(cam.fov_max_rad)));
}
/*
@@ -705,9 +746,9 @@
// put the camera back to center
usleep(PTZ_SLEEP_TIME_USEC);
- SendAbsPanTilt(0,0);
+ SendAbsPanTilt(evid_cam_pan_cu(this->cam_config_, 0.0), evid_cam_tilt_cu(this->cam_config_, 0.0));
usleep(PTZ_SLEEP_TIME_USEC);
- SendAbsZoom(cam_config_.fov_max_cu);
+ SendAbsZoom(evid_cam_zoom_cu(this->cam_config_, 0.0));
if(close(ptz_fd))
perror("SonyEVID30::Shutdown():close():");
@@ -1201,12 +1242,12 @@
cam_config_ = SONY_EVI_CAMERAS[0];
}
- if (maxfov != 0) {
- cam_config_.fov_max_rad = maxfov;
+ if (!(fabs(this->maxfov) < EPS)) {
+ this->cam_config_.fov_max_rad = this->maxfov;
+ }
+ if (!(fabs(this->minfov) < EPS)) {
+ this->cam_config_.fov_min_rad = this->minfov;
}
- if (minfov != 0) {
- cam_config_.fov_min_rad = minfov;
- }
}
return 0;
}
@@ -1297,7 +1338,7 @@
switch (control_mode_) {
case PLAYER_PTZ_VELOCITY_CONTROL:
// Change of velocity requested?
- if (cmd->panspeed != panspeed_demand_rad
+ if (cmd->panspeed != panspeed_demand_rad
|| cmd->tiltspeed != tiltspeed_demand_rad) {
panspeed_demand_rad = cmd->panspeed;
tiltspeed_demand_rad = cmd->tiltspeed;
@@ -1497,6 +1538,7 @@
{
while (1) {
player_ptz_data_t data;
+ struct timespec ts;
// Process incoming requests
pthread_testcancel();
@@ -1514,7 +1556,9 @@
pthread_testcancel();
Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_PTZ_DATA_STATE, &data,sizeof(player_ptz_data_t),NULL);
- usleep(PTZ_SLEEP_TIME_USEC);
+ ts.tv_sec = 0;
+ ts.tv_nsec = PTZ_SLEEP_TIME_USEC * 1000;
+ nanosleep(&ts, NULL);
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-08-26 06:09:54
|
Revision: 8226
http://playerstage.svn.sourceforge.net/playerstage/?rev=8226&view=rev
Author: hsujohnhsu
Date: 2009-08-26 06:09:46 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
pass through ODE flags for DdSINGLE or DdDOUBLE.
Modified Paths:
--------------
code/gazebo/trunk/cmake/FindOde.cmake
code/gazebo/trunk/server/gui/CMakeLists.txt
Modified: code/gazebo/trunk/cmake/FindOde.cmake
===================================================================
--- code/gazebo/trunk/cmake/FindOde.cmake 2009-08-26 06:08:55 UTC (rev 8225)
+++ code/gazebo/trunk/cmake/FindOde.cmake 2009-08-26 06:09:46 UTC (rev 8226)
@@ -36,6 +36,15 @@
IF (NOT ODE_FOUND)
MESSAGE (SEND_ERROR "\nError: ODE and development files not found. See the following website: http://www.ode.org")
ELSE (NOT ODE_FOUND)
+ #SET (CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} ${ODE_CFLAGS_OTHER}" CACHE INTERNAL "added dSINGLE" FORCE)
+ #SET (CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} ${ODE_CFLAGS_OTHER}" CACHE INTERNAL "added dSINGLE" FORCE)
+ #SET (CMAKE_C_FLAGS_PROFILE "${CMAKE_C_FLAGS_PROFILE} ${ODE_CFLAGS_OTHER}" CACHE INTERNAL "added dSINGLE" FORCE)
+ #MESSAGE (STATUS "\n\ndebug\n" ${CMAKE_C_FLAGS_DEBUG} "\n\n")
+
+ APPEND_TO_CACHED_LIST(gazeboserver_cflags
+ ${gazeboserver_cflags_desc}
+ ${ODE_CFLAGS_OTHER})
+
APPEND_TO_CACHED_LIST(gazeboserver_include_dirs
${gazeboserver_include_dirs_desc}
${ODE_INCLUDE_DIRS})
Modified: code/gazebo/trunk/server/gui/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/server/gui/CMakeLists.txt 2009-08-26 06:08:55 UTC (rev 8225)
+++ code/gazebo/trunk/server/gui/CMakeLists.txt 2009-08-26 06:09:46 UTC (rev 8226)
@@ -1,3 +1,5 @@
+INCLUDE (${gazebo_cmake_dir}/GazeboUtils.cmake)
+
SET (sources Gui.cc
GLWindow.cc
MainMenu.cc
@@ -16,6 +18,9 @@
GLFrame.hh
)
+LIST_TO_STRING(GAZEBO_CFLAGS "${gazeboserver_cflags}")
+SET_SOURCE_FILES_PROPERTIES(${sources} PROPERTIES COMPILE_FLAGS "${GAZEBO_CFLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE}}")
+
#ADD_LIBRARY(gazebo_gui STATIC ${sources})
ADD_LIBRARY(gazebo_gui-shared SHARED ${sources})
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-08-26 06:09:04
|
Revision: 8225
http://playerstage.svn.sourceforge.net/playerstage/?rev=8225&view=rev
Author: hsujohnhsu
Date: 2009-08-26 06:08:55 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
fix race condition errors:
* call msync() for opening mmap, seems to need it mostly running on slow file systems such as nfs.
* remove toAddModels, add to models queue directly at spawn time.
simplify code:
* removed ProcessMessages() and call UpdateSimulationIface() directly.
Modified Paths:
--------------
code/gazebo/trunk/libgazebo/Iface.cc
code/gazebo/trunk/server/Simulator.cc
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/World.hh
Modified: code/gazebo/trunk/libgazebo/Iface.cc
===================================================================
--- code/gazebo/trunk/libgazebo/Iface.cc 2009-08-25 18:58:06 UTC (rev 8224)
+++ code/gazebo/trunk/libgazebo/Iface.cc 2009-08-26 06:08:55 UTC (rev 8225)
@@ -288,6 +288,12 @@
// Map the mmap file
this->mMap = mmap(0, this->size, PROT_READ | PROT_WRITE, MAP_SHARED, this->mmapFd, 0);
+ // flushes changes made to the in-core copy of a mmap file
+ if (msync( this->mMap, this->size, MS_SYNC))
+ {
+ stream << "error msync mmap: " << strerror(errno);
+ throw(stream.str());
+ }
if (this->mMap == MAP_FAILED)
{
Modified: code/gazebo/trunk/server/Simulator.cc
===================================================================
--- code/gazebo/trunk/server/Simulator.cc 2009-08-25 18:58:06 UTC (rev 8224)
+++ code/gazebo/trunk/server/Simulator.cc 2009-08-26 06:08:55 UTC (rev 8225)
@@ -676,7 +676,7 @@
nanosleep(&req, &rem);
// Process all incoming messages from simiface
- world->ProcessMessages();
+ world->UpdateSimulationIface();
if (this->timeout > 0 && this->GetRealTime() > this->timeout)
{
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-08-25 18:58:06 UTC (rev 8224)
+++ code/gazebo/trunk/server/World.cc 2009-08-26 06:08:55 UTC (rev 8225)
@@ -218,7 +218,6 @@
if (this->openAL)
this->openAL->Init();
- this->toAddModels.clear();
this->toDeleteModels.clear();
this->toLoadEntities.clear();
@@ -304,13 +303,6 @@
}
////////////////////////////////////////////////////////////////////////////////
-// Process messages
-void World::ProcessMessages()
-{
- this->UpdateSimulationIface();
-}
-
-////////////////////////////////////////////////////////////////////////////////
// Finilize the world
void World::Fini()
{
@@ -477,13 +469,10 @@
this->SetModelPose(model, model->GetInitPose());
// Add the model to our list
- if (Simulator::Instance()->GetSimTime() == 0)
- this->models.push_back(model);
- else
- {
+ this->models.push_back(model);
+
+ if (Simulator::Instance()->GetSimTime() > 0)
model->Init();
- this->toAddModels.push_back(model);
- }
if (parent != NULL)
model->Attach(node->GetChild("attach"));
@@ -1257,16 +1246,8 @@
this->simIface->Unlock();
-
- std::vector< Model* >::iterator miter;
-
- // Copy the newly created models into the main model vector
- std::copy(this->toAddModels.begin(), this->toAddModels.end(),
- std::back_inserter(this->models));
- this->toAddModels.clear();
-
-
// Remove and delete all models that are marked for deletion
+ std::vector< Model* >::iterator miter;
for (miter=this->toDeleteModels.begin();
miter!=this->toDeleteModels.end(); miter++)
{
Modified: code/gazebo/trunk/server/World.hh
===================================================================
--- code/gazebo/trunk/server/World.hh 2009-08-25 18:58:06 UTC (rev 8224)
+++ code/gazebo/trunk/server/World.hh 2009-08-26 06:08:55 UTC (rev 8225)
@@ -176,8 +176,6 @@
/// \brief Get whether to view as wireframe
public: bool GetShowPhysics();
- public: void ProcessMessages();
-
/// Set to true to show bounding boxes
private: bool showBoundingBoxes;
@@ -202,7 +200,7 @@
private: void SetModelPose(Model *model , Pose3d pose);
/// \brief Update the simulation iface
- private: void UpdateSimulationIface();
+ public: void UpdateSimulationIface();
/// \brif Get the names of interfaces defined in the tree of a model
private: void GetInterfaceNames(Entity* m, std::vector<std::string>& list);
@@ -216,9 +214,6 @@
/// List of all the registered geometries
private: std::vector< Geom* > geometries;
- /// List of models to add into the world
- private: std::vector< Model* > toAddModels;
-
/// List of models to delete from the world
private: std::vector< Model* > toDeleteModels;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rt...@us...> - 2009-08-25 18:58:15
|
Revision: 8224
http://playerstage.svn.sourceforge.net/playerstage/?rev=8224&view=rev
Author: rtv
Date: 2009-08-25 18:58:06 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
better fix for removing update events on zero subscriptions
Modified Paths:
--------------
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/world.cc
code/stage/trunk/webstage/CMakeLists.txt
code/stage/trunk/webstage/webstage.cc
code/stage/trunk/worlds/benchmark/expand_swarm.cc
code/stage/trunk/worlds/benchmark/hospital.world
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-08-25 10:44:06 UTC (rev 8223)
+++ code/stage/trunk/libstage/model.cc 2009-08-25 18:58:06 UTC (rev 8224)
@@ -665,12 +665,9 @@
void Model::Update( void )
{
- CallCallbacks( &hooks.update );
-
- last_update = world->sim_time;
-
- if( subs > 0 )
- world->Enqueue( event_queue_num, World::Event::UPDATE, interval, this );
+ CallCallbacks( &hooks.update );
+ last_update = world->sim_time;
+ world->Enqueue( event_queue_num, World::Event::UPDATE, interval, this );
}
@@ -790,7 +787,6 @@
}
// set up the next event
- if( subs > 0 ) // TODO XX ?
world->Enqueue( 0, World::Event::ENERGY, interval_energy, this );
}
@@ -851,11 +847,8 @@
if( trail.size() > trail_length )
trail.pop_front();
}
-
- //if( ! velocity.IsZero() )
-
- if( subs > 0 )// TODO XX ?
- world->Enqueue( 0, World::Event::POSE, interval_pose, this );
+
+ world->Enqueue( 0, World::Event::POSE, interval_pose, this );
}
Model* Model::GetUnsubscribedModelOfType( const std::string& type ) const
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-08-25 10:44:06 UTC (rev 8223)
+++ code/stage/trunk/libstage/world.cc 2009-08-25 18:58:06 UTC (rev 8224)
@@ -477,12 +477,15 @@
{
// printf( "@ %llu next event <%s %llu %s>\n", sim_time, ev.TypeStr( ev.type ), ev.time, ev.mod->Token() );
queue.pop();
+
// only update events are allowed in queues other than zero
if( queue_num > 0 && ev.type != Event::UPDATE )
- PRINT_WARN1( "event type %d in async queue", queue_num );
+ PRINT_WARN1( "event type %d in async queue", queue_num );
- //process the event and move to the next
- ev.Execute();
+ if( ev.mod->subs > 0 ) // no subscriptions means the event is discarded
+ ev.Execute(); // simulate the event
+
+ // and move to the next
ev = queue.top();
}
}
@@ -536,14 +539,14 @@
// world callbacks
CallUpdateCallbacks();
-
- ++updates;
-
+
if( show_clock && ((this->updates % show_clock_interval) == 0) )
{
printf( "\r[Stage: %s]", ClockString().c_str() );
fflush( stdout );
}
+
+ ++updates;
return false;
}
Modified: code/stage/trunk/webstage/CMakeLists.txt
===================================================================
--- code/stage/trunk/webstage/CMakeLists.txt 2009-08-25 10:44:06 UTC (rev 8223)
+++ code/stage/trunk/webstage/CMakeLists.txt 2009-08-25 18:58:06 UTC (rev 8224)
@@ -1,19 +1,8 @@
MESSAGE( STATUS "Configuring webstage" )
-#include(FindPkgConfig)
-
-#pkg_search_module( LIBXML2 REQUIRED libxml-2.0)
-#IF( LIBXML2_FOUND )
-# MESSAGE( STATUS ${INDENT} "libxml version ${LIBXML2_VERSION} detected at ${LIBXML2_PREFIX}" )
-#ELSE( LIBXML2_FOUND )
-# MESSAGE( ${INDENT} "libxml2 not detected" )
-#ENDIF( LIBXML2_FOUND )
-
-#include_directories( ${LIBXML2_INCLUDE_DIRS} )
-
add_executable( webstage webstage.cc )
-target_link_libraries( webstage ${WEBSIM_LIBRARIES} stage )
+target_link_libraries( webstage ${WEBSIM_LIBRARIES} event stage )
INSTALL(TARGETS webstage
RUNTIME DESTINATION bin
Modified: code/stage/trunk/webstage/webstage.cc
===================================================================
--- code/stage/trunk/webstage/webstage.cc 2009-08-25 10:44:06 UTC (rev 8223)
+++ code/stage/trunk/webstage/webstage.cc 2009-08-25 18:58:06 UTC (rev 8224)
@@ -49,19 +49,29 @@
return true;
}
- virtual bool ClockRunFor( double usec )
+ virtual bool ClockRunFor( uint32_t msec )
{
puts( "[WebStage] Clock tick" );
-
+
world->paused = true;
- // when paused, the world will run while steps > 0, decrementing
- // steps each cycle.
- // TODO
- world->steps = 1;//(usec * 1e6) / world->GetSimInterval();
+ world->sim_interval = msec * 1e3; // usec
+ world->Update();
return true;
}
+ virtual bool GetModelType( const std::string& name, std::string& type )
+ {
+ Stg::Model* mod = world->GetModel( name.c_str() );
+
+ if( ! mod )
+ return false;
+
+ type = mod->GetModelType();
+ return true;
+ }
+
+
void Push( const std::string& name )
{
Stg::Model* mod = world->GetModel( name.c_str() );
@@ -193,7 +203,7 @@
Model*mod = world->GetModel( name.c_str() );
if(mod){
- stg_model_type_t type = mod->GetModelType();
+ std::string type = mod->GetModelType();
if(type == "position") {
websim::Pose p;
@@ -333,232 +343,249 @@
return true;
}
-virtual bool GetModelPVA(const std::string& name,
- websim::Time& t,
- websim::Pose& p,
- websim::Velocity& v,
- websim::Acceleration& a,
- std::string& error)
-{
- //printf( "get model name:%s\n", name.c_str() );
+ virtual bool GetModelPVA(const std::string& name,
+ websim::Time& t,
+ websim::Pose& p,
+ websim::Velocity& v,
+ websim::Acceleration& a,
+ std::string& error)
+ {
+ //printf( "get model name:%s\n", name.c_str() );
- t = GetTime();
+ t = GetTime();
- Model* mod = world->GetModel( name.c_str() );
- if( mod )
- {
- Stg::Pose sp = mod->GetPose();
- p.x = sp.x;
- p.y = sp.y;
- p.z = sp.z;
- p.a = sp.a;
+ Model* mod = world->GetModel( name.c_str() );
+ if( mod )
+ {
+ Stg::Pose sp = mod->GetPose();
+ p.x = sp.x;
+ p.y = sp.y;
+ p.z = sp.z;
+ p.a = sp.a;
- Stg::Velocity sv = mod->GetVelocity();
- v.x = sv.x;
- v.y = sv.y;
- v.z = sv.z;
- v.a = sv.a;
- }
- else
- printf( "Warning: attempt to set PVA for unrecognized model \"%s\"\n",
- name.c_str() );
+ Stg::Velocity sv = mod->GetVelocity();
+ v.x = sv.x;
+ v.y = sv.y;
+ v.z = sv.z;
+ v.a = sv.a;
+ }
+ else
+ printf( "Warning: attempt to set PVA for unrecognized model \"%s\"\n",
+ name.c_str() );
- return true;
-}
-/*
- virtual bool GetLaserData(const std::string& name,
- websim::Time& t,
- uint32_t& resolution,
- double& fov,
- websim::Pose& p,
- std::vector<double>& ranges,
- std::string& error,
- void* parent){
+ return true;
+ }
+ /*
+ virtual bool GetLaserData(const std::string& name,
+ websim::Time& t,
+ uint32_t& resolution,
+ double& fov,
+ websim::Pose& p,
+ std::vector<double>& ranges,
+ std::string& error,
+ void* parent){
- t = GetTime();
+ t = GetTime();
- Model* mod = world->GetModel( name.c_str() );
- if( mod )
- {
- ModelLaser* laser = (ModelLaser*)mod->GetModel("laser:0");
+ Model* mod = world->GetModel( name.c_str() );
+ if( mod )
+ {
+ ModelLaser* laser = (ModelLaser*)mod->GetModel("laser:0");
- if(laser){
- uint32_t sample_count=0;
- ModelLaser::Sample* scan = laser->GetSamples( &sample_count );
- assert(scan);
+ if(laser){
+ uint32_t sample_count=0;
+ ModelLaser::Sample* scan = laser->GetSamples( &sample_count );
+ assert(scan);
- ModelLaser::Config cfg = laser->GetConfig();
- resolution = cfg.resolution;
- fov = cfg.fov;
+ ModelLaser::Config cfg = laser->GetConfig();
+ resolution = cfg.resolution;
+ fov = cfg.fov;
- for(unsigned int i=0;i<sample_count;i++)
- ranges.push_back(scan[i].range);
- }else{
+ for(unsigned int i=0;i<sample_count;i++)
+ ranges.push_back(scan[i].range);
+ }else{
- printf( "Warning: attempt to get laser data for unrecognized laser model of model \"%s\"\n",
- name.c_str() );
- return false;
+ printf( "Warning: attempt to get laser data for unrecognized laser model of model \"%s\"\n",
+ name.c_str() );
+ return false;
- }
+ }
- }
- else{
- printf( "Warning: attempt to get laser data for unrecognized model \"%s\"\n",
- name.c_str() );
- return false;
- }
+ }
+ else{
+ printf( "Warning: attempt to get laser data for unrecognized model \"%s\"\n",
+ name.c_str() );
+ return false;
+ }
- return true;
+ return true;
- }
- virtual bool GetRangerData(const std::string& name,
- websim::Time& t,
- std::vector<websim::Pose>& p,
- std::vector<double>& ranges,
- std::string& response,
- xmlNode* parent){
- t = GetTime();
+ }
+ virtual bool GetRangerData(const std::string& name,
+ websim::Time& t,
+ std::vector<websim::Pose>& p,
+ std::vector<double>& ranges,
+ std::string& response,
+ xmlNode* parent){
+ t = GetTime();
- Model* mod = world->GetModel( name.c_str() );
- if( mod )
- {
- ModelRanger* ranger = (ModelRanger*)mod->GetModel("ranger:0");
+ Model* mod = world->GetModel( name.c_str() );
+ if( mod )
+ {
+ ModelRanger* ranger = (ModelRanger*)mod->GetModel("ranger:0");
- if(ranger){
- uint32_t count = ranger->sensors.size();
- for(unsigned int i=0;i<count;i++)
- ranges.push_back(ranger->sensors[i].range);
- //std::copy(ranger->samples,ranger->samples+ranger->sensor_count,ranges.begin());
+ if(ranger){
+ uint32_t count = ranger->sensors.size();
+ for(unsigned int i=0;i<count;i++)
+ ranges.push_back(ranger->sensors[i].range);
+ //std::copy(ranger->samples,ranger->samples+ranger->sensor_count,ranges.begin());
- for(unsigned int i=0;i<count;i++){
- websim::Pose pos;
- Pose rpos;
- rpos = ranger->sensors[i].pose;
- pos.x = rpos.x;
- pos.y = rpos.y;
- pos.z = rpos.z;
- pos.a = rpos.a;
- p.push_back(pos);
- }
+ for(unsigned int i=0;i<count;i++){
+ websim::Pose pos;
+ Pose rpos;
+ rpos = ranger->sensors[i].pose;
+ pos.x = rpos.x;
+ pos.y = rpos.y;
+ pos.z = rpos.z;
+ pos.a = rpos.a;
+ p.push_back(pos);
+ }
- }else{
+ }else{
- printf( "Warning: attempt to get ranger data for unrecognized ranger model of model \"%s\"\n",
- name.c_str() );
- return false;
- }
- }
- else{
- printf( "Warning: attempt to get ranger data for unrecognized model \"%s\"\n",
- name.c_str() );
- return false;
- }
+ printf( "Warning: attempt to get ranger data for unrecognized ranger model of model \"%s\"\n",
+ name.c_str() );
+ return false;
+ }
+ }
+ else{
+ printf( "Warning: attempt to get ranger data for unrecognized model \"%s\"\n",
+ name.c_str() );
+ return false;
+ }
- return true;
+ return true;
- }*/
+ }*/
-virtual bool GetModelGeometry(const std::string& name,
- double& x,
- double& y,
- double& z,
- websim::Pose& center,
- std::string& response)
-{
- if(name == "sim"){
+ virtual bool GetModelGeometry(const std::string& name,
+ double& x,
+ double& y,
+ double& z,
+ websim::Pose& center,
+ std::string& response)
+ {
+ if(name == "sim"){
- stg_bounds3d_t ext = world->GetExtent();
+ stg_bounds3d_t ext = world->GetExtent();
- x = ext.x.max - ext.x.min;
- y = ext.y.max - ext.y.min;
- z = ext.z.max - ext.z.min;
+ x = ext.x.max - ext.x.min;
+ y = ext.y.max - ext.y.min;
+ z = ext.z.max - ext.z.min;
- }
- else
- {
- Model* mod = world->GetModel(name.c_str());
- if(mod){
- Geom ext = mod->GetGeom();
+ }
+ else
+ {
+ Model* mod = world->GetModel(name.c_str());
+ if(mod){
+ Geom ext = mod->GetGeom();
- x = ext.size.x;
- y = ext.size.y;
- z = ext.size.z;
- center.x = ext.pose.x;
- center.y = ext.pose.y;
- center.a = ext.pose.a;
+ x = ext.size.x;
+ y = ext.size.y;
+ z = ext.size.z;
+ center.x = ext.pose.x;
+ center.y = ext.pose.y;
+ center.a = ext.pose.a;
+ }
+ else
+ {
+ printf("Warning: attemp to get the extent of unrecognized model \"%s\"\n", name.c_str());
+ return false;
+ }
}
- else
- {
- printf("Warning: attemp to get the extent of unrecognized model \"%s\"\n", name.c_str());
- return false;
- }
- }
- return true;
-}
+ return true;
+ }
-static int CountRobots(Model * mod, int* n ){
+ static int CountRobots(Model * mod, int* n ){
- if(n && mod->GetModelType() == MODEL_TYPE_POSITION)
- (*n)++;
+ if(n && mod->GetModelType() == "position")
+ (*n)++;
- return 0;
-}
+ return 0;
+ }
-virtual bool GetNumberOfRobots(unsigned int& n)
-{
+ virtual bool GetNumberOfRobots(unsigned int& n)
+ {
- world->ForEachDescendant((stg_model_callback_t)CountRobots, &n);
- return true;
+ world->ForEachDescendant((stg_model_callback_t)CountRobots, &n);
+ return true;
-}
-/*
- virtual bool GetModelTree()
- {
+ }
+ /*
+ virtual bool GetModelTree()
+ {
- // world->ForEachDescendant((stg_model_callback_t)printname, NULL);
+ // world->ForEachDescendant((stg_model_callback_t)printname, NULL);
- return true;
- }
-*/
-virtual bool GetSayStrings(std::vector<std::string>& sayings)
-{
- unsigned int n=0;
- this->GetNumberOfRobots(n);
+ return true;
+ }
+ */
+ virtual bool GetSayStrings(std::vector<std::string>& sayings)
+ {
+ unsigned int n=0;
+ this->GetNumberOfRobots(n);
- for(unsigned int i=0;i<n;i++){
- char temp[128];
- sprintf(temp,"position:%d",i);
- Model *mod = world->GetModel(temp);
- if(mod->GetSayString() != "")
- {
+ for(unsigned int i=0;i<n;i++){
+ char temp[128];
+ sprintf(temp,"position:%d",i);
+ Model *mod = world->GetModel(temp);
+ if(mod->GetSayString() != "")
+ {
- std::string str = temp;
- str += " says: \" ";
- str += mod->GetSayString();
- str += " \" ";
+ std::string str = temp;
+ str += " says: \" ";
+ str += mod->GetSayString();
+ str += " \" ";
- sayings.push_back(str);
+ sayings.push_back(str);
- }
+ }
+ }
+
+ return true;
}
- return true;
-}
+ virtual websim::Time GetTime()
+ {
+ stg_usec_t stgtime = world->SimTimeNow();
-virtual websim::Time GetTime()
-{
- stg_usec_t stgtime = world->SimTimeNow();
+ websim::Time t;
+ t.sec = stgtime / 1e6;
+ t.usec = stgtime - (t.sec * 1e6);
+ return t;
+ }
+
+ // add an FLTK event loop update to WebSim's implementation
+ virtual void Wait()
+ {
+ Fl::check();
+
+ while( unacknowledged_ticks || unacknowledged_pushes || ticks_remaining )
+ {
+ printf( "event loop in wait (%d %d %d)\n",
+ unacknowledged_ticks, unacknowledged_pushes, ticks_remaining );
- websim::Time t;
- t.sec = stgtime / 1e6;
- t.usec = stgtime - (t.sec * 1e6);
- return t;
-}
-
+ event_loop( EVLOOP_NONBLOCK );
+
+ puts( "fl::check\n" );
+ Fl::check();
+ }
+ }
+
};
@@ -636,20 +663,22 @@
// todo? check for changes?
// send my updates
ws.Push();
- //puts( "pushes done" );
+ //puts( "push done" );
- // tell my friends to start simulating
- ws.Go();
+ // run one step of the simulation
+ ws.Tick();
- // puts( "go done" );
+ //puts( "tick done" );
// update Stage
world->Update();
//puts( "update done" );
+
+ Fl::check();
- // wait for goes from all my friends
+ // wait until everyone report simulation step done
ws.Wait();
- //puts( "wait done" );
+ puts( "wait done" );
}
printf( "Webstage done.\n" );
Modified: code/stage/trunk/worlds/benchmark/expand_swarm.cc
===================================================================
--- code/stage/trunk/worlds/benchmark/expand_swarm.cc 2009-08-25 10:44:06 UTC (rev 8223)
+++ code/stage/trunk/worlds/benchmark/expand_swarm.cc 2009-08-25 18:58:06 UTC (rev 8224)
@@ -39,7 +39,8 @@
// subscribe to the ranger, which we use for navigating
robot->ranger = (ModelRanger*)mod->GetModel( "ranger:0" );
assert( robot->ranger );
-
+
+
// ask Stage to call into our ranger update function
robot->ranger->AddUpdateCallback( (stg_model_callback_t)RangerUpdate, robot );
Modified: code/stage/trunk/worlds/benchmark/hospital.world
===================================================================
--- code/stage/trunk/worlds/benchmark/hospital.world 2009-08-25 10:44:06 UTC (rev 8223)
+++ code/stage/trunk/worlds/benchmark/hospital.world 2009-08-25 18:58:06 UTC (rev 8224)
@@ -43,6 +43,7 @@
color "random"
ranger( pose [ 0 0 -0.050 0 ]
+ alwayson 1
scount 12
spose[0] [0 0 0]
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ro...@us...> - 2009-08-25 10:44:20
|
Revision: 8223
http://playerstage.svn.sourceforge.net/playerstage/?rev=8223&view=rev
Author: robotos
Date: 2009-08-25 10:44:06 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
max and min vel and angular_vel for the goto driver
Modified Paths:
--------------
code/player/trunk/server/drivers/position/goto/goto.cc
Modified: code/player/trunk/server/drivers/position/goto/goto.cc
===================================================================
--- code/player/trunk/server/drivers/position/goto/goto.cc 2009-08-24 07:39:06 UTC (rev 8222)
+++ code/player/trunk/server/drivers/position/goto/goto.cc 2009-08-25 10:44:06 UTC (rev 8223)
@@ -67,6 +67,18 @@
- send_everything (integer)
- Default: 1
- If set to 1, data and commands are sent at once
+- max_vel
+ - Default: 0.7
+ - Maximum speed forward
+- min_vel
+ - Default: 0.1
+ - Minimum speed forward
+- max_angular_vel
+ - Default: 45.0
+ - Maximum rotation speed
+- min_angular_vel
+ - Default: 10.0
+ - Minimum rotation speed
@par Example
@@ -105,7 +117,7 @@
#define AVMAX 45.0
#define MAXD 2.0
#define TVMIN 0.1
-#define TVMAX 0.7
+#define TVMAX 3
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -152,6 +164,10 @@
double dist_tol;
double angle_tol;
double max_dist;
+ double max_vel;
+ double min_vel;
+ double max_angular_vel;
+ double min_angular_vel;
int debug;
int reactive;
int forward_enabled;
@@ -283,6 +299,39 @@
this->forward_enabled = cf->ReadInt(section, "forward_enabled", 0);
this->early_check = cf->ReadInt(section, "early_check", 1);
this->send_everything = cf->ReadInt(section, "send_everything", 1);
+
+ this->max_vel = cf->ReadFloat(section, "max_vel", TVMAX);
+ if ((this->max_vel) < 0.0)
+ {
+ PLAYER_ERROR1("Invalid max_vel %.4f", this->max_vel);
+ this->SetError(-1);
+ return;
+ }
+
+ this->min_vel = cf->ReadFloat(section, "min_vel", TVMIN);
+ if ((this->min_vel) < 0.0)
+ {
+ PLAYER_ERROR1("Invalid min_vel %.4f", this->min_vel);
+ this->SetError(-1);
+ return;
+ }
+
+ this->max_angular_vel = cf->ReadFloat(section, "max_angular_vel", AVMAX);
+ if ((this->max_angular_vel) < 0.0)
+ {
+ PLAYER_ERROR1("Invalid max_angular_vel %.4f", this->max_angular_vel);
+ this->SetError(-1);
+ return;
+ }
+
+ this->min_angular_vel = cf->ReadFloat(section, "min_angular_vel", AVMIN);
+ if ((this->min_angular_vel) < 0.0)
+ {
+ PLAYER_ERROR1("Invalid min_angular_vel %.4f", this->min_angular_vel);
+ this->SetError(-1);
+ return;
+ }
+
}
Goto::~Goto() { }
@@ -474,9 +523,9 @@
tv = 0.0;
} else
{
- tv = (TVMIN) + (dist / ((M_SQRT2) * (MAXD))) * ((TVMAX) - (TVMIN));
+ tv = (this->min_vel) + (dist / ((M_SQRT2) * (MAXD))) * ((this->max_vel) - (this->min_vel));
}
- av = DTOR(AVMIN) + (fabs(ad) / (M_PI)) * (DTOR(AVMAX) - DTOR(AVMIN));
+ av = DTOR(this->min_angular_vel) + (fabs(ad) / (M_PI)) * (DTOR(this->max_angular_vel) - DTOR(this->min_angular_vel));
if (ad < 0.0) av = -av;
vel_cmd.vel.px = tv;
vel_cmd.vel.py = 0.0;
@@ -534,7 +583,7 @@
GlobalTime->GetTimeDouble(&(this->stall_start_time));
this->stall_length = (static_cast<double>(rand()) / static_cast<double>(RAND_MAX)) + 0.3;
this->stall_turn = ((static_cast<double>(rand()) / static_cast<double>(RAND_MAX)) * 4.0) - 2.0;
- vel_cmd.vel.px = -(TVMAX);
+ vel_cmd.vel.px = -(this->max_vel);
vel_cmd.vel.py = 0.0;
vel_cmd.vel.pa = this->stall_turn;
this->stall_state++;
@@ -545,13 +594,13 @@
if ((t - (this->stall_start_time)) >= (this->stall_length))
{
this->stall_start_time = t;
- vel_cmd.vel.px = ((TVMAX) * 0.65);
+ vel_cmd.vel.px = ((this->max_vel) * 0.65);
vel_cmd.vel.py = 0.0;
vel_cmd.vel.pa = 0.0;
this->stall_state++;
} else
{
- vel_cmd.vel.px = -(TVMAX);
+ vel_cmd.vel.px = -(this->max_vel);
vel_cmd.vel.py = 0.0;
vel_cmd.vel.pa = this->stall_turn;
}
@@ -567,7 +616,7 @@
this->stall_state++;
} else
{
- vel_cmd.vel.px = ((TVMAX) * 0.65);
+ vel_cmd.vel.px = ((this->max_vel) * 0.65);
vel_cmd.vel.py = 0.0;
vel_cmd.vel.pa = 0.0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ro...@us...> - 2009-08-24 07:39:14
|
Revision: 8222
http://playerstage.svn.sourceforge.net/playerstage/?rev=8222&view=rev
Author: robotos
Date: 2009-08-24 07:39:06 +0000 (Mon, 24 Aug 2009)
Log Message:
-----------
raygeom should not create lines if not to be rendered
Modified Paths:
--------------
code/gazebo/trunk/server/physics/RayGeom.cc
Modified: code/gazebo/trunk/server/physics/RayGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/RayGeom.cc 2009-08-23 18:54:14 UTC (rev 8221)
+++ code/gazebo/trunk/server/physics/RayGeom.cc 2009-08-24 07:39:06 UTC (rev 8222)
@@ -49,7 +49,7 @@
// Create default ray with unit length
this->SetGeom( dCreateRay( this->spaceId, 1.0 ), false );
- if (displayRays)
+ if ( displayRays && Simulator::Instance()->GetRenderEngineEnabled() )
{
this->line = OgreCreator::Instance()->CreateDynamicLine(OgreDynamicRenderable::OT_LINE_LIST);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-08-23 18:54:24
|
Revision: 8221
http://playerstage.svn.sourceforge.net/playerstage/?rev=8221&view=rev
Author: gerkey
Date: 2009-08-23 18:54:14 +0000 (Sun, 23 Aug 2009)
Log Message:
-----------
Commented out references to missing source files
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/CMakeLists.txt
Modified: code/stage/trunk/examples/ctrl/CMakeLists.txt
===================================================================
--- code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-08-23 18:50:39 UTC (rev 8220)
+++ code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-08-23 18:54:14 UTC (rev 8221)
@@ -1,8 +1,8 @@
SET( PLUGINS
- convoy
+ # convoy
fasr
- fasr2
+ # fasr2
lasernoise
sink
source
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-08-23 18:50:51
|
Revision: 8220
http://playerstage.svn.sourceforge.net/playerstage/?rev=8220&view=rev
Author: gerkey
Date: 2009-08-23 18:50:39 +0000 (Sun, 23 Aug 2009)
Log Message:
-----------
Added missing dependency for swig
Modified Paths:
--------------
code/player/trunk/cmake/internal/UseSwig_Player.cmake
Modified: code/player/trunk/cmake/internal/UseSwig_Player.cmake
===================================================================
--- code/player/trunk/cmake/internal/UseSwig_Player.cmake 2009-08-23 09:22:35 UTC (rev 8219)
+++ code/player/trunk/cmake/internal/UseSwig_Player.cmake 2009-08-23 18:50:39 UTC (rev 8220)
@@ -126,7 +126,7 @@
-o "${swig_generated_file_fullname}"
"${infile}"
MAIN_DEPENDENCY "${swig_source_file_fullname}"
- DEPENDS ${SWIG_MODULE_${name}_EXTRA_DEPS}
+ DEPENDS ${SWIG_MODULE_${name}_EXTRA_DEPS} ${infile}
COMMENT "Swig source")
SET_SOURCE_FILES_PROPERTIES("${swig_generated_file_fullname}" ${swig_extra_generated_files}
PROPERTIES GENERATED 1)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ro...@us...> - 2009-08-23 09:22:42
|
Revision: 8219
http://playerstage.svn.sourceforge.net/playerstage/?rev=8219&view=rev
Author: robotos
Date: 2009-08-23 09:22:35 +0000 (Sun, 23 Aug 2009)
Log Message:
-----------
actually setting the fiducial and retro info model-wise
Modified Paths:
--------------
code/gazebo/trunk/server/Model.cc
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-08-22 18:01:15 UTC (rev 8218)
+++ code/gazebo/trunk/server/Model.cc 2009-08-23 09:22:35 UTC (rev 8219)
@@ -257,6 +257,12 @@
if (**this->staticP == false)
this->SetGravityMode( **this->enableGravityP );
+
+ //global fiducial and retro id
+ this->SetLaserFiducialId(**this->laserFiducialP);
+ this->SetLaserRetro(**this->laserRetroP);
+
+
// Create the graphics iface handler
this->graphicsHandler = new GraphicsIfaceHandler();
this->graphicsHandler->Load(this->GetScopedName(), this);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rt...@us...> - 2009-08-22 01:38:32
|
Revision: 8216
http://playerstage.svn.sourceforge.net/playerstage/?rev=8216&view=rev
Author: rtv
Date: 2009-08-22 01:38:22 +0000 (Sat, 22 Aug 2009)
Log Message:
-----------
fixed event queuing bug when unsubscribing & subscribing
Modified Paths:
--------------
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_fiducial.cc
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-08-21 22:35:30 UTC (rev 8215)
+++ code/stage/trunk/libstage/model.cc 2009-08-22 01:38:22 UTC (rev 8216)
@@ -790,6 +790,7 @@
}
// set up the next event
+ if( subs > 0 ) // TODO XX ?
world->Enqueue( 0, World::Event::ENERGY, interval_energy, this );
}
@@ -852,7 +853,9 @@
}
//if( ! velocity.IsZero() )
- world->Enqueue( 0, World::Event::POSE, interval_pose, this );
+
+ if( subs > 0 )// TODO XX ?
+ world->Enqueue( 0, World::Event::POSE, interval_pose, this );
}
Model* Model::GetUnsubscribedModelOfType( const std::string& type ) const
Modified: code/stage/trunk/libstage/model_fiducial.cc
===================================================================
--- code/stage/trunk/libstage/model_fiducial.cc 2009-08-21 22:35:30 UTC (rev 8215)
+++ code/stage/trunk/libstage/model_fiducial.cc 2009-08-22 01:38:22 UTC (rev 8216)
@@ -18,12 +18,6 @@
#include "worldfile.hh"
using namespace Stg;
-static const stg_meters_t DEFAULT_RANGEMIN = 0.0;
-static const stg_meters_t DEFAULT_RANGEMAXID = 5.0;
-static const stg_meters_t DEFAULT_RANGEMAXANON = 8.0;
-static const stg_radians_t DEFAULT_FOV = M_PI;
-static const stg_watts_t DEFAULT_WATTS = 10.0;
-
//TODO make instance attempt to register an option (as customvisualizations do)
Option ModelFiducial::showData( "Fiducials", "show_fiducial", "", true, NULL );
Option ModelFiducial::showFov( "Fiducial FOV", "show_fiducial_fov", "", false, NULL );
@@ -72,10 +66,10 @@
const std::string& type ) :
Model( world, parent, type ),
fiducials(),
- max_range_anon( DEFAULT_RANGEMAXANON ),
- max_range_id( DEFAULT_RANGEMAXID ),
- min_range( DEFAULT_RANGEMIN ),
- fov( DEFAULT_FOV ),
+ max_range_anon( 8.0 ),
+ max_range_id( 5.0 ),
+ min_range( 0.0 ),
+ fov( M_PI ),
heading( 0 ),
key( 0 )
{
@@ -178,32 +172,36 @@
// him->Token() );
// if it was him, we can see him
- if( hitmod == him )
- {
- Geom hisgeom( him->GetGeom() );
-
- // record where we saw him and what he looked like
- Fiducial fid;
- fid.mod = him;
- fid.range = range;
- fid.bearing = dtheta;
- fid.geom.x = hisgeom.size.x;
- fid.geom.y = hisgeom.size.y;
- fid.geom.a = normalize( hispose.a - mypose.a);
-
- // store the global pose of the fiducial (mainly for the GUI)
- fid.pose = hispose;
+ if( hitmod != him )
+ return;
- // if he's within ID range, get his fiducial.return value, else
- // we see value 0
- fid.id = range < max_range_id ? hitmod->vis.fiducial_return : 0;
-
- PRINT_DEBUG2( "adding %s's value %d to my list of fiducials",
- him->Token(), him->vis.fiducial_return );
-
- fiducials.push_back( fid );
- }
-}
+ assert( range >= 0 );
+
+ // passed all the tests! record the fiducial hit
+
+ Geom hisgeom( him->GetGeom() );
+
+ // record where we saw him and what he looked like
+ Fiducial fid;
+ fid.mod = him;
+ fid.range = range;
+ fid.bearing = dtheta;
+ fid.geom.x = hisgeom.size.x;
+ fid.geom.y = hisgeom.size.y;
+ fid.geom.a = normalize( hispose.a - mypose.a);
+
+ // store the global pose of the fiducial (mainly for the GUI)
+ fid.pose = hispose;
+
+ // if he's within ID range, get his fiducial.return value, else
+ // we see value 0
+ fid.id = range < max_range_id ? hitmod->vis.fiducial_return : 0;
+
+ PRINT_DEBUG2( "adding %s's value %d to my list of fiducials",
+ him->Token(), him->vis.fiducial_return );
+
+ fiducials.push_back( fid );
+}
///////////////////////////////////////////////////////////////////////////
// Update the beacon data
@@ -220,10 +218,8 @@
// reset the array of detected fiducials
fiducials.clear();
- for( std::set<Model*>::iterator it( world->models_with_fiducials.begin() );
- it != world->models_with_fiducials.end();
- ++it )
- AddModelIfVisible( *it );
+ FOR_EACH( it, world->models_with_fiducials )
+ AddModelIfVisible( *it );
}
void ModelFiducial::Load( void )
@@ -272,9 +268,9 @@
glLineStipple( 1, 0x00FF );
// draw lines to the fiducials
- for( unsigned int f=0; f<fiducials.size(); f++ )
+ FOR_EACH( it, fiducials )
{
- Fiducial& fid = fiducials[f];
+ Fiducial& fid = *it;
double dx = fid.range * cos( fid.bearing);
double dy = fid.range * sin( fid.bearing);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rt...@us...> - 2009-08-21 22:35:39
|
Revision: 8215
http://playerstage.svn.sourceforge.net/playerstage/?rev=8215&view=rev
Author: rtv
Date: 2009-08-21 22:35:30 +0000 (Fri, 21 Aug 2009)
Log Message:
-----------
disabled visualization drawing for unsubscribed models
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/CMakeLists.txt
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_draw.cc
code/stage/trunk/libstage/model_load.cc
code/stage/trunk/libstage/stage.hh
Modified: code/stage/trunk/examples/ctrl/CMakeLists.txt
===================================================================
--- code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-08-21 03:48:00 UTC (rev 8214)
+++ code/stage/trunk/examples/ctrl/CMakeLists.txt 2009-08-21 22:35:30 UTC (rev 8215)
@@ -1,6 +1,8 @@
SET( PLUGINS
+ convoy
fasr
+ fasr2
lasernoise
sink
source
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-08-21 03:48:00 UTC (rev 8214)
+++ code/stage/trunk/libstage/model.cc 2009-08-21 22:35:30 UTC (rev 8215)
@@ -657,6 +657,9 @@
{
//printf( "Shutdown model %s\n", this->token );
CallCallbacks( &hooks.shutdown );
+
+ // allows data visualizations to be cleared.
+ NeedRedraw();
}
@@ -760,7 +763,7 @@
FOR_EACH( it, touchers )
{
- Model* toucher = (*it); //(Model*)touchers->data;
+ Model* toucher = (*it);
PowerPack* hispp =toucher->FindPowerPack();
if( hispp && toucher->watts_take > 0.0)
Modified: code/stage/trunk/libstage/model_draw.cc
===================================================================
--- code/stage/trunk/libstage/model_draw.cc 2009-08-21 03:48:00 UTC (rev 8214)
+++ code/stage/trunk/libstage/model_draw.cc 2009-08-21 22:35:30 UTC (rev 8215)
@@ -525,17 +525,19 @@
void Model::DataVisualizeTree( Camera* cam )
{
PushLocalCoords();
- DataVisualize( cam ); // virtual function overridden by most model types
- for( std::list<Visualizer*>::iterator it = cv_list.begin();
- it != cv_list.end();
- it++ )
+ if( subs > 0 )
{
- Visualizer* vis = *it;
- if( world_gui->GetCanvas()->_custom_options[ vis->GetMenuName() ]->isEnabled() )
- vis->Visualize( this, cam );
+ DataVisualize( cam ); // virtual function overridden by some model types
+
+ FOR_EACH( it, cv_list )
+ {
+ Visualizer* vis = *it;
+ if( world_gui->GetCanvas()->_custom_options[ vis->GetMenuName() ]->isEnabled() )
+ vis->Visualize( this, cam );
+ }
}
-
+
// and draw the children
FOR_EACH( it, children )
(*it)->DataVisualizeTree( cam );
Modified: code/stage/trunk/libstage/model_load.cc
===================================================================
--- code/stage/trunk/libstage/model_load.cc 2009-08-21 03:48:00 UTC (rev 8214)
+++ code/stage/trunk/libstage/model_load.cc 2009-08-21 22:35:30 UTC (rev 8215)
@@ -145,6 +145,16 @@
}
}
+ if( wf->ReadInt( wf_entity, "noblocks", 0 ) )
+ {
+ if( has_default_block )
+ {
+ blockgroup.Clear();
+ has_default_block = false;
+ blockgroup.CalcSize();
+ }
+ }
+
if( wf->PropertyExists( wf_entity, "bitmap" ) )
{
const char* bitmapfile = wf->ReadString( wf_entity, "bitmap", NULL );
@@ -162,21 +172,21 @@
if( wf->PropertyExists( wf_entity, "boundary" ))
{
this->SetBoundary( wf->ReadInt(wf_entity, "boundary", this->boundary ));
-
+
if( boundary )
- {
- //PRINT_WARN1( "setting boundary for %s\n", token );
+ {
+ //PRINT_WARN1( "setting boundary for %s\n", token );
- blockgroup.CalcSize();
-
- double epsilon = 0.01;
- Size bgsize = blockgroup.GetSize();
-
- AddBlockRect(blockgroup.minx,blockgroup.miny, epsilon, bgsize.y, bgsize.z );
- AddBlockRect(blockgroup.minx,blockgroup.miny, bgsize.x, epsilon, bgsize.z );
- AddBlockRect(blockgroup.minx,blockgroup.maxy-epsilon, bgsize.x, epsilon, bgsize.z );
- AddBlockRect(blockgroup.maxx-epsilon,blockgroup.miny, epsilon, bgsize.y, bgsize.z );
- }
+ blockgroup.CalcSize();
+
+ double epsilon = 0.01;
+ Size bgsize = blockgroup.GetSize();
+
+ AddBlockRect(blockgroup.minx,blockgroup.miny, epsilon, bgsize.y, bgsize.z );
+ AddBlockRect(blockgroup.minx,blockgroup.miny, bgsize.x, epsilon, bgsize.z );
+ AddBlockRect(blockgroup.minx,blockgroup.maxy-epsilon, bgsize.x, epsilon, bgsize.z );
+ AddBlockRect(blockgroup.maxx-epsilon,blockgroup.miny, epsilon, bgsize.y, bgsize.z );
+ }
}
if( wf->PropertyExists( wf_entity, "mass" ))
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-08-21 03:48:00 UTC (rev 8214)
+++ code/stage/trunk/libstage/stage.hh 2009-08-21 22:35:30 UTC (rev 8215)
@@ -2326,11 +2326,17 @@
and has the type indicated by the string. This model is tagged as used. */
Model* GetUnusedModelOfType( const std::string& type );
-
/** Returns the value of the model's stall boolean, which is true
iff the model has crashed into another model */
bool Stalled() const { return this->stall; }
+ /** Returns the current number of subscriptions. If alwayson, this
+ is never less than 1.*/
+ unsigned int GetSubscriptionCount() const { return subs; }
+
+ /** Returns true if the model has one or more subscribers, else false. */
+ bool HasSubscribers() const { return( subs > 0 ); }
+
static std::map< std::string, creator_t> name_map;
};
@@ -2433,8 +2439,7 @@
stg_usec_t interval; ///< Time interval between updates (TODO: is this used?)
};
- private:
-
+ private:
class Vis : public Visualizer
{
private:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
2441 messages has been excluded from this view by a project administrator.
