Revision: 7191
http://playerstage.svn.sourceforge.net/playerstage/?rev=7191&view=rev
Author: rtv
Date: 2008-12-07 02:07:21 +0000 (Sun, 07 Dec 2008)
Log Message:
-----------
fixed tricky bug in fasr.cc when used in worlds bigger than 8m square. awful.
Added some cautious NAN checks to avoid similar horror in future.
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/libstage/canvas.cc
code/stage/trunk/libstage/gl.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/model_position.cc
code/stage/trunk/libstage/region.cc
code/stage/trunk/libstage/stage.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2008-12-06 21:25:20 UTC (rev
7190)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2008-12-07 02:07:21 UTC (rev
7191)
@@ -182,12 +182,23 @@
int x = (pose.x + 8) / 4;
int y = (pose.y + 8) / 4;
+
+ // oh what an awful bug - 5 hours to track this down. When using
+ // this controller in a world larger than 8*8 meters, a_goal can
+ // sometimes be NAN. Causing trouble WAY upstream.
+ if( x > 3 ) x = 3;
+ if( y > 3 ) y = 3;
double a_goal =
dtor( robot->pos->GetFlagCount() ? have[y][x] : need[y][x] );
+ assert( ! isnan(a_goal ) );
+ assert( ! isnan(pose.a ) );
+
double a_error = normalize( a_goal - pose.a );
-
+
+ assert( ! isnan(a_error) );
+
robot->pos->SetTurnSpeed( a_error );
}
Modified: code/stage/trunk/libstage/canvas.cc
===================================================================
--- code/stage/trunk/libstage/canvas.cc 2008-12-06 21:25:20 UTC (rev 7190)
+++ code/stage/trunk/libstage/canvas.cc 2008-12-07 02:07:21 UTC (rev 7191)
@@ -592,20 +592,25 @@
{
stg_bounds3d_t bounds = world->GetExtent();
- char str[16];
- PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray
- 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 );
- }
+// printf( "bounds [%.2f %.2f] [%.2f %.2f] [%.2f %.2f]\n",
+// bounds.x.min, bounds.x.max,
+// bounds.y.min, bounds.y.max,
+// bounds.z.min, bounds.z.max );
+
+ char str[64];
+ PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray
+ 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 );
+ }
- 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 );
- }
- PopColor();
+ 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 );
+ }
+ PopColor();
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
Modified: code/stage/trunk/libstage/gl.cc
===================================================================
--- code/stage/trunk/libstage/gl.cc 2008-12-06 21:25:20 UTC (rev 7190)
+++ code/stage/trunk/libstage/gl.cc 2008-12-07 02:07:21 UTC (rev 7191)
@@ -21,10 +21,10 @@
}
-// TODO - this could be faster, but we don't draw a lot of text
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);
}
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2008-12-06 21:25:20 UTC (rev 7190)
+++ code/stage/trunk/libstage/model.cc 2008-12-07 02:07:21 UTC (rev 7191)
@@ -566,7 +566,22 @@
// should one day do all this with affine transforms for neatness?
inline stg_pose_t StgModel::LocalToGlobal( stg_pose_t pose )
{
- return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose );
+ stg_pose_t gpose = pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose );
+
+ if( isnan(gpose.x) || isnan(gpose.y) | isnan(gpose.z) || isnan(gpose.a ) )
+ {
+ stg_pose_t g = GetGlobalPose();
+ stg_pose_t p = parent ? parent->GetPose() : stg_pose_t();
+
+ printf( "model %p %s gpose BAD [%.2f %.2f %.2f %.2f] pose [%.2f
%.2f %.2f %.2f] globalpose [%.2f %.2f %.2f %.2f] parent [%.2f %.2f %.2f %.2f]
\n",
+ this, token, gpose.x, gpose.y, gpose.z,
gpose.a,
+ pose.x, pose.y, pose.z, pose.a,
+ g.x, g.y, g.z, g.a,
+ p.x, p.y, p.z, p.a );
+ return gpose;
+
+ }
+ //return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose );
}
void StgModel::MapWithChildren()
@@ -1223,6 +1238,11 @@
// vel.z,
// vel.a );
+ assert( ! isnan(vel.x) );
+ assert( ! isnan(vel.y) );
+ assert( ! isnan(vel.z) );
+ assert( ! isnan(vel.a) );
+
this->velocity = vel;
if( on_velocity_list && velocity_is_zero( vel ) )
@@ -1250,28 +1270,24 @@
world->NeedRedraw();
}
-void StgModel::SetPose( stg_pose_t pose )
+void StgModel::SetPose( stg_pose_t newpose )
{
//PRINT_DEBUG5( "%s.SetPose(%.2f %.2f %.2f %.2f)",
// this->token, pose->x, pose->y, pose->z, pose->a );
// if the pose has changed, we need to do some work
- if( memcmp( &this->pose, &pose, sizeof(stg_pose_t) ) != 0 )
+ if( memcmp( &pose, &newpose, sizeof(stg_pose_t) ) != 0 )
{
- //puts( "SETPOSE" );
-
- // UnMapWithChildren();
+ pose = newpose;
+ pose.a = normalize(pose.a);
-
- pose.a = normalize( pose.a );
- this->pose = pose;
- this->pose.a = normalize(this->pose.a);
-
- this->NeedRedraw();
-
- this->map_caches_are_invalid = true;
+ if( isnan( pose.a ) )
+ printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n",
+ token, pose.x, pose.y, pose.z,
pose.a );
+
+ NeedRedraw();
+ map_caches_are_invalid = true;
MapWithChildren();
-
world->dirty = true;
}
@@ -1483,6 +1499,10 @@
StgModel* StgModel::ConditionalMove( stg_pose_t newpose )
{
+ if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a
) )
+ printf( "ConditionalMove bad newpose %s [%.2f %.2f %.2f %.2f]\n",
+ token, newpose.x, newpose.y, newpose.z,
newpose.a );
+
stg_pose_t startpose = pose;
pose = newpose; // do the move provisionally - we might undo it below
@@ -1496,6 +1516,11 @@
world->dirty = true; // need redraw
}
+
+ if( isnan( pose.x ) || isnan( pose.y ) || isnan( pose.z ) || isnan( pose.a
) )
+ printf( "ConditionalMove bad pose %s [%.2f %.2f %.2f %.2f]\n",
+ token, pose.x, pose.y, pose.z, pose.a );
+
return hitmod;
}
@@ -1532,6 +1557,10 @@
p.z = velocity.z * interval;
p.a = velocity.a * interval;
+ if( isnan( p.x ) || isnan( p.y ) || isnan( p.z ) || isnan( p.a ) )
+ printf( "UpdatePose bad vel %s [%.2f %.2f %.2f %.2f]\n",
+ token, p.x, p.y, p.z, p.a );
+
// attempts to move to the new pose. If the move fails because we'd
// hit another model, that model is returned.
StgModel* hitthing = ConditionalMove( pose_sum( pose, p ) );
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2008-12-06 21:25:20 UTC (rev
7190)
+++ code/stage/trunk/libstage/model_laser.cc 2008-12-07 02:07:21 UTC (rev
7191)
@@ -176,7 +176,7 @@
double sample_incr = fov / (double)(sample_count-1);
samples = g_renew( stg_laser_sample_t, samples, sample_count );
-
+
stg_pose_t rayorg = geom.pose;
bzero( &rayorg, sizeof(rayorg));
rayorg.z += geom.size.z/2;
Modified: code/stage/trunk/libstage/model_position.cc
===================================================================
--- code/stage/trunk/libstage/model_position.cc 2008-12-06 21:25:20 UTC (rev
7190)
+++ code/stage/trunk/libstage/model_position.cc 2008-12-07 02:07:21 UTC (rev
7191)
@@ -472,41 +472,54 @@
void StgModelPosition::SetSpeed( double x, double y, double a )
{
- control_mode = STG_POSITION_CONTROL_VELOCITY;
- goal.x = x;
- goal.y = y;
- goal.z = 0;
- goal.a = a;
+ assert( ! isnan(x) );
+ assert( ! isnan(y) );
+ assert( ! isnan(a) );
+
+ control_mode = STG_POSITION_CONTROL_VELOCITY;
+ goal.x = x;
+ goal.y = y;
+ goal.z = 0;
+ goal.a = a;
}
void StgModelPosition::SetXSpeed( double x )
{
- control_mode = STG_POSITION_CONTROL_VELOCITY;
- goal.x = x;
+ assert( ! isnan(x) );
+ control_mode = STG_POSITION_CONTROL_VELOCITY;
+ goal.x = x;
}
void StgModelPosition::SetYSpeed( double y )
{
- control_mode = STG_POSITION_CONTROL_VELOCITY;
- goal.y = y;
+ assert( ! isnan(y) );
+ control_mode = STG_POSITION_CONTROL_VELOCITY;
+ goal.y = y;
}
void StgModelPosition::SetZSpeed( double z )
{
- control_mode = STG_POSITION_CONTROL_VELOCITY;
- goal.z = z;
+ assert( ! isnan(z) );
+ control_mode = STG_POSITION_CONTROL_VELOCITY;
+ goal.z = z;
}
void StgModelPosition::SetTurnSpeed( double a )
{
- control_mode = STG_POSITION_CONTROL_VELOCITY;
- goal.a = a;
+ assert( ! isnan(a) );
+ control_mode = STG_POSITION_CONTROL_VELOCITY;
+ goal.a = a;
}
void StgModelPosition::SetSpeed( stg_velocity_t vel )
{
+ assert( ! isnan(vel.x) );
+ assert( ! isnan(vel.y) );
+ assert( ! isnan(vel.z) );
+ assert( ! isnan(vel.a) );
+
control_mode = STG_POSITION_CONTROL_VELOCITY;
goal.x = vel.x;
goal.y = vel.y;
@@ -516,6 +529,10 @@
void StgModelPosition::GoTo( double x, double y, double a )
{
+ assert( ! isnan(x) );
+ assert( ! isnan(y) );
+ assert( ! isnan(a) );
+
control_mode = STG_POSITION_CONTROL_POSITION;
goal.x = x;
goal.y = y;
Modified: code/stage/trunk/libstage/region.cc
===================================================================
--- code/stage/trunk/libstage/region.cc 2008-12-06 21:25:20 UTC (rev 7190)
+++ code/stage/trunk/libstage/region.cc 2008-12-07 02:07:21 UTC (rev 7191)
@@ -14,7 +14,7 @@
Region::Region()
- : count(0), cells(NULL)
+ : cells(NULL), count(0)
{
//for( unsigned int i=0; i<Region::SIZE; i++ )
//cells[i].region = this;
@@ -52,18 +52,66 @@
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- // outline regions
+ // outline superregion
+ glColor3f( 0,0,1 );
+ glRecti( 0,0, 1<<SRBITS, 1<<SRBITS );
+
+ // outline regions
glColor3f( 0,1,0 );
for( unsigned int x=0; x<SuperRegion::WIDTH; x++ )
for( unsigned int y=0; y<SuperRegion::WIDTH; y++ )
- glRecti( x<<RBITS, y<<RBITS,
- (x+1)<<RBITS, (y+1)<<RBITS );
+ {
+ Region* r = GetRegion(x,y);
- // outline superregion
- glColor3f( 0,0,1 );
-
- glRecti( 0,0, 1<<SRBITS, 1<<SRBITS );
-
+ if( r->count )
+ // outline regions with contents
+ glRecti( x<<RBITS, y<<RBITS,
+ (x+1)<<RBITS, (y+1)<<RBITS );
+ else if( r->cells )
+ {
+ double left = x << RBITS;
+ double right = (x+1) << RBITS;
+ double bottom = y << RBITS;
+ double top = (y+1) << RBITS;
+
+ double d = 3.0;
+
+ // draw little corner markers for regions with
memory
+ // allocated but no contents
+ glBegin( GL_LINES );
+
+ glVertex2f( left, bottom );
+ glVertex2f( left+d, bottom );
+
+ glVertex2f( left, bottom );
+ glVertex2f( left, bottom+d );
+
+
+ glVertex2f( left, top );
+ glVertex2f( left+d, top );
+
+ glVertex2f( left, top );
+ glVertex2f( left, top-d );
+
+
+
+ glVertex2f( right, top );
+ glVertex2f( right-d, top );
+
+ glVertex2f( right, top );
+ glVertex2f( right, top-d );
+
+
+ glVertex2f( right, bottom );
+ glVertex2f( right-d, bottom );
+
+ glVertex2f( right, bottom );
+ glVertex2f( right, bottom+d );
+
+ glEnd();
+ }
+ }
+
char buf[32];
snprintf( buf, 15, "%lu", count );
gl_draw_string( 1<<SBITS, 1<<SBITS, 0, buf );
Modified: code/stage/trunk/libstage/stage.cc
===================================================================
--- code/stage/trunk/libstage/stage.cc 2008-12-06 21:25:20 UTC (rev 7190)
+++ code/stage/trunk/libstage/stage.cc 2008-12-07 02:07:21 UTC (rev 7191)
@@ -52,7 +52,18 @@
return init_called;
}
+double Stg::normalize( double a )
+{
+ assert( ! isnan(a) );
+
+ //return( atan2(sin(a), cos(a)));
+ // faster than return( atan2(sin(a), cos(a)));
+ while( a < -M_PI ) a += (2.0 * M_PI);
+ while( a > M_PI ) a -= (2.0 * M_PI);
+ return a;
+};
+
void Stg::RegisterModel( stg_model_type_t type,
const char*
name,
stg_creator_t
creator )
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2008-12-06 21:25:20 UTC (rev 7190)
+++ code/stage/trunk/libstage/stage.hh 2008-12-07 02:07:21 UTC (rev 7191)
@@ -168,15 +168,7 @@
/** Normalize an angle to within +/_ M_PI
*/
- inline double normalize( double a )
- {
- return( atan2(sin(a), cos(a)));
- // faster than return( atan2(sin(a), cos(a)));
- //while( a < -M_PI ) a += 2.0 * M_PI;
- //while( a > M_PI ) a -= 2.0 * M_PI;
- //return a;
- }
-
+ double normalize( double a );
/** take binary sign of a, either -1, or 1 if >= 0
*/
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2008-12-06 21:25:20 UTC (rev 7190)
+++ code/stage/trunk/libstage/world.cc 2008-12-07 02:07:21 UTC (rev 7191)
@@ -519,20 +519,22 @@
// inline functions for converting from global cell coordinates to
// superregions, regions and local cells
-inline int32_t SUPERREGION( const int32_t& x )
-{
+inline int32_t SUPERREGION( const int32_t x )
+{
+ if( abs((x>>SRBITS) > 100 ) )
+ printf( "MACRO[ %d %d ]", x, (x>>SRBITS) );
return( x >> SRBITS );
}
-inline int32_t REGION( const int32_t& x )
+inline int32_t REGION( const int32_t x )
{
const int32_t _region_coord_mask = ~ ( ( ~ 0x00 ) << SRBITS );
return( ( x & _region_coord_mask ) >> RBITS );
}
-inline int32_t CELL( const int32_t& x )
+inline int32_t CELL( const int32_t x )
{
- static const int32_t _cell_coord_mask = ~ ( ( ~ 0x00 ) << RBITS );
+ const int32_t _cell_coord_mask = ~ ( ( ~ 0x00 ) << RBITS );
return( x & _cell_coord_mask );
}
@@ -568,14 +570,14 @@
const bool ztest )
{
stg_raytrace_result_t sample;
-
- // printf( "raytracing at [ %.2f %.2f %.2f %.2f ] for %.2f \n",
- // pose.x,
- // pose.y,
- // pose.z,
- // pose.a,
- // range );
-
+
+// printf( "raytracing at [ %.2f %.2f %.2f %.2f ] for %.2f \n",
+// gpose.x,
+// gpose.y,
+// gpose.z,
+// gpose.a,
+// range );
+
// initialize the sample
sample.pose = gpose;
sample.range = range; // we might change this below
@@ -585,7 +587,7 @@
stg_point_int_t glob;
glob.x = (int32_t)(gpose.x*ppm);
glob.y = (int32_t)(gpose.y*ppm);
-
+
// record our starting position
stg_point_int_t start = glob;
@@ -640,6 +642,18 @@
else if( dy > 0 )
n++;
+ if( abs(sup.x) > 20 )
+ printf( "raytracing at [ %.2f %.2f %.2f %.2f ] GLOB( %d %d ) SUP( %d
%d )\n",
+ gpose.x,
+ gpose.y,
+ gpose.z,
+ gpose.a,
+ glob.x,
+ glob.y,
+ sup.x,
+ sup.y );
+
+
// find the starting superregion
sr = GetSuperRegionCached( sup ); // possibly NULL, but unlikely
@@ -784,6 +798,7 @@
SuperRegion* StgWorld::AddSuperRegion( const stg_point_int_t& sup )
{
//printf( "Creating super region [ %d %d ]\n", sup.x, sup.y );
+
SuperRegion* sr = CreateSuperRegion( sup );
// the bounds of the world have changed
@@ -815,6 +830,8 @@
inline SuperRegion* StgWorld::GetSuperRegion( const stg_point_int_t& sup )
{
+ //printf( "SUP[ %d %d ] ", sup.x, sup.y );
+
// no, so we try to fetch it out of the hash table
SuperRegion* sr = (SuperRegion*)
g_hash_table_lookup( superregions, (gpointer)&sup );
@@ -835,6 +852,8 @@
glob.x = x;
glob.y = y;
+ //printf( "GC[ %d %d ] ", glob.x, glob.y );
+
return( GetSuperRegionCached( SUPERREGION(glob) )
->GetRegion( REGION(x), REGION(y) )
->GetCell( CELL(x), CELL(y) )) ;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
SF.Net email is Sponsored by MIX09, March 18-20, 2009 in Las Vegas, Nevada.
The future of the web can't happen without you. Join us at MIX09 to help
pave the way to the Next Web now. Learn more and register at
http://ad.doubleclick.net/clk;208669438;13503038;i?http://2009.visitmix.com/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
