Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

cleaning up API some more - moved real time stuff out of world and in…

…to worldgui where it belongs
  • Loading branch information...
commit 772b106176d87ae225e0cd8e7d852e3068680c3e 1 parent f4914b8
rtv authored
View
30 CMakeLists.txt
@@ -8,12 +8,7 @@ SET( VERSION ${V_MAJOR}.${V_MINOR}.${V_BUGFIX} )
SET( APIVERSION ${V_MAJOR}.${V_MINOR} )
# minimum version of Player to build the plugin
-SET( MIN_PLAYER 99.0.0 ) # change once plugin is fixed
-
-#SET(CMAKE_BUILD_TYPE debug)
-#SET(CMAKE_CXX_FLAGS_DISTRIBUTION "-O3")
-#SET(CMAKE_C_FLAGS_DISTRIBUTION "-O3")
-
+SET( MIN_PLAYER 2.2.0 ) # change once plugin is fixed
cmake_minimum_required( VERSION 2.4 FATAL_ERROR )
@@ -35,22 +30,8 @@ include(FindPkgConfig)
pkg_search_module( GLIB REQUIRED glib-2.0 )
find_package( OpenGL REQUIRED )
-# Look for player v${MIN_PLAYER} or higher and set flags
-# built-ins don't work properly, so use pkg-config directly
-FIND_PROGRAM (PKGCONFIG NAMES pkg-config)
-IF (PKGCONFIG)
- EXECUTE_PROCESS (COMMAND pkg-config --atleast-version=${MIN_PLAYER} playercore --silence-errors
- RESULT_VARIABLE PLAYER_FOUND)
- IF (PLAYER_FOUND EQUAL 0)
- # found the correct version
- pkg_search_module( PLAYER playercore )
- ADD_SUBDIRECTORY( libstageplugin )
- ELSE (PLAYER_FOUND EQUAL 0)
- MESSAGE(STATUS "Player >=v${MIN_PLAYER} not found, skipping Player plugin")
- ENDIF (PLAYER_FOUND EQUAL 0)
-ELSE (PKGCONFIG)
- MESSAGE (STATUS "pkg-config not found, skipping Player plugin")
-ENDIF (PKGCONFIG)
+pkg_search_module( PLAYER playercore )
+
# find FLTK and set flags
FIND_PROGRAM (FLTKCONFIG NAMES fltk-config)
@@ -99,6 +80,11 @@ link_directories(${GLIB_LIBRARY_DIRS}
ADD_SUBDIRECTORY(libstage)
ADD_SUBDIRECTORY(examples)
+if( PLAYER_FOUND )
+ADD_SUBDIRECTORY(libstageplugin)
+endif( PLAYER_FOUND )
+
+
INSTALL(FILES rgb.txt stagelogo.png
DESTINATION share/stage
)
View
8 docsrc/stage.dox
@@ -159,7 +159,7 @@ DETAILS_AT_TOP = YES
# member inherits the documentation from any documented member that it
# re-implements.
-INHERIT_DOCS = NO
+INHERIT_DOCS = YES
# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
# tag is set to YES, then doxygen will reuse the documentation of the first
@@ -219,7 +219,7 @@ SUBGROUPING = NO
# Private class members and static file members will be hidden unless
# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES
-EXTRACT_ALL = YES
+EXTRACT_ALL = NO
# If the EXTRACT_PRIVATE tag is set to YES all private members of a class
# will be included in the documentation.
@@ -569,13 +569,13 @@ STRIP_CODE_COMMENTS = YES
# then for each documented function all documented
# functions referencing it will be listed.
-REFERENCED_BY_RELATION = YES
+REFERENCED_BY_RELATION = NO
# If the REFERENCES_RELATION tag is set to YES (the default)
# then for each documented function all documented entities
# called/used by that function will be listed.
-REFERENCES_RELATION = YES
+REFERENCES_RELATION = NO
# If the USE_HTAGS tag is set to YES then the references to source code
# will point to the HTML generated by the htags(1) tool instead of doxygen
View
2  docsrc/stage.txt
@@ -85,6 +85,8 @@ Stage was written by:
<li>Reed Hedges <tt>(hedges@mobilerobots.com)</tt>
<li>Andrew Howard <tt>(ahoward@robotics.usc.edu)</tt>
<li>Pooya Karimian <tt>(pooyak@sfu.ca)</tt>
+<li>Jeremy Asher <tt>(jra11@sfu.ca)</tt>
+<li>Alex Couture-Beil <tt>(asc17@sfu.ca)</tt>
</ul>
Many patches and bug reports have been contributed by users around the
View
6 libstage/CMakeLists.txt
@@ -1,5 +1,8 @@
add_library( stage SHARED
+ world.cc
+ worldgui.cc
+ worldfile.cc
stage.cc
typetable.cc
stage.hh # get headers into IDE projects
@@ -23,9 +26,6 @@ add_library( stage SHARED
model_props.cc
model_ranger.cc
resource.cc
- world.cc
- worldfile.cc
- worldgui.cc
texture_manager.cc
)
View
14 libstage/canvas.cc
@@ -21,7 +21,7 @@ void StgCanvas::TimerCallback( StgCanvas* c )
}
- StgCanvas::StgCanvas( StgWorld* world, int x, int y, int w, int h)
+ StgCanvas::StgCanvas( StgWorldGui* world, int x, int y, int w, int h)
: Fl_Gl_Window(x,y,w,h)
{
end();
@@ -88,7 +88,7 @@ StgModel* StgCanvas::Select( int x, int y )
// render all top-level, draggable models in a color that is their
// id
- for( GList* it=world->children; it; it=it->next )
+ for( GList* it= world->StgWorld::children; it; it=it->next )
{
StgModel* mod = (StgModel*)it->data;
@@ -444,7 +444,7 @@ void StgCanvas::renderFrame( bool robot_camera )
glDisable( GL_DEPTH_TEST );
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL );
- for( GList* it=world->children; it; it=it->next )
+ for( GList* it=world->StgWorld::children; it; it=it->next )
{
((StgModel*)it->data)->DrawTrailFootprint();
}
@@ -455,7 +455,7 @@ void StgCanvas::renderFrame( bool robot_camera )
{
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL );
- for( GList* it=world->children; it; it=it->next )
+ for( GList* it=world->StgWorld::children; it; it=it->next )
{
((StgModel*)it->data)->DrawTrailBlocks();
}
@@ -464,7 +464,7 @@ void StgCanvas::renderFrame( bool robot_camera )
if( showflags & STG_SHOW_ARROWS )
{
glEnable( GL_DEPTH_TEST );
- for( GList* it=world->children; it; it=it->next )
+ for( GList* it=world->StgWorld::children; it; it=it->next )
{
((StgModel*)it->data)->DrawTrailArrows();
}
@@ -472,7 +472,7 @@ void StgCanvas::renderFrame( bool robot_camera )
if( showflags & STG_SHOW_BLOCKS )
{
- for( GList* it=world->children; it; it=it->next )
+ for( GList* it=world->StgWorld::children; it; it=it->next )
{
StgModel* mod = ((StgModel*)it->data);
@@ -500,7 +500,7 @@ void StgCanvas::renderFrame( bool robot_camera )
//mod->Draw( showflags ); // draw the stuff that changes every update
// draw everything else
- for( GList* it=world->children; it; it=it->next )
+ for( GList* it=world->StgWorld::children; it; it=it->next )
((StgModel*)it->data)->Draw( showflags, this );
}
View
111 libstage/stage.hh
@@ -988,7 +988,7 @@ private:
static int AddBlockPixel( int x, int y, int z,
stg_render_info_t* rinfo ) ; //< used as a callback by StgModel
- stg_usec_t real_time_next_update;
+ //stg_usec_t real_time_next_update;
stg_usec_t real_time_start;
bool quit; // quit this world ASAP
@@ -996,11 +996,10 @@ private:
// convert a distance in meters to a distance in world occupancy grid pixels
int32_t MetersToPixels( stg_meters_t x ){ return (int32_t)floor(x * ppm) ; };
- void Initialize( const char* token,
- stg_msec_t interval_sim,
- stg_msec_t interval_real,
- double ppm );
-
+ void Initialize( const char* token,
+ stg_msec_t interval_sim,
+ double ppm );
+
virtual void PushColor( stg_color_t col ) { /* do nothing */ };
virtual void PushColor( double r, double g, double b, double a ) { /* do nothing */ };
virtual void PopColor(){ /* do nothing */ };
@@ -1018,23 +1017,14 @@ private:
GHashTable* models_by_name; ///< the models that make up the world, indexed by name
GList* velocity_list; ///< a list of models that have non-zero velocity, for efficient updating
- stg_usec_t sim_time; ///< the current sim time in this world in ms
-
//stg_usec_t wall_last_update; ///< the real time of the last update in ms
- long unsigned int updates; ///< the number of simulated time steps executed so far
-
bool dirty; ///< iff true, a gui redraw would be required
- stg_usec_t interval_sim; ///< temporal resolution: milliseconds that elapse between simulated time steps
-
- stg_usec_t interval_log[INTERVAL_LOG_LEN];
int total_subs; ///< the total number of subscriptions to all models
double ppm; ///< the resolution of the world model in pixels per meter
- bool paused; ///< the world only updates when this is false
-
GList* update_list; //< the descendants that need Update() called
void StartUpdatingModel( StgModel* mod );
@@ -1063,14 +1053,13 @@ private:
void RemoveBlock( int x, int y, StgBlock* block );
-
-public:
- stg_usec_t interval_real; ///< real-time interval between updates - set this to zero for 'as fast as possible
-
protected:
GHashTable* superregions;
+ stg_usec_t interval_sim; ///< temporal resolution: milliseconds that elapse between simulated time steps
static void UpdateCb( StgWorld* world);
+
+ stg_usec_t sim_time; ///< the current sim time in this world in ms
GList* ray_list;
// store rays traced for debugging purposes
@@ -1091,15 +1080,14 @@ protected:
GList* GetRayList(){ return ray_list; };
void ClearRays();
+ long unsigned int updates; ///< the number of simulated time steps executed so far
public:
static const int DEFAULT_PPM = 50; // default resolution in pixels per meter
- static const stg_msec_t DEFAULT_INTERVAL_REAL = 100; ///< real time between updates
static const stg_msec_t DEFAULT_INTERVAL_SIM = 100; ///< duration of sim timestep
StgWorld( const char* token = "MyWorld",
stg_msec_t interval_sim = DEFAULT_INTERVAL_SIM,
- stg_msec_t interval_real = DEFAULT_INTERVAL_REAL,
double ppm = DEFAULT_PPM );
virtual ~StgWorld();
@@ -1125,9 +1113,6 @@ public:
virtual bool Save( const char* filename );
virtual bool Update(void);
- void Start(){ paused = false; };
- void Stop(){ paused = true; };
- void TogglePause(){ paused = !paused; };
bool TestQuit(){ return( quit || quit_all ); }
void Quit(){ quit = true; }
void QuitAll(){ quit_all = true; }
@@ -1146,11 +1131,6 @@ public:
nonexistent */
StgModel* GetModel( const char* name );
-
- /** Get human readable string that describes the current simulation
- time. */
- void ClockString( char* str, size_t maxlen );
-
/** Return the 3D bounding box of the world, in meters */
stg_bounds3d_t GetExtent(){ return extent; };
@@ -1969,11 +1949,12 @@ class StgCanvas : public Fl_Gl_Window
void DrawGlobalGrid();
public:
- StgCanvas( StgWorld* world, int x, int y, int W,int H);
+
+ StgCanvas( StgWorldGui* world, int x, int y, int W,int H);
~StgCanvas();
bool graphics;
- StgWorld* world;
+ StgWorldGui* world;
void FixViewport(int W,int H);
//robot_camera = true
@@ -2005,48 +1986,62 @@ class StgCanvas : public Fl_Gl_Window
};
-
-
/** Extends StgWorld to implements an FLTK / OpenGL graphical user
interface.
*/
class StgWorldGui : public StgWorld, public Fl_Window
{
- friend class StgCanvas;
-
- private:
- int wf_section;
- StgCanvas* canvas;
- Fl_Menu_Bar* mbar;
-
+ friend class StgCanvas;
+ friend class StgModelCamera;
+
+private:
+ bool paused; ///< the world only updates when this is false
+ //int wf_section;
+ StgCanvas* canvas;
+ Fl_Menu_Bar* mbar;
+ stg_usec_t interval_log[INTERVAL_LOG_LEN];
+
stg_usec_t real_time_of_last_update;
+ stg_usec_t interval_real; ///< real-time interval between updates - set this to zero for 'as fast as possible
- public:
- StgWorldGui(int W,int H,const char*L=0);
- ~StgWorldGui();
+public:
+ static const stg_msec_t DEFAULT_INTERVAL_REAL = 100; ///< real time between updates
+ StgWorldGui(int W,int H,const char*L=0);
+ ~StgWorldGui();
+
virtual bool Update();
-
- virtual void Load( const char* filename );
- virtual void UnLoad();
- virtual bool Save( const char* filename );
+
+ virtual void Load( const char* filename );
+ virtual void UnLoad();
+ virtual bool Save( const char* filename );
+
+
+ void Start(){ paused = false; };
+ void Stop(){ paused = true; };
+ void TogglePause(){ paused = !paused; };
+
+ /** Get human readable string that describes the current simulation
+ time. */
+ void ClockString( char* str, size_t maxlen );
/** Set the minimum real time interval between world updates, in
microeconds. */
void SetRealTimeInterval( stg_usec_t usec )
{ interval_real = usec; }
-
- // static callback functions
- static void LoadCallback( Fl_Widget* wid, StgWorldGui* world );
- static void SaveCallback( Fl_Widget* wid, StgWorldGui* world );
- static void SaveAsCallback( Fl_Widget* wid, StgWorldGui* world );
- static void QuitCallback( Fl_Widget* wid, StgWorldGui* world );
- static void About_cb( Fl_Widget* wid, StgWorldGui* world );
- static void HelpAboutCallback( Fl_Widget* wid );
- static void view_toggle_cb( Fl_Menu_Bar* menubar, StgCanvas* canvas );
- static void WindowCallback( Fl_Widget* wid, StgWorldGui* world );
-
+
+ // static callback functions
+protected:
+ static void LoadCallback( Fl_Widget* wid, StgWorldGui* world );
+ static void SaveCallback( Fl_Widget* wid, StgWorldGui* world );
+ static void SaveAsCallback( Fl_Widget* wid, StgWorldGui* world );
+ static void QuitCallback( Fl_Widget* wid, StgWorldGui* world );
+ static void About_cb( Fl_Widget* wid, StgWorldGui* world );
+ static void HelpAboutCallback( Fl_Widget* wid );
+ static void view_toggle_cb( Fl_Menu_Bar* menubar, StgCanvas* canvas );
+ static void WindowCallback( Fl_Widget* wid, StgWorldGui* world );
+
bool SaveAsDialog();
bool CloseWindowQuery();
View
104 libstage/world.cc
@@ -95,15 +95,13 @@ void StgWorld::UpdateAll()
StgWorld::StgWorld( const char* token,
stg_msec_t interval_sim,
- stg_msec_t interval_real,
double ppm )
{
- Initialize( token, interval_sim, interval_real, ppm );
+ Initialize( token, interval_sim, ppm );
}
void StgWorld::Initialize( const char* token,
stg_msec_t interval_sim,
- stg_msec_t interval_real,
double ppm )
{
if( ! Stg::InitDone() )
@@ -130,11 +128,10 @@ void StgWorld::Initialize( const char* token,
this->models_by_name = g_hash_table_new( g_str_hash, g_str_equal );
this->sim_time = 0;
this->interval_sim = (stg_usec_t)thousand * interval_sim;
- this->interval_real = (stg_usec_t)thousand * interval_real;
this->ppm = ppm; // this is the raytrace resolution
this->real_time_start = RealTimeNow();
- this->real_time_next_update = 0;
+ //this->real_time_next_update = 0;
this->update_list = NULL;
this->velocity_list = NULL;
@@ -143,7 +140,6 @@ void StgWorld::Initialize( const char* token,
(GEqualFunc)PointIntEqual );
this->total_subs = 0;
- this->paused = false;
this->destroy = false;
// store a global table of all blocks, so they can be rendered all
@@ -152,8 +148,6 @@ void StgWorld::Initialize( const char* token,
bzero( &this->extent, sizeof(this->extent));
- for( unsigned int i=0; i<INTERVAL_LOG_LEN; i++ )
- this->interval_log[i] = this->interval_real;
this->real_time_now = 0;
}
@@ -179,52 +173,6 @@ void StgWorld::RemoveModel( StgModel* mod )
g_hash_table_remove( models_by_name, mod );
}
-void StgWorld::ClockString( char* str, size_t maxlen )
-{
- const uint32_t usec_per_hour = 360000000;
- const uint32_t usec_per_minute = 60000000;
- const uint32_t usec_per_second = 1000000;
- const uint32_t usec_per_msec = 1000;
-
- uint32_t hours = sim_time / usec_per_hour;
- uint32_t minutes = (sim_time % usec_per_hour) / usec_per_minute;
- uint32_t seconds = (sim_time % usec_per_minute) / usec_per_second;
- uint32_t msec = (sim_time % usec_per_second) / usec_per_msec;
-
- // find the average length of the last few realtime intervals;
- stg_usec_t average_real_interval = 0;
- for( uint32_t i=0; i<INTERVAL_LOG_LEN; i++ )
- average_real_interval += interval_log[i];
- average_real_interval /= INTERVAL_LOG_LEN;
-
- double localratio = (double)interval_sim / (double)average_real_interval;
-
-#ifdef DEBUG
- if( hours > 0 )
- snprintf( str, maxlen, "Time: %uh%02um%02u.%03us\t[%.6f]\tsubs: %d %s",
- hours, minutes, seconds, msec,
- localratio,
- total_subs,
- paused ? "--PAUSED--" : "" );
- else
- snprintf( str, maxlen, "Time: %02um%02u.%03us\t[%.6f]\tsubs: %d %s",
- minutes, seconds, msec,
- localratio,
- total_subs,
- paused ? "--PAUSED--" : "" );
-#else
- if( hours > 0 )
- snprintf( str, maxlen, "%uh%02um%02u.%03us\t[%.2f] %s",
- hours, minutes, seconds, msec,
- localratio,
- paused ? "--PAUSED--" : "" );
- else
- snprintf( str, maxlen, "%02um%02u.%03us\t[%.2f] %s",
- minutes, seconds, msec,
- localratio,
- paused ? "--PAUSED--" : "" );
-#endif
-}
// wrapper to startup all models from the hash table
void init_models( gpointer dummy1, StgModel* mod, gpointer dummy2 )
@@ -266,9 +214,6 @@ void StgWorld::Load( const char* worldfile_path )
this->token = (char*)
wf->ReadString( entity, "name", token );
- this->interval_real = (stg_usec_t)thousand *
- wf->ReadInt( entity, "interval_real", this->interval_real/thousand );
-
this->interval_sim = (stg_usec_t)thousand *
wf->ReadInt( entity, "interval_sim", this->interval_sim/thousand );
@@ -280,9 +225,6 @@ void StgWorld::Load( const char* worldfile_path )
this->ppm =
1.0 / wf->ReadFloat( entity, "resolution", 1.0 / this->ppm );
- this->paused =
- wf->ReadInt( entity, "paused", this->paused );
-
//_stg_disable_gui = wf->ReadInt( entity, "gui_disable", _stg_disable_gui );
// Iterate through entitys and create client-side models
@@ -414,32 +356,24 @@ stg_usec_t StgWorld::RealTimeSinceStart()
bool StgWorld::Update()
{
- PRINT_DEBUG( "StgWorld::Update()" );
-
- if( paused )
- return false;
-
- // update any models that are due to be updated
- for( GList* it=this->update_list; it; it=it->next )
- ((StgModel*)it->data)->UpdateIfDue();
-
- // update any models with non-zero velocity
- for( GList* it=this->velocity_list; it; it=it->next )
- ((StgModel*)it->data)->UpdatePose();
-
- this->sim_time += this->interval_sim;
- this->updates++;
-
- // if we've run long enough, set the quit flag
- if( (quit_time > 0) && (sim_time >= quit_time) )
- quit = true;
-
- stg_usec_t timenow = RealTimeSinceStart();
-
- interval_log[updates%INTERVAL_LOG_LEN] = timenow - real_time_now;
- real_time_now = timenow;
+ PRINT_DEBUG( "StgWorld::Update()" );
+
+ // update any models that are due to be updated
+ for( GList* it=this->update_list; it; it=it->next )
+ ((StgModel*)it->data)->UpdateIfDue();
+
+ // update any models with non-zero velocity
+ for( GList* it=this->velocity_list; it; it=it->next )
+ ((StgModel*)it->data)->UpdatePose();
- return true;
+ this->sim_time += this->interval_sim;
+ this->updates++;
+
+ // if we've run long enough, set the quit flag
+ if( (quit_time > 0) && (sim_time >= quit_time) )
+ quit = true;
+
+ return true;
}
void StgWorld::AddModel( StgModel* mod )
View
82 libstage/worldgui.cc
@@ -134,6 +134,12 @@ static const char* MITEM_VIEW_PERSPECTIVE = "&View/Perspective camera";
//size_range( 100,100 ); // set minimum window size
graphics = true;
+ paused = false;
+
+ interval_real = (stg_usec_t)thousand * DEFAULT_INTERVAL_REAL;
+
+ for( unsigned int i=0; i<INTERVAL_LOG_LEN; i++ )
+ this->interval_log[i] = this->interval_real;
// build the menus
mbar = new Fl_Menu_Bar(0,0, W, 30);// 640, 30);
@@ -191,15 +197,72 @@ StgWorldGui::~StgWorldGui()
delete canvas;
}
+
+void StgWorldGui::ClockString( char* str, size_t maxlen )
+{
+ const uint32_t usec_per_hour = 360000000;
+ const uint32_t usec_per_minute = 60000000;
+ const uint32_t usec_per_second = 1000000;
+ const uint32_t usec_per_msec = 1000;
+
+ uint32_t hours = sim_time / usec_per_hour;
+ uint32_t minutes = (sim_time % usec_per_hour) / usec_per_minute;
+ uint32_t seconds = (sim_time % usec_per_minute) / usec_per_second;
+ uint32_t msec = (sim_time % usec_per_second) / usec_per_msec;
+
+ // find the average length of the last few realtime intervals;
+ stg_usec_t average_real_interval = 0;
+ for( uint32_t i=0; i<INTERVAL_LOG_LEN; i++ )
+ average_real_interval += interval_log[i];
+ average_real_interval /= INTERVAL_LOG_LEN;
+
+ double localratio = (double)interval_sim / (double)average_real_interval;
+
+#ifdef DEBUG
+ if( hours > 0 )
+ snprintf( str, maxlen, "Time: %uh%02um%02u.%03us\t[%.6f]\tsubs: %d %s",
+ hours, minutes, seconds, msec,
+ localratio,
+ total_subs,
+ paused ? "--PAUSED--" : "" );
+ else
+ snprintf( str, maxlen, "Time: %02um%02u.%03us\t[%.6f]\tsubs: %d %s",
+ minutes, seconds, msec,
+ localratio,
+ total_subs,
+ paused ? "--PAUSED--" : "" );
+#else
+ if( hours > 0 )
+ snprintf( str, maxlen, "%uh%02um%02u.%03us\t[%.2f] %s",
+ hours, minutes, seconds, msec,
+ localratio,
+ paused ? "--PAUSED--" : "" );
+ else
+ snprintf( str, maxlen, "%02um%02u.%03us\t[%.2f] %s",
+ minutes, seconds, msec,
+ localratio,
+ paused ? "--PAUSED--" : "" );
+#endif
+}
+
+
void StgWorldGui::Load( const char* filename )
{
PRINT_DEBUG1( "%s.Load()", token );
StgWorld::Load( filename );
- wf_section = wf->LookupEntity( "window" );
- if( wf_section < 1) // no section defined
- return;
+// wf_section = wf->LookupEntity( "window" );
+// if( wf_section < 1) // no section defined
+// return;
+
+ int wf_section = 0; // root section
+
+ this->paused =
+ wf->ReadInt( wf_section, "paused", this->paused );
+
+ this->interval_real = (stg_usec_t)thousand *
+ wf->ReadInt( wf_section, "interval_real", this->interval_real/thousand );
int width = (int)wf->ReadTupleFloat(wf_section, "size", 0, w() );
int height = (int)wf->ReadTupleFloat(wf_section, "size", 1, h() );
@@ -486,6 +549,8 @@ bool StgWorldGui::Save( const char* filename )
{
PRINT_DEBUG1( "%s.Save()", token );
+ int wf_section = 0;
+
wf->WriteTupleFloat( wf_section, "size", 0, w() );
wf->WriteTupleFloat( wf_section, "size", 1, h() );
@@ -516,10 +581,9 @@ bool StgWorldGui::Update()
{
if( real_time_of_last_update == 0 )
real_time_of_last_update = RealTimeNow();
-
- bool val = StgWorld::Update();
-
+ bool val = paused ? true : StgWorld::Update();
+
stg_usec_t interval;
stg_usec_t timenow;
@@ -540,6 +604,12 @@ bool StgWorldGui::Update()
real_time_of_last_update = timenow;
+ //stg_usec_t timenow = RealTimeSinceStart();
+
+ interval_log[updates%INTERVAL_LOG_LEN] = interval_real;//timenow - real_time_now;
+ // real_time_now = timenow;
+
+
return val;
}
View
1  libstageplugin/CMakeLists.txt
@@ -1,6 +1,7 @@
link_directories( ${PLAYER_LIBDIR} )
include_directories( ${PLAYER_INCLUDE_DIRS})
+message( PLAYER INCLUDE ${PLAYER_INCLUDE_DIRS} )
add_library( stageplugin MODULE
p_driver.cc
View
30 libstageplugin/p_driver.cc
@@ -225,10 +225,10 @@ Interface::Interface( player_devaddr_t addr,
}
InterfaceModel::InterfaceModel( player_devaddr_t addr,
- StgDriver* driver,
- ConfigFile* cf,
- int section,
- char* typestr )
+ StgDriver* driver,
+ ConfigFile* cf,
+ int section,
+ stg_model_type_t type )
: Interface( addr, driver, cf, section )
{
char* model_name = (char*)cf->ReadString(section, "model", NULL );
@@ -246,7 +246,7 @@ InterfaceModel::InterfaceModel( player_devaddr_t addr,
// find a model of the right type
this->mod = driver->LocateModel( model_name,
&addr,
- typestr );
+ type );
if( !this->mod )
{
@@ -407,30 +407,30 @@ StgDriver::StgDriver(ConfigFile* cf, int section)
StgModel* StgDriver::LocateModel( char* basename,
player_devaddr_t* addr,
- char* typestr )
+ stg_model_type_t type )
{
- printf( "attempting to find a model under model \"%s\" of type [%s]\n",
- basename, typestr );
+ printf( "attempting to find a model under model \"%s\" of type [%d]\n",
+ basename, type );
StgModel* base_model = world->GetModel( basename );
if( base_model == NULL )
{
PRINT_ERR1( " Error! can't find a Stage model named \"%s\"",
- basename );
+ basename );
return NULL;
}
- if( typestr == NULL ) // if we don't care what type the model is
+ if( type == MODEL_TYPE_PLAIN ) // if we don't care what type the model is
return base_model;
-
+
// printf( "found base model %s\n", base_model->Token() );
-
+
// we find the first model in the tree that is the right
// type (i.e. has the right initialization function) and has not
// been used before
//return( model_match( base_model, addr, typestr, this->devices ) );
- return( base_model->GetUnsubscribedModelOfType( typestr ) );
+ return( base_model->GetUnsubscribedModelOfType( type ) );
}
////////////////////////////////////////////////////////////////////////////////
@@ -511,7 +511,7 @@ StgDriver::~StgDriver()
// Shutdown the device
int StgDriver::Shutdown()
{
- puts("Shutting stage driver down");
+ //puts("Shutting stage driver down");
// Stop and join the driver thread
// this->StopThread(); // todo - the thread only runs in the sim instance
@@ -523,7 +523,7 @@ int StgDriver::Shutdown()
// stg_model_unsubscribe( device->mod );
// }
- puts("stage driver has been shutdown");
+ puts("Stage driver has been shutdown");
return(0);
}
View
4 libstageplugin/p_driver.h
@@ -45,7 +45,7 @@ class StgDriver : public Driver
StgModel* LocateModel( char* basename,
player_devaddr_t* addr,
- char* typestr);
+ stg_model_type_t type );
protected:
@@ -100,7 +100,7 @@ class InterfaceModel
StgDriver* driver,
ConfigFile* cf,
int section,
- char* typestr );
+ stg_model_type_t type );
StgModel* mod;
View
2  libstageplugin/p_graphics3d.cc
@@ -58,7 +58,7 @@ InterfaceGraphics3d::InterfaceGraphics3d( player_devaddr_t addr,
StgDriver* driver,
ConfigFile* cf,
int section )
- : InterfaceModel( addr, driver, cf, section, NULL )
+ : InterfaceModel( addr, driver, cf, section, MODEL_TYPE_PLAIN )
{
// data members
commands = NULL;
View
2  libstageplugin/p_laser.cc
@@ -44,7 +44,7 @@ InterfaceLaser::InterfaceLaser( player_devaddr_t addr,
StgDriver* driver,
ConfigFile* cf,
int section )
- : InterfaceModel( addr, driver, cf, section, "laser" )
+ : InterfaceModel( addr, driver, cf, section, MODEL_TYPE_LASER )
{
this->scan_id = 0;
}
View
2  libstageplugin/p_position.cc
@@ -51,7 +51,7 @@ InterfacePosition::InterfacePosition( player_devaddr_t addr,
ConfigFile* cf,
int section )
- : InterfaceModel( addr, driver, cf, section, "position" )
+ : InterfaceModel( addr, driver, cf, section, MODEL_TYPE_POSITION )
{
//puts( "InterfacePosition constructor" );
}
View
2  libstageplugin/p_sonar.cc
@@ -46,7 +46,7 @@ InterfaceSonar::InterfaceSonar( player_devaddr_t id,
StgDriver* driver,
ConfigFile* cf,
int section )
- : InterfaceModel( id, driver, cf, section, "ranger" )
+ : InterfaceModel( id, driver, cf, section, MODEL_TYPE_RANGER )
{
//this->data_len = sizeof(player_sonar_data_t);
//this->cmd_len = 0;
View
15 worlds/fasr.world
@@ -19,15 +19,12 @@ interval_real 20 # real-time interval between simulation updates in millisecond
paused 1
# configure the GUI window
-window
-(
- #size [ 698.000 628.000 ]
- center [6.990 -4.040]
- rotate [ 0.000 0.000 ]
- scale 33.306
- show_data 0
- #interval 10
-)
+size [ 698.000 628.000 ]
+center [6.990 -4.040]
+rotate [ 0.000 0.000 ]
+scale 33.306
+show_data 0
+
# load an environment bitmap
floorplan
View
26 worlds/simple.world
@@ -20,22 +20,20 @@ interval_real 100 # real-time interval between simulation updates in millisecon
paused 0
# configure the GUI window
-window
-(
- size [ 745.000 448.000 ]
- center [-7.010 5.960]
- rotate [ 0.920 -0.430 ]
- scale 28.806
-)
+size [ 745.000 448.000 ]
+center [-7.010 5.960]
+rotate [ 0.920 -0.430 ]
+scale 28.806
+
# load an environment bitmap
-#floorplan
-#(
-# name "cave"
-# size3 [16 16 0.5]
-# pose [0.000 0.000 0.000]
-# bitmap "bitmaps/cave.png"
-#)
+floorplan
+(
+ name "cave"
+ size3 [16 16 1.5]
+ pose [0.000 0.000 0.000]
+ bitmap "bitmaps/cave.png"
+)
define autorob fancypioneer2dx
(
Please sign in to comment.
Something went wrong with that request. Please try again.