Browse files

cleaning up

  • Loading branch information...
1 parent c69a1c9 commit 1ce2551ba2c627ee0e5d2701663be69c1c6ec594 rtv committed Aug 15, 2009
Showing with 1,609 additions and 1,642 deletions.
  1. +12 −13 libstage/block.cc
  2. +0 −2 libstage/blockgroup.cc
  3. +1 −1 libstage/canvas.cc
  4. +5 −6 libstage/model.cc
  5. +76 −86 libstage/model_draw.cc
  6. +3 −3 libstage/model_getset.cc
  7. +3 −3 libstage/model_laser.cc
  8. +1,409 −1,415 libstage/stage.hh
  9. +72 −101 libstage/world.cc
  10. +25 −9 libstage/worldgui.cc
  11. +1 −1 worlds/everything.world
  12. +2 −2 worlds/map.inc
View
25 libstage/block.cc
@@ -8,13 +8,12 @@ using namespace Stg;
blocks. The point data is copied, so pts can safely be freed
after calling this.*/
Block::Block( Model* mod,
- stg_point_t* pts,
- size_t pt_count,
- stg_meters_t zmin,
- stg_meters_t zmax,
- Color color,
- bool inherit_color
- ) :
+ stg_point_t* pts,
+ size_t pt_count,
+ stg_meters_t zmin,
+ stg_meters_t zmax,
+ Color color,
+ bool inherit_color ) :
mod( mod ),
mpts(),
pt_count( pt_count ),
@@ -33,21 +32,21 @@ Block::Block( Model* mod,
// copy the argument point data into the member vector
this->pts.reserve( pt_count );
for( size_t p=0; p<pt_count; p++ )
- this->pts.push_back( pts[p] );
+ this->pts.push_back( pts[p] );
}
/** A from-file constructor */
Block::Block( Model* mod,
- Worldfile* wf,
- int entity)
+ Worldfile* wf,
+ int entity)
: mod( mod ),
- mpts(),
+ mpts(),
pt_count(0),
pts(),
color(0),
inherit_color(true),
- rendered_cells( new CellPtrVec ),
- candidate_cells( new CellPtrVec )
+ rendered_cells( new CellPtrVec ),
+ candidate_cells( new CellPtrVec )
{
assert(mod);
assert(wf);
View
2 libstage/blockgroup.cc
@@ -154,7 +154,6 @@ void BlockGroup::BuildDisplayList( Model* mod )
return;
//printf( "display list for model %s\n", mod->token );
-
if( displaylist == 0 )
{
displaylist = glGenLists(1);
@@ -163,7 +162,6 @@ void BlockGroup::BuildDisplayList( Model* mod )
glNewList( displaylist, GL_COMPILE );
-
Geom geom = mod->GetGeom();
Gl::pose_shift( geom.pose );
View
2 libstage/canvas.cc
@@ -228,7 +228,7 @@ Model* Canvas::getModel( int x, int y )
{
Model* mod = (*it);
- if( mod->gui.mask & (STG_MOVE_TRANS | STG_MOVE_ROT ))
+ if( mod->gui.move )
{
uint8_t rByte, gByte, bByte, aByte;
uint32_t modelId = mod->id;
View
11 libstage/model.cc
@@ -172,7 +172,7 @@ void Visibility::Load( Worldfile* wf, int wf_entity )
GuiState:: GuiState() :
grid( false ),
- mask( 0 ),
+ move( false ),
nose( false ),
outline( false )
{ /* nothing to do */}
@@ -182,14 +182,13 @@ void GuiState::Load( Worldfile* wf, int wf_entity )
nose = wf->ReadInt( wf_entity, "gui_nose", nose);
grid = wf->ReadInt( wf_entity, "gui_grid", grid);
outline = wf->ReadInt( wf_entity, "gui_outline", outline);
- mask = wf->ReadInt( wf_entity, "gui_movemask", mask);
+ move = wf->ReadInt( wf_entity, "gui_move", move );
}
-
// constructor
Model::Model( World* world,
- Model* parent,
- const stg_model_type_t type )
+ Model* parent,
+ const stg_model_type_t type )
: Ancestor(),
blockgroup(),
blocks_dl(0),
@@ -254,7 +253,7 @@ Model::Model( World* world,
{
world->AddChild( this );
// top level models are draggable in the GUI
- gui.mask = (STG_MOVE_TRANS | STG_MOVE_ROT);
+ gui.move = true;
}
world->AddModel( this );
View
162 libstage/model_draw.cc
@@ -267,10 +267,9 @@ void Model::AddVisualizer( Visualizer* cv, bool on_by_default )
void Model::RemoveVisualizer( Visualizer* cv )
{
if( cv )
- cv_list.erase( remove( cv_list.begin(), cv_list.end(), cv ));
-
-
- //TODO unregister option - tricky because there might still be instances attached to different models which have the same name
+ cv_list.erase( remove( cv_list.begin(), cv_list.end(), cv ));
+
+ //TODO unregister option - tricky because there might still be instances attached to different models which have the same name
}
@@ -285,107 +284,98 @@ void Model::DrawStatusTree( Camera* cam )
void Model::DrawStatus( Camera* cam )
{
- // quick hack
-// if( power_pack && power_pack->stored < 0.0 )
-// {
-// glPushMatrix();
-// glTranslatef( 0.3, 0, 0.0 );
-// DrawImage( TextureManager::getInstance()._mains_texture_id, cam, 0.85 );
-// glPopMatrix();
-// }
-
if( say_string || power_pack )
{
float yaw, pitch;
pitch = - cam->pitch();
yaw = - cam->yaw();
- Pose gpz = GetGlobalPose();
-
+ Pose gpz = GetGlobalPose();
+
float robotAngle = -rtod(gpz.a);
glPushMatrix();
-
+
// move above the robot
glTranslatef( 0, 0, 0.5 );
-
+
// rotate to face screen
glRotatef( robotAngle - yaw, 0,0,1 );
glRotatef( -pitch, 1,0,0 );
-
- if( power_pack )
- power_pack->Visualize( cam );
-
- if( say_string )
- {
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
-
- //get raster positition, add gl_width, then project back to world coords
- glRasterPos3f( 0, 0, 0 );
- GLfloat pos[ 4 ];
- glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
-
- GLboolean valid;
- glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid );
-
- if( valid )
- {
- //fl_font( FL_HELVETICA, 12 );
- float w = gl_width( this->say_string ); // scaled text width
- float h = gl_height(); // scaled text height
-
- GLdouble wx, wy, wz;
- GLint viewport[4];
- glGetIntegerv(GL_VIEWPORT, viewport);
-
- GLdouble modelview[16];
- glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
-
- GLdouble projection[16];
- glGetDoublev(GL_PROJECTION_MATRIX, projection);
-
- //get width and height in world coords
- gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz );
- w = wx;
- gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz );
- h = wy;
-
- // calculate speech bubble margin
- const float m = h/10;
-
- // draw inside of bubble
- PushColor( BUBBLE_FILL );
- glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT );
- glPolygonMode( GL_FRONT, GL_FILL );
- glEnable( GL_POLYGON_OFFSET_FILL );
- glPolygonOffset( 1.0, 1.0 );
- Gl::draw_octagon( w, h, m );
- glDisable( GL_POLYGON_OFFSET_FILL );
- PopColor();
-
- // draw outline of bubble
- PushColor( BUBBLE_BORDER );
- glLineWidth( 1 );
- glEnable( GL_LINE_SMOOTH );
- glPolygonMode( GL_FRONT, GL_LINE );
- Gl::draw_octagon( w, h, m );
- glPopAttrib();
- PopColor();
-
- PushColor( BUBBLE_TEXT );
- // draw text inside the bubble
- Gl::draw_string( m, 2.5*m, 0, this->say_string );
- PopColor();
- }
- }
- glPopMatrix();
- }
+
+ if( power_pack )
+ power_pack->Visualize( cam );
+
+ if( say_string )
+ {
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+
+ //get raster positition, add gl_width, then project back to world coords
+ glRasterPos3f( 0, 0, 0 );
+ GLfloat pos[ 4 ];
+ glGetFloatv(GL_CURRENT_RASTER_POSITION, pos);
+
+ GLboolean valid;
+ glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid );
+
+ if( valid )
+ {
+ //fl_font( FL_HELVETICA, 12 );
+ float w = gl_width( this->say_string ); // scaled text width
+ float h = gl_height(); // scaled text height
+
+ GLdouble wx, wy, wz;
+ GLint viewport[4];
+ glGetIntegerv(GL_VIEWPORT, viewport);
+
+ GLdouble modelview[16];
+ glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
+
+ GLdouble projection[16];
+ glGetDoublev(GL_PROJECTION_MATRIX, projection);
+
+ //get width and height in world coords
+ gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz );
+ w = wx;
+ gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz );
+ h = wy;
+
+ // calculate speech bubble margin
+ const float m = h/10;
+
+ // draw inside of bubble
+ PushColor( BUBBLE_FILL );
+ glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT );
+ glPolygonMode( GL_FRONT, GL_FILL );
+ glEnable( GL_POLYGON_OFFSET_FILL );
+ glPolygonOffset( 1.0, 1.0 );
+ Gl::draw_octagon( w, h, m );
+ glDisable( GL_POLYGON_OFFSET_FILL );
+ PopColor();
+
+ // draw outline of bubble
+ PushColor( BUBBLE_BORDER );
+ glLineWidth( 1 );
+ glEnable( GL_LINE_SMOOTH );
+ glPolygonMode( GL_FRONT, GL_LINE );
+ Gl::draw_octagon( w, h, m );
+ glPopAttrib();
+ PopColor();
+
+ PushColor( BUBBLE_TEXT );
+ // draw text inside the bubble
+ Gl::draw_string( m, 2.5*m, 0, this->say_string );
+ PopColor();
+ }
+ }
+ glPopMatrix();
+ }
if( stall )
{
DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 );
}
-// extern GLuint glowTex;
+ // extern GLuint glowTex;
// extern GLuint checkTex;
// if( parent == NULL )
View
6 libstage/model_getset.cc
@@ -98,10 +98,10 @@ void Model::SetGuiNose( int val )
CallCallbacks( &gui.nose );
}
-void Model::SetGuiMask( int val )
+void Model::SetGuiMove( int val )
{
- gui.mask = val;
- CallCallbacks( &gui.mask );
+ gui.move = val;
+ CallCallbacks( &gui.move );
}
void Model::SetGuiGrid( int val )
View
6 libstage/model_laser.cc
@@ -71,11 +71,11 @@ Option ModelLaser::Vis::showBeams( "Laser beams", "show_laser_beams", "", false,
Only calculate the true range of every nth laser sample. The missing samples are filled in with a linear interpolation. Generally it would be better to use fewer samples, but some (poorly implemented!) programs expect a fixed number of samples. Setting this number > 1 allows you to reduce the amount of computation required for your fixed-size laser vector.
*/
-
+
ModelLaser::ModelLaser( World* world,
- Model* parent )
+ Model* parent )
: Model( world, parent, MODEL_TYPE_LASER ),
- vis( world ),
+ vis( world ),
sample_count( DEFAULT_SAMPLES ),
samples(),
range_max( DEFAULT_MAXRANGE ),
View
2,824 libstage/stage.hh
1,409 additions, 1,415 deletions not shown because the diff is too large. Please use a local Git client to view these changes.
View
173 libstage/world.cc
@@ -106,8 +106,6 @@ World::World( const char* token,
models_with_fiducials(),
ppm( ppm ), // raytrace resolution
quit( false ),
- real_time_now( RealTimeNow() ),
- real_time_start( real_time_now ),
show_clock( false ),
show_clock_interval( 100 ), // 10 simulated seconds using defaults
thread_mutex(),
@@ -302,8 +300,6 @@ void World::Load( const char* worldfile_path )
printf( " [Loading %s]", worldfile_path );
fflush(stdout);
- stg_usec_t load_start_time = RealTimeNow();
-
this->wf = new Worldfile();
wf->Load( worldfile_path );
PRINT_DEBUG1( "wf has %d entitys", wf->GetEntityCount() );
@@ -312,80 +308,74 @@ void World::Load( const char* worldfile_path )
//puts("");
int entity = 0;
-
+
this->token = (char*)
- wf->ReadString( entity, "name", token );
-
- this->quit_time = (stg_usec_t)( million *
- wf->ReadFloat( entity, "quit_time", 0 ) );
+ wf->ReadString( entity, "name", this->token );
+
+ this->quit_time =
+ (stg_usec_t)( million * wf->ReadFloat( entity, "quit_time", this->quit_time ) );
this->ppm =
1.0 / wf->ReadFloat( entity, "resolution", 1.0 / this->ppm );
this->show_clock =
- wf->ReadInt( entity, "show_clock", this->show_clock );
+ wf->ReadInt( entity, "show_clock", this->show_clock );
this->show_clock_interval =
- wf->ReadInt( entity, "show_clock_interval", this->show_clock_interval );
+ wf->ReadInt( entity, "show_clock_interval", this->show_clock_interval );
// read msec instead of usec: easier for user
this->sim_interval =
- 1e3 * wf->ReadFloat( entity, "interval_sim", this->sim_interval / 1e3 );
+ 1e3 * wf->ReadFloat( entity, "interval_sim", this->sim_interval / 1e3 );
this->worker_threads = wf->ReadInt( entity, "threads", this->worker_threads );
if( worker_threads > 0 )
{
PRINT_WARN( "\nmulti-thread support is experimental and may not work properly, if at all." );
-
+
event_queues.resize( worker_threads + 1 );
-
+
// kick off the threads
for( unsigned int t=0; t<worker_threads; t++ )
- {
- std::pair<World*,int> *p = new std::pair<World*,int>( this, t+1 );
-
- //normal posix pthread C function pointer
- typedef void* (*func_ptr) (void*);
-
- pthread_t pt;
- pthread_create( &pt,
- NULL,
- (func_ptr)World::update_thread_entry,
- p );
- }
-
+ {
+ std::pair<World*,int> *p = new std::pair<World*,int>( this, t+1 );
+
+ //normal posix pthread C function pointer
+ typedef void* (*func_ptr) (void*);
+
+ pthread_t pt;
+ pthread_create( &pt,
+ NULL,
+ (func_ptr)World::update_thread_entry,
+ p );
+ }
+
printf( "[threads %u]", worker_threads );
}
-
+
// Iterate through entitys and create objects of the appropriate type
for( int entity = 1; entity < wf->GetEntityCount(); entity++ )
{
const char *typestr = (char*)wf->GetEntityType(entity);
-
+
// don't load window entries here
if( strcmp( typestr, "window" ) == 0 )
- {
- /* do nothing here */
- }
+ {
+ /* do nothing here */
+ }
else if( strcmp( typestr, "block" ) == 0 )
- LoadBlock( wf, entity );
+ LoadBlock( wf, entity );
else
- LoadModel( wf, entity );
+ LoadModel( wf, entity );
}
-
+
// warn about unused WF lines
wf->WarnUnused();
-
+
FOR_EACH( it, children )
(*it)->InitRecursive();
-
- stg_usec_t load_end_time = RealTimeNow();
-
- if( debug )
- printf( "[Load time %.3fsec]\n",
- (load_end_time - load_start_time) / 1e6 );
- else
- putchar( '\n' );
+
+ putchar( '\n' );
}
void World::UnLoad()
@@ -406,31 +396,12 @@ void World::UnLoad()
token = NULL;
}
-// cant inline a symbol that is used externally
-stg_usec_t World::RealTimeNow()
-{
- struct timeval tv;
- gettimeofday( &tv, NULL ); // slow system call: use sparingly
- return( tv.tv_sec*1000000 + tv.tv_usec );
-}
-
-inline stg_usec_t World::RealTimeSinceStart()
-{
- stg_usec_t timenow = RealTimeNow();
-
- // subtract the start time from the current time to get the elapsed
- // time
-
- return timenow - real_time_start;
-}
-
-
bool World::PastQuitTime()
{
return( (quit_time > 0) && (sim_time >= quit_time) );
}
-std::string World::ClockString()
+std::string World::ClockString() const
{
const uint32_t usec_per_hour = 3600000000U;
const uint32_t usec_per_minute = 60000000U;
@@ -458,25 +429,25 @@ std::string World::ClockString()
}
void World::AddUpdateCallback( stg_world_callback_t cb,
- void* user )
+ void* user )
{
// add the callback & argument to the list
std::pair<stg_world_callback_t,void*> p(cb, user);
cb_list.push_back( p );
}
int World::RemoveUpdateCallback( stg_world_callback_t cb,
- void* user )
+ void* user )
{
std::pair<stg_world_callback_t,void*> p( cb, user );
FOR_EACH( it, cb_list )
{
if( (*it) == p )
- {
- cb_list.erase( it );
- break;
- }
+ {
+ cb_list.erase( it );
+ break;
+ }
}
// return the number of callbacks now in the list. Useful for
@@ -496,44 +467,22 @@ void World::CallUpdateCallbacks()
void World::ConsumeQueue( unsigned int queue_num )
{
std::priority_queue<Event>& queue = event_queues[queue_num];
-
+
if( queue.empty() )
return;
-
+
// update everything on the event queue that happens at this time or earlier
Event ev( queue.top() );
- while( ev.time <= sim_time )
+ while( ev.time <= sim_time )
{
- // printf( "@ %llu next event <%s %llu %s>\n",
- // sim_time,
- // ev.TypeStr( ev.type ),
- // ev.time,
- // ev.mod->Token() );
-
- queue.pop();
-
+ // 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 );
-
- switch( ev.type )
- {
- case Event::UPDATE:
- ev.mod->Update();
- break;
-
- case Event::POSE:
- ev.mod->UpdatePose();
- break;
-
- case Event::ENERGY:
- ev.mod->UpdateCharge();
- break;
-
- default:
- PRINT_WARN1( "unknown event type %d", ev.type );
- }
-
+ PRINT_WARN1( "event type %d in async queue", queue_num );
+
+ //process the event and move to the next
+ ev.Execute();
ev = queue.top();
}
}
@@ -1102,6 +1051,7 @@ void World::Enqueue( unsigned int queue_num, Event::type_t type, stg_usec_t dela
++event_pending_count;
}
+
bool World::Event::operator<( const Event& other ) const
{
// sort by time, type, then model, in that order.
@@ -1135,3 +1085,24 @@ const char* World::Event::TypeStr( type_t type )
default: return "<unknown>";
}
}
+
+void World::Event::Execute()
+{
+ switch( type )
+ {
+ case UPDATE:
+ mod->Update();
+ break;
+
+ case POSE:
+ mod->UpdatePose();
+ break;
+
+ case ENERGY:
+ mod->UpdateCharge();
+ break;
+
+ default:
+ PRINT_WARN1( "unknown event type %d", type );
+ }
+}
View
34 libstage/worldgui.cc
@@ -194,7 +194,8 @@ WorldGui::WorldGui(int W,int H,const char* L) :
oDlg( NULL ),
pause_time( false ),
real_time_interval( sim_interval ),
- real_time_recorded( RealTimeNow() ),
+ real_time_now( RealTimeNow() ),
+ real_time_recorded( real_time_now ),
timing_interval( 20 )
{
Fl::scheme( "gtk+" );
@@ -261,7 +262,9 @@ void WorldGui::Load( const char* filename )
Fl::check();
fileMan->newWorld( filename );
-
+
+ stg_usec_t load_start_time = RealTimeNow();
+
World::Load( filename );
// worldgui exclusive properties live in the top-level section
@@ -290,6 +293,12 @@ void WorldGui::Load( const char* filename )
}
label( title.c_str() );
}
+
+ stg_usec_t load_end_time = RealTimeNow();
+
+ if( debug )
+ printf( "[Load time %.3fsec]\n",
+ (load_end_time - load_start_time) / 1e6 );
Show();
}
@@ -352,15 +361,15 @@ bool WorldGui::Update()
bool done = World::Update();
if( done )
- {
- quit_time = 0; // allows us to continue by un-pausing
- Stop();
- }
+ {
+ quit_time = 0; // allows us to continue by un-pausing
+ Stop();
+ }
return done;
}
-std::string WorldGui::ClockString()
+std::string WorldGui::ClockString() const
{
std::string str = World::ClockString();
@@ -788,7 +797,7 @@ bool WorldGui::closeWindowQuery()
void WorldGui::DrawBoundingBoxTree()
{
FOR_EACH( it, World::children )
- (*it)->DrawBoundingBoxTree();
+ (*it)->DrawBoundingBoxTree();
}
void WorldGui::PushColor( Color col )
@@ -800,5 +809,12 @@ void WorldGui::PushColor( double r, double g, double b, double a )
void WorldGui::PopColor()
{ canvas->PopColor(); }
-Model* WorldGui::RecentlySelectedModel()
+Model* WorldGui::RecentlySelectedModel() const
{ return canvas->last_selection; }
+
+stg_usec_t WorldGui::RealTimeNow() const
+{
+ struct timeval tv;
+ gettimeofday( &tv, NULL ); // slow system call: use sparingly
+ return( tv.tv_sec*1000000 + tv.tv_usec );
+}
View
2 worlds/everything.world
@@ -40,7 +40,7 @@ floorplan
define puck model(
size [ 0.080 0.080 0.100 ]
gripper_return 1
- gui_movemask 3
+ gui_move 1
gui_nose 0
fiducial_return 10
)
View
4 worlds/map.inc
@@ -12,7 +12,7 @@ define floorplan model
gui_nose 0
gui_grid 0
- gui_movemask 0
+ gui_move 0
gui_outline 0
gripper_return 0
fiducial_return 0
@@ -26,7 +26,7 @@ define zone model
gui_nose 0
gui_grid 0
- gui_movemask 0
+ gui_move 0
gui_outline 0
# insensible to collision and range sensors

0 comments on commit 1ce2551

Please sign in to comment.