Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

moved Model::Move into ModelPosition::Move. Now only position models …

…can have velocity (makes code more simple)
  • Loading branch information...
commit 78cb1d5eb46274b75713ba4e39697af960dc1f27 1 parent bc4d761
@rtv authored
View
4 CMakeLists.txt
@@ -1,8 +1,8 @@
PROJECT(Stage)
SET( V_MAJOR 4 )
-SET( V_MINOR 0 )
-SET( V_BUGFIX 1 )
+SET( V_MINOR 1 )
+SET( V_BUGFIX dev )
SET( VERSION ${V_MAJOR}.${V_MINOR}.${V_BUGFIX} )
SET( APIVERSION ${V_MAJOR}.${V_MINOR} )
View
11 libstage/gl.cc
@@ -64,9 +64,14 @@ void Stg::Gl::draw_array( float x, float y, float w, float h,
void Stg::Gl::draw_string( float x, float y, float z, const char *str )
{
- // glRasterPos2f( x, y );
- // printf( "[%.2f %.2f %.2f] string %u %s\n", x,y,z,(unsigned int)strlen(str), str );
- gl_draw( str, x, y ); // fltk function
+ glRasterPos2f( x, y );
+
+ GLboolean b;
+ glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &b );
+
+ //printf( "[%.2f %.2f %.2f] %d string %u %s\n", x,y,z, (int)b, (unsigned int)strlen(str), str );
+
+ if( b ) gl_draw( str, x, y ); // fltk function
}
void Stg::Gl::draw_string_multiline( float x, float y, float w, float h, const char *str, Fl_Align align )
View
86 libstage/model.cc
@@ -5,7 +5,7 @@
/** @defgroup model
The basic model simulates an object with basic properties; position,
- size, velocity, color, visibility to various sensors, etc. The basic
+ size, color, visibility to various sensors, etc. The basic
model also has a body made up of a list of lines. Internally, the
basic model is used as the base class for all other model types. You
can use the basic model to simulate environmental objects.
@@ -22,7 +22,6 @@
pose [ 0.0 0.0 0.0 0.0 ]
size [ 0.1 0.1 0.1 ]
origin [ 0.0 0.0 0.0 0.0 ]
- velocity [ 0.0 0.0 0.0 0.0 ]
update_interval 100
@@ -66,24 +65,11 @@
- origin [ x:<float> y:<float> z:<float> heading:<float> ]\n
specify the position of the object's center, relative to its pose
-
- - velocity [ x:<float> y:<float> z:<float> heading:<float>
- omega:<float> ]\n Specify the initial velocity of the model. Note
- that if the model hits an obstacle, its velocity will be set to
- zero.
- update_interval int (defaults to 100) The amount of simulated
time in milliseconds between calls to Model::Update(). Controls
the frequency with which this model's data is generated.
- - velocity_enable int (defaults to 0)\n Most models ignore their
- velocity state. This saves on processing time since most models
- never have their velocity set to anything but zero. Some
- subclasses (e.g. ModelPosition) change this default as they are
- expecting to move. Users can specify a non-zero value here to
- enable velocity control of this model. This achieves the same as
- calling Model::VelocityEnable()
-
- color <string>\n specify the color of the object using a color
name from the X11 database (rgb.txt)
@@ -144,13 +130,6 @@
easy to define objects in a single local coordinate system.
*/
-// todo
-// - 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).
-
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
@@ -310,8 +289,6 @@ Model::Model( World* world,
type(type),
event_queue_num(0),
used(false),
- velocity(),
- velocity_enable(false),
watts(0.0),
watts_give(0.0),
watts_take(0.0),
@@ -739,10 +716,7 @@ void Model::Startup( void )
event_queue_num = thread_safe ? world->GetEventQueue( this ) : 0;
world->Enqueue( event_queue_num, interval, this, UpdateWrapper, NULL );
-
- if( velocity_enable )
- world->EnableVelocity( this );
-
+
if( FindPowerPack() )
world->EnableEnergy( this );
@@ -755,8 +729,6 @@ void Model::Shutdown( void )
CallCallbacks( CB_SHUTDOWN );
world->DisableEnergy( this );
- world->DisableVelocity( this );
- velocity_enable = false;
// allows data visualizations to be cleared.
NeedRedraw();
@@ -896,60 +868,6 @@ void Model::UpdateCharge()
}
-void Model::Move( void )
-{
- if( velocity.IsZero() )
- return;
-
- if( disabled )
- return;
-
- // convert usec to sec
- const double interval( (double)world->sim_interval / 1e6 );
-
- // find the change in velcity due to our acceleration vector
- // velocity.x *= 1.0 + acceleration.x * interval;
- //velocity.y *= 1.0 + acceleration.y * interval;
- //velocity.z *= 1.0 + acceleration.z * interval;
- //velocity.a *= 1.0 + acceleration.a * interval;
-
- // find the change of pose due to our velocity vector
- const Pose p( velocity.x * interval,
- velocity.y * interval,
- velocity.z * interval,
- normalize( velocity.a * interval ));
-
- // the pose we're trying to achieve (unless something stops us)
- const Pose newpose( pose + p );
-
- // stash the original pose so we can put things back if we hit
- const Pose startpose( pose );
-
- pose = newpose; // do the move provisionally - we might undo it below
-
- const unsigned int layer( world->updates%2 );
-
- UnMapWithChildren( layer ); // remove from all blocks
- MapWithChildren( layer ); // render into new blocks
-
- if( TestCollision() ) // crunch!
- {
- // put things back the way they were
- // this is expensive, but it happens _very_ rarely for most people
- pose = startpose;
- UnMapWithChildren( layer );
- MapWithChildren( layer );
-
- SetStall(true);
- }
- else
- {
- world->dirty = true; // need redraw
- SetStall(false);
- }
-}
-
-
void Model::UpdateTrail()
{
// get the current item and increment the counter
View
9 libstage/model_actuator.cc
@@ -76,11 +76,11 @@ ModelActuator::ModelActuator( World* world,
this->SetWatts( WATTS_BASE );
// sensible position defaults
- this->SetVelocity( Velocity(0,0,0,0) );
+ //this->SetVelocity( Velocity(0,0,0,0) );
this->SetBlobReturn(true);
// Allow the models to move
- VelocityEnable();
+ //VelocityEnable();
}
@@ -282,7 +282,8 @@ void ModelActuator::Update( void )
PRINT_ERR1( "unrecognized actuator type %d", actuator_type );
}
- this->SetVelocity( outvel );
+ // TODO - deal with velocity
+ //this->SetVelocity( outvel );
//this->GPoseDirtyTree();
PRINT_DEBUG4( " Set Velocity: [ %.4f %.4f %.4f %.4f ]\n",
outvel.x, outvel.y, outvel.z, outvel.a );
@@ -305,7 +306,7 @@ void ModelActuator::Shutdown( void )
// safety features!
goal = 0;
- velocity.Zero();
+ // velocity.Zero();
Model::Shutdown();
}
View
63 libstage/model_getset.cc
@@ -137,31 +137,31 @@ int Model::SetParent( Model* newparent)
return 0; //ok
}
-// get the model's velocity in the global frame
-Velocity Model::GetGlobalVelocity() const
-{
- const Pose gpose(GetGlobalPose());
- const double cosa(cos(gpose.a));
- const double sina(sin(gpose.a));
+// // get the model's velocity in the global frame
+// Velocity Model::GetGlobalVelocity() const
+// {
+// const Pose gpose(GetGlobalPose());
+// const double cosa(cos(gpose.a));
+// const double sina(sin(gpose.a));
- return Velocity( velocity.x * cosa - velocity.y * sina,
- velocity.x * sina + velocity.y * cosa,
- velocity.z,
- velocity.a );
-}
-
-// set the model's velocity in the global frame
-void Model::SetGlobalVelocity( const Velocity& gv )
-{
- const Pose gpose = GetGlobalPose();
- const double cosa(cos(gpose.a));
- const double sina(sin(gpose.a));
+// return Velocity( velocity.x * cosa - velocity.y * sina,
+// velocity.x * sina + velocity.y * cosa,
+// velocity.z,
+// velocity.a );
+// }
+
+// // set the model's velocity in the global frame
+// void Model::SetGlobalVelocity( const Velocity& gv )
+// {
+// const Pose gpose = GetGlobalPose();
+// const double cosa(cos(gpose.a));
+// const double sina(sin(gpose.a));
- this->SetVelocity( Velocity( gv.x * cosa + gv.y * sina,
- -gv.x * sina + gv.y * cosa,
- gv.z,
- gv.a ));
-}
+// this->SetVelocity( Velocity( gv.x * cosa + gv.y * sina,
+// -gv.x * sina + gv.y * cosa,
+// gv.z,
+// gv.a ));
+// }
// get the model's position in the global frame
Pose Model::GetGlobalPose() const
@@ -179,23 +179,6 @@ Pose Model::GetGlobalPose() const
return global_pose;
}
-void Model::VelocityEnable()
-{
- velocity_enable = true;
- world->EnableVelocity(this);
-}
-
-void Model::VelocityDisable()
-{
- velocity_enable = false;
- world->DisableVelocity( this );
-}
-
-void Model::SetVelocity( const Velocity& val )
-{
- velocity = val;
- CallCallbacks( CB_VELOCITY );
-}
// set the model's pose in the local frame
void Model::SetPose( const Pose& newpose )
View
4 libstage/model_load.cc
@@ -98,8 +98,6 @@ void Model::Load()
if( wf->PropertyExists( wf_entity, "pose" ))
SetPose( GetPose().Load( wf, wf_entity, "pose" ) );
- if( wf->PropertyExists( wf_entity, "velocity" ))
- SetVelocity( GetVelocity().Load( wf, wf_entity, "velocity" ));
if( wf->PropertyExists( wf_entity, "color" ))
{
@@ -178,8 +176,6 @@ void Model::Load()
if( res != this->map_resolution )
SetMapResolution( res );
- velocity_enable = wf->ReadInt( wf_entity, "enable_velocity", velocity_enable );
-
if( wf->PropertyExists( wf_entity, "friction" ))
{
this->SetFriction( wf->ReadFloat(wf_entity, "friction", this->friction ));
View
87 libstage/model_position.cc
@@ -39,6 +39,9 @@ using namespace Stg;
(
# position properties
drive "diff"
+
+ velocity [ 0.0 0.0 0.0 0.0 ]
+
localization "gps"
localization_origin [ <defaults to model's start pose> ]
@@ -55,8 +58,6 @@ using namespace Stg;
# model properties
- # update position according to the current velocity state
- velocity_enable 1
)
@endverbatim
@@ -65,18 +66,22 @@ using namespace Stg;
@par Details
- - drive "diff", "omni" or "car"\n
+ - acceleration_bounds [ xmin xmax ymin ymax zmin zmax amin amax ] x,y,z in meters per second squared, a in degrees per second squared
+- drive "diff", "omni" or "car"\n
select differential-steer model(like a Pioneer), omnidirectional mode or carlike (velocity and steering angle).
+ omega:<float> ]\n Specify the initial velocity of the model. Note
+ that if the model hits an obstacle, its velocity will be set to
+ zero.
- localization "gps" or "odom"\n
if "gps" the position model reports its position with perfect accuracy. If "odom", a simple odometry model is used and position data drifts from the ground truth over time. The odometry model is parameterized by the odom_error property.
- localization_origin [x y z theta]
- set the origin of the localization coordinate system. By default, this is copied from the model's initial pose, so the robot reports its position relative to the place it started out. Tip: If localization_origin is set to [0 0 0 0] and localization is "gps", the model will return its true global position. This is unrealistic, but useful if you want to abstract away the details of localization. Be prepared to justify the use of this mode in your research!
- odom_error [x y z theta]
- parameters for the odometry error model used when specifying localization "odom". Each value is the maximum proportion of error in intergrating x, y, and theta velocities to compute odometric position estimate. For each axis, if the the value specified here is E, the actual proportion is chosen at startup at random in the range -E/2 to +E/2. Note that due to rounding errors, setting these values to zero does NOT give you perfect localization - for that you need to choose localization "gps".
- - wheelbase <float,meters>\nThe wheelbase used for the car steering model. Only used if drive is set to "car". Defaults to 1.0m\n
- - velocity_bounds [ xmin xmax ymin ymax zmin zmax amin amax ] x,y,z in meters per second, a in degrees per second - acceleration_bounds [ xmin xmax ymin ymax zmin zmax amin amax ] x,y,z in meters per second squared, a in degrees per second squared
-
-*/
+ - velocity [ x:<float> y:<float> z:<float> heading:<float>
+ - velocity_bounds [ xmin xmax ymin ymax zmin zmax amin amax ] x,y,z in meters per second, a in degrees per second
+ - wheelbase <float,meters>
+ The wheelbase used for the car steering model. Only used if drive is set to "car". Defaults to 1.0m\*/
@@ -95,6 +100,7 @@ ModelPosition::ModelPosition( World* world,
const std::string& type ) :
Model( world, parent, type ),
// private
+ velocity(),
goal(0,0,0,0),
control_mode( CONTROL_VELOCITY ),
drive_mode( DRIVE_DIFFERENTIAL ),
@@ -116,9 +122,6 @@ ModelPosition::ModelPosition( World* world,
// assert that Update() is reentrant for this derived model
thread_safe = false;
-
- // position devices respond to velocity settings by default
- velocity_enable = true;
// install sensible velocity and acceleration bounds
for( int i=0; i<3; i++ )
@@ -149,10 +152,20 @@ ModelPosition::~ModelPosition( void )
// nothing to do
}
+void ModelPosition::SetVelocity( const Velocity& val )
+{
+ velocity = val;
+ CallCallbacks( CB_VELOCITY );
+}
+
+
void ModelPosition::Load( void )
{
Model::Load();
+ if( wf->PropertyExists( wf_entity, "velocity" ))
+ SetVelocity( GetVelocity().Load( wf, wf_entity, "velocity" ));
+
// load steering mode
if( wf->PropertyExists( wf_entity, "drive" ) )
{
@@ -514,13 +527,63 @@ void ModelPosition::Update( void )
}
PRINT_DEBUG3( " READING POSITION: [ %.4f %.4f %.4f ]\n",
- est_pose.x, est_pose.y, est_pose.a );
+ est_pose.x, est_pose.y, est_pose.a );
Model::Update();
}
+void ModelPosition::Move( void )
+{
+ if( velocity.IsZero() )
+ return;
+
+ if( disabled )
+ return;
+
+ // convert usec to sec
+ const double interval( (double)world->sim_interval / 1e6 );
+
+ // find the change of pose due to our velocity vector
+ const Pose p( velocity.x * interval,
+ velocity.y * interval,
+ velocity.z * interval,
+ normalize( velocity.a * interval ));
+
+ // the pose we're trying to achieve (unless something stops us)
+ const Pose newpose( pose + p );
+
+ // stash the original pose so we can put things back if we hit
+ const Pose startpose( pose );
+
+ pose = newpose; // do the move provisionally - we might undo it below
+
+ const unsigned int layer( world->UpdateCount()%2 );
+
+ UnMapWithChildren( layer ); // remove from all blocks
+ MapWithChildren( layer ); // render into new blocks
+
+ if( TestCollision() ) // crunch!
+ {
+ // put things back the way they were
+ // this is expensive, but it happens _very_ rarely for most people
+ pose = startpose;
+ UnMapWithChildren( layer );
+ MapWithChildren( layer );
+
+ SetStall(true);
+ }
+ else
+ {
+ // world->dirty = true; // need redraw
+ SetStall(false);
+ }
+}
+
+
void ModelPosition::Startup( void )
{
+ world->active_velocity.insert( this );
+
Model::Startup();
PRINT_DEBUG( "position startup" );
@@ -534,6 +597,8 @@ void ModelPosition::Shutdown( void )
goal.Zero();
velocity.Zero();
+ world->active_velocity.erase( this );
+
Model::Shutdown();
}
View
4 libstage/model_ranger.cc
@@ -262,7 +262,7 @@ void ModelRanger::Sensor::Visualize( ModelRanger::Vis* vis, ModelRanger* rgr ) c
Color c( col );
c.a = 0.15; // transparent version of sensor color
rgr->PushColor( c );
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+ glPolygonMode( GL_FRONT, GL_FILL );
if( ranges.size() ) // if we have some data
{
@@ -282,7 +282,7 @@ void ModelRanger::Sensor::Visualize( ModelRanger::Vis* vis, ModelRanger* rgr ) c
}
else
{
- for( size_t s(0); s<sample_count; s++ )
+ for( size_t s(0); s<sample_count+1; s++ )
{
double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0;
pts[2*s+2] = (float)(ranges[s] * cos(ray_angle) );
View
38 libstage/stage.hh
@@ -806,6 +806,8 @@ namespace Stg
CtrlArgs( std::string w, std::string c ) : worldfile(w), cmdline(c) {}
};
+ class ModelPosition;
+
/// %World class
class World : public Ancestor
{
@@ -912,6 +914,8 @@ namespace Stg
void CallUpdateCallbacks(); ///< Call all calbacks in cb_list, removing any that return true;
public:
+
+ uint64_t UpdateCount(){ return updates; }
bool paused; ///< if true, the simulation is stopped
@@ -1076,9 +1080,7 @@ namespace Stg
void DisableEnergy( Model* m ) { active_energy.erase( m ); };
/** Set of models that require their positions to be recalculated at each World::Update(). */
- std::set<Model*> active_velocity;
- void EnableVelocity( Model* m ) { active_velocity.insert( m ); };
- void DisableVelocity( Model* m ) { active_velocity.erase( m ); };
+ std::set<ModelPosition*> active_velocity;
/** The amount of simulated time to run for each call to Update() */
usec_t sim_interval;
@@ -1992,13 +1994,7 @@ namespace Stg
-1, to indicate that it is not on a list yet. */
unsigned int event_queue_num;
bool used; ///< TRUE iff this model has been returned by GetUnusedModelOfType()
- Velocity velocity;
- /** respond to velocity state by changing position. Initially
- false, set to true by subclass, worldfile, or explcicit call
- to Model::VelocityEnable(). */
- bool velocity_enable;
-
watts_t watts;///< power consumed by this model
/** If >0, this model can transfer energy to models that have
@@ -2144,11 +2140,9 @@ namespace Stg
virtual void Startup();
virtual void Shutdown();
virtual void Update();
- virtual void Move();
virtual void UpdateCharge();
static int UpdateWrapper( Model* mod, void* arg ){ mod->Update(); return 0; }
- static int MoveWrapper( Model* mod, void* arg ){ mod->Move(); return 0; }
/** Calls CallCallback( CB_UPDATE ) */
void CallUpdateCallbacks( void );
@@ -2217,7 +2211,7 @@ namespace Stg
/** Alternate constructor that creates dummy models with only a pose */
Model()
: mapped(false), alwayson(false), blocks_dl(0),
- boundary(false), data_fresh(false), disabled(true), friction(0), has_default_block(false), log_state(false), map_resolution(0), mass(0), parent(NULL), rebuild_displaylist(false), stack_children(true), stall(false), subs(0), thread_safe(false),trail_index(0), event_queue_num(0), used(false), velocity_enable(false), watts(0), watts_give(0),watts_take(0),wf(NULL), wf_entity(0), world(NULL)
+ boundary(false), data_fresh(false), disabled(true), friction(0), has_default_block(false), log_state(false), map_resolution(0), mass(0), parent(NULL), rebuild_displaylist(false), stack_children(true), stall(false), subs(0), thread_safe(false),trail_index(0), event_queue_num(0), used(false), watts(0), watts_give(0),watts_take(0),wf(NULL), wf_entity(0), world(NULL)
{}
void Say( const std::string& str );
@@ -2329,14 +2323,11 @@ namespace Stg
/** set the pose of model in global coordinates */
void SetGlobalPose( const Pose& gpose );
- /** set a model's velocity in its parent's coordinate system */
- void SetVelocity( const Velocity& vel );
-
/** Enable update of model pose according to velocity state */
- void VelocityEnable();
+ // void VelocityEnable();
/** Disable update of model pose according to velocity state */
- void VelocityDisable();
+ //void VelocityDisable();
/** set a model's pose in its parent's coordinate system */
void SetPose( const Pose& pose );
@@ -2383,9 +2374,6 @@ namespace Stg
system. */
Pose GetPose() const { return pose; }
- /** Get (a copy of) the model's velocity in its local reference
- frame. */
- Velocity GetVelocity() const { return velocity; }
// guess what these do?
void SetColor( Color col );
@@ -2954,6 +2942,7 @@ namespace Stg
} DriveMode;
private:
+ Velocity velocity;
Pose goal;///< the current velocity or pose to reach, depending on the value of control_mode
ControlMode control_mode;
DriveMode drive_mode;
@@ -2967,6 +2956,7 @@ namespace Stg
/** Set the min and max velocity in all 4 DOF */
Bounds velocity_bounds[4];
+
public:
// constructor
ModelPosition( World* world,
@@ -2975,11 +2965,17 @@ namespace Stg
// destructor
~ModelPosition();
+ virtual void Move();
virtual void Startup();
virtual void Shutdown();
virtual void Update();
virtual void Load();
-
+
+ /** Get (a copy of) the model's velocity in its local reference
+ frame. */
+ Velocity GetVelocity() const { return velocity; }
+ void SetVelocity( const Velocity& val );
+
/** Specify a point in space. Arrays of Waypoints can be attached to
Models and visualized. */
class Waypoint
View
2  libstage/world.cc
@@ -636,7 +636,7 @@ bool World::Update()
pthread_cond_broadcast( &threads_start_cond );
pthread_mutex_unlock( &sync_mutex );
- // update the position of all models based on their velocity
+ // update the position of all position models based on their velocity
FOR_EACH( it, active_velocity )
(*it)->Move();
View
5 todo.txt
@@ -1,4 +1,9 @@
+** BUGS **
+
+ visualization of ranger data (see eg. hospital.world)
+
+
** Optimization **
Timing benchmarks 3600 seconds of virtual time in real time seconds:
View
4 worlds/fasr.world
@@ -11,8 +11,8 @@ speedup -1
paused 0
# time at which to pause (in GUI mode) or quit (in headless mode) the simulation
-#quit_time 3600 # 1 hour of simulated time
-quit_time 1800 # 30 mins of simulated time
+quit_time 3600 # 1 hour of simulated time
+#quit_time 1800 # 30 mins of simulated time
resolution 0.02
Please sign in to comment.
Something went wrong with that request. Please try again.