Revision: 7834
http://playerstage.svn.sourceforge.net/playerstage/?rev=7834&view=rev
Author: rtv
Date: 2009-06-10 16:55:56 +0000 (Wed, 10 Jun 2009)
Log Message:
-----------
accepted Toby's recommended Inro patch: new models, friction, stickiness, fltk
fix - good stuff.
Modified Paths:
--------------
code/stage/trunk/libstage/CMakeLists.txt
code/stage/trunk/libstage/model.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/typetable.cc
code/stage/trunk/libstage/worldgui.cc
code/stage/trunk/libstageplugin/CMakeLists.txt
code/stage/trunk/libstageplugin/p_driver.cc
code/stage/trunk/libstageplugin/p_driver.h
code/stage/trunk/libstageplugin/p_simulation.cc
Modified: code/stage/trunk/libstage/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstage/CMakeLists.txt 2009-06-10 15:09:50 UTC (rev
7833)
+++ code/stage/trunk/libstage/CMakeLists.txt 2009-06-10 16:55:56 UTC (rev
7834)
@@ -4,7 +4,6 @@
include_directories(${PROJECT_BINARY_DIR})
set( stageSrcs
- world.cc
ancestor.cc
block.cc
blockgroup.cc
@@ -15,6 +14,7 @@
glcolorstack.cc
logentry.cc
model.cc
+ model_actuator.cc
model_blinkenlight.cc
model_blobfinder.cc
model_callbacks.cc
@@ -28,6 +28,8 @@
model_position.cc
model_props.cc
model_ranger.cc
+ model_loadcell.cc
+ model_lightindicator.cc
option.cc
powerpack.cc
region.cc
@@ -38,6 +40,7 @@
typetable.cc
vis_strip.cc
waypoint.cc
+ world.cc
worldfile.cc
worldgui.cc
canvas.cc
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-06-10 15:09:50 UTC (rev 7833)
+++ code/stage/trunk/libstage/model.cc 2009-06-10 16:55:56 UTC (rev 7834)
@@ -33,6 +33,8 @@
blob_return 1
laser_return LaserVisible
gripper_return 0
+ gravity_return 0
+ sticky_return 0
# GUI properties
gui_nose 0
@@ -90,20 +92,10 @@
- gui_movemask <int>\n
define how the model can be moved by the mouse in the GUI window
+ - friction <float>\n
+ Determines the proportion of velocity lost per second. For example, 0.1
would mean that the object would lose 10% of its speed due to friction per
second. A value of zero (the default) means this model can not be pushed around
(infinite friction).
*/
-/*
- TODO
-
- - friction float [WARNING: Friction model is not yet implemented;
- - details may change] if > 0 the model can be pushed around by other
- - moving objects. The value determines the proportion of velocity
- - lost per second. For example, 0.1 would mean that the object would
- - lose 10% of its speed due to friction per second. A value of zero
- - (the default) means this model can not be pushed around (infinite
- - friction).
-*/
-
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
@@ -160,6 +152,9 @@
ranger_return( true )
{ /* nothing do do */ }
+//static const members
+static const double DEFAULT_FRICTION = 0.0;
+
void Visibility::Load( Worldfile* wf, int wf_entity )
{
blob_return = wf->ReadInt( wf_entity, "blob_return", blob_return);
@@ -169,6 +164,8 @@
laser_return = (stg_laser_return_t)wf->ReadInt( wf_entity, "laser_return",
laser_return);
obstacle_return = wf->ReadInt( wf_entity, "obstacle_return",
obstacle_return);
ranger_return = wf->ReadInt( wf_entity, "ranger_return", ranger_return);
+ gravity_return = wf->ReadInt( wf_entity, "gravity_return", gravity_return);
+ sticky_return = wf->ReadInt( wf_entity, "sticky_return", sticky_return);
}
GuiState:: GuiState() :
@@ -261,6 +258,7 @@
world->AddModel( this );
+ this->friction = DEFAULT_FRICTION;
g_datalist_init( &this->props );
// now we can add the basic square shape
@@ -575,6 +573,11 @@
((Model*)it->data)->MapWithChildren();
}
+void Model::MapFromRoot()
+{
+ Root()->MapWithChildren();
+}
+
void Model::UnMapWithChildren()
{
UnMap();
@@ -584,6 +587,11 @@
((Model*)it->data)->UnMapWithChildren();
}
+void Model::UnMapFromRoot()
+{
+ Root()->UnMapWithChildren();
+}
+
void Model::Map()
{
//PRINT_DEBUG1( "%s.Map()", token );
@@ -612,7 +620,7 @@
world->total_subs--;
world->dirty = true; // need redraw
- printf( "unsubscribe from %s %d\n", token, subs );
+ //printf( "unsubscribe from %s %d\n", token, subs );
// if this is the last sub, call shutdown
if( subs < 1 )
@@ -918,7 +926,7 @@
Model* Model::GetUnsubscribedModelOfType( stg_model_type_t type ) const
{
if( (this->type == type) && (this->subs == 0) )
- return (Model*)this; // discard const
+ return const_cast<Model*> (this); // discard const
// this model is no use. try children recursively
for( GList* it = children; it; it=it->next )
@@ -968,7 +976,18 @@
return NULL;
}
+stg_kg_t Model::GetTotalMass()
+{
+ stg_kg_t sum = mass;
+ for (GList* child = this->children; child; child = child->next) {
+ Model* childModel = (Model*) child->data;
+ sum += childModel->GetTotalMass();
+ }
+ return sum;
+}
+
+
Model* Model::GetModel( const char* modelname ) const
{
// construct the full model name and look it up
@@ -1035,6 +1054,12 @@
rastervis.SetData( data, width, height, cellwidth, cellheight );
}
+void Model::SetFriction( double friction )
+{
+ this->friction = friction;
+ CallCallbacks( &this->friction );
+}
+
//***************************************************************
// Raster data visualizer
//
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2009-06-10 15:09:50 UTC (rev
7833)
+++ code/stage/trunk/libstage/model_laser.cc 2009-06-10 16:55:56 UTC (rev
7834)
@@ -199,6 +199,7 @@
PRINT_DEBUG2( "Constructing ModelLaser %d (%s)\n",
id, typestr );
+
// Model data members
interval = DEFAULT_INTERVAL_MS * (int)thousand;
@@ -285,6 +286,8 @@
{
assert( samples.size() == sample_count );
+ UnMapFromRoot(); // Don't raytrace self
+
double bearing = -fov/2.0;
// make the first and last rays exactly at the extremes of the FOV
double sample_incr = fov / MAX(sample_count-1,1);
@@ -335,6 +338,8 @@
}
}
+ MapFromRoot();
+
Model::Update();
}
Modified: code/stage/trunk/libstage/model_load.cc
===================================================================
--- code/stage/trunk/libstage/model_load.cc 2009-06-10 15:09:50 UTC (rev
7833)
+++ code/stage/trunk/libstage/model_load.cc 2009-06-10 16:55:56 UTC (rev
7834)
@@ -135,6 +135,12 @@
if( wf->PropertyExists( wf_entity, "color_rgba" ))
{
+ if (wf->GetProperty(wf_entity,"color_rgba")->value_count < 4)
+ {
+ PRINT_ERR1( "model %s color_rgba requires 4 values\n", this->token );
+ }
+ else
+ {
double red = wf->ReadTupleFloat( wf_entity, "color_rgba", 0, 0 );
double green = wf->ReadTupleFloat( wf_entity, "color_rgba", 1, 0 );
double blue = wf->ReadTupleFloat( wf_entity, "color_rgba", 2, 0 );
@@ -142,6 +148,7 @@
this->SetColor( stg_color_pack( red, green, blue, alpha ));
}
+ }
if( wf->PropertyExists( wf_entity, "bitmap" ) )
{
@@ -193,6 +200,18 @@
if( wf->PropertyExists( wf_entity, "db_count" ) )
LoadDataBaseEntries( wf, wf_entity );
+ if (vis.gravity_return)
+ {
+ Velocity vel = GetVelocity();
+ this->SetVelocity( vel );
+ world->StartUpdatingModel( this );
+ }
+
+ if( wf->PropertyExists( wf_entity, "friction" ))
+ {
+ this->SetFriction( wf->ReadFloat(wf_entity, "friction", this->friction ));
+ }
+
if( wf->PropertyExists( wf_entity, "ctrl" ))
{
char* lib = (char*)wf->ReadString(wf_entity, "ctrl", NULL );
@@ -339,10 +358,6 @@
sscanf( entry, "%[^<]<%[^>]>%[^\"]", key, type, value );
- assert( key );
- assert( type );
- assert( value );
-
if( strcmp( type, "int" ) == 0 )
SetPropertyInt( strdup(key), atoi(value) );
else if( strcmp( type, "float" ) == 0 )
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-06-10 15:09:50 UTC (rev 7833)
+++ code/stage/trunk/libstage/stage.hh 2009-06-10 16:55:56 UTC (rev 7834)
@@ -102,6 +102,9 @@
MODEL_TYPE_BLINKENLIGHT,
MODEL_TYPE_CAMERA,
MODEL_TYPE_GRIPPER,
+ MODEL_TYPE_ACTUATOR,
+ MODEL_TYPE_LOADCELL,
+ MODEL_TYPE_LIGHTINDICATOR,
MODEL_TYPE_COUNT // must be the last entry, to count the number of
// types
} stg_model_type_t;
@@ -981,6 +984,9 @@
/** hint that the world needs to be redrawn if a GUI is attached */
void NeedRedraw(){ dirty = true; };
+ /** Special model for the floor of the world */
+ Model* ground;
+
/** Get human readable string that describes the current simulation
time. */
virtual std::string ClockString( void );
@@ -1127,6 +1133,9 @@
/// Control printing time to stdout
void ShowClock( bool enable ){ show_clock = enable; };
+
+ /** Return the floor model */
+ Model* GetGround() {return ground;};
};
class Block
@@ -1681,6 +1690,8 @@
stg_laser_return_t laser_return;
bool obstacle_return;
bool ranger_return;
+ bool gravity_return;
+ bool sticky_return;
Visibility();
void Load( Worldfile* wf, int wf_entity );
@@ -1756,6 +1767,7 @@
/** Default color of the model's blocks.*/
stg_color_t color;
+ double friction;
/** This can be set to indicate that the model has new data that
may be of interest to users. This allows polling the model
@@ -1826,6 +1838,9 @@
stg_meters_t cellwidth,
stg_meters_t cellheight );
+ int subs; //< the number of subscriptions to this model
+ int used; //< the number of connections to this model
+
void AddPoint( stg_meters_t x, stg_meters_t y );
void ClearPts();
@@ -1924,6 +1939,10 @@
void MapWithChildren();
void UnMapWithChildren();
+ // Find the root model, and map/unmap the whole tree.
+ void MapFromRoot();
+ void UnMapFromRoot();
+
int TreeToPtrArray( GPtrArray* array ) const;
/** raytraces a single ray from the point and heading identified by
@@ -2209,6 +2228,9 @@
/** return a model's unique process-wide identifier */
uint32_t GetId() const { return id; }
+ /** Get the total mass of a model and it's children recursively */
+ stg_kg_t GetTotalMass();
+
// stg_laser_return_t GetLaserReturn(){ return laser_return; }
/** Change a model's parent - experimental*/
@@ -2230,7 +2252,9 @@
void SetColor( stg_color_t col );
void SetMass( stg_kg_t mass );
void SetStall( stg_bool_t stall );
+ void SetGravityReturn( int val );
void SetGripperReturn( int val );
+ void SetStickyReturn( int val );
void SetLaserReturn( stg_laser_return_t val );
void SetObstacleReturn( int val );
void SetBlobReturn( int val );
@@ -2242,6 +2266,7 @@
void SetGuiOutline( int val );
void SetWatts( stg_watts_t watts );
void SetMapResolution( stg_meters_t res );
+ void SetFriction( double friction );
bool DataIsFresh() const { return this->data_fresh; }
@@ -2376,6 +2401,7 @@
and has the type indicated by the string. This model is
tagged as used. */
Model* GetUnusedModelOfType( const stg_model_type_t type );
+
/** Returns the value of the model's stall boolean, which is true
iff the model has crashed into another model */
bool Stalled() const { return this->stall; }
@@ -2447,6 +2473,8 @@
};
+
+
// LASER MODEL --------------------------------------------------------
/// %ModelLaser class
@@ -2528,7 +2556,45 @@
void SetConfig( Config& cfg );
};
- // GRIPPER MODEL --------------------------------------------------------
+
+// LOAD CELL MODEL --------------------------------------------------------
+
+/// %ModelLoadCell class
+class ModelLoadCell : public Model
+{
+private:
+public:
+ //static const char* typestr;
+ // constructor
+ ModelLoadCell( World* world,
+ Model* parent );
+
+ // destructor
+ ~ModelLoadCell();
+
+ float GetVoltage();
+};
+
+
+// Light indicator model
+class ModelLightIndicator : public Model
+{
+public:
+ ModelLightIndicator(World* world, Model* parent);
+ ~ModelLightIndicator();
+
+ void SetState(bool isOn);
+
+protected:
+ virtual void DrawBlocks();
+
+private:
+ bool m_IsOn;
+};
+
+// \todo GRIPPER MODEL
--------------------------------------------------------
+
+
class ModelGripper : public Model
{
public:
@@ -2931,6 +2997,69 @@
Pose est_origin; ///< global origin of the local coordinate system
};
+
+// ACTUATOR MODEL --------------------------------------------------------
+
+/** Define a actuator control method */
+typedef enum
+{ STG_ACTUATOR_CONTROL_VELOCITY,
+ STG_ACTUATOR_CONTROL_POSITION
+} stg_actuator_control_mode_t;
+
+/** Define an actuator type */
+typedef enum
+{ STG_ACTUATOR_TYPE_LINEAR,
+ STG_ACTUATOR_TYPE_ROTATIONAL
+} stg_actuator_type_t;
+
+
+/// %ModelActuator class
+class ModelActuator : public Model
+{
+ private:
+ double goal; //< the current velocity or pose to reach,
depending on the value of control_mode
+ double pos;
+ double max_speed;
+ double min_position;
+ double max_position;
+ stg_actuator_control_mode_t control_mode;
+ stg_actuator_type_t actuator_type;
+ stg_point3_t axis;
+
+ Pose InitialPose;
+ public:
+ static const char* typestr;
+
+ // constructor
+ ModelActuator( World* world,
+ Model* parent );
+ // destructor
+ ~ModelActuator();
+
+ virtual void Startup();
+ virtual void Shutdown();
+ virtual void Update();
+ virtual void Load();
+
+ /** Sets the control_mode to STG_ACTUATOR_CONTROL_VELOCITY and
sets
+ the goal velocity. */
+ void SetSpeed( double speed );
+
+ double GetSpeed() const {return goal;}
+
+ /** Sets the control mode to STG_ACTUATOR_CONTROL_POSITION and
sets
+ the goal pose */
+ void GoTo( double pose );
+
+ double GetPosition() const {return pos;};
+
+ double GetMaxPosition() const {return max_position;};
+
+ double GetMinPosition() const {return min_position;};
+
+};
+
+
}; // end namespace stg
#endif
Modified: code/stage/trunk/libstage/typetable.cc
===================================================================
--- code/stage/trunk/libstage/typetable.cc 2009-06-10 15:09:50 UTC (rev
7833)
+++ code/stage/trunk/libstage/typetable.cc 2009-06-10 16:55:56 UTC (rev
7834)
@@ -19,6 +19,9 @@
static Model* CreateModelRanger( World* world, Model* parent )
{ return new ModelRanger( world, parent ); }
+static Model* CreateModelLoadCell( World* world, Model* parent )
+{ return new ModelLoadCell( world, parent ); }
+
static Model* CreateModelCamera( World* world, Model* parent )
{ return new ModelCamera( world, parent ); }
@@ -31,7 +34,12 @@
static Model* CreateModelGripper( World* world, Model* parent )
{ return new ModelGripper( world, parent ); }
+static Model* CreateModelActuator( World* world, Model* parent )
+{ return new ModelActuator( world, parent ); }
+static Model* CreateModelLightIndicator( World* world, Model* parent )
+{ return new ModelLightIndicator( world, parent ); }
+
void Stg::RegisterModels()
{
RegisterModel( MODEL_TYPE_PLAIN, "model", CreateModel );
@@ -43,6 +51,9 @@
RegisterModel( MODEL_TYPE_BLOBFINDER, "blobfinder", CreateModelBlobfinder );
RegisterModel( MODEL_TYPE_BLINKENLIGHT, "blinkenlight",
CreateModelBlinkenlight);
RegisterModel( MODEL_TYPE_GRIPPER, "gripper", CreateModelGripper);
+ RegisterModel( MODEL_TYPE_ACTUATOR, "actuator", CreateModelActuator);
+ RegisterModel( MODEL_TYPE_LOADCELL, "loadcell", CreateModelLoadCell );
+ RegisterModel( MODEL_TYPE_LIGHTINDICATOR, "lightindicator",
CreateModelLightIndicator );
#if DEBUG // human-readable view of the table
puts( "Stg::Typetable" );
Modified: code/stage/trunk/libstage/worldgui.cc
===================================================================
--- code/stage/trunk/libstage/worldgui.cc 2009-06-10 15:09:50 UTC (rev
7833)
+++ code/stage/trunk/libstage/worldgui.cc 2009-06-10 16:55:56 UTC (rev
7834)
@@ -256,6 +256,9 @@
{
PRINT_DEBUG1( "%s.Load()", token );
+ // needs to happen before StgWorld load, or we segfault with GL calls on
some graphics cards
+ Fl::check();
+
fileMan->newWorld( filename );
World::Load( filename );
Modified: code/stage/trunk/libstageplugin/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstageplugin/CMakeLists.txt 2009-06-10 15:09:50 UTC
(rev 7833)
+++ code/stage/trunk/libstageplugin/CMakeLists.txt 2009-06-10 16:55:56 UTC
(rev 7834)
@@ -5,6 +5,7 @@
set( stagepluginSrcs p_driver.h
p_driver.cc
+ p_actarray.cc
p_blobfinder.cc
p_simulation.cc
p_laser.cc
@@ -12,6 +13,8 @@
p_position.cc
p_sonar.cc
p_speech.cc
+ p_aio.cc
+ p_dio.cc
stg_time.cc
)
Modified: code/stage/trunk/libstageplugin/p_driver.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_driver.cc 2009-06-10 15:09:50 UTC (rev
7833)
+++ code/stage/trunk/libstageplugin/p_driver.cc 2009-06-10 16:55:56 UTC (rev
7834)
@@ -306,10 +306,22 @@
switch( player_addr.interf )
{
+ case PLAYER_ACTARRAY_CODE:
+ ifsrc = new InterfaceActArray( player_addr, this, cf, section );
+ break;
+
+ case PLAYER_AIO_CODE:
+ ifsrc = new InterfaceAio( player_addr, this, cf, section );
+ break;
+
case PLAYER_BLOBFINDER_CODE:
ifsrc = new InterfaceBlobfinder( player_addr, this, cf, section );
break;
+ case PLAYER_DIO_CODE:
+ ifsrc = new InterfaceDio(player_addr, this, cf, section);
+ break;
+
case PLAYER_FIDUCIAL_CODE:
ifsrc = new InterfaceFiducial( player_addr, this, cf, section
);
break;
Modified: code/stage/trunk/libstageplugin/p_driver.h
===================================================================
--- code/stage/trunk/libstageplugin/p_driver.h 2009-06-10 15:09:50 UTC (rev
7833)
+++ code/stage/trunk/libstageplugin/p_driver.h 2009-06-10 16:55:56 UTC (rev
7834)
@@ -168,6 +168,28 @@
virtual void Publish( void );
};
+class InterfaceAio : public InterfaceModel
+{
+ public:
+ InterfaceAio( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int
section );
+ virtual ~InterfaceAio( void ){ /* TODO: clean up*/ };
+ virtual int ProcessMessage(QueuePointer & resp_queue,
+ player_msghdr_t* hdr,
+ void* data);
+ virtual void Publish( void );
+};
+
+
+class InterfaceDio : public InterfaceModel
+{
+public:
+ InterfaceDio(player_devaddr_t addr, StgDriver* driver, ConfigFile* cf,
int section);
+ virtual ~InterfaceDio();
+ virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr_t*
hdr, void* data);
+ virtual void Publish();
+};
+
+
class InterfacePower : public InterfaceModel
{
public:
@@ -194,6 +216,18 @@
};
+class InterfaceActArray : public InterfaceModel
+{
+ public:
+ InterfaceActArray( player_devaddr_t addr, StgDriver* driver,
ConfigFile* cf, int section );
+ virtual ~InterfaceActArray( void ){ /* TODO: clean up*/ };
+
+ virtual int ProcessMessage( QueuePointer & resp_queue,
+ player_msghdr * hdr,
+ void * data );
+ virtual void Publish( void );
+};
+
class InterfaceBlobfinder : public InterfaceModel
{
public:
Modified: code/stage/trunk/libstageplugin/p_simulation.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_simulation.cc 2009-06-10 15:09:50 UTC
(rev 7833)
+++ code/stage/trunk/libstageplugin/p_simulation.cc 2009-06-10 16:55:56 UTC
(rev 7834)
@@ -148,6 +148,17 @@
player_msghdr_t* hdr,
void* data)
{
+ if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_CAPABILTIES_REQ, addr))
+ {
+ PLAYER_ERROR1("%p\n", data);
+ player_capabilities_req_t & cap_req = *
reinterpret_cast<player_capabilities_req_t *> (data);
+ if (cap_req.type == PLAYER_MSGTYPE_REQ && (cap_req.subtype ==
PLAYER_SIMULATION_REQ_SET_POSE3D || cap_req.subtype ==
PLAYER_SIMULATION_REQ_GET_POSE3D))
+ {
+ this->driver->Publish(addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_CAPABILTIES_REQ);
+ return 0;
+ }
+ }
+
// Is it a request to get a model's pose in 2D?
if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_GET_POSE2D,
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Crystal Reports - New Free Runtime and 30 Day Trial
Check out the new simplified licensing option that enables unlimited
royalty-free distribution of the report engine for externally facing
server and web deployment.
http://p.sf.net/sfu/businessobjects
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
