Revision: 6755
http://playerstage.svn.sourceforge.net/playerstage/?rev=6755&view=rev
Author: rtv
Date: 2008-07-03 16:46:19 -0700 (Thu, 03 Jul 2008)
Log Message:
-----------
fixed data vis bug whereby vis coord system was not moved on to top of parent
Modified Paths:
--------------
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/model_ranger.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/worlds/everything.world
code/stage/trunk/worlds/fasr.world
code/stage/trunk/worlds/pioneer.inc
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2008-07-03 23:39:37 UTC (rev 6754)
+++ code/stage/trunk/libstage/model.cc 2008-07-03 23:46:19 UTC (rev 6755)
@@ -1155,15 +1155,6 @@
glPopMatrix(); // drop out of local coords
}
-// void StgModel::BuildDisplayList()
-// {
-// glNewList( blocks_dl, GL_COMPILE );
-// DrawBlocks();
-// glEndList();
-
-// rebuild_displaylist = false; // we just did it
-// }
-
void StgModel::DataVisualize( void )
{
// do nothing
@@ -1173,7 +1164,13 @@
{
PushLocalCoords();
DataVisualize(); // virtual function overridden by most model types
+
+ // shift to top of this model
+ glTranslatef( 0,0, geom.size.z );
+
+ // and draw the children
LISTMETHOD( children, StgModel*, DataVisualizeTree );
+
PopCoords();
}
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2008-07-03 23:39:37 UTC (rev
6754)
+++ code/stage/trunk/libstage/model_laser.cc 2008-07-03 23:46:19 UTC (rev
6755)
@@ -34,6 +34,7 @@
//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 );
/**
@ingroup model
@@ -76,11 +77,11 @@
*/
StgModelLaser::StgModelLaser( StgWorld* world,
- StgModel* parent )
+
StgModel* parent )
: StgModel( world, parent, MODEL_TYPE_LASER )
{
PRINT_DEBUG2( "Constructing StgModelLaser %d (%s)\n",
- id, typestr );
+ id, typestr );
// sensible laser defaults
interval = DEFAULT_INTERVAL_MS * (int)thousand;
@@ -107,6 +108,7 @@
data_dl = glGenLists(1);
registerOption( &showLaserData );
+ registerOption( &showLaserStrikes );
}
@@ -155,8 +157,8 @@
}
static bool laser_raytrace_match( StgBlock* testblock,
- StgModel* finder,
- const void* dummy )
+
StgModel* finder,
+
const void* dummy )
{
// Ignore the model that's looking and things that are invisible to
// lasers
@@ -186,20 +188,20 @@
rayorg.a = bearing;
Raytrace( rayorg,
- range_max,
- laser_raytrace_match,
- NULL,
- &sample,
- true ); // z testing enabled
+ range_max,
+ laser_raytrace_match,
+ NULL,
+ &sample,
+ true ); // z testing enabled
samples[t].range = sample.range;
// if we hit a model and it reflects brightly, we set
// reflectance high, else low
if( sample.block && ( sample.block->Model()->GetLaserReturn() >=
LaserBright ) )
- samples[t].reflectance = 1;
+ samples[t].reflectance = 1;
else
- samples[t].reflectance = 0;
+ samples[t].reflectance = 0;
// todo - lower bound on range
bearing += sample_incr;
@@ -209,22 +211,22 @@
if( resolution > 1 )
{
for( unsigned int t=resolution; t<sample_count; t+=resolution )
- for( unsigned int g=1; g<resolution; g++ )
- {
- if( t >= sample_count )
- break;
+ for( unsigned int g=1; g<resolution; g++ )
+ {
+ if( t >= sample_count )
+ break;
- // copy the rightmost sample data into this point
- memcpy( &samples[t-g],
- &samples[t-resolution],
- sizeof(stg_laser_sample_t));
+ // copy the rightmost sample data into this
point
+ memcpy( &samples[t-g],
+ &samples[t-resolution],
+ sizeof(stg_laser_sample_t));
- double left = samples[t].range;
- double right = samples[t-resolution].range;
+ double left = samples[t].range;
+ double right = samples[t-resolution].range;
- // linear range interpolation between the left and right samples
- samples[t-g].range = (left-g*(left-right)/resolution);
- }
+ // linear range interpolation between the left
and right samples
+ samples[t-g].range =
(left-g*(left-right)/resolution);
+ }
}
data_dirty = true;
@@ -302,12 +304,12 @@
void StgModelLaser::DataVisualize( void )
{
- if ( !showLaserData )
- return;
-
if( ! (samples && sample_count) )
return;
-
+
+ if ( ! (showLaserData || showLaserStrikes) )
+ return;
+
// we only regenerate the list if there's new data
if( data_dirty )
{
@@ -331,42 +333,50 @@
glVertexPointer( 2, GL_FLOAT, 0, pts );
for( unsigned int s=0; s<sample_count; s++ )
- {
- double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0;
- pts[2*s+2] = (float)(samples[s].range * cos(ray_angle) );
- pts[2*s+3] = (float)(samples[s].range * sin(ray_angle) );
+ {
+ double ray_angle = (s * (fov / (sample_count-1))) -
fov/2.0;
+ pts[2*s+2] = (float)(samples[s].range * cos(ray_angle)
);
+ pts[2*s+3] = (float)(samples[s].range * sin(ray_angle)
);
- // if the sample is unusually bright, draw a little blob
- if( samples[s].reflectance > 0 )
- {
- glBegin( GL_POINTS );
- glVertex2f( pts[2*s+2], pts[2*s+3] );
- glEnd();
+ // if the sample is unusually bright, draw a little
blob
+ if( samples[s].reflectance > 0 )
+ {
+ glBegin( GL_POINTS );
+ glVertex2f( pts[2*s+2], pts[2*s+3] );
+ glEnd();
- // why doesn't this work?
- //glDrawArrays( GL_POINTS, 2*s+2, 1 );
- }
-
- }
+ // why doesn't this work?
+ //glDrawArrays( GL_POINTS, 2*s+2, 1 );
+ }
+
+ }
PopColor();
+
glDepthMask( GL_FALSE );
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
// draw the filled polygon in transparent blue
PushColor( 0, 0, 1, 0.1 );
-
+
+
glDrawArrays( GL_POLYGON, 0, sample_count+1 );
-
- // draw the beam strike points in black
- PushColor( 0, 0, 0, 1.0 );
- glPointSize( 1.0 );
- glDrawArrays( GL_POINTS, 0, sample_count+1 );
-
+
// reset
PopColor();
- PopColor();
+
+ if( showLaserStrikes )
+ {
+ // draw the beam strike points in black
+ PushColor( 0, 0, 0, 1.0 );
+ glPointSize( 1.0 );
+ glDrawArrays( GL_POINTS, 0, sample_count+1 );
+ PopColor();
+ }
+
glDepthMask( GL_TRUE );
+
+
glEndList();
}
Modified: code/stage/trunk/libstage/model_ranger.cc
===================================================================
--- code/stage/trunk/libstage/model_ranger.cc 2008-07-03 23:39:37 UTC (rev
6754)
+++ code/stage/trunk/libstage/model_ranger.cc 2008-07-03 23:46:19 UTC (rev
6755)
@@ -93,6 +93,7 @@
static const char RANGER_GEOM_COLOR[] = "orange";
Option StgModelRanger::showRangerData( "Show Ranger Data", "show_ranger", "",
true );
+Option StgModelRanger::showRangerTransducers( "Show Ranger transducer
locations", "show_ranger_transducers", "", false );
StgModelRanger::StgModelRanger( StgWorld* world,
@@ -139,6 +140,7 @@
}
registerOption( &showRangerData );
+ registerOption( &showRangerTransducers );
}
StgModelRanger::~StgModelRanger()
@@ -314,23 +316,39 @@
void StgModelRanger::DataVisualize( void )
{
+ if( ! (samples && sensors && sensor_count) )
+ return;
+
+ if( showRangerTransducers )
+ {
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+ PushColor( 0,0,0,1 );
+
+ for( unsigned int s=0; s<sensor_count; s++ )
+ {
+ if( samples[s] > 0.0 )
+ {
+ stg_ranger_sensor_t* rngr = &sensors[s];
+
+ glPointSize( 4 );
+ glBegin( GL_POINTS );
+ glVertex3f( rngr->pose.x, rngr->pose.y,
rngr->pose.z );
+ glEnd();
+ }
+ }
+ PopColor();
+ }
+
if ( !showRangerData )
return;
- if( ! (samples && sensors && sensor_count) )
- return;
-
- // if all models have the same number of sensors, this is fast
- // as it will probably not use a system call or cause a cache
- // miss
+ // if all models have the same number of sensors, this is fast as
+ // it will probably not use a system call
static float* pts = NULL;
size_t memsize = 9 * sensor_count * sizeof(float);
pts = (float*)g_realloc( pts, memsize );
bzero( pts, memsize );
- //glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
- //PushColor( 0,0,0,1 );
-
// calculate a triangle for each non-zero sensor range
for( unsigned int s=0; s<sensor_count; s++ )
{
@@ -342,11 +360,6 @@
//double dy = rngr->size.y/2.0;
//double dz = rngr->size.z/2.0;
- // DEBUG: draw a point for the sensor pose
- glPointSize( 6 );
- glBegin( GL_POINTS );
- glVertex3f( rngr->pose.x, rngr->pose.y, rngr->pose.z );
- glEnd();
// sensor FOV
double sidelen = samples[s];
@@ -366,9 +379,7 @@
pts[index+8] = rngr->pose.z;
}
}
-
- //PopColor();
-
+
// draw the filled triangles in transparent blue
glDepthMask( GL_FALSE );
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2008-07-03 23:39:37 UTC (rev 6754)
+++ code/stage/trunk/libstage/stage.hh 2008-07-03 23:46:19 UTC (rev 6755)
@@ -2208,9 +2208,10 @@
stg_meters_t range_min, range_max;
stg_radians_t fov;
uint32_t resolution;
-
- static Option showLaserData;
+ static Option showLaserData;
+ static Option showLaserStrikes;
+
public:
static const char* typestr;
// constructor
@@ -2443,6 +2444,7 @@
private:
static Option showRangerData;
+ static Option showRangerTransducers;
};
Modified: code/stage/trunk/worlds/everything.world
===================================================================
--- code/stage/trunk/worlds/everything.world 2008-07-03 23:39:37 UTC (rev
6754)
+++ code/stage/trunk/worlds/everything.world 2008-07-03 23:46:19 UTC (rev
6755)
@@ -54,7 +54,7 @@
#
define trickedoutpioneer pioneer2dx
(
- sicklaser( alwayson 1
+ sicklaser( alwayson 1
pose [0.030 0.000 0.000 ]
fiducial( range_max 8 range_max_id 5 )
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2008-07-03 23:39:37 UTC (rev 6754)
+++ code/stage/trunk/worlds/fasr.world 2008-07-03 23:46:19 UTC (rev 6755)
@@ -21,10 +21,10 @@
# configure the GUI window
window
(
- size [ 641.000 480.000 ]
- center [-0.294 0.114]
- rotate [ -52.500 -18.500 ]
- scale 27.525
+ size [ 1641.000 953.000 ]
+ center [0.265 0.145]
+ rotate [ -57.500 -29.500 ]
+ scale 62.271
show_data 0
show_flags 1
@@ -60,7 +60,7 @@
color "red"
sicklaser( pose [ 0.040 0.000 0.000 ] samples 30 range_max 5 laser_return 2 )
- #camera( pose [ 0.000 0.000 0.000 ] width 100 height 100 horizfov 70 vertfov
40 yaw 0 )
+ camera( pose [ 0.000 0.000 0.000 ] width 100 height 100 horizfov 70 vertfov
40 yaw 0 )
ctrl "fasr"
# say "Autolab"
@@ -68,23 +68,23 @@
autorob( pose [4.461 6.406 1.930] )
-autorob( pose [6.635 6.458 -52.629] )
-autorob( pose [6.385 5.805 -87.082] )
-autorob( pose [7.004 5.327 170.536] )
-autorob( pose [5.770 6.492 -10.539] )
-autorob( pose [7.493 4.852 -156.719] )
-autorob( pose [5.078 6.853 -37.549] )
-autorob( pose [6.147 7.399 4.964] )
-autorob( pose [4.058 5.283 125.796] )
-autorob( pose [7.487 6.926 -40.634] )
+#autorob( pose [6.635 6.458 -52.629] )
+#autorob( pose [6.385 5.805 -87.082] )
+#autorob( pose [7.004 5.327 170.536] )
+#autorob( pose [5.770 6.492 -10.539] )
+#autorob( pose [7.493 4.852 -156.719] )
+#autorob( pose [5.078 6.853 -37.549] )
+#autorob( pose [6.147 7.399 4.964] )
+#autorob( pose [4.058 5.283 125.796] )
+#autorob( pose [7.487 6.926 -40.634] )
-autorob( pose [4.530 7.367 -113.456] )
-autorob( pose [6.071 5.138 -1.177] )
-autorob( pose [6.591 4.622 -68.007] )
-autorob( pose [5.454 7.540 135.162] )
-autorob( pose [4.707 5.855 -3.588] )
-autorob( pose [6.714 7.447 -73.332] )
-autorob( pose [5.582 5.724 -48.161] )
-autorob( pose [3.958 6.851 -102.651] )
-autorob( pose [5.127 5.109 23.582] )
-autorob( pose [7.534 5.825 -70.230] )
+#autorob( pose [4.530 7.367 -113.456] )
+#autorob( pose [6.071 5.138 -1.177] )
+#autorob( pose [6.591 4.622 -68.007] )
+#autorob( pose [5.454 7.540 135.162] )
+#autorob( pose [4.707 5.855 -3.588] )
+#autorob( pose [6.714 7.447 -73.332] )
+#autorob( pose [5.582 5.724 -48.161] )
+#autorob( pose [3.958 6.851 -102.651] )
+#autorob( pose [5.127 5.109 23.582] )
+#autorob( pose [7.534 5.825 -70.230] )
Modified: code/stage/trunk/worlds/pioneer.inc
===================================================================
--- code/stage/trunk/worlds/pioneer.inc 2008-07-03 23:39:37 UTC (rev 6754)
+++ code/stage/trunk/worlds/pioneer.inc 2008-07-03 23:46:19 UTC (rev 6755)
@@ -52,7 +52,7 @@
# use the sonar array defined above with a small vertical offset to
# drop the sensors into the robot body
- p2dx_sonar( pose4 [.04 0 -0.05 0] )
+ p2dx_sonar( pose4 [0.04 0 -0.03 0] )
# differential steering model
drive "diff"
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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
