Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

header clean up

  • Loading branch information...
commit ec56d041526d5500e45e01bd3a4dd1358291a168 1 parent 1085f4e
rtv authored
Showing with 1,740 additions and 1,806 deletions.
  1. +1 −2  libstage/CMakeLists.txt
  2. +2 −1  libstage/ancestor.cc
  3. +3 −2 libstage/block.cc
  4. +6 −3 libstage/blockgroup.cc
  5. +3 −1 libstage/camera.cc
  6. +15 −7 libstage/canvas.cc
  7. +140 −0 libstage/canvas.hh
  8. +3 −1 libstage/charger.cc
  9. +20 −19 libstage/gl.cc
  10. +20 −0 libstage/gl.hh
  11. +3 −1 libstage/glcolorstack.cc
  12. +2 −1  libstage/main.cc
  13. +69 −14 libstage/model.cc
  14. +0 −1,190 libstage/model.hh
  15. +3 −1 libstage/model_blinkenlight.cc
  16. +5 −1 libstage/model_blobfinder.cc
  17. +2 −1  libstage/model_callbacks.cc
  18. +5 −1 libstage/model_camera.cc
  19. +13 −6 libstage/model_fiducial.cc
  20. +5 −1 libstage/model_laser.cc
  21. +3 −1 libstage/model_load.cc
  22. +17 −12 libstage/model_position.cc
  23. +2 −1  libstage/model_props.cc
  24. +6 −1 libstage/model_ranger.cc
  25. +4 −1 libstage/option.cc
  26. +1 −0  libstage/options_dlg.hh
  27. +2 −1  libstage/powerpack.cc
  28. +5 −3 libstage/region.cc
  29. +124 −170 libstage/region.hh
  30. +2 −1  libstage/resource.cc
  31. +2 −1  libstage/stage.cc
  32. +1,203 −22 libstage/stage.hh
  33. +0 −303 libstage/stage_internal.hh
  34. +3 −1 libstage/texture_manager.cc
  35. +26 −24 libstage/texture_manager.hh
  36. +2 −1  libstage/typetable.cc
  37. +2 −1  libstage/waypoint.cc
  38. +5 −1 libstage/world.cc
  39. +3 −2 libstage/worldfile.cc
  40. +0 −2  libstage/worldfile.hh
  41. +8 −4 libstage/worldgui.cc
