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.
------------------------------------------------------------------------------
This SF.net email is sponsored by:
SourcForge Community
SourceForge wants to tell your story.
http://p.sf.net/sfu/sf-spreadtheword
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
