Revision: 6463
http://playerstage.svn.sourceforge.net/playerstage/?rev=6463&view=rev
Author: rtv
Date: 2008-06-03 15:27:00 -0700 (Tue, 03 Jun 2008)
Log Message:
-----------
fixed z-aware raytracing
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_blobfinder.cc
code/stage/trunk/libstage/model_fiducial.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/model_load.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/worlds/fasr.world
code/stage/trunk/worlds/simple.world
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2008-06-03 17:18:26 UTC (rev
6462)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2008-06-03 22:27:00 UTC (rev
6463)
@@ -88,6 +88,8 @@
// there's anything in front
double minleft = 1e6;
double minright = 1e6;
+
+ //return 0;
for (uint32_t i = 0; i < sample_count; i++)
{
@@ -155,6 +157,9 @@
//printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
// pose.x, pose.y, pose.z, pose.a );
+ //pose.z += 0.0001;
+ //robot->pos->SetPose( pose );
+
if( robot->pos->GetFlagCount() < payload &&
hypot( -7-pose.x, -7-pose.y ) < 2.0 )
{
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2008-06-03 17:18:26 UTC (rev 6462)
+++ code/stage/trunk/libstage/model.cc 2008-06-03 22:27:00 UTC (rev 6463)
@@ -349,67 +349,68 @@
AddBlock( pts, 4, 0, 1, 0, true );
}
+
void StgModel::Raytrace( stg_pose_t pose,
stg_meters_t range,
stg_block_match_func_t func,
const void* arg,
- stg_raytrace_sample_t* sample )
+ stg_raytrace_sample_t* sample,
+ bool ztest )
{
world->Raytrace( LocalToGlobal(pose),
range,
func,
this,
arg,
- sample );
+ sample,
+ ztest );
}
void StgModel::Raytrace( stg_radians_t bearing,
stg_meters_t range,
stg_block_match_func_t func,
const void* arg,
- stg_raytrace_sample_t* sample )
+ stg_raytrace_sample_t* sample,
+ bool ztest )
{
- stg_pose_t pose;
- bzero(&pose,sizeof(pose));
- pose.a = bearing;
-
- Raytrace( pose, range, func, arg, sample );
+ stg_pose_t raystart;
+ bzero( &raystart, sizeof(raystart));
+ raystart.a = bearing;
+
+ world->Raytrace( LocalToGlobal(raystart),
+ range,
+ func,
+ this,
+ arg,
+ sample,
+ ztest );
}
-void StgModel::Raytrace( stg_pose_t pose,
+
+void StgModel::Raytrace( stg_radians_t bearing,
stg_meters_t range,
stg_radians_t fov,
stg_block_match_func_t func,
const void* arg,
stg_raytrace_sample_t* samples,
- uint32_t sample_count )
+ uint32_t sample_count,
+ bool ztest )
{
- world->Raytrace( LocalToGlobal(pose),
+ stg_pose_t raystart;
+ bzero( &raystart, sizeof(raystart));
+ raystart.a = bearing;
+
+ world->Raytrace( LocalToGlobal(raystart),
range,
fov,
func,
this,
arg,
samples,
- sample_count );
+ sample_count,
+ ztest );
}
-void StgModel::Raytrace( stg_radians_t bearing,
- stg_meters_t range,
- stg_radians_t fov,
- stg_block_match_func_t func,
- const void* arg,
- stg_raytrace_sample_t* samples,
- uint32_t sample_count )
-{
- stg_pose_t pose;
- bzero(&pose,sizeof(pose));
- pose.a = bearing;
-
- Raytrace( pose, range, fov, func, arg, samples, sample_count );
-}
-
-
// utility for g_free()ing everything in a list
void list_gfree( GList* list )
{
@@ -948,11 +949,15 @@
if( blinkenlights )
DrawBlinkenlights();
- if( stall )
- gl_draw_string( 0,0,0.5, "X" );
-
+ if( stall )
+ {
+ PushColor( 1,0,0,1 );
+ gl_draw_string( 0,0,0.5, "!" );
+ PopColor();
+ }
+
// shift up the CS to the top of this model
- gl_coord_shift( 0,0, this->geom.size.z, 0 );
+ //gl_coord_shift( 0,0, this->geom.size.z, 0 );
// recursively draw the tree below this model
for( GList* it=children; it; it=it->next )
@@ -1415,7 +1420,8 @@
range,
(stg_block_match_func_t)collision_match,
NULL,
- &sample );
+ &sample,
+ true );
if( sample.block )
hitmod = sample.block->Model();
Modified: code/stage/trunk/libstage/model_blobfinder.cc
===================================================================
--- code/stage/trunk/libstage/model_blobfinder.cc 2008-06-03 17:18:26 UTC
(rev 6462)
+++ code/stage/trunk/libstage/model_blobfinder.cc 2008-06-03 22:27:00 UTC
(rev 6463)
@@ -184,8 +184,7 @@
stg_raytrace_sample_t* samples = new stg_raytrace_sample_t[scan_width];
- Raytrace( pan, range, fov, blob_match, NULL, samples, scan_width );
-
+ Raytrace( pan, range, fov, blob_match, NULL, samples, scan_width, false );
// now the colors and ranges are filled in - time to do blob detection
double yRadsPerPixel = fov / scan_height;
Modified: code/stage/trunk/libstage/model_fiducial.cc
===================================================================
--- code/stage/trunk/libstage/model_fiducial.cc 2008-06-03 17:18:26 UTC (rev
6462)
+++ code/stage/trunk/libstage/model_fiducial.cc 2008-06-03 22:27:00 UTC (rev
6463)
@@ -157,12 +157,13 @@
//printf( "bearing %.2f\n", RTOD(bearing) );
stg_raytrace_sample_t ray;
-
+
Raytrace( dtheta,
max_range_anon,
fiducial_raytrace_match,
NULL,
- &ray );
+ &ray,
+ false );
range = ray.range;
StgModel* hitmod = ray.block->Model();
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2008-06-03 17:18:26 UTC (rev
6462)
+++ code/stage/trunk/libstage/model_laser.cc 2008-06-03 22:27:00 UTC (rev
6463)
@@ -158,15 +158,12 @@
static bool laser_raytrace_match( StgBlock* testblock,
StgModel* finder,
- const void* zp )
+ const void* dummy )
{
// Ignore the model that's looking and things that are invisible to
// lasers
- double z = *(stg_meters_t*)zp;
-
if( (testblock->Model() != finder) &&
- testblock->IntersectGlobalZ( z ) &&
(testblock->Model()->GetLaserReturn() > 0 ) )
return true; // match!
@@ -178,19 +175,24 @@
double bearing = -fov/2.0;
double sample_incr = fov / (double)(sample_count-1);
- stg_pose_t gpose = GetGlobalPose();
- stg_meters_t zscan = gpose.z + geom.size.z /2.0;
-
samples = g_renew( stg_laser_sample_t, samples, sample_count );
+
+ stg_pose_t rayorg;
+ bzero( &rayorg, sizeof(rayorg));
+ rayorg.z = geom.size.z/2;
for( unsigned int t=0; t<sample_count; t += resolution )
{
stg_raytrace_sample_t sample;
- Raytrace( bearing,
+
+ rayorg.a = pose.a + bearing;
+
+ Raytrace( rayorg,
range_max,
laser_raytrace_match,
- &zscan, // height of scan line
- &sample );
+ NULL,
+ &sample,
+ true ); // z testing enabled
samples[t].range = sample.range;
@@ -306,6 +308,7 @@
return;
glPushMatrix();
+ //glTranslatef( 0,0, 0 ); // shoot the laser beam out at the right height
glTranslatef( 0,0, geom.size.z/2.0 ); // shoot the laser beam out at the
right height
// pack the laser hit points into a vertex array for fast rendering
Modified: code/stage/trunk/libstage/model_load.cc
===================================================================
--- code/stage/trunk/libstage/model_load.cc 2008-06-03 17:18:26 UTC (rev
6462)
+++ code/stage/trunk/libstage/model_load.cc 2008-06-03 22:27:00 UTC (rev
6463)
@@ -350,6 +350,12 @@
// call any type-specific load callbacks
this->CallCallbacks( &this->load );
+ // MUST BE THE LAST THING LOADED
+ if( wf->PropertyExists( this->id, "alwayson" ))
+ {
+ if( wf->ReadInt( this->id, "alwayson", 0) > 0 )
+ Startup();
+ }
if( this->debug )
printf( "Model \"%s\" is in debug mode\n", token );
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2008-06-03 17:18:26 UTC (rev 6462)
+++ code/stage/trunk/libstage/stage.hh 2008-06-03 22:27:00 UTC (rev 6463)
@@ -995,7 +995,8 @@
stg_block_match_func_t func,
StgModel* finder,
const void* arg,
- stg_raytrace_sample_t* sample );
+ stg_raytrace_sample_t* sample,
+ bool ztest );
void Raytrace( stg_pose_t pose,
stg_meters_t range,
@@ -1004,7 +1005,8 @@
StgModel* finder,
const void* arg,
stg_raytrace_sample_t* samples,
- uint32_t sample_count );
+ uint32_t sample_count,
+ bool ztest );
void RemoveBlock( int x, int y, StgBlock* block );
//{ }//bgrid->RemoveBlock( x, y, block ); };
@@ -1195,39 +1197,43 @@
int TreeToPtrArray( GPtrArray* array );
- // raytraces from the point and heading identified by pose, in local coords
+ /** raytraces a single ray from the point and heading identified by
+ pose, in local coords */
void Raytrace( stg_pose_t pose,
stg_meters_t range,
stg_block_match_func_t func,
const void* arg,
- stg_raytrace_sample_t* sample );
-
- // this version raytraces from our local origin in the direction [angle]
- void Raytrace( stg_radians_t angle,
- stg_meters_t range,
- stg_block_match_func_t func,
- const void* arg,
- stg_raytrace_sample_t* sample );
-
- // raytraces from the point and heading identified by pose, in local coords
+ stg_raytrace_sample_t* sample,
+ bool ztest = true );
+
+ /** raytraces multiple rays around the point and heading identified
+ by pose, in local coords */
void Raytrace( stg_pose_t pose,
stg_meters_t range,
stg_radians_t fov,
stg_block_match_func_t func,
const void* arg,
stg_raytrace_sample_t* samples,
- uint32_t sample_count );
+ uint32_t sample_count,
+ bool ztest = true );
- // this version raytraces from our local origin in the direction [angle]
- void Raytrace( stg_radians_t angle,
- stg_meters_t range,
- stg_radians_t fov,
+ void Raytrace( stg_radians_t bearing,
+ stg_meters_t range,
stg_block_match_func_t func,
const void* arg,
+ stg_raytrace_sample_t* sample,
+ bool ztest = true );
+
+ void Raytrace( stg_radians_t bearing,
+ stg_meters_t range,
+ stg_radians_t fov,
+ stg_block_match_func_t func,
+ const void* arg,
stg_raytrace_sample_t* samples,
- uint32_t sample_count );
+ uint32_t sample_count,
+ bool ztest = true );
-
+
/** Causes this model and its children to recompute their global
position instead of using a cached pose in
StgModel::GetGlobalPose()..*/
@@ -1614,7 +1620,7 @@
{ if( count ) *count = pt_count; return pts; };
bool IntersectGlobalZ( stg_meters_t z )
- { return( z >= global_zmin && z < global_zmax ); }
+ { return( z >= global_zmin && z <= global_zmax ); }
stg_color_t Color()
{ return( inherit_color ? mod->GetColor() : color ); }
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2008-06-03 17:18:26 UTC (rev 6462)
+++ code/stage/trunk/libstage/world.cc 2008-06-03 22:27:00 UTC (rev 6463)
@@ -485,14 +485,15 @@
StgModel* model,
const void* arg,
stg_raytrace_sample_t* samples, // preallocated
storage for samples
- uint32_t sample_count ) // number of samples
+ uint32_t sample_count,
+ bool ztest ) // number of samples
{
pose.a -= fov/2.0; // direction of first ray
stg_radians_t angle_incr = fov/(double)sample_count;
for( uint32_t s=0; s < sample_count; s++ )
{
- Raytrace( pose, range, func, model, arg, &samples[s] );
+ Raytrace( pose, range, func, model, arg, &samples[s], ztest );
pose.a += angle_incr;
}
}
@@ -504,7 +505,8 @@
stg_block_match_func_t func,
StgModel* mod,
const void* arg,
- stg_raytrace_sample_t* sample )
+ stg_raytrace_sample_t* sample,
+ bool ztest )
{
// initialize the sample
memcpy( &sample->pose, &pose, sizeof(stg_pose_t)); // pose stays fixed
@@ -606,7 +608,7 @@
// if this block does not belong to the searching model and it
// matches the predicate and it's in the right z range
if( //block && (block->Model() != finder) &&
- //block->IntersectGlobalZ( pose.z ) &&
+ (ztest ? block->IntersectGlobalZ( pose.z ) : true) &&
(*func)( block, mod, arg ) )
{
// a hit!
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2008-06-03 17:18:26 UTC (rev 6462)
+++ code/stage/trunk/worlds/fasr.world 2008-06-03 22:27:00 UTC (rev 6463)
@@ -21,7 +21,7 @@
# configure the GUI window
window
(
- size [ 698.000 628.000 ]
+ #size [ 698.000 628.000 ]
center [6.990 -4.040]
rotate [ 0.000 0.000 ]
scale 33.306
@@ -30,13 +30,13 @@
)
# 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 0.8]
+ pose [0.000 0.000 0.000]
+ bitmap "bitmaps/cave.png"
+)
zone
(
@@ -57,20 +57,24 @@
define autorob pioneer2dx
(
color "red"
+
sicklaser( pose [ 0.040 0.000 0.000 ] samples 32 range_max 5 laser_return 2 )
- ctrl "fasr"
+
+ ctrl "fasr"
)
-autorob( pose [4.461 6.406 1.930] say "Booga" )
+sicklaser( pose [0 0 0] alwayson 1 color "white")
+
+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.065 5.583 125.796] )
-#autorob( pose [7.487 6.926 -40.634] )
+autorob( pose [6.147 7.399 4.964] )
+autorob( pose [4.065 5.583 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] )
Modified: code/stage/trunk/worlds/simple.world
===================================================================
--- code/stage/trunk/worlds/simple.world 2008-06-03 17:18:26 UTC (rev
6462)
+++ code/stage/trunk/worlds/simple.world 2008-06-03 22:27:00 UTC (rev
6463)
@@ -37,10 +37,10 @@
bitmap "bitmaps/cave.png"
)
-define autorob pioneer2dx
+define autorob fancypioneer2dx
(
color "red"
- #sicklaser( pose [ 0.040 0.000 0.000 ] samples 32 )
+ fancysicklaser( pose [ 0.040 0.000 0.000 ] samples 32 )
#ctrl "wander"
blinkenlight( pose [ 0.15 0.1 0 ] color "red" )
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
Check out the new SourceForge.net Marketplace.
It's the best place to buy or sell services for
just about anything Open Source.
http://sourceforge.net/services/buy/index.php
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