View
3  libstage/CMakeLists.txt
@@ -25,7 +25,6 @@ set( stageSrcs ancestor.cc
model_props.cc
model_ranger.cc
option.cc
- option.hh
options_dlg.cc
options_dlg.hh
powerpack.cc
@@ -82,7 +81,7 @@ INSTALL(TARGETS stagebinary stage
LIBRARY DESTINATION lib
)
-INSTALL(FILES stage.hh option.hh worldfile.hh
+INSTALL(FILES stage.hh
DESTINATION include/${PROJECT_NAME}-${V_MAJOR}.${V_MINOR})
ADD_TEST( test1 ${EXECUTABLE_OUTPUT_PATH}stagetest )
View
3  libstage/ancestor.cc
@@ -1,4 +1,5 @@
-#include "stage_internal.hh"
+#include "stage.hh"
+using namespace Stg;
Ancestor::Ancestor()
{
View
5 libstage/block.cc
@@ -1,7 +1,8 @@
-#include "stage_internal.hh"
+#include "region.hh"
+#include "worldfile.hh"
-//GPtrArray* Block::global_verts = g_ptr_array_sized_new( 1024 );
+using namespace Stg;
/** Create a new block. A model's body is a list of these
blocks. The point data is copied, so pts can safely be freed
View
9 libstage/blockgroup.cc
@@ -1,5 +1,8 @@
-#include "stage_internal.hh"
+#include "stage.hh"
+#include "gl.hh"
+#include "worldfile.hh"
+
#include <libgen.h> // for dirname(3)
#undef DEBUG
@@ -123,7 +126,7 @@ void BlockGroup::DrawSolid( const Geom & geom )
{
glPushMatrix();
- gl_pose_shift( geom.pose );
+ Gl::pose_shift( geom.pose );
glScalef( geom.size.x / size.x,
geom.size.y / size.y,
@@ -166,7 +169,7 @@ void BlockGroup::BuildDisplayList( Model* mod )
Geom geom = mod->GetGeom();
- gl_pose_shift( geom.pose );
+ Gl::pose_shift( geom.pose );
glScalef( geom.size.x / size.x,
geom.size.y / size.y,
View
4 libstage/camera.cc
@@ -7,7 +7,9 @@
*/
-#include "stage_internal.hh"
+#include "stage.hh"
+#include "worldfile.hh"
+using namespace Stg;
#include <iostream>
View
22 libstage/canvas.cc
@@ -9,15 +9,18 @@
$Id$
*/
-#include "stage_internal.hh"
+#include "stage.hh"
+#include "gl.hh"
+#include "canvas.hh"
+#include "worldfile.hh"
#include "texture_manager.hh"
#include "replace.h"
#include <string>
-//#include <map>
#include <sstream>
#include <png.h>
+
#include "file_manager.hh"
#include "options_dlg.hh"
@@ -49,6 +52,12 @@ Canvas::Canvas( WorldGui* world,
int x, int y,
int width, int height) :
Fl_Gl_Window( x, y, width, height ),
+ colorstack(),
+ models_sorted( NULL ),
+ current_camera( NULL ),
+ camera(),
+ perspective_camera(),
+ dirty_buffer( false ),
wf( NULL ),
startx( -1 ),
starty( -1 ),
@@ -79,8 +88,7 @@ Canvas::Canvas( WorldGui* world,
graphics( true ),
world( world ),
frames_rendered_count( 0 ),
- screenshot_frame_skip( 1 ),
- models_sorted( NULL )
+ screenshot_frame_skip( 1 )
{
end();
@@ -608,13 +616,13 @@ void Canvas::DrawGlobalGrid()
for( double i = floor(bounds.x.min); i < bounds.x.max; i++)
{
snprintf( str, 16, "%d", (int)i );
- gl_draw_string( i, 0, 0.00, str );
+ Gl::draw_string( i, 0, 0.00, str );
}
for( double i = floor(bounds.y.min); i < bounds.y.max; i++)
{
snprintf( str, 16, "%d", (int)i );
- gl_draw_string( 0, i, 0.00, str );
+ Gl::draw_string( 0, i, 0.00, str );
}
PopColor();
@@ -996,7 +1004,7 @@ void Canvas::renderFrame()
colorstack.Pop();
colorstack.Push( 0,0,0 ); // black
- gl_draw_string( margin, margin, 0, clockstr.c_str() );
+ Gl::draw_string( margin, margin, 0, clockstr.c_str() );
colorstack.Pop();
glEnable( GL_DEPTH_TEST );
View
140 libstage/canvas.hh
@@ -0,0 +1,140 @@
+
+#include "stage.hh"
+#include "option.hh"
+
+#include <map>
+
+namespace Stg
+{
+// COLOR STACK CLASS
+class GlColorStack
+{
+ public:
+ GlColorStack();
+ ~GlColorStack();
+
+ void Push( GLdouble col[4] );
+ void Push( double r, double g, double b, double a );
+ void Push( double r, double g, double b );
+ void Push( stg_color_t col );
+
+ void Pop();
+
+ unsigned int Length()
+ { return g_queue_get_length( colorstack ); }
+
+ private:
+ GQueue* colorstack;
+};
+
+
+class Canvas : public Fl_Gl_Window
+{
+ friend class WorldGui; // allow access to private members
+ friend class Model;
+
+private:
+ GlColorStack colorstack;
+
+ GList* models_sorted;
+
+ Camera* current_camera;
+ OrthoCamera camera;
+ PerspectiveCamera perspective_camera;
+ bool dirty_buffer;
+ Worldfile* wf;
+
+ int startx, starty;
+ bool selectedModel;
+ bool clicked_empty_space;
+ int empty_space_startx, empty_space_starty;
+ GList* selected_models; ///< a list of models that are currently
+ ///selected by the user
+ Model* last_selection; ///< the most recently selected model
+ ///(even if it is now unselected).
+
+ stg_msec_t interval; // window refresh interval in ms
+
+ GList* ray_list;
+ void RecordRay( double x1, double y1, double x2, double y2 );
+ void DrawRays();
+ void ClearRays();
+ void DrawGlobalGrid();
+
+ void AddModel( Model* mod );
+
+ Option showBlinken,
+ showBlocks,
+ showClock,
+ showData,
+ showFlags,
+ showFollow,
+ showFootprints,
+ showGrid,
+ showOccupancy,
+ showScreenshots,
+ showStatus,
+ showTrailArrows,
+ showTrailRise,
+ showTrails,
+ showTree,
+ showBBoxes,
+ showBlur,
+ pCamOn,
+ visualizeAll;
+
+public:
+ Canvas( WorldGui* world, int x, int y, int width, int height);
+ ~Canvas();
+
+ bool graphics;
+ WorldGui* world;
+ unsigned long frames_rendered_count;
+ int screenshot_frame_skip;
+
+ std::map< std::string, Option* > _custom_options;
+
+ void Screenshot();
+ void InitGl();
+ void createMenuItems( Fl_Menu_Bar* menu, std::string path );
+
+ void FixViewport(int W,int H);
+ void DrawFloor(); //simpler floor compared to grid
+ void DrawBlocks();
+ void DrawBoundingBoxes();
+ void resetCamera();
+ virtual void renderFrame();
+ virtual void draw();
+ virtual int handle( int event );
+ void resize(int X,int Y,int W,int H);
+
+ void CanvasToWorld( int px, int py,
+ double *wx, double *wy, double* wz );
+
+ Model* getModel( int x, int y );
+ bool selected( Model* mod );
+ void select( Model* mod );
+ void unSelect( Model* mod );
+ void unSelectAll();
+
+ inline void setDirtyBuffer( void ) { dirty_buffer = true; }
+ inline bool dirtyBuffer( void ) const { return dirty_buffer; }
+
+ inline void PushColor( stg_color_t col )
+ { colorstack.Push( col ); }
+
+ void PushColor( double r, double g, double b, double a )
+ { colorstack.Push( r,g,b,a ); }
+
+ void PopColor(){ colorstack.Pop(); }
+
+ void InvertView( uint32_t invertflags );
+
+ static void TimerCallback( Canvas* canvas );
+ static void perspectiveCb( Fl_Widget* w, void* p );
+
+ void Load( Worldfile* wf, int section );
+ void Save( Worldfile* wf, int section );
+};
+
+} // namespace Stg
View
4 libstage/charger.cc
@@ -6,7 +6,9 @@
*/
-#include "stage_internal.hh"
+#include "stage.hh"
+#include "worldfile.hh"
+using namespace Stg;
Charger::Charger( World* world )
: world( world ), watts( 1000.0 )
View
39 libstage/gl.cc
@@ -1,41 +1,42 @@
-#include "stage_internal.hh"
+#include "gl.hh"
+using namespace Stg;
// transform the current coordinate frame by the given pose
-void Stg::gl_coord_shift( double x, double y, double z, double a )
+void Stg::Gl::coord_shift( double x, double y, double z, double a )
{
glTranslatef( x,y,z );
glRotatef( rtod(a), 0,0,1 );
}
// transform the current coordinate frame by the given pose
-void Stg::gl_pose_shift( const Pose &pose )
+void Stg::Gl::pose_shift( const Pose &pose )
{
- gl_coord_shift( pose.x, pose.y, pose.z, pose.a );
+ coord_shift( pose.x, pose.y, pose.z, pose.a );
}
-void Stg::gl_pose_inverse_shift( const Pose &pose )
+void Stg::Gl::pose_inverse_shift( const Pose &pose )
{
- gl_coord_shift( 0,0,0, -pose.a );
- gl_coord_shift( -pose.x, -pose.y, -pose.z, 0 );
+ coord_shift( 0,0,0, -pose.a );
+ coord_shift( -pose.x, -pose.y, -pose.z, 0 );
}
-void Stg::gl_draw_string( float x, float y, float z, const char *str )
+void Stg::Gl::draw_string( float x, float y, float z, const char *str )
{
glRasterPos3f( x, y, z );
//printf( "[%.2f %.2f %.2f] string %u %s\n", x,y,z,(unsigned int)strlen(str), str );
gl_draw(str);
}
-void Stg::gl_speech_bubble( float x, float y, float z, const char* str )
+void Stg::Gl::draw_speech_bubble( float x, float y, float z, const char* str )
{
- gl_draw_string( x, y, z, str );
+ draw_string( x, y, z, str );
}
// draw an octagon with center rectangle dimensions w/h
// and outside margin m
-void Stg::gl_draw_octagon( float w, float h, float m )
+void Stg::Gl::draw_octagon( float w, float h, float m )
{
glBegin(GL_POLYGON);
glVertex2f( m+w, 0 );
@@ -49,7 +50,7 @@ void Stg::gl_draw_octagon( float w, float h, float m )
glEnd();
}
-void Stg::gl_draw_vector( double x, double y, double z )
+void Stg::Gl::draw_vector( double x, double y, double z )
{
glBegin( GL_LINES );
glVertex3f( 0,0,0 );
@@ -57,14 +58,14 @@ void Stg::gl_draw_vector( double x, double y, double z )
glEnd();
}
-void Stg::gl_draw_origin( double len )
+void Stg::Gl::draw_origin( double len )
{
- gl_draw_vector( len,0,0 );
- gl_draw_vector( 0,len,0 );
- gl_draw_vector( 0,0,len );
+ draw_vector( len,0,0 );
+ draw_vector( 0,len,0 );
+ draw_vector( 0,0,len );
}
-void Stg::gl_draw_grid( stg_bounds3d_t vol )
+void Stg::Gl::draw_grid( stg_bounds3d_t vol )
{
glBegin(GL_LINES);
@@ -87,13 +88,13 @@ void Stg::gl_draw_grid( stg_bounds3d_t vol )
for( double i = floor(vol.x.min); i < vol.x.max; i++)
{
snprintf( str, 16, "%d", (int)i );
- gl_draw_string( i, 0, 0.00, str );
+ draw_string( i, 0, 0.00, str );
}
for( double i = floor(vol.y.min); i < vol.y.max; i++)
{
snprintf( str, 16, "%d", (int)i );
- gl_draw_string( 0, i, 0.00, str );
+ draw_string( 0, i, 0.00, str );
}
}
View
20 libstage/gl.hh
@@ -0,0 +1,20 @@
+
+#include "stage.hh"
+
+namespace Stg
+{
+ /** @brief Internal low-level drawing convenience routines. */
+ namespace Gl
+ {
+ void pose_shift( const Pose &pose );
+ void pose_inverse_shift( const Pose &pose );
+ void coord_shift( double x, double y, double z, double a );
+ void draw_grid( stg_bounds3d_t vol );
+ /** Render a string at [x,y,z] in the current color */
+ void draw_string( float x, float y, float z, const char *string);
+ void draw_speech_bubble( float x, float y, float z, const char* str );
+ void draw_octagon( float w, float h, float m );
+ void draw_vector( double x, double y, double z );
+ void draw_origin( double len );
+ }
+}
View
4 libstage/glcolorstack.cc
@@ -1,4 +1,6 @@
-#include "stage_internal.hh"
+#include "canvas.hh"
+
+using namespace Stg;
GlColorStack::GlColorStack()
{
View
3  libstage/main.cc
@@ -6,8 +6,9 @@
#include <getopt.h>
-#include "stage_internal.hh"
+#include "stage.hh"
#include "config.h"
+using namespace Stg;
/* options descriptor */
static struct option longopts[] = {
View
83 libstage/model.cc
@@ -109,8 +109,15 @@
#endif
//#define DEBUG 0
-#include "stage_internal.hh"
+#include "stage.hh"
+#include "gl.hh"
+
+#include <map>
+
+#include "worldfile.hh"
+#include "canvas.hh"
#include "texture_manager.hh"
+using namespace Stg;
// speech bubble colors
static const stg_color_t BUBBLE_FILL = 0xFFC8C8FF; // light blue/grey
@@ -121,6 +128,42 @@ static const stg_color_t BUBBLE_TEXT = 0xFF000000; // black
uint32_t Model::count = 0;
GHashTable* Model::modelsbyid = g_hash_table_new( NULL, NULL );
+Visibility::Visibility() :
+ blob_return( true ),
+ fiducial_key( 0 ),
+ fiducial_return( 0 ),
+ gripper_return( false ),
+ laser_return( LaserVisible ),
+ obstacle_return( true ),
+ ranger_return( true )
+{ /* nothing do do */ }
+
+void Visibility::Load( Worldfile* wf, int wf_entity )
+{
+ blob_return = wf->ReadInt( wf_entity, "blob_return", blob_return);
+ fiducial_key = wf->ReadInt( wf_entity, "fiducial_key", fiducial_key);
+ fiducial_return = wf->ReadInt( wf_entity, "fiducial_return", fiducial_return);
+ gripper_return = wf->ReadInt( wf_entity, "gripper_return", gripper_return);
+ laser_return = (stg_laser_return_t)wf->ReadInt( wf_entity, "laser_return", laser_return);
+ obstacle_return = wf->ReadInt( wf_entity, "obstacle_return", obstacle_return);
+ ranger_return = wf->ReadInt( wf_entity, "ranger_return", ranger_return);
+}
+
+GuiState:: GuiState() :
+ grid( false ),
+ mask( 0 ),
+ nose( false ),
+ outline( false )
+{ /* nothing to do */}
+
+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);
+}
+
// constructor
Model::Model( World* world,
@@ -136,8 +179,8 @@ Model::Model( World* world,
color( 0xFFFF0000 ), // red
data_fresh(false),
disabled(false),
- flag_list(NULL),
custom_visual_list( NULL ),
+ flag_list(NULL),
geom(),
has_default_block( true ),
id( Model::count++ ),
@@ -166,7 +209,7 @@ Model::Model( World* world,
wf(NULL),
wf_entity(0),
world(world),
- world_gui( dynamic_cast< WorldGui* >( world ) )
+ world_gui( dynamic_cast< WorldGui* >( world ) )
{
assert( modelsbyid );
assert( world );
@@ -467,6 +510,18 @@ bool Model::IsRelated( Model* mod2 )
return( (this == mod2) || IsAntecedent( mod2 ) || IsDescendent( mod2 ) );
}
+
+// bool Model::IsRelated( Model* that )
+// {
+// if( this == that )
+// return true;
+
+// for( GList* it = children; it; it=it->next )
+// {
+// if(
+// }
+
+
// get the model's velocity in the global frame
Velocity Model::GetGlobalVelocity()
{
@@ -702,11 +757,11 @@ void Model::DrawSelected()
token, gpose.x, gpose.y, gpose.z, rtod(gpose.a) );
PushColor( 0,0,0,1 ); // text color black
- gl_draw_string( 0.5,0.5,0.5, buf );
+ Gl::draw_string( 0.5,0.5,0.5, buf );
glRotatef( rtod(pose.a), 0,0,1 );
- gl_pose_shift( geom.pose );
+ Gl::pose_shift( geom.pose );
double dx = geom.size.x / 2.0 * 1.6;
double dy = geom.size.y / 2.0 * 1.6;
@@ -737,8 +792,8 @@ void Model::DrawTrailFootprint()
stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i );
glPushMatrix();
- gl_pose_shift( checkpoint->pose );
- gl_pose_shift( geom.pose );
+ Gl::pose_shift( checkpoint->pose );
+ Gl::pose_shift( geom.pose );
stg_color_unpack( checkpoint->color, &r, &g, &b, &a );
PushColor( r, g, b, 0.1 );
@@ -872,7 +927,7 @@ void Model::DrawBoundingBoxTree()
void Model::DrawBoundingBox()
{
- gl_pose_shift( geom.pose );
+ Gl::pose_shift( geom.pose );
PushColor( color );
@@ -917,7 +972,7 @@ void Model::PushLocalCoords()
if( parent )
glTranslatef( 0,0, parent->geom.size.z );
- gl_pose_shift( pose );
+ Gl::pose_shift( pose );
}
void Model::PopCoords()
@@ -943,7 +998,7 @@ void Model::AddCustomVisualizer( CustomVisualizer* custom_visual )
Canvas* canvas = world_gui->GetCanvas();
std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( custom_visual->name() );
if( i == canvas->_custom_options.end() ) {
- Option* op = new Option( custom_visual->name(), custom_visual->name(), "", true, world );
+ Option* op = new Option( custom_visual->name(), custom_visual->name(), "", true, world_gui );
canvas->_custom_options[ custom_visual->name() ] = op;
registerOption( op );
}
@@ -1038,7 +1093,7 @@ void Model::DrawStatus( Camera* cam )
glPolygonMode( GL_FRONT, GL_FILL );
glEnable( GL_POLYGON_OFFSET_FILL );
glPolygonOffset( 1.0, 1.0 );
- gl_draw_octagon( w, h, m );
+ Gl::draw_octagon( w, h, m );
glDisable( GL_POLYGON_OFFSET_FILL );
PopColor();
@@ -1047,13 +1102,13 @@ void Model::DrawStatus( Camera* cam )
glLineWidth( 1 );
glEnable( GL_LINE_SMOOTH );
glPolygonMode( GL_FRONT, GL_LINE );
- gl_draw_octagon( w, h, m );
+ Gl::draw_octagon( w, h, m );
glPopAttrib();
PopColor();
PushColor( BUBBLE_TEXT );
// draw text inside the bubble
- gl_draw_string( 2.5*m, 2.5*m, 0, this->say_string );
+ Gl::draw_string( 2.5*m, 2.5*m, 0, this->say_string );
PopColor();
}
}
@@ -1268,7 +1323,7 @@ void Model::DrawGrid( void )
vol.z.max = geom.size.z;
PushColor( 0,0,1,0.4 );
- gl_draw_grid(vol);
+ Gl::draw_grid(vol);
PopColor();
PopCoords();
}
View
1,190 libstage/model.hh
@@ -1,1190 +0,0 @@
-#ifndef MODEL_H
-#define MODEL_H
-
-class Camera;
-
-/** energy data packet */
-class PowerPack
-{
-public:
- PowerPack( Model* mod );
-
- /** The model that owns this object */
- Model* mod;
-
- /** Energy stored */
- stg_joules_t stored;
-
- /** Energy capacity */
- stg_joules_t capacity;
-
- /** TRUE iff the device is receiving energy from a charger */
- bool charging;
-
- /** OpenGL visualization of the powerpack state */
- void Visualize( Camera* cam );
-
- /** Print human-readable status on stdout, prefixed with the
- argument string */
- void Print( char* prefix );
-};
-
-class Visibility
-{
-public:
- bool blob_return;
- int fiducial_key;
- int fiducial_return;
- bool gripper_return;
- stg_laser_return_t laser_return;
- bool obstacle_return;
- bool ranger_return;
-
- Visibility() :
- blob_return( true ),
- fiducial_key( 0 ),
- fiducial_return( 0 ),
- gripper_return( false ),
- laser_return( LaserVisible ),
- obstacle_return( true ),
- ranger_return( true )
- { /* nothing do do */ }
-
- void Load( Worldfile* wf, int wf_entity )
- {
- blob_return = wf->ReadInt( wf_entity, "blob_return", blob_return);
- fiducial_key = wf->ReadInt( wf_entity, "fiducial_key", fiducial_key);
- fiducial_return = wf->ReadInt( wf_entity, "fiducial_return", fiducial_return);
- gripper_return = wf->ReadInt( wf_entity, "gripper_return", gripper_return);
- laser_return = (stg_laser_return_t)wf->ReadInt( wf_entity, "laser_return", laser_return);
- obstacle_return = wf->ReadInt( wf_entity, "obstacle_return", obstacle_return);
- ranger_return = wf->ReadInt( wf_entity, "ranger_return", ranger_return);
- }
-};
-
-/** Abstract class for adding visualizations to models. DataVisualize must be overloaded, and is then called in the models local coord system */
-class CustomVisualizer {
-public:
- //TODO allow user to specify name - which will show up in display filter
- virtual ~CustomVisualizer( void ) { }
- virtual void DataVisualize( Camera* cam ) = 0;
- virtual const std::string& name() = 0; //must return a name for visualization (careful not to return stack-memory)
-};
-
-
-/* Hooks for attaching special callback functions (not used as
- variables - we just need unique addresses for them.) */
-class CallbackHooks
-{
-public:
- char load;
- char save;
- char shutdown;
- char startup;
- char update;
-};
-
-/** Records model state and functionality in the GUI, if used */
-class GuiState
-{
-public:
- bool grid;
- unsigned int mask;
- bool nose;
- bool outline;
-
- GuiState() :
- grid( false ),
- mask( 0 ),
- nose( false ),
- outline( false )
- { /* nothing to do */}
-
- void 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);
- }
-
-};
-
-/// %Model class
-class Model : public Ancestor
-{
- friend class Ancestor;
- friend class World;
- friend class WorldGui;
- friend class Canvas;
- friend class Block;
- friend class Region;
- friend class BlockGroup;
-
-private:
- /** the number of models instatiated - used to assign unique IDs */
- static uint32_t count;
- static GHashTable* modelsbyid;
- std::vector<Option*> drawOptions;
- const std::vector<Option*>& getOptions() const { return drawOptions; }
-
-protected:
- GMutex* access_mutex;
- GPtrArray* blinkenlights;
- BlockGroup blockgroup;
- /** OpenGL display list identifier for the blockgroup */
- int blocks_dl;
-
- /** Iff true, 4 thin blocks are automatically added to the model,
- forming a solid boundary around the bounding box of the
- model. */
- int boundary;
-
- /** callback functions can be attached to any field in this
- structure. When the internal function model_change(<mod>,<field>)
- is called, the callback is triggered */
- GHashTable* callbacks;
-
- /** Default color of the model's blocks.*/
- stg_color_t color;
-
- /** This can be set to indicate that the model has new data that
- may be of interest to users. This allows polling the model
- instead of adding a data callback. */
- bool data_fresh;
- stg_bool_t disabled; //< if non-zero, the model is disabled
- GList* custom_visual_list;
- GList* flag_list;
- Geom geom;
- Pose global_pose;
- bool gpose_dirty; //< set this to indicate that global pose may have changed
- /** Controls our appearance and functionality in the GUI, if used */
- GuiState gui;
-
- bool has_default_block;
-
- /* hooks for attaching special callback functions (not used as
- variables - we just need unique addresses for them.) */
- CallbackHooks hooks;
-
- /** unique process-wide identifier for this model */
- uint32_t id;
- ctrlinit_t* initfunc;
- stg_usec_t interval; //< time between updates in us
- stg_usec_t last_update; //< time of last update in us
- bool map_caches_are_invalid;
- stg_meters_t map_resolution;
- stg_kg_t mass;
- bool on_update_list;
- bool on_velocity_list;
-
- /** Pointer to the parent of this model, possibly NULL. */
- Model* parent;
-
- /** The pose of the model in it's parents coordinate frame, or the
- global coordinate frame is the parent is NULL. */
- Pose pose;
-
- /** Optional attached PowerPack, defaults to NULL */
- PowerPack* power_pack;
-
- /** GData datalist can contain arbitrary named data items. Can be used
- by derived model types to store properties, and for user code
- to associate arbitrary items with a model. */
- GData* props;
- bool rebuild_displaylist; //< iff true, regenerate block display list before redraw
- char* say_string; //< if non-null, this string is displayed in the GUI
-
- stg_bool_t stall;
- /** Thread safety flag. Iff true, Update() may be called in
- parallel with other models. Defaults to false for safety */
- int subs; //< the number of subscriptions to this model
- bool thread_safe;
- GArray* trail;
- stg_model_type_t type;
- bool used; //< TRUE iff this model has been returned by GetUnusedModelOfType()
- Velocity velocity;
- stg_watts_t watts; //< power consumed by this model
- Worldfile* wf;
- int wf_entity;
- World* world; // pointer to the world in which this model exists
- WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode
-
-public:
-
- Visibility vis;
-
- void Lock()
- {
- if( access_mutex == NULL )
- access_mutex = g_mutex_new();
-
- assert( access_mutex );
- g_mutex_lock( access_mutex );
- }
-
- void Unlock()
- {
- assert( access_mutex );
- g_mutex_unlock( access_mutex );
- }
-
-
-private:
- /** Private copy constructor declared but not defined, to make it
- impossible to copy models. */
- explicit Model(const Model& original);
-
- /** Private assignment operator declared but not defined, to make it
- impossible to copy models */
- Model& operator=(const Model& original);
-
-protected:
-
- /// Register an Option for pickup by the GUI
- void registerOption( Option* opt )
- { drawOptions.push_back( opt ); }
-
- /** Check to see if the current pose will yield a collision with
- obstacles. Returns a pointer to the first entity we are in
- collision with, or NULL if no collision exists. */
- Model* TestCollision();
-
- void CommitTestedPose();
-
- void Map();
- void UnMap();
-
- void MapWithChildren();
- void UnMapWithChildren();
-
- int TreeToPtrArray( GPtrArray* array );
-
- /** raytraces a single ray from the point and heading identified by
- pose, in local coords */
- stg_raytrace_result_t Raytrace( const Pose &pose,
- const stg_meters_t range,
- const stg_ray_test_func_t func,
- const void* arg,
- const bool ztest = true );
-
- /** raytraces multiple rays around the point and heading identified
- by pose, in local coords */
- void Raytrace( const Pose &pose,
- const stg_meters_t range,
- const stg_radians_t fov,
- const stg_ray_test_func_t func,
- const void* arg,
- stg_raytrace_result_t* samples,
- const uint32_t sample_count,
- const bool ztest = true );
-
- stg_raytrace_result_t Raytrace( const stg_radians_t bearing,
- const stg_meters_t range,
- const stg_ray_test_func_t func,
- const void* arg,
- const bool ztest = true );
-
- void Raytrace( const stg_radians_t bearing,
- const stg_meters_t range,
- const stg_radians_t fov,
- const stg_ray_test_func_t func,
- const void* arg,
- stg_raytrace_result_t* samples,
- const uint32_t sample_count,
- const bool ztest = true );
-
-
- /** Causes this model and its children to recompute their global
- position instead of using a cached pose in
- Model::GetGlobalPose()..*/
- void GPoseDirtyTree();
-
- virtual void Startup();
- virtual void Shutdown();
- virtual void Update();
- virtual void UpdatePose();
-
- void StartUpdating();
- void StopUpdating();
-
- Model* ConditionalMove( Pose newpose );
-
- stg_meters_t ModelHeight();
-
- bool UpdateDue( void );
- void UpdateIfDue();
-
- void DrawBlocksTree();
- virtual void DrawBlocks();
- void DrawBoundingBox();
- void DrawBoundingBoxTree();
- virtual void DrawStatus( Camera* cam );
- void DrawStatusTree( Camera* cam );
-
- void DrawOriginTree();
- void DrawOrigin();
-
- void PushLocalCoords();
- void PopCoords();
-
- /** Draw the image stored in texture_id above the model */
- void DrawImage( uint32_t texture_id, Camera* cam, float alpha );
-
-
- /** static wrapper for DrawBlocks() */
- static void DrawBlocks( gpointer dummykey,
- Model* mod,
- void* arg );
-
- virtual void DrawPicker();
- virtual void DataVisualize( Camera* cam );
-
- virtual void DrawSelected(void);
-
- void DrawTrailFootprint();
- void DrawTrailBlocks();
- void DrawTrailArrows();
- void DrawGrid();
-
-
- void DrawBlinkenlights();
-
- void DataVisualizeTree( Camera* cam );
-
- virtual void PushColor( stg_color_t col )
- { world->PushColor( col ); }
-
- virtual void PushColor( double r, double g, double b, double a )
- { world->PushColor( r,g,b,a ); }
-
- virtual void PopColor(){ world->PopColor(); }
-
- void DrawFlagList();
-
- void DrawPose( Pose pose );
-
-public:
- void RecordRenderPoint( GSList** head, GSList* link,
- unsigned int* c1, unsigned int* c2 );
-
- void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax,
- stg_meters_t ymin, stg_meters_t ymax );
-
- /** Look up a model pointer by a unique model ID */
- static Model* LookupId( uint32_t id )
- { return (Model*)g_hash_table_lookup( modelsbyid, (void*)id ); }
-
- /** Constructor */
- Model( World* world,
- Model* parent,
- stg_model_type_t type = MODEL_TYPE_PLAIN );
-
- /** Destructor */
- virtual ~Model();
-
- void Say( const char* str );
- /** Attach a user supplied visualization to a model */
- void AddCustomVisualizer( CustomVisualizer* custom_visual );
- /** remove user supplied visualization to a model - supply the same ptr passed to AddCustomVisualizer */
- void RemoveCustomVisualizer( CustomVisualizer* custom_visual );
-
-
- void Load( Worldfile* wf, int wf_entity )
- {
- /** Set the worldfile and worldfile entity ID - must be called
- before Load() */
- SetWorldfile( wf, wf_entity );
- Load(); // call virtual load
- }
-
- /** Set the worldfile and worldfile entity ID */
- void SetWorldfile( Worldfile* wf, int wf_entity )
- { this->wf = wf; this->wf_entity = wf_entity; }
-
- /** configure a model by reading from the current world file */
- virtual void Load();
-
- /** save the state of the model to the current world file */
- virtual void Save();
-
- /** Should be called after all models are loaded, to do any last-minute setup */
- void Init();
- void InitRecursive();
-
- void AddFlag( Flag* flag );
- void RemoveFlag( Flag* flag );
-
- void PushFlag( Flag* flag );
- Flag* PopFlag();
-
- int GetFlagCount(){ return g_list_length( flag_list ); }
-
- /** Add a pointer to a blinkenlight to the model. */
- void AddBlinkenlight( stg_blinkenlight_t* b )
- { g_ptr_array_add( this->blinkenlights, b ); }
-
- /** Clear all blinkenlights from the model. Does not destroy the
- blinkenlight objects. */
- void ClearBlinkenlights()
- { g_ptr_array_set_size( this->blinkenlights, 0 ); }
-
- /** Disable the model. Its pose will not change due to velocity
- until re-enabled using Enable(). This is used for example when
- dragging a model with the mouse pointer. The model is enabled by
- default. */
- void Disable(){ disabled = true; };
-
- /** Enable the model, so that non-zero velocities will change the
- model's pose. Models are enabled by default. */
- void Enable(){ disabled = false; };
-
- /** Load a control program for this model from an external
- library */
- void LoadControllerModule( char* lib );
-
- /** Sets the redraw flag, so this model will be redrawn at the
- earliest opportunity */
- void NeedRedraw();
-
- /** Add a block to this model by loading it from a worldfile
- entity */
- void LoadBlock( Worldfile* wf, int entity );
-
- /** Add a block to this model centered at [x,y] with extent [dx, dy,
- dz] */
- void AddBlockRect( stg_meters_t x, stg_meters_t y,
- stg_meters_t dx, stg_meters_t dy,
- stg_meters_t dz );
-
- /** remove all blocks from this model, freeing their memory */
- void ClearBlocks();
-
- /** Returns a pointer to this model's parent model, or NULL if this
- model has no parent */
- Model* Parent(){ return this->parent; }
-
- Model* GetModel( const char* name );
- //int GuiMask(){ return this->gui_mask; };
-
- /** Returns a pointer to the world that contains this model */
- World* GetWorld(){ return this->world; }
-
- /** return the root model of the tree containing this model */
- Model* Root(){ return( parent ? parent->Root() : this ); }
-
- bool IsAntecedent( Model* testmod );
-
- /** returns true if model [testmod] is a descendent of this model */
- bool IsDescendent( Model* testmod );
-
- /** returns true if model [testmod] is a descendent or antecedent of this model */
- bool IsRelated( Model* testmod );
-
- /** get the pose of a model in the global CS */
- Pose GetGlobalPose();
-
- /** get the velocity of a model in the global CS */
- Velocity GetGlobalVelocity();
-
- /* set the velocity of a model in the global coordinate system */
- void SetGlobalVelocity( Velocity gvel );
-
- /** subscribe to a model's data */
- void Subscribe();
-
- /** unsubscribe from a model's data */
- void Unsubscribe();
-
- /** set the pose of model in global coordinates */
- void SetGlobalPose( Pose gpose );
-
- /** set a model's velocity in its parent's coordinate system */
- void SetVelocity( Velocity vel );
-
- /** set a model's pose in its parent's coordinate system */
- void SetPose( Pose pose );
-
- /** add values to a model's pose in its parent's coordinate system */
- void AddToPose( Pose pose );
-
- /** add values to a model's pose in its parent's coordinate system */
- void AddToPose( double dx, double dy, double dz, double da );
-
- /** set a model's geometry (size and center offsets) */
- void SetGeom( Geom src );
-
- /** set a model's geometry (size and center offsets) */
- void SetFiducialReturn( int fid );
-
- /** set a model's fiducial key: only fiducial finders with a
- matching key can detect this model as a fiducial. */
- void SetFiducialKey( int key );
-
- stg_color_t GetColor(){ return color; }
-
- // stg_laser_return_t GetLaserReturn(){ return laser_return; }
-
- /** Change a model's parent - experimental*/
- int SetParent( Model* newparent);
-
- /** Get a model's geometry - it's size and local pose (offset from
- origin in local coords) */
- Geom GetGeom(){ return geom; }
-
- /** Get the pose of a model in its parent's coordinate system */
- Pose GetPose(){ return pose; }
-
- /** Get a model's velocity (in its local reference frame) */
- Velocity GetVelocity(){ return velocity; }
-
- // guess what these do?
- void SetColor( stg_color_t col );
- void SetMass( stg_kg_t mass );
- void SetStall( stg_bool_t stall );
- void SetGripperReturn( int val );
- void SetLaserReturn( stg_laser_return_t val );
- void SetObstacleReturn( int val );
- void SetBlobReturn( int val );
- void SetRangerReturn( int val );
- void SetBoundary( int val );
- void SetGuiNose( int val );
- void SetGuiMask( int val );
- void SetGuiGrid( int val );
- void SetGuiOutline( int val );
- void SetWatts( stg_watts_t watts );
- void SetMapResolution( stg_meters_t res );
-
- bool DataIsFresh(){ return this->data_fresh; }
-
- /* attach callback functions to data members. The function gets
- called when the member is changed using SetX() accessor method */
-
- void AddCallback( void* address,
- stg_model_callback_t cb,
- void* user );
-
- int RemoveCallback( void* member,
- stg_model_callback_t callback );
-
- void CallCallbacks( void* address );
-
- /* wrappers for the generic callback add & remove functions that hide
- some implementation detail */
-
- void AddStartupCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &hooks.startup, cb, user ); };
-
- void RemoveStartupCallback( stg_model_callback_t cb )
- { RemoveCallback( &hooks.startup, cb ); };
-
- void AddShutdownCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &hooks.shutdown, cb, user ); };
-
- void RemoveShutdownCallback( stg_model_callback_t cb )
- { RemoveCallback( &hooks.shutdown, cb ); }
-
- void AddLoadCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &hooks.load, cb, user ); }
-
- void RemoveLoadCallback( stg_model_callback_t cb )
- { RemoveCallback( &hooks.load, cb ); }
-
- void AddSaveCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &hooks.save, cb, user ); }
-
- void RemoveSaveCallback( stg_model_callback_t cb )
- { RemoveCallback( &hooks.save, cb ); }
-
- void AddUpdateCallback( stg_model_callback_t cb, void* user )
- { AddCallback( &hooks.update, cb, user ); }
-
- void RemoveUpdateCallback( stg_model_callback_t cb )
- { RemoveCallback( &hooks.update, cb ); }
-
- /** named-property interface
- */
- void* GetProperty( char* key );
-
- /** @brief Set a named property of a Stage model.
-
- Set a property of a Stage model.
-
- This function can set both predefined and user-defined
- properties of a model. Predefined properties are intrinsic to
- every model, such as pose and color. Every supported predefined
- properties has its identifying string defined as a preprocessor
- macro in stage.h. Users should use the macro instead of a
- hard-coded string, so that the compiler can help you to avoid
- mis-naming properties.
-
- User-defined properties allow the user to attach arbitrary data
- pointers to a model. User-defined property data is not copied,
- so the original pointer must remain valid. User-defined property
- names are simple strings. Names beginning with an underscore
- ('_') are reserved for internal libstage use: users should not
- use names beginning with underscore (at risk of causing very
- weird behaviour).
-
- Any callbacks registered for the named property will be called.
-
- Returns 0 on success, or a positive error code on failure.
-
- *CAUTION* The caller is responsible for making sure the pointer
- points to data of the correct type for the property, so use
- carefully. Check the docs or the equivalent
- stg_model_set_<property>() function definition to see the type
- of data required for each property.
- */
- int SetProperty( char* key, void* data );
- void UnsetProperty( char* key );
-
- virtual void Print( char* prefix );
- virtual const char* PrintWithPose();
-
- /** Convert a pose in the world coordinate system into a model's
- local coordinate system. Overwrites [pose] with the new
- coordinate. */
- Pose GlobalToLocal( const Pose pose );
-
- /** Return the global pose (i.e. pose in world coordinates) of a
- pose specified in the model's local coordinate system */
- Pose LocalToGlobal( const Pose pose );
-
- /** Return the 3d point in world coordinates of a 3d point
- specified in the model's local coordinate system */
- stg_point3_t LocalToGlobal( const stg_point3_t local );
-
- /** returns the first descendent of this model that is unsubscribed
- and has the type indicated by the string */
- Model* GetUnsubscribedModelOfType( const stg_model_type_t type );
-
- /** returns the first descendent of this model that is unused
- and has the type indicated by the string. This model is tagged as used. */
- Model* GetUnusedModelOfType( const stg_model_type_t type );
-
- /** Returns the value of the model's stall boolean, which is true
- iff the model has crashed into another model */
- bool Stalled(){ return this->stall; }
-};
-
-
-// BLOBFINDER MODEL --------------------------------------------------------
-
-/** blobfinder data packet
- */
-typedef struct
-{
- stg_color_t color;
- uint32_t left, top, right, bottom;
- stg_meters_t range;
-} stg_blobfinder_blob_t;
-
-/// %ModelBlobfinder class
-class ModelBlobfinder : public Model
-{
-private:
- GArray* colors;
- GArray* blobs;
-
- // predicate for ray tracing
- static bool BlockMatcher( Block* testblock, Model* finder );
-
- static Option showBlobData;
-
- virtual void DataVisualize( Camera* cam );
-
-public:
- unsigned int scan_width;
- unsigned int scan_height;
- stg_meters_t range;
- stg_radians_t fov;
- stg_radians_t pan;
-
- static const char* typestr;
-
- // constructor
- ModelBlobfinder( World* world,
- Model* parent );
- // destructor
- ~ModelBlobfinder();
-
- virtual void Startup();
- virtual void Shutdown();
- virtual void Update();
- virtual void Load();
-
- stg_blobfinder_blob_t* GetBlobs( unsigned int* count )
- {
- if( count ) *count = blobs->len;
- return (stg_blobfinder_blob_t*)blobs->data;
- }
-
- /** Start finding blobs with this color.*/
- void AddColor( stg_color_t col );
-
- /** Stop tracking blobs with this color */
- void RemoveColor( stg_color_t col );
-
- /** Stop tracking all colors. Call this to clear the defaults, then
- add colors individually with AddColor(); */
- void RemoveAllColors();
-};
-
-
-
-
-// LASER MODEL --------------------------------------------------------
-
-/** laser sample packet
- */
-typedef struct
-{
- stg_meters_t range; ///< range to laser hit in meters
- double reflectance; ///< intensity of the reflection 0.0 to 1.0
-} stg_laser_sample_t;
-
-typedef struct
-{
- uint32_t sample_count;
- uint32_t resolution;
- Bounds range_bounds;
- stg_radians_t fov;
- stg_usec_t interval;
-} stg_laser_cfg_t;
-
-/// %ModelLaser class
-class ModelLaser : public Model
-{
-private:
- /** OpenGL displaylist for laser data */
- int data_dl;
- bool data_dirty;
-
- stg_laser_sample_t* samples;
- uint32_t sample_count;
- stg_meters_t range_min, range_max;
- stg_radians_t fov;
- uint32_t resolution;
-
- static Option showLaserData;
- static Option showLaserStrikes;
-
-public:
- static const char* typestr;
- // constructor
- ModelLaser( World* world,
- Model* parent );
-
- // destructor
- ~ModelLaser();
-
- virtual void Startup();
- virtual void Shutdown();
- virtual void Update();
- virtual void Load();
- virtual void Print( char* prefix );
- virtual void DataVisualize( Camera* cam );
-
- uint32_t GetSampleCount(){ return sample_count; }
-
- stg_laser_sample_t* GetSamples( uint32_t* count=NULL);
-
- void SetSamples( stg_laser_sample_t* samples, uint32_t count);
-
- // Get the user-tweakable configuration of the laser
- stg_laser_cfg_t GetConfig( );
-
- // Set the user-tweakable configuration of the laser
- void SetConfig( stg_laser_cfg_t cfg );
-};
-
-// \todo GRIPPER MODEL --------------------------------------------------------
-
-// typedef enum {
-// STG_GRIPPER_PADDLE_OPEN = 0, // default state
-// STG_GRIPPER_PADDLE_CLOSED,
-// STG_GRIPPER_PADDLE_OPENING,
-// STG_GRIPPER_PADDLE_CLOSING,
-// } stg_gripper_paddle_state_t;
-
-// typedef enum {
-// STG_GRIPPER_LIFT_DOWN = 0, // default state
-// STG_GRIPPER_LIFT_UP,
-// STG_GRIPPER_LIFT_UPPING, // verbed these to match the paddle state
-// STG_GRIPPER_LIFT_DOWNING,
-// } stg_gripper_lift_state_t;
-
-// typedef enum {
-// STG_GRIPPER_CMD_NOP = 0, // default state
-// STG_GRIPPER_CMD_OPEN,
-// STG_GRIPPER_CMD_CLOSE,
-// STG_GRIPPER_CMD_UP,
-// STG_GRIPPER_CMD_DOWN
-// } stg_gripper_cmd_type_t;
-
-// /** gripper configuration packet
-// */
-// typedef struct
-// {
-// Size paddle_size; ///< paddle dimensions
-
-// stg_gripper_paddle_state_t paddles;
-// stg_gripper_lift_state_t lift;
-
-// double paddle_position; ///< 0.0 = full open, 1.0 full closed
-// double lift_position; ///< 0.0 = full down, 1.0 full up
-
-// stg_meters_t inner_break_beam_inset; ///< distance from the end of the paddle
-// stg_meters_t outer_break_beam_inset; ///< distance from the end of the paddle
-// stg_bool_t paddles_stalled; // true iff some solid object stopped
-// // the paddles closing or opening
-
-// GSList *grip_stack; ///< stack of items gripped
-// int grip_stack_size; ///< maximum number of objects in stack, or -1 for unlimited
-
-// double close_limit; ///< How far the gripper can close. If < 1.0, the gripper has its mouth full.
-
-// } stg_gripper_config_t;
-
-// /** gripper command packet
-// */
-// typedef struct
-// {
-// stg_gripper_cmd_type_t cmd;
-// int arg;
-// } stg_gripper_cmd_t;
-
-
-// /** gripper data packet
-// */
-// typedef struct
-// {
-// stg_gripper_paddle_state_t paddles;
-// stg_gripper_lift_state_t lift;
-
-// double paddle_position; ///< 0.0 = full open, 1.0 full closed
-// double lift_position; ///< 0.0 = full down, 1.0 full up
-
-// stg_bool_t inner_break_beam; ///< non-zero iff beam is broken
-// stg_bool_t outer_break_beam; ///< non-zero iff beam is broken
-
-// stg_bool_t paddle_contacts[2]; ///< non-zero iff paddles touch something
-
-// stg_bool_t paddles_stalled; // true iff some solid object stopped
-// // the paddles closing or opening
-
-// int stack_count; ///< number of objects in stack
-
-
-// } stg_gripper_data_t;
-
-
-// \todo BUMPER MODEL --------------------------------------------------------
-
-// typedef struct
-// {
-// Pose pose;
-// stg_meters_t length;
-// } stg_bumper_config_t;
-
-// typedef struct
-// {
-// Model* hit;
-// stg_point_t hit_point;
-// } stg_bumper_sample_t;
-
-
-// FIDUCIAL MODEL --------------------------------------------------------
-
-/** fiducial config packet
- */
-typedef struct
-{
- stg_meters_t max_range_anon; //< maximum detection range
- stg_meters_t max_range_id; ///< maximum range at which the ID can be read
- stg_meters_t min_range; ///< minimum detection range
- stg_radians_t fov; ///< field of view
- stg_radians_t heading; ///< center of field of view
-
- /// only detects fiducials with a key string that matches this one
- /// (defaults to NULL)
- int key;
-} stg_fiducial_config_t;
-
-/** fiducial data packet
- */
-typedef struct
-{
- stg_meters_t range; ///< range to the target
- stg_radians_t bearing; ///< bearing to the target
- Pose geom; ///< size and relative angle of the target
- Pose pose; ///< Absolute accurate position of the target in world coordinates (it's cheating to use this in robot controllers!)
- int id; ///< the identifier of the target, or -1 if none can be detected.
-
-} stg_fiducial_t;
-
-/// %ModelFiducial class
-class ModelFiducial : public Model
-{
-private:
- // if neighbor is visible, add him to the fiducial scan
- void AddModelIfVisible( Model* him );
-
- // static wrapper function can be used as a function pointer
- static int AddModelIfVisibleStatic( Model* him, ModelFiducial* me )
- { if( him != me ) me->AddModelIfVisible( him ); return 0; }
-
- virtual void Update();
- virtual void DataVisualize( Camera* cam );
-
- GArray* data;
-
- static Option showFiducialData;
-
-public:
- static const char* typestr;
- // constructor
- ModelFiducial( World* world,
- Model* parent );
- // destructor
- virtual ~ModelFiducial();
-
- virtual void Load();
- void Shutdown( void );
-
- stg_meters_t max_range_anon; //< maximum detection range
- stg_meters_t max_range_id; ///< maximum range at which the ID can be read
- stg_meters_t min_range; ///< minimum detection range
- stg_radians_t fov; ///< field of view
- stg_radians_t heading; ///< center of field of view
- int key; ///< /// only detect fiducials with a key that matches this one (defaults 0)
-
- stg_fiducial_t* fiducials; ///< array of detected fiducials
- uint32_t fiducial_count; ///< the number of fiducials detected
-};
-
-
-// RANGER MODEL --------------------------------------------------------
-
-typedef struct
-{
- Pose pose;
- Size size;
- Bounds bounds_range;
- stg_radians_t fov;
- int ray_count;
-} stg_ranger_sensor_t;
-
-/// %ModelRanger class
-class ModelRanger : public Model
-{
-protected:
-
- virtual void Startup();
- virtual void Shutdown();
- virtual void Update();
- virtual void DataVisualize( Camera* cam );
-
-public:
- static const char* typestr;
- // constructor
- ModelRanger( World* world,
- Model* parent );
- // destructor
- virtual ~ModelRanger();
-
- virtual void Load();
- virtual void Print( char* prefix );
-
- uint32_t sensor_count;
- stg_ranger_sensor_t* sensors;
- stg_meters_t* samples;
-
-private:
- static Option showRangerData;
- static Option showRangerTransducers;
-
-};
-
-// BLINKENLIGHT MODEL ----------------------------------------------------
-class ModelBlinkenlight : public Model
-{
-private:
- double dutycycle;
- bool enabled;
- stg_msec_t period;
- bool on;
-
- static Option showBlinkenData;
-public:
-
- static const char* typestr;
- ModelBlinkenlight( World* world,
- Model* parent );
-
- ~ModelBlinkenlight();
-
- virtual void Load();
- virtual void Update();
- virtual void DataVisualize( Camera* cam );
-};
-
-// CAMERA MODEL ----------------------------------------------------
-typedef struct {
- // GL_V3F
- GLfloat x, y, z;
-} ColoredVertex;
-
-/// %ModelCamera class
-class ModelCamera : public Model
-{
-private:
- Canvas* _canvas;
-
- GLfloat* _frame_data; //opengl read buffer
- GLubyte* _frame_color_data; //opengl read buffer
-
- bool _valid_vertexbuf_cache;
- ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length)
-
- int _width; //width of buffer
- int _height; //height of buffer
- static const int _depth = 4;
-
- int _camera_quads_size;
- GLfloat* _camera_quads;
- GLubyte* _camera_colors;
-
- static Option showCameraData;
-
- PerspectiveCamera _camera;
- float _yaw_offset; //position camera is mounted at
- float _pitch_offset;
-
- ///Take a screenshot from the camera's perspective. return: true for sucess, and data is available via FrameDepth() / FrameColor()
- bool GetFrame();
-
-public:
- ModelCamera( World* world,
- Model* parent );
-
- ~ModelCamera();
-
- virtual void Load();
-
- ///Capture a new frame ( calls GetFrame )
- virtual void Update();
-
- ///Draw Camera Model - TODO
- //virtual void Draw( uint32_t flags, Canvas* canvas );
-
- ///Draw camera visualization
- virtual void DataVisualize( Camera* cam );
-
- ///width of captured image
- inline int getWidth( void ) const { return _width; }
-
- ///height of captured image
- inline int getHeight( void ) const { return _height; }
-
- ///get reference to camera used
- inline const PerspectiveCamera& getCamera( void ) const { return _camera; }
-
- ///get a reference to camera depth buffer
- inline const GLfloat* FrameDepth() const { return _frame_data; }
-
- ///get a reference to camera color image. 3 bytes (RGB) per pixel
- inline const GLubyte* FrameColor() const { return _frame_color_data; }
-
- ///change the pitch
- inline void setPitch( float pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; }
-
- ///change the yaw
- inline void setYaw( float yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; }
-};
-
-// POSITION MODEL --------------------------------------------------------
-
-/** Define a position control method */
-typedef enum
- { STG_POSITION_CONTROL_VELOCITY,
- STG_POSITION_CONTROL_POSITION
- } stg_position_control_mode_t;
-
-/** Define a localization method */
-typedef enum
- { STG_POSITION_LOCALIZATION_GPS,
- STG_POSITION_LOCALIZATION_ODOM
- } stg_position_localization_mode_t;
-
-/** Define a driving method */
-typedef enum
- { STG_POSITION_DRIVE_DIFFERENTIAL,
- STG_POSITION_DRIVE_OMNI,
- STG_POSITION_DRIVE_CAR
- } stg_position_drive_mode_t;
-
-
-/// %ModelPosition class
-class ModelPosition : public Model
-{
- friend class Canvas;
-
-private:
- Pose goal; //< the current velocity or pose to reach, depending on the value of control_mode
- stg_position_control_mode_t control_mode;
- stg_position_drive_mode_t drive_mode;
- stg_position_localization_mode_t localization_mode; ///< global or local mode
- Velocity integration_error; ///< errors to apply in simple odometry model
-
- Waypoint* waypoints;
- uint32_t waypoint_count;
- void DrawWaypoints();
-
-private:
- static Option showCoords;
- static Option showWaypoints;
-
-public:
- static const char* typestr;
- // constructor
- ModelPosition( World* world,
- Model* parent );
- // destructor
- ~ModelPosition();
-
- virtual void Startup();
- virtual void Shutdown();
- virtual void Update();
- virtual void Load();
-
- virtual void DataVisualize( Camera* cam );
-
- /** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */
- Waypoint* SetWaypoints( Waypoint* wps, uint32_t count );
-
- /** Set the current pose estimate.*/
- void SetOdom( Pose odom );
-
- /** Sets the control_mode to STG_POSITION_CONTROL_VELOCITY and sets
- the goal velocity. */
- void SetSpeed( double x, double y, double a );
- void SetXSpeed( double x );
- void SetYSpeed( double y );
- void SetZSpeed( double z );
- void SetTurnSpeed( double a );
- void SetSpeed( Velocity vel );
-
- /** Sets the control mode to STG_POSITION_CONTROL_POSITION and sets
- the goal pose */
- void GoTo( double x, double y, double a );
- void GoTo( Pose pose );
-
- // localization state
- Pose est_pose; ///< position estimate in local coordinates
- Pose est_pose_error; ///< estimated error in position estimate
- Pose est_origin; ///< global origin of the local coordinate system
-};
-
-#endif // MODEL_H
View
4 libstage/model_blinkenlight.cc
@@ -48,8 +48,10 @@ enabled 1
*/
//#define DEBUG 1
-#include "stage_internal.hh"
+#include "stage.hh"
+#include "worldfile.hh"
#include "option.hh"
+using namespace Stg;
//TODO make instance attempt to register an option (as customvisualizations do)
Option ModelBlinkenlight::showBlinkenData( "Show Blink", "show_blinken", "", true, NULL );
View
6 libstage/model_blobfinder.cc
@@ -14,7 +14,11 @@
//#define DEBUG
#include <sys/time.h>
-#include "stage_internal.hh"
+
+#include "stage.hh"
+#include "option.hh"
+#include "worldfile.hh"
+using namespace Stg;
static const stg_watts_t DEFAULT_BLOBFINDERWATTS = 2.0;
static const stg_meters_t DEFAULT_BLOBFINDERRANGE = 12.0;
View
3  libstage/model_callbacks.cc
@@ -1,4 +1,5 @@
-#include "stage_internal.hh"
+#include "stage.hh"
+using namespace Stg;
int key_gen( Model* mod, void* address )
{
View
6 libstage/model_camera.cc
@@ -13,7 +13,11 @@
#define CAMERA_FAR_CLIP 8.0
//#define DEBUG 1
-#include "stage_internal.hh"
+#include "canvas.hh"
+#include "worldfile.hh"
+
+using namespace Stg;
+
#include <sstream>
#include <iomanip>
View
19 libstage/model_fiducial.cc
@@ -15,7 +15,12 @@
#include <assert.h>
#include <math.h>
-#include "stage_internal.hh"
+
+#include "stage.hh"
+#include "gl.hh"
+#include "option.hh"
+#include "worldfile.hh"
+using namespace Stg;
const stg_meters_t DEFAULT_FIDUCIAL_RANGEMIN = 0.0;
const stg_meters_t DEFAULT_FIDUCIAL_RANGEMAXID = 5.0;
@@ -176,9 +181,11 @@ void ModelFiducial::AddModelIfVisible( Model* him )
range = ray.range;
Model* hitmod = ray.mod;
- // printf( "ray hit %s and was seeking LOS to %s\n",
- //hitmod ? hitmod->Token() : "null",
- //him->Token() );
+// printf( "ray hit %s and was seeking LOS to %s\n",
+// hitmod ? hitmod->Token() : "null",
+// him->Token() );
+
+ //assert( ! (hitmod == this) );
// if it was him, we can see him
if( hitmod == him )
@@ -296,7 +303,7 @@ void ModelFiducial::DataVisualize( Camera* cam )
glEnd();
glPushMatrix();
- gl_coord_shift( dx,dy,0,fid.geom.a );
+ Gl::coord_shift( dx,dy,0,fid.geom.a );
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
glRectf( -fid.geom.x/2.0, -fid.geom.y/2.0,
@@ -307,7 +314,7 @@ void ModelFiducial::DataVisualize( Camera* cam )
snprintf(idstr, 31, "%d", fid.id );
PushColor( 0,0,0,1 ); // black
- gl_draw_string( 0,0,0, idstr );
+ Gl::draw_string( 0,0,0, idstr );
PopColor();
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
View
6 libstage/model_laser.cc
@@ -14,7 +14,11 @@
//#define DEBUG
#include <sys/time.h>
-#include "stage_internal.hh"
+
+#include "stage.hh"
+#include "worldfile.hh"
+#include "option.hh"
+using namespace Stg;
// DEFAULT PARAMETERS FOR LASER MODEL
static const bool DEFAULT_FILLED = true;
View
4 libstage/model_load.cc
@@ -7,8 +7,10 @@
#include <string.h>
#include <ltdl.h> // for library module loading
-#include "stage_internal.hh"
+#include "stage.hh"
+#include "worldfile.hh"
#include "file_manager.hh"
+using namespace Stg;
//#define DEBUG
View
29 libstage/model_position.cc
@@ -17,7 +17,12 @@
#include <stdlib.h>
//#define DEBUG
-#include "stage_internal.hh"
+
+#include "stage.hh"
+#include "gl.hh"
+#include "option.hh"
+#include "worldfile.hh"
+using namespace Stg;
/**
@ingroup model
@@ -589,11 +594,11 @@ void ModelPosition::DataVisualize( Camera* cam )
glPushMatrix();
// back into global coords
- gl_pose_inverse_shift( GetGlobalPose() );
+ Gl::pose_inverse_shift( GetGlobalPose() );
- gl_pose_shift( est_origin );
+ Gl::pose_shift( est_origin );
PushColor( 1,0,0,1 ); // origin in red
- gl_draw_origin( 0.5 );
+ Gl::draw_origin( 0.5 );
glEnable (GL_LINE_STIPPLE);
glLineStipple (3, 0xAAAA);
@@ -609,22 +614,22 @@ void ModelPosition::DataVisualize( Camera* cam )
char label[64];
snprintf( label, 64, "x:%.3f", est_pose.x );
- gl_draw_string( est_pose.x / 2.0, -0.5, 0, label );
+ Gl::draw_string( est_pose.x / 2.0, -0.5, 0, label );
snprintf( label, 64, "y:%.3f", est_pose.y );
- gl_draw_string( est_pose.x + 0.5 , est_pose.y / 2.0, 0, (const char*)label );
+ Gl::draw_string( est_pose.x + 0.5 , est_pose.y / 2.0, 0, (const char*)label );
PopColor();
- gl_pose_shift( est_pose );
+ Gl::pose_shift( est_pose );
PushColor( 0,1,0,1 ); // pose in green
- gl_draw_origin( 0.5 );
+ Gl::draw_origin( 0.5 );
PopColor();
- gl_pose_shift( geom.pose );
+ Gl::pose_shift( geom.pose );
PushColor( 0,0,1,1 ); // offset in blue
- gl_draw_origin( 0.5 );
+ Gl::draw_origin( 0.5 );
PopColor();
double r,g,b,a;
@@ -650,8 +655,8 @@ void ModelPosition::DrawWaypoints()
glPushMatrix();
PushColor( color );
- gl_pose_inverse_shift( pose );
- gl_pose_shift( est_origin );
+ Gl::pose_inverse_shift( pose );
+ Gl::pose_shift( est_origin );
for( unsigned int i=0; i < waypoint_count; i++ )
waypoints[i].Draw();
View
3  libstage/model_props.cc
@@ -1,5 +1,6 @@
-#include "stage_internal.hh"
+#include "stage.hh"
+using namespace Stg;
#define MATCH(A,B) (strcmp(A,B)== 0)
View
7 libstage/model_ranger.cc
@@ -76,7 +76,12 @@
*/
//#define DEBUG 1
-#include "stage_internal.hh"
+
+#include "stage.hh"
+#include "worldfile.hh"
+#include "option.hh"
+using namespace Stg;
+
#include <math.h>
static const stg_watts_t RANGER_WATTSPERSENSOR = 0.2;
View
5 libstage/option.cc
@@ -1,5 +1,8 @@
-#include "stage_internal.hh"
+#include "stage.hh"
+#include "worldfile.hh"
+#include "option.hh"
+#include "canvas.hh"
using namespace Stg;
View
1  libstage/options_dlg.hh
@@ -10,6 +10,7 @@
#include <vector>
#include <set>
+#include "stage.hh"
#include "option.hh"
namespace Stg {
View
3  libstage/powerpack.cc
@@ -5,7 +5,8 @@
SVN: $Id: stage.hh 7279 2009-01-18 00:10:21Z rtv $
*/
-#include "stage_internal.hh"
+#include "stage.hh"
+using namespace Stg;
PowerPack::PowerPack( Model* mod ) :
mod( mod), stored( 0.0 ), capacity( 0.0 ), charging( false )
View
8 libstage/region.cc
@@ -4,7 +4,9 @@
Copyright Richard Vaughan 2008
*/
-#include "stage_internal.hh"
+#include "region.hh"
+#include "gl.hh"
+using namespace Stg;
const uint32_t Region::WIDTH = REGIONWIDTH;
const uint32_t Region::SIZE = REGIONSIZE;
@@ -114,7 +116,7 @@ void SuperRegion::Draw( bool drawall )
char buf[32];
snprintf( buf, 15, "%lu", count );
- gl_draw_string( 1<<SBITS, 1<<SBITS, 0, buf );
+ Gl::draw_string( 1<<SBITS, 1<<SBITS, 0, buf );
glColor3f( 1.0,0,0 );
@@ -127,7 +129,7 @@ void SuperRegion::Draw( bool drawall )
continue;
snprintf( buf, 15, "%lu", r->count );
- gl_draw_string( x<<RBITS, y<<RBITS, 0, buf );
+ Gl::draw_string( x<<RBITS, y<<RBITS, 0, buf );
for( unsigned int p=0; p<Region::WIDTH; p++ )
View
294 libstage/region.hh
@@ -1,195 +1,149 @@
+#pragma once
/*
region.hh
data structures supporting multi-resolution ray tracing in world class.
Copyright Richard Vaughan 2008
*/
-const uint32_t RBITS = 6; // regions contain (2^RBITS)^2 pixels
-const uint32_t SBITS = 5; // superregions contain (2^SBITS)^2 regions
-const uint32_t SRBITS = RBITS+SBITS;
+#include "stage.hh"
-class Stg::Region
+namespace Stg
{
- friend class SuperRegion;
-
-private:
- static const uint32_t REGIONWIDTH = 1<<RBITS;
- static const uint32_t REGIONSIZE = REGIONWIDTH*REGIONWIDTH;
-
- //stg_cell_t cells[REGIONSIZE];
- stg_cell_t* cells;
-public:
- uint32_t count; // number of blocks rendered into these cells
-
- Region()
- : cells( new stg_cell_t[REGIONSIZE] ),
- count(0)
- {
- /* do nothing */
- }
+// a bit of experimenting suggests that these values are fast. YMMV.
+#define RBITS 4 // regions contain (2^RBITS)^2 pixels
+#define SBITS 4 // superregions contain (2^SBITS)^2 regions
+#define SRBITS (RBITS+SBITS)
- ~Region()
- {
- delete[] cells;
- }
+#define REGIONWIDTH (1<<RBITS)
+#define REGIONSIZE REGIONWIDTH*REGIONWIDTH
- stg_cell_t* GetCell( int32_t x, int32_t y )
- {
- uint32_t index = x + (y*REGIONWIDTH);
-#ifdef DEBUG
- assert( index >=0 );
- assert( index < REGIONSIZE );
-#endif
-
- printf("GET CELL [%d %d] index %d gives %p\n",
- x, y, index, &cells[index] );
-
- return &cells[index];
- }
-
- // add a block to a region cell specified in REGION coordinates
- void AddBlock( Model* mod,
- stg_color_t col,
- Bounds zbounds,
- int32_t x, int32_t y,
- unsigned int* count2 )
- {
- assert( mod );
-
- stg_cell_t* cell = GetCell( x, y );
- assert( cell );
-
- // put a new empty pointer on the front of the list to use below
- cell->list = g_slist_prepend( cell->list, NULL );
-
- // this is the data to be stored in this cell
- stg_list_entry_t el;
- el.cell = cell;
- el.link = cell->list;
- el.counter1 = &count;
- el.counter2 = count2;
- el.mod = mod;
- el.color = col;
- el.zbounds = zbounds;
-
- // copy this struct onto the end of the model's array
- g_array_append_val( mod->rendered_points, el );
-
- // stash this record at the head of the list
- cell->list->data =
- & g_array_index( mod->rendered_points,
- stg_list_entry_t,
- mod->rendered_points->len-1 );
-
- count++;
- }
-
-};
+#define SUPERREGIONWIDTH (1<<SBITS)
+#define SUPERREGIONSIZE SUPERREGIONWIDTH*SUPERREGIONWIDTH
-
-class Stg::SuperRegion
+class Cell
{
+ friend class Region;
+ friend class SuperRegion;
friend class World;
-
-private:
- static const uint32_t SUPERREGIONWIDTH = 1<<SBITS;
- static const uint32_t SUPERREGIONSIZE = SUPERREGIONWIDTH*SUPERREGIONWIDTH;
-
- Region regions[SUPERREGIONSIZE];
- uint32_t count; // number of blocks rendered into these regions
-
- stg_point_int_t origin;
+ friend class Block;
+private:
+ Region* region;
+ GSList* list;
public:
+ Cell()
+ : region( NULL),
+ list(NULL)
+ { /* do nothing */ }
- SuperRegion( int32_t x, int32_t y )
- {
- count = 0;
- origin.x = x;
- origin.y = y;
- }
-
- ~SuperRegion()
- {
- }
+ inline void RemoveBlock( Block* b );
+ inline void AddBlock( Block* b );
+ inline void AddBlockNoRecord( Block* b );
+};
- // get the region x,y from the region array
- Region* GetRegion( int32_t x, int32_t y )
- {
- int32_t index = x + (y*SUPERREGIONWIDTH);
- assert( index >=0 );
- assert( index < (int)SUPERREGIONSIZE );
- return &regions[ index ];
- }
+
+class Region
+{
+ friend class SuperRegion;
- // add a block to a cell specified in superregion coordinates
-