Revision: 6841
http://playerstage.svn.sourceforge.net/playerstage/?rev=6841&view=rev
Author: rtv
Date: 2008-07-10 23:31:47 -0700 (Thu, 10 Jul 2008)
Log Message:
-----------
fixed long-standing model geometry bug
Modified Paths:
--------------
code/stage/trunk/docsrc/stage.txt
code/stage/trunk/libstage/block.cc
code/stage/trunk/libstage/canvas.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/libstage/worldgui.cc
code/stage/trunk/worlds/fasr.world
Added Paths:
-----------
code/stage/trunk/worlds/CMakeLists.txt
code/stage/trunk/worlds/benchmark/CMakeLists.txt
Modified: code/stage/trunk/docsrc/stage.txt
===================================================================
--- code/stage/trunk/docsrc/stage.txt 2008-07-10 22:45:42 UTC (rev 6840)
+++ code/stage/trunk/docsrc/stage.txt 2008-07-11 06:31:47 UTC (rev 6841)
@@ -81,9 +81,10 @@
<ul>
<li>Richard Vaughan <tt>([EMAIL PROTECTED])</tt>
-<li>Brian Gerkey <tt>([EMAIL PROTECTED])</tt>
+<li>Brian Gerkey <tt>([EMAIL PROTECTED])</tt>
<li>Reed Hedges <tt>([EMAIL PROTECTED])</tt>
-<li>Andrew Howard <tt>([EMAIL PROTECTED])</tt>
+<li>Andrew Howard <tt>([EMAIL PROTECTED])</tt>
+<li>Toby Collett <<tt>([EMAIL PROTECTED])</tt>
<li>Pooya Karimian <tt>([EMAIL PROTECTED])</tt>
<li>Jeremy Asher <tt>([EMAIL PROTECTED])</tt>
<li>Alex Couture-Beil <tt>([EMAIL PROTECTED])</tt>
Modified: code/stage/trunk/libstage/block.cc
===================================================================
--- code/stage/trunk/libstage/block.cc 2008-07-10 22:45:42 UTC (rev 6840)
+++ code/stage/trunk/libstage/block.cc 2008-07-11 06:31:47 UTC (rev 6841)
@@ -145,6 +145,7 @@
local.z = zmin;
global = mod->LocalToGlobal( local );
+ //global = local;
pts_global_pixels[p].x = mod->GetWorld()->MetersToPixels(
global.x );
pts_global_pixels[p].y = mod->GetWorld()->MetersToPixels(
global.y );
Modified: code/stage/trunk/libstage/canvas.cc
===================================================================
--- code/stage/trunk/libstage/canvas.cc 2008-07-10 22:45:42 UTC (rev 6840)
+++ code/stage/trunk/libstage/canvas.cc 2008-07-11 06:31:47 UTC (rev 6841)
@@ -410,7 +410,8 @@
world->TogglePause();
break;
case ' ': // space bar
- //current_camera->reset();
+
+ //current_camera->reset();
if ( wf )
current_camera->Load( wf, wf->LookupEntity(
"window" ) );
else
@@ -659,6 +660,10 @@
if( showBlocks )
DrawBlocks();
+ // useful debug - puts a point at the origin of each model
+ //for( GList* it = world->StgWorld::children; it; it=it->next )
+ // ((StgModel*)it->data)->DrawOriginTree();
+
// draw the model-specific visualizations
if( showData ) {
if ( visualizeAll ) {
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2008-07-10 22:45:42 UTC (rev 6840)
+++ code/stage/trunk/libstage/model.cc 2008-07-11 06:31:47 UTC (rev 6841)
@@ -571,7 +571,7 @@
// should one day do all this with affine transforms for neatness?
stg_pose_t StgModel::LocalToGlobal( stg_pose_t pose )
{
- return pose_sum( pose_sum( geom.pose, GetGlobalPose()), pose );
+ return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose );
}
stg_point3_t StgModel::LocalToGlobal( stg_point3_t point )
@@ -868,12 +868,67 @@
}
}
+void StgModel::PushMyPose()
+{
+ world->PushPose();
+ world->ShiftPose( &pose );
+}
+
+void StgModel::PopPose()
+{
+ world->PopPose();
+}
+
+void StgModel::ShiftPose( stg_pose_t* pose )
+{
+ world->ShiftPose( pose );
+}
+
+void StgModel::ShiftToTop()
+{
+ stg_pose_t top;
+ bzero( &top, sizeof(top));
+ top.z = geom.size.z;
+ ShiftPose( &top );
+}
+
+void StgModel::DrawOriginTree()
+{
+ PushMyPose();
+
+ world->DrawPose();
+
+ ShiftToTop();
+
+ for( GList* it=children; it; it=it->next )
+ ((StgModel*)it->data)->DrawOriginTree();
+
+ PopPose();
+}
+
+void StgWorld::DrawPose()
+{
+ PushColor( 0,0,0,1 );
+ glPointSize( 4 );
+
+ stg_pose_t* gpose = PeekPose();
+
+ glBegin( GL_POINTS );
+ glVertex3f( gpose->x, gpose->y, gpose->z );
+ glEnd();
+
+ PopColor();
+}
+
+
void StgModel::DrawBlocksTree( )
{
PushLocalCoords();
+ LISTMETHOD( children, StgModel*, DrawBlocksTree );
+
+ gl_pose_shift( &geom.pose );
DrawBlocks();
- LISTMETHOD( children, StgModel*, DrawBlocksTree );
PopCoords();
}
@@ -904,13 +959,13 @@
// TODO - fix this!
//if( rebuild_displaylist )
- {
+ //{
//rebuild_displaylist = false;
//glNewList( blocks_dl, GL_COMPILE );
LISTMETHOD( this->blocks, StgBlock*, Draw );
//glEndList();
- }
+ //}
//glCallList( blocks_dl );
}
@@ -924,7 +979,7 @@
glTranslatef( 0,0, parent->geom.size.z );
gl_pose_shift( &pose );
- gl_pose_shift( &geom.pose );
+ //gl_pose_shift( &geom.pose );
// useful debug - draw a point at the local origin
// PushColor( color );
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2008-07-10 22:45:42 UTC (rev
6840)
+++ code/stage/trunk/libstage/model_laser.cc 2008-07-11 06:31:47 UTC (rev
6841)
@@ -26,13 +26,8 @@
static const unsigned int DEFAULT_SAMPLES = 180;
static const stg_msec_t DEFAULT_INTERVAL_MS = 100;
static const unsigned int DEFAULT_RESOLUTION = 1;
+static const char* DEFAULT_COLOR = "blue";
-static const char DEFAULT_GEOM_COLOR[] = "blue";
-//static const char BRIGHT_COLOR[] = "blue";
-//static const char CFG_COLOR[] = "light steel blue";
-//static const char COLOR[] = "steel blue";
-//static const char FILL_COLOR[] = "powder blue";
-
Option StgModelLaser::showLaserData( "Show Laser Data", "show_laser", "", true
);
Option StgModelLaser::showLaserStrikes( "Show Laser Data",
"show_laser_strikes", "", false );
@@ -55,12 +50,11 @@
range_min 0.0
range_max 8.0
fov 3.14159
- laser_sample_skip 1
+ resolution 1
# model properties
size [ 0.15 0.15 0.2 ]
color "blue"
- watts 17.5 # approximately correct for SICK LMS200
)
@endverbatim
@@ -74,7 +68,7 @@
the maximum range reported by the scanner, in meters. The scanner will not
detect objects beyond this range.
- fov <float>\n
the angular field of view of the scanner, in radians.
-- laser_sample_skip <int>\n
+- resolution <int>\n
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.
*/
@@ -95,7 +89,7 @@
SetGeom( geom );
// set default color
- SetColor( stg_lookup_color(DEFAULT_GEOM_COLOR));
+ SetColor( stg_lookup_color(DEFAULT_COLOR));
range_min = DEFAULT_MINRANGE;
range_max = DEFAULT_MAXRANGE;
@@ -130,7 +124,7 @@
range_min = wf->ReadLength( wf_entity, "range_min", range_min);
range_max = wf->ReadLength( wf_entity, "range_max", range_max );
fov = wf->ReadAngle( wf_entity, "fov", fov );
- resolution = wf->ReadInt( wf_entity, "laser_sample_skip", resolution );
+ resolution = wf->ReadInt( wf_entity, "resolution", resolution );
if( resolution < 1 )
{
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2008-07-10 22:45:42 UTC (rev 6840)
+++ code/stage/trunk/libstage/stage.hh 2008-07-11 06:31:47 UTC (rev 6841)
@@ -112,7 +112,7 @@
/// Author string
const char AUTHORS[] =
- "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges,
Pooya Karimian and contributors.";
+ "Richard Vaughan, Brian Gerkey, Andrew Howard, Reed Hedges,
Pooya Karimian, Toby Collett, Jeremy Asher, Alex Couture-Beil and
contributors.";
/// Project website string
const char WEBSITE[] = "http://playerstage.org";;
@@ -960,46 +960,50 @@
private:
static GList* world_list;
- /** Update all existing worlds */
+
+ /** Coordinate system stack - experimental*/
+ GQueue* csstack;
+
+ void PushPose();
+ void PopPose();
+ stg_pose_t* PeekPose();
+ void ShiftPose( stg_pose_t* pose );
+ void DrawPose();
- static bool quit_all; // quit all worlds ASAP
- static unsigned int next_id; //< initialized to zero, used tob
- //allocate unique sequential world ids
- 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_start;
-
- bool quit; // quit this world ASAP
-
- // 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,
+ static bool quit_all; ///< quit all worlds ASAP
+ static unsigned int next_id; ///<initially zero, used to allocate unique
sequential world ids
+ 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_start; ///< the real time at which this world was
created
+
+ bool quit; ///< quit this world ASAP
+
+ ///< 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) ; };
+
+ /** Called by all constructors to set up the object */
+ 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 */ };
-
- stg_usec_t real_time_now;
-
+ // dummy implementations to be overloaded by GUI subclasses
+ 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 */ };
+
+ /** The current real time in microseconds */
+ stg_usec_t real_time_now;
+
/** StgWorld::quit is set true when this simulation time is reached */
stg_usec_t quit_time;
bool destroy;
- //stg_id_t id;
-
- //GHashTable* models_by_id; ///< the models that make up the world,
indexed by id
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 wall_last_update; ///< the real time of the last update in ms
-
bool dirty; ///< iff true, a gui redraw would be required
int total_subs; ///< the total number of subscriptions to all models
@@ -1298,6 +1302,9 @@
virtual void DrawStatus( StgCanvas* canvas );
void DrawStatusTree( StgCanvas* canvas );
+ void DrawOriginTree();
+ void DrawOrigin();
+
void PushLocalCoords();
void PopCoords();
@@ -1355,6 +1362,12 @@
virtual void PopColor(){ world->PopColor(); }
void DrawFlagList();
+
+ void PushMyPose();
+ void PopPose();
+ void ShiftPose( stg_pose_t* pose );
+ void ShiftToTop();
+
public:
void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax,
stg_meters_t ymin,
stg_meters_t ymax );
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2008-07-10 22:45:42 UTC (rev 6840)
+++ code/stage/trunk/libstage/world.cc 2008-07-11 06:31:47 UTC (rev 6841)
@@ -94,6 +94,7 @@
Initialize( token, interval_sim, ppm );
}
+
void StgWorld::Initialize( const char* token,
stg_msec_t interval_sim,
double ppm )
@@ -105,6 +106,12 @@
}
StgWorld::world_list = g_list_append( StgWorld::world_list, this );
+ // initialize the global stack with the origin
+ this->csstack = g_queue_new();
+ stg_pose_t* pose_zero = new stg_pose_t;
+ bzero( pose_zero, sizeof(stg_pose_t));
+ g_queue_push_head( csstack, pose_zero );
+
this->ray_list = NULL;
this->quit_time = 0;
@@ -285,7 +292,7 @@
g_hash_table_insert( entitytable, (gpointer)entity, mod );
}
- // warn about unused WF linesa
+ // warn about unused WF lines
wf->WarnUnused();
// run through the loaded models and initialise them
@@ -686,3 +693,51 @@
extent.z.min = MIN( extent.z.min, pt.z );
extent.z.max = MAX( extent.z.max, pt.z );
}
+
+
+// coordinate system stack - like OpenGL's matrix stack model
+
+/** Pop a copy of the current stack head onto the stack */
+void StgWorld::PushPose()
+{
+ stg_pose_t* newhead = new stg_pose_t;
+
+ // copy the current head into the new head
+ memcpy( newhead, PeekPose(), sizeof(stg_pose_t));
+
+ // and push the new head
+ g_queue_push_head( csstack, newhead );
+}
+
+/** Pop and free the pose at the head of the stack. */
+void StgWorld::PopPose()
+{
+ stg_pose_t* popped = (stg_pose_t*)g_queue_pop_head( csstack );
+ if( popped )
+ delete popped;
+ else
+ PRINT_WARN( "Empty CS stack popped" );
+}
+
+/** return a pointer to the pose at the head of the queue */
+stg_pose_t* StgWorld::PeekPose()
+{
+ return (stg_pose_t*)g_queue_peek_head( csstack );
+}
+
+void StgWorld::ShiftPose( stg_pose_t* p2 )
+{
+ stg_pose_t* p1 = PeekPose();
+ double cosa = cos(p1->a);
+ double sina = sin(p1->a);
+
+ stg_pose_t result;
+ result.x = p1->x + p2->x * cosa - p2->y * sina;
+ result.y = p1->y + p2->x * sina + p2->y * cosa;
+ result.z = p1->z + p2->z;
+ result.a = normalize(p1->a + p2->a);
+
+ // copy the new pose onto the existing pose at head of stack
+ memcpy( p1, &result, sizeof(stg_pose_t));
+}
+
Modified: code/stage/trunk/libstage/worldgui.cc
===================================================================
--- code/stage/trunk/libstage/worldgui.cc 2008-07-10 22:45:42 UTC (rev
6840)
+++ code/stage/trunk/libstage/worldgui.cc 2008-07-11 06:31:47 UTC (rev
6841)
@@ -148,9 +148,12 @@
"http://playerstage.org\n";
"\n"
"Copyright 2000-2008 Richard Vaughan,\n"
- " Brian Gerkey, Andrew Howard, Reed Hedges, \n"
- " Toby Collett, Alex Couture-Beil, Jeremy Asher \n"
- " and contributors";
+ "Brian Gerkey, Andrew Howard, Reed Hedges, \n"
+ "Toby Collett, Alex Couture-Beil, Jeremy Asher \n"
+ "and contributors\n"
+ "\n"
+ "Distributed under the terms of the \n"
+ "GNU General Public License v2";
StgWorldGui::StgWorldGui(int W,int H,const char* L) :
Fl_Window(W,H,L )
@@ -537,7 +540,7 @@
fl_register_images();
const int Width = 420;
- const int Height = 300;
+ const int Height = 330;
const int Spc = 10;
const int ButtonH = 25;
const int ButtonW = 60;
Added: code/stage/trunk/worlds/CMakeLists.txt
===================================================================
--- code/stage/trunk/worlds/CMakeLists.txt (rev 0)
+++ code/stage/trunk/worlds/CMakeLists.txt 2008-07-11 06:31:47 UTC (rev
6841)
@@ -0,0 +1 @@
+ADD_SUBDIRECTORY(benchmark)
Added: code/stage/trunk/worlds/benchmark/CMakeLists.txt
===================================================================
--- code/stage/trunk/worlds/benchmark/CMakeLists.txt
(rev 0)
+++ code/stage/trunk/worlds/benchmark/CMakeLists.txt 2008-07-11 06:31:47 UTC
(rev 6841)
@@ -0,0 +1,4 @@
+ADD_LIBRARY( expand MODULE expand.cc )
+TARGET_LINK_LIBRARIES( expand stage )
+SET_TARGET_PROPERTIES( expand PROPERTIES PREFIX "" )
+INSTALL( TARGETS expand DESTINATION lib)
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2008-07-10 22:45:42 UTC (rev 6840)
+++ code/stage/trunk/worlds/fasr.world 2008-07-11 06:31:47 UTC (rev 6841)
@@ -54,7 +54,7 @@
define autorob pioneer2dx
(
- sicklaser( pose [ 1.040 0 0 0 ] origin [ 0 0 0 0 ] samples 32 range_max 5
laser_return 2 )
+ sicklaser( samples 32 range_max 5 laser_return 2 )
ctrl "fasr"
#camera( pose [ 0 0 0 0 ] width 100 height 100 horizfov 70 vertfov 40 yaw 0 )
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
Sponsored by: SourceForge.net Community Choice Awards: VOTE NOW!
Studies have shown that voting for your favorite open source project,
along with a healthy diet, reduces your potential for chronic lameness
and boredom. Vote Now at http://www.sourceforge.net/community/cca08
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
