From: <rt...@us...> - 2009-01-18 00:10:27
|
Revision: 7279 http://playerstage.svn.sourceforge.net/playerstage/?rev=7279&view=rev Author: rtv Date: 2009-01-18 00:10:21 +0000 (Sun, 18 Jan 2009) Log Message: ----------- added simple power model and some cosmetic and gui fixes Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model.hh code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/worlds/fasr.world code/stage/trunk/worlds/map.inc code/stage/trunk/worlds/pioneer.inc Added Paths: ----------- code/stage/trunk/libstage/powerpack.cc Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-01-18 00:10:21 UTC (rev 7279) @@ -27,6 +27,7 @@ option.hh options_dlg.cc options_dlg.hh + powerpack.cc region.cc resource.cc stage.cc Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/model.cc 2009-01-18 00:10:21 UTC (rev 7279) @@ -151,6 +151,7 @@ on_velocity_list( false ), parent(parent), pose(), + power_pack( NULL ), props(NULL), rebuild_displaylist(true), say_string(NULL), @@ -656,7 +657,26 @@ { // printf( "[%llu] %s update (%d subs)\n", // this->world->sim_time, this->token, this->subs ); + + // f we're drawing current and a power pack has been installed + if( power_pack && (watts > 0) ) + { + // consume energy stored in the power pack + stg_joules_t consumed = watts * (world->interval_sim * 1e-6); + power_pack->stored -= consumed; + /* + printf ( "%s current %.2f consumed %.6f ppack @ %p [ %.2f/%.2f (%.0f)\n", + token, + watts, + consumed, + power_pack, + power_pack->stored, + power_pack->capacity, + power_pack->stored / power_pack->capacity * 100.0 ); + */ + } + CallCallbacks( &hooks.update ); last_update = world->sim_time; } @@ -920,18 +940,19 @@ void Model::DrawStatus( Camera* cam ) { - if( say_string ) + + + if( say_string || power_pack ) { float yaw, pitch; pitch = - cam->pitch(); yaw = - cam->yaw(); - float robotAngle = -rtod(pose.a); + Pose gpz = GetGlobalPose(); + + float robotAngle = -rtod(gpz.a); glPushMatrix(); - fl_font( FL_HELVETICA, 12 ); - float w = gl_width( this->say_string ); // scaled text width - float h = gl_height(); // scaled text height // move above the robot glTranslatef( 0, 0, 0.5 ); @@ -939,61 +960,77 @@ // rotate to face screen glRotatef( robotAngle - yaw, 0,0,1 ); glRotatef( -pitch, 1,0,0 ); + + + //if( ! parent ) + // glRectf( 0,0,1,1 ); - //get raster positition, add gl_width, then project back to world coords - glRasterPos3f( 0, 0, 0 ); - GLfloat pos[ 4 ]; - glGetFloatv(GL_CURRENT_RASTER_POSITION, pos); - - GLboolean valid; - glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid ); - if( valid ) - { - GLdouble wx, wy, wz; - GLint viewport[4]; - glGetIntegerv(GL_VIEWPORT, viewport); + if( power_pack && (power_pack->mod == this) ) + power_pack->Visualize( cam ); + + if( say_string ) + { + //get raster positition, add gl_width, then project back to world coords + glRasterPos3f( 0, 0, 0 ); + GLfloat pos[ 4 ]; + glGetFloatv(GL_CURRENT_RASTER_POSITION, pos); - GLdouble modelview[16]; - glGetDoublev(GL_MODELVIEW_MATRIX, modelview); + GLboolean valid; + glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid ); - GLdouble projection[16]; - glGetDoublev(GL_PROJECTION_MATRIX, projection); - - //get width and height in world coords - gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz ); - w = wx; - gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz ); - h = wy; - - // calculate speech bubble margin - const float m = h/10; - - // draw inside of bubble - PushColor( BUBBLE_FILL ); - glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); - glPolygonMode( GL_FRONT, GL_FILL ); - glEnable( GL_POLYGON_OFFSET_FILL ); - glPolygonOffset( 1.0, 1.0 ); - gl_draw_octagon( w, h, m ); - glDisable( GL_POLYGON_OFFSET_FILL ); - PopColor(); - - // draw outline of bubble - PushColor( BUBBLE_BORDER ); - glLineWidth( 1 ); - glEnable( GL_LINE_SMOOTH ); - glPolygonMode( GL_FRONT, GL_LINE ); - gl_draw_octagon( w, h, m ); - glPopAttrib(); - PopColor(); - - PushColor( BUBBLE_TEXT ); - // draw text inside the bubble - gl_draw_string( 2.5*m, 2.5*m, 0, this->say_string ); - PopColor(); - } - glPopMatrix(); - } + if( valid ) + { + + fl_font( FL_HELVETICA, 12 ); + float w = gl_width( this->say_string ); // scaled text width + float h = gl_height(); // scaled text height + + GLdouble wx, wy, wz; + GLint viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + + GLdouble modelview[16]; + glGetDoublev(GL_MODELVIEW_MATRIX, modelview); + + GLdouble projection[16]; + glGetDoublev(GL_PROJECTION_MATRIX, projection); + + //get width and height in world coords + gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz ); + w = wx; + gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz ); + h = wy; + + // calculate speech bubble margin + const float m = h/10; + + // draw inside of bubble + PushColor( BUBBLE_FILL ); + glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); + glPolygonMode( GL_FRONT, GL_FILL ); + glEnable( GL_POLYGON_OFFSET_FILL ); + glPolygonOffset( 1.0, 1.0 ); + gl_draw_octagon( w, h, m ); + glDisable( GL_POLYGON_OFFSET_FILL ); + PopColor(); + + // draw outline of bubble + PushColor( BUBBLE_BORDER ); + glLineWidth( 1 ); + glEnable( GL_LINE_SMOOTH ); + glPolygonMode( GL_FRONT, GL_LINE ); + gl_draw_octagon( w, h, m ); + glPopAttrib(); + PopColor(); + + PushColor( BUBBLE_TEXT ); + // draw text inside the bubble + gl_draw_string( 2.5*m, 2.5*m, 0, this->say_string ); + PopColor(); + } + } + glPopMatrix(); + } if( stall ) { @@ -1074,9 +1111,13 @@ PushColor( flag->color ); + + glEnable(GL_POLYGON_OFFSET_FILL); + glPolygonOffset(1.0, 1.0); gluQuadricDrawStyle( quadric, GLU_FILL ); gluSphere( quadric, flag->size/2.0, 4,2 ); - + glDisable(GL_POLYGON_OFFSET_FILL); + // draw the edges darker version of the same color double r,g,b,a; stg_color_unpack( flag->color, &r, &g, &b, &a ); @@ -1149,7 +1190,20 @@ void Model::DataVisualize( Camera* cam ) { - // do nothing +// if( power_pack ) +// { +// // back into global coords to get rid of my rotation +// glPushMatrix(); +// gl_pose_inverse_shift( GetGlobalPose() ); + +// // shift to the top left corner of the model (roughly) +// glTranslatef( pose.x - geom.size.x/2.0, +// pose.y + geom.size.y/2.0, +// pose.z + geom.size.z ); + +// power_pack->Visualize( cam ); +// glPopMatrix(); +// } } void Model::DataVisualizeTree( Camera* cam ) Modified: code/stage/trunk/libstage/model.hh =================================================================== --- code/stage/trunk/libstage/model.hh 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/model.hh 2009-01-18 00:10:21 UTC (rev 7279) @@ -1,6 +1,30 @@ #ifndef MODEL_H #define MODEL_H +class Camera; + +/** energy data packet */ +class PowerPack +{ +public: + PowerPack( Model* mod ); + + /** The model that owns this object */ + Model* mod; + + /** Energy stored */ + stg_joules_t stored; + + /** Energy capacity */ + stg_joules_t capacity; + + /** TRUE iff the device is receiving energy from a charger */ + bool charging; + + /** OpenGL visualization of the powerpack state */ + void Visualize( Camera* cam ); +}; + class Visibility { public: @@ -76,7 +100,7 @@ nose = wf->ReadInt( wf_entity, "gui_nose", nose); grid = wf->ReadInt( wf_entity, "gui_grid", grid); outline = wf->ReadInt( wf_entity, "gui_outline", outline); - mask = wf->ReadInt( wf_entity, "gui_mask", mask); + mask = wf->ReadInt( wf_entity, "gui_movemask", mask); } }; @@ -129,7 +153,6 @@ Geom geom; Pose global_pose; bool gpose_dirty; //< set this to indicate that global pose may have changed - /** Controls our appearance and functionality in the GUI, if used */ GuiState gui; @@ -157,6 +180,9 @@ global coordinate frame is the parent is NULL. */ Pose pose; + /** Optional attached PowerPack, defaults to NULL */ + PowerPack* power_pack; + /** GData datalist can contain arbitrary named data items. Can be used by derived model types to store properties, and for user code to associate arbitrary items with a model. */ @@ -699,47 +725,9 @@ void RemoveAllColors(); }; -// ENERGY model -------------------------------------------------------------- -/** energy data packet */ -typedef struct -{ - /** estimate of current energy stored */ - stg_joules_t stored; - /** TRUE iff the device is receiving energy from a charger */ - stg_bool_t charging; - /** diatance to charging device */ - stg_meters_t range; - - /** an array of pointers to connected models */ - GPtrArray* connections; -} stg_energy_data_t; - -/** energy config packet (use this to set or get energy configuration)*/ -typedef struct -{ - /** maximum storage capacity */ - stg_joules_t capacity; - - /** When charging another device, supply this many Joules/sec at most*/ - stg_watts_t give_rate; - - /** When charging from another device, receive this many Joules/sec at most*/ - stg_watts_t take_rate; - - /** length of the charging probe */ - stg_meters_t probe_range; - - /** iff TRUE, this device will supply power to connected devices */ - stg_bool_t give; - -} stg_energy_config_t; - -// there is currently no energy command packet - - // LASER MODEL -------------------------------------------------------- /** laser sample packet Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/model_load.cc 2009-01-18 00:10:21 UTC (rev 7279) @@ -19,7 +19,59 @@ PRINT_DEBUG1( "Model \"%s\" loading...", token ); + if( wf->PropertyExists( wf_entity, "joules" ) ) + { + if( !power_pack ) + power_pack = new PowerPack( this ); + + power_pack->stored = + wf->ReadFloat( wf_entity, "joules", power_pack->stored ); + /* assume that the store is full, so the capacity is the same as + the charge */ + power_pack->capacity = power_pack->stored; + } + + if( wf->PropertyExists( wf_entity, "joules_capacity" ) ) + { + if( !power_pack ) + power_pack = new PowerPack( this ); + + power_pack->capacity = + wf->ReadFloat( wf_entity, "joules_stored", power_pack->capacity ); + + } + + /** if the capacity has been specified, limit the store to the capacity */ + if( power_pack && (power_pack->stored > power_pack->capacity) ) + { + power_pack->stored = power_pack->capacity; + PRINT_WARN3( "model %s energy storage exceeds capacity (%.2f / %.2f joules). Limited stored energy to max capactity.", + token, + power_pack->stored, + power_pack->capacity ); + } + + if( wf->PropertyExists( wf_entity, "watts" ) ) + { + watts = wf->ReadFloat( wf_entity, "watts", watts ); + + if( watts > 0 ) + { + // find a power pack attached to me or an ancestor in my tree + while( (!power_pack) && parent ) + { + power_pack = parent->power_pack; + } + + if( power_pack == NULL ) + { + PRINT_WARN2( "worldfile requests %.2f watts for model %s, but can not find an energy source. Setting watts has no effect unless you also specify a \"joules\" value for this model or an ancestor.", watts, token ); + exit(-1); + } + } + } + if( wf->PropertyExists( wf_entity, "debug" ) ) { PRINT_WARN2( "debug property specified for model %d %s\n", Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/model_position.cc 2009-01-18 00:10:21 UTC (rev 7279) @@ -69,8 +69,8 @@ -const double STG_POSITION_WATTS_KGMS = 5.0; // cost per kg per meter per second -const double STG_POSITION_WATTS = 2.0; // base cost of position device +const double STG_POSITION_WATTS_KGMS = 10.0; // current per kg per meter per second +const double STG_POSITION_WATTS = 1.0; // base cost of position device // simple odometry error model parameters. the error is selected at // random in the interval -MAX/2 to +MAX/2 at startup @@ -385,13 +385,13 @@ default: PRINT_ERR1( "unrecognized position command mode %d", control_mode ); } - + // simple model of power consumption - // this->watts = STG_POSITION_WATTS + - //fabs(vel->x) * STG_POSITION_WATTS_KGMS * this->mass + - //fabs(vel->y) * STG_POSITION_WATTS_KGMS * this->mass + - //fabs(vel->a) * STG_POSITION_WATTS_KGMS * this->mass; - + watts = STG_POSITION_WATTS + + fabs(vel.x) * STG_POSITION_WATTS_KGMS * mass + + fabs(vel.y) * STG_POSITION_WATTS_KGMS * mass + + fabs(vel.a) * STG_POSITION_WATTS_KGMS * mass; + //PRINT_DEBUG4( "model %s velocity (%.2f %.2f %.2f)", // this->token, // this->velocity.x, @@ -636,6 +636,9 @@ glPopMatrix(); } + + // inherit more viz + Model::DataVisualize( cam ); } void ModelPosition::DrawWaypoints() Added: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc (rev 0) +++ code/stage/trunk/libstage/powerpack.cc 2009-01-18 00:10:21 UTC (rev 7279) @@ -0,0 +1,58 @@ +/** powerpack.cc + Simple model of energy storage + Richard Vaughan + Created 2009.1.15 + $Id$ +*/ + +#include "stage_internal.hh" + +PowerPack::PowerPack( Model* mod ) : + mod( mod), stored( 0.0 ), capacity( 0.0 ) +{ + // nothing to do +}; + + +/** OpenGL visualization of the powerpack state */ +void PowerPack::Visualize( Camera* cam ) +{ + const double height = 0.5; + const double width = 0.3; + + double percent = stored/capacity * 100.0; + + if( percent > 50 ) + glColor4f( 0,1,0, 0.7 ); // green + else if( percent > 25 ) + glColor4f( 1,0,1, 0.7 ); // magenta + else + glColor4f( 1,0,0, 0.7 ); // red + + static char buf[6]; + snprintf( buf, 6, "%.0f", percent ); + + glTranslatef( -width, 0.0, 0.0 ); + + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + GLfloat fullness = height * (percent * 0.01); + glRectf( 0,0,width, fullness); + + // outline the charge-o-meter + glTranslatef( 0,0,0.001 ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + glColor4f( 0,0,0,0.7 ); + glRectf( 0,0,width, height ); + + glBegin( GL_LINES ); + glVertex2f( 0, fullness ); + glVertex2f( width, fullness ); + glEnd(); + + // draw the percentage + //gl_draw_string( -0.2, 0, 0, buf ); + + // ? + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); +} Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/libstage/stage.hh 2009-01-18 00:10:21 UTC (rev 7279) @@ -847,7 +847,7 @@ stg_bounds3d_t extent; ///< Describes the 3D volume of the world bool graphics;///< true iff we have a GUI - stg_usec_t interval_sim; ///< temporal resolution: milliseconds that elapse between simulated time steps + stg_usec_t interval_sim; ///< temporal resolution: microseconds that elapse between simulated time steps GList* ray_list;///< List of rays traced for debug visualization stg_usec_t sim_time; ///< the current sim time in this world in ms GHashTable* superregions; Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/worlds/fasr.world 2009-01-18 00:10:21 UTC (rev 7279) @@ -7,7 +7,7 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 0 # real-time interval between simulation updates in milliseconds +interval_real 00 # real-time interval between simulation updates in milliseconds paused 1 resolution 0.02 @@ -22,7 +22,7 @@ ( size [ 600.000 600.000 ] - center [ -0.118 -0.212 ] + center [ -0.521 -0.218 ] rotate [ 0 0 ] scale 30.883 @@ -61,13 +61,20 @@ ctrl "sink" ) +#charger +#( +# pose +# +#) + define autorob pioneer2dx ( - sicklaser( samples 32 range_max 5 laser_return 2 ) + sicklaser( samples 32 range_max 5 laser_return 2 watts 30 ) ctrl "fasr" + joules 1000000 ) -autorob( pose [5.099 4.804 0 -73.937] ) +autorob( pose [4.116 6.107 0 -147.323] ) autorob( pose [6.471 5.304 0 14.941] ) autorob( pose [5.937 4.858 0 -147.503] ) autorob( pose [7.574 6.269 0 -111.715] ) Modified: code/stage/trunk/worlds/map.inc =================================================================== --- code/stage/trunk/worlds/map.inc 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/worlds/map.inc 2009-01-18 00:10:21 UTC (rev 7279) @@ -24,6 +24,11 @@ color "orange" size [ 2 2 0.02 ] + gui_nose 0 + gui_grid 0 + gui_movemask 0 + gui_outline 0 + # insensible to collision and range sensors obstacle_return 0 laser_return 0 Modified: code/stage/trunk/worlds/pioneer.inc =================================================================== --- code/stage/trunk/worlds/pioneer.inc 2009-01-16 22:32:00 UTC (rev 7278) +++ code/stage/trunk/worlds/pioneer.inc 2009-01-18 00:10:21 UTC (rev 7279) @@ -127,7 +127,7 @@ gui_nose 1 # estimated mass in KG - mass 15.0 + mass 23.0 # use the sonar array defined above with a small vertical offset to # drop the sensors into the robot body This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |