Revision: 7148 http://playerstage.svn.sourceforge.net/playerstage/?rev=7148&view=rev Author: rtv Date: 2008-11-15 03:00:57 +0000 (Sat, 15 Nov 2008)
Log Message: ----------- major changes for 3.1 Modified Paths: -------------- code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_ranger.cc Property Changed: ---------------- code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_props.cc code/stage/trunk/libstage/model_ranger.cc code/stage/trunk/libstage/model_wifi.cc Modified: code/stage/trunk/libstage/model_blinkenlight.cc =================================================================== --- code/stage/trunk/libstage/model_blinkenlight.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_blinkenlight.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -106,7 +106,7 @@ } -void StgModelBlinkenlight::DataVisualize( StgCamera* cam ) +void StgModelBlinkenlight::DataVisualize( Camera* cam ) { if( on && showBlinkenData ) { @@ -115,7 +115,8 @@ gl_pose_shift( &this->pose ); gl_pose_shift( &this->geom.pose ); glLineWidth( 3 ); - LISTMETHOD( this->blocks, StgBlock*, Draw ); + // TODO + //LISTMETHOD( this->blocks, StgBlock*, Draw ); glLineWidth( 1 ); glPopMatrix(); // drop out of local coords } Property changes on: code/stage/trunk/libstage/model_blinkenlight.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_blobfinder.cc =================================================================== --- code/stage/trunk/libstage/model_blobfinder.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_blobfinder.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -108,11 +108,11 @@ g_array_free( colors, true ); } -static bool blob_match( StgBlock* testblock, - StgModel* finder, - const void* dummy ) +static bool blob_match( StgModel* candidate, + StgModel* finder, + const void* dummy ) { - return( ! finder->IsRelated( testblock->Model() )); + return( ! finder->IsRelated( candidate )); } @@ -178,9 +178,9 @@ StgModel::Update(); // generate a scan for post-processing into a blob image + + stg_raytrace_result_t* samples = new stg_raytrace_result_t[scan_width]; - stg_raytrace_sample_t* samples = new stg_raytrace_sample_t[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 @@ -191,22 +191,22 @@ // scan through the samples looking for color blobs for(unsigned int s=0; s < scan_width; s++ ) { - if( samples[s].block == NULL ) + if( samples[s].mod == NULL ) continue; // we saw nothing //if( samples[s].block->Color() != 0xFFFF0000 ) //continue; // we saw nothing - + unsigned int right = s; - stg_color_t blobcol = samples[s].block->Color(); + stg_color_t blobcol = samples[s].color; //printf( "blob start %d color %X\n", blobleft, blobcol ); // loop until we hit the end of the blob // there has to be a gap of >1 pixel to end a blob // this avoids getting lots of crappy little blobs - while( s < scan_width && samples[s].block && - ColorMatchIgnoreAlpha( samples[s].block->Color(), blobcol) ) + while( s < scan_width && samples[s].mod && + ColorMatchIgnoreAlpha( samples[s].color, blobcol) ) { //printf( "%u blobcol %X block %p %s color %X\n", s, blobcol, samples[s].block, samples[s].block->Model()->Token(), samples[s].block->Color() ); s++; @@ -295,7 +295,7 @@ StgModel::Shutdown(); } -void StgModelBlobfinder::DataVisualize( StgCamera* cam ) +void StgModelBlobfinder::DataVisualize( Camera* cam ) { if ( !showBlobData ) return; Property changes on: code/stage/trunk/libstage/model_blobfinder.cc ___________________________________________________________________ Added: svn:eol + native Property changes on: code/stage/trunk/libstage/model_callbacks.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_camera.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -19,7 +19,7 @@ Option StgModelCamera::showCameraData( "Show Camera Data", "show_camera", "", true ); -static const stg_size_t DEFAULT_SIZE = { 0.1, 0.07, 0.05 }; +static const stg_size_t DEFAULT_SIZE( 0.1, 0.07, 0.05 ); static const char DEFAULT_GEOM_COLOR[] = "black"; static const float DEFAULT_HFOV = 70; static const float DEFAULT_VFOV = 40; @@ -235,7 +235,7 @@ } //TODO create lines outlining camera frustrum, then iterate over each depth measurement and create a square -void StgModelCamera::DataVisualize( StgCamera* cam ) +void StgModelCamera::DataVisualize( Camera* cam ) { if( _frame_data == NULL || !showCameraData ) Property changes on: code/stage/trunk/libstage/model_camera.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_fiducial.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -71,6 +71,9 @@ PRINT_DEBUG2( "Constructing StgModelFiducial %d (%s)\n", id, typestr ); + // assert that Update() is reentrant for this derived model + thread_safe = true; + // sensible fiducial defaults // interval = 200; // common for a SICK LMS200 @@ -100,9 +103,11 @@ g_array_free( data, true ); } -static bool fiducial_raytrace_match( StgBlock* testblock, StgModel* finder, const void* dummy ) +static bool fiducial_raytrace_match( StgModel* candidate, + StgModel* finder, + const void* dummy ) { - return( ! finder->IsRelated( testblock->Model() ) ); + return( ! finder->IsRelated( candidate ) ); } @@ -160,18 +165,15 @@ //printf( "bearing %.2f\n", RTOD(bearing) ); - - stg_raytrace_sample_t ray; - - Raytrace( dtheta, - max_range_anon, - fiducial_raytrace_match, - NULL, - &ray, - false ); - + + stg_raytrace_result_t ray = Raytrace( dtheta, + max_range_anon, + fiducial_raytrace_match, + NULL, + false ); + range = ray.range; - StgModel* hitmod = ray.block->Model(); + StgModel* hitmod = ray.mod; // printf( "ray hit %s and was seeking LOS to %s\n", //hitmod ? hitmod->Token() : "null", @@ -248,7 +250,7 @@ } -void StgModelFiducial::DataVisualize( StgCamera* cam ) +void StgModelFiducial::DataVisualize( Camera* cam ) { if ( !showFiducialData ) return; Property changes on: code/stage/trunk/libstage/model_fiducial.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_load.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -39,9 +39,9 @@ else PRINT_ERR1( "Name blank for model %s. Check your worldfile\n", this->token ); } - + //PRINT_WARN1( "%s::Load", token ); - + if( wf->PropertyExists( wf_entity, "origin" ) ) { stg_geom_t geom = GetGeom(); @@ -79,17 +79,8 @@ vel.z = wf->ReadTupleLength(wf_entity, "velocity", 2, vel.z ); vel.a = wf->ReadTupleAngle(wf_entity, "velocity", 3, vel.a ); this->SetVelocity( vel ); - - if( vel.x || vel.y || vel.z || vel.a ) - world->StartUpdatingModel( this ); } - - if( wf->PropertyExists( wf_entity, "boundary" )) - { - this->SetBoundary( wf->ReadInt(wf_entity, "boundary", this->boundary )); - } - if( wf->PropertyExists( wf_entity, "color" )) { stg_color_t col = 0xFFFF0000; // red; @@ -118,148 +109,51 @@ this->SetColor( stg_color_pack( red, green, blue, alpha )); } - + if( wf->PropertyExists( wf_entity, "bitmap" ) ) { const char* bitmapfile = wf->ReadString( wf_entity, "bitmap", NULL ); assert( bitmapfile ); - char full[_POSIX_PATH_MAX]; + if( has_default_block ) + { + blockgroup.Clear(); + has_default_block = false; + } - if( bitmapfile[0] == '/' ) - strcpy( full, bitmapfile ); - else - { - char *tmp = strdup(wf->filename); - snprintf( full, _POSIX_PATH_MAX, - "%s/%s", dirname(tmp), bitmapfile ); - free(tmp); - } - - PRINT_DEBUG1( "attempting to load image %s", full ); - - stg_rotrect_t* rects = NULL; - unsigned int rect_count = 0; - unsigned int width, height; - if( stg_rotrects_from_image_file( full, - &rects, - &rect_count, - &width, &height ) ) - { - PRINT_ERR1( "failed to load rects from image file \"%s\"", - full ); - return; - } - - this->UnMap(); - this->ClearBlocks(); - - //printf( "found %d rects\n", rect_count ); - - if( rects && (rect_count > 0) ) - { - //puts( "loading rects" ); - for( unsigned int r=0; r<rect_count; r++ ) - this->AddBlockRect( rects[r].pose.x, rects[r].pose.y, - rects[r].size.x, rects[r].size.y ); - - if( this->boundary ) - { - // add thin bounding blocks - double epsilon = 0.01; - this->AddBlockRect(0,0, epsilon, height ); - this->AddBlockRect(0,0, width, epsilon ); - this->AddBlockRect(0, height-epsilon, width, epsilon ); - this->AddBlockRect(width-epsilon,0, epsilon, height ); - } - - StgBlock::ScaleList( this->blocks, &this->geom.size ); - this->Map(); - this->NeedRedraw(); - - g_free( rects ); - } - - //printf( "model %s block count %d\n", - // token, g_list_length( blocks )); + //puts( "clearing blockgroup" ); + //blockgroup.Clear(); + //puts( "loading bitmap" ); + blockgroup.LoadBitmap( this, bitmapfile, wf ); } - - if( wf->PropertyExists( wf_entity, "blocks" ) ) + + if( wf->PropertyExists( wf_entity, "boundary" )) { - int blockcount = wf->ReadInt( wf_entity, "blocks", 0 ); + this->SetBoundary( wf->ReadInt(wf_entity, "boundary", this->boundary )); - this->UnMap(); - this->ClearBlocks(); + if( boundary ) + { + //PRINT_WARN1( "setting boundary for %s\n", token ); + + blockgroup.CalcSize(); - //printf( "expecting %d blocks\n", blockcount ); + double epsilon = 0.01; + double width = blockgroup.size.x; + double height = blockgroup.size.y; + double dx = width/2.0; + double dy = height/2.0; + double dz = blockgroup.size.z; - char key[256]; - for( int l=0; l<blockcount; l++ ) - { - snprintf(key, sizeof(key), "block[%d].points", l); - int pointcount = wf->ReadInt(wf_entity,key,0); + // add thin bounding blocks + AddBlockRect(-dx,-dy, epsilon, height, dz ); + AddBlockRect(-dx,-dy, width, epsilon, dz ); + AddBlockRect(-dx, -dy+height-epsilon, width, epsilon, dz ); + AddBlockRect(-dx+width-epsilon,-dy, epsilon, height, dz ); + } + } - //printf( "expecting %d points in block %d\n", - //pointcount, l ); - - stg_point_t* pts = stg_points_create( pointcount ); - - int p; - for( p=0; p<pointcount; p++ ) { - snprintf(key, sizeof(key), "block[%d].point[%d]", l, p ); - - pts[p].x = wf->ReadTupleLength(wf_entity, key, 0, 0); - pts[p].y = wf->ReadTupleLength(wf_entity, key, 1, 0); - - //printf( "key %s x: %.2f y: %.2f\n", - // key, pt.x, pt.y ); - } - - // block Z axis - snprintf(key, sizeof(key), "block[%d].z", l); - - stg_meters_t zmin = - wf->ReadTupleLength(wf_entity, key, 0, 0.0 ); - - stg_meters_t zmax = - wf->ReadTupleLength(wf_entity, key, 1, 1.0 ); - - // block color - stg_color_t blockcol = this->color; - bool inherit_color = true; - - snprintf(key, sizeof(key), "block[%d].color", l); - - const char* colorstr = wf->ReadString( wf_entity, key, NULL ); - if( colorstr ) - { - blockcol = stg_lookup_color( colorstr ); - inherit_color = false; - } - - this->AddBlock( pts, pointcount, zmin, zmax, blockcol, inherit_color ); - - stg_points_destroy( pts ); - } - - StgBlock::ScaleList( this->blocks, &this->geom.size ); - - if( this->boundary ) - { - // add thin bounding blocks - double epsilon = 0.001; - double width = geom.size.x; - double height = geom.size.y; - this->AddBlockRect(-width/2.0, -height/2.0, epsilon, height ); - this->AddBlockRect(-width/2.0, -height/2.0, width, epsilon ); - this->AddBlockRect(-width/2.0, height/2.0-epsilon, width, epsilon ); - this->AddBlockRect(width/2.0-epsilon, -height/2.0, epsilon, height ); - } - - this->Map(); - } - - if( wf->PropertyExists( wf_entity, "mass" )) + + if( wf->PropertyExists( wf_entity, "mass" )) this->SetMass( wf->ReadFloat(wf_entity, "mass", this->mass )); if( wf->PropertyExists( wf_entity, "fiducial_return" )) @@ -301,13 +195,13 @@ if( wf->PropertyExists( wf_entity, "ctrl" )) { char* lib = (char*)wf->ReadString(wf_entity, "ctrl", NULL ); - + if( !lib ) - puts( "Error - NULL library name" ); + puts( "Error - NULL library name" ); else - LoadControllerModule( lib ); + LoadControllerModule( lib ); } - + if( wf->PropertyExists( wf_entity, "say" )) this->Say( wf->ReadString(wf_entity, "say", NULL )); @@ -318,8 +212,10 @@ if( wf->PropertyExists( wf_entity, "alwayson" )) { if( wf->ReadInt( wf_entity, "alwayson", 0) > 0 ) - Startup(); + Startup(); } + + MapWithChildren(); if( this->debug ) printf( "Model \"%s\" is in debug mode\n", token ); @@ -406,6 +302,9 @@ } fflush(stdout); + + // as we now have a controller, the world needs to call our update function + StartUpdating(); } Property changes on: code/stage/trunk/libstage/model_load.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_position.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -89,6 +89,9 @@ PRINT_DEBUG2( "Constructing StgModelPosition %d (%s)\n", id, typestr ); + // assert that Update() is reentrant for this derived model + thread_safe = true; + // no power consumed until we're subscribed this->SetWatts( 0 ); @@ -434,6 +437,7 @@ PRINT_DEBUG3( " READING POSITION: [ %.4f %.4f %.4f ]\n", est_pose.x, est_pose.y, est_pose.a ); + StgModel::Update(); } Property changes on: code/stage/trunk/libstage/model_position.cc ___________________________________________________________________ Added: svn:eol + native Property changes on: code/stage/trunk/libstage/model_props.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_ranger.cc =================================================================== --- code/stage/trunk/libstage/model_ranger.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_ranger.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -108,6 +108,9 @@ // Set up sensible defaults + // assert that Update() is reentrant for this derived model + thread_safe = true; + stg_color_t col = stg_lookup_color( RANGER_CONFIG_COLOR ); this->SetColor( col ); @@ -255,14 +258,15 @@ } } -static bool ranger_match( StgBlock* block, StgModel* finder, const void* dummy ) +static bool ranger_match( StgModel* candidate, + StgModel* finder, const void* dummy ) { //printf( "ranger match sees %s %p %d \n", // block->Model()->Token(), block->Model(), block->Model()->LaserReturn() ); // Ignore myself, my children, and my ancestors. - return( block->Model()->GetRangerReturn() && - !block->Model()->IsRelated( finder ) ); + return( candidate->GetRangerReturn() && + !candidate->IsRelated( finder ) ); } void StgModelRanger::Update( void ) @@ -285,13 +289,11 @@ // TODO - reinstate multi-ray rangers //for( int r=0; r<sensors[t].ray_count; r++ ) //{ - stg_raytrace_sample_t ray; - Raytrace( sensors[t].pose, - sensors[t].bounds_range.max, - ranger_match, - NULL, - &ray ); - + stg_raytrace_result_t ray = Raytrace( sensors[t].pose, + sensors[t].bounds_range.max, + ranger_match, + NULL ); + samples[t] = MAX( ray.range, sensors[t].bounds_range.min ); //sensors[t].error = TODO; } @@ -321,7 +323,7 @@ puts( " ]" ); } -void StgModelRanger::DataVisualize( StgCamera* cam ) +void StgModelRanger::DataVisualize( Camera* cam ) { if( ! (samples && sensors && sensor_count) ) return; @@ -381,16 +383,14 @@ } } - // draw the filled triangles in transparent blue + // draw the filled triangles in transparent pale green glDepthMask( GL_FALSE ); - PushColor( 0, 1, 0, 0.1 ); // transparent pale green - //glEnableClientState( GL_VERTEX_ARRAY ); + PushColor( 0, 1, 0, 0.1 ); glVertexPointer( 3, GL_FLOAT, 0, pts ); glDrawArrays( GL_TRIANGLES, 0, 3 * sensor_count ); // restore state glDepthMask( GL_TRUE ); - //glDisableClientState( GL_VERTEX_ARRAY ); PopColor(); } Property changes on: code/stage/trunk/libstage/model_ranger.cc ___________________________________________________________________ Added: svn:eol + native Property changes on: code/stage/trunk/libstage/model_wifi.cc ___________________________________________________________________ Added: svn:eol + native This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------- This SF.Net email is sponsored by the Moblin Your Move Developer's challenge Build the coolest Linux based applications with Moblin SDK & win great prizes Grand prize is a trip for two to an Open Source event anywhere in the world http://moblin-contest.org/redirect.php?banner_id=100&url=/ _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit