Revision: 8587
http://playerstage.svn.sourceforge.net/playerstage/?rev=8587&view=rev
Author: rtv
Date: 2010-03-12 08:06:06 +0000 (Fri, 12 Mar 2010)
Log Message:
-----------
cleaning up API - may break some clients
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/CMakeLists.txt
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/examples/ctrl/fasr2.cc
code/stage/trunk/examples/ctrl/lasernoise.cc
code/stage/trunk/examples/ctrl/pioneer_flocking.cc
code/stage/trunk/examples/ctrl/wander.cc
code/stage/trunk/examples/ctrl/wander_pioneer.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/model_position.cc
code/stage/trunk/libstage/model_ranger.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/webstage/webstage.cc
code/stage/trunk/worlds/benchmark/expand_pioneer.cc
code/stage/trunk/worlds/benchmark/expand_swarm.cc
Modified: code/stage/trunk/examples/ctrl/CMakeLists.txt
===================================================================
--- code/stage/trunk/examples/ctrl/CMakeLists.txt 2010-03-12 04:55:07 UTC
(rev 8586)
+++ code/stage/trunk/examples/ctrl/CMakeLists.txt 2010-03-12 08:06:06 UTC
(rev 8587)
@@ -1,7 +1,6 @@
SET( PLUGINS
fasr
- lasernoise
sink
source
wander
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2010-03-12 04:55:07 UTC (rev
8586)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2010-03-12 08:06:06 UTC (rev
8587)
@@ -212,9 +212,9 @@
double minright = 1e6;
// Get the data
- uint32_t sample_count=0;
- ModelLaser::Sample* scan = laser->GetSamples( &sample_count );
-
+ const std::vector<ModelLaser::Sample>& scan = laser->GetSamples();
+ uint32_t sample_count = scan.size();
+
for (uint32_t i = 0; i < sample_count; i++)
{
if( verbose ) printf( "%.3f ", scan[i].range );
@@ -339,10 +339,9 @@
// if( laser->power_pack && laser->power_pack->charging )
// printf( "model %s power pack @%p is charging\n",
// laser->Token(), laser->power_pack );
-
- if( laser->GetSamples(NULL) == NULL )
- return 0;
-
+
+ assert( laser->GetSamples().size() > 0 );
+
switch( robot->mode )
{
case MODE_DOCK:
Modified: code/stage/trunk/examples/ctrl/fasr2.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr2.cc 2010-03-12 04:55:07 UTC (rev
8586)
+++ code/stage/trunk/examples/ctrl/fasr2.cc 2010-03-12 08:06:06 UTC (rev
8587)
@@ -509,8 +509,10 @@
pos->Say( "" );
// stay put while anything is close behind
+ const std::vector<ModelRanger::Sensor>& sensors =
ranger->GetSensors();
+
for( unsigned int s = BACK_SENSOR_FIRST; s <=
BACK_SENSOR_LAST; ++s )
- if( ranger->sensors[s].range < wait_distance)
+ if( sensors[s].range < wait_distance)
{
pos->Say( "Waiting..." );
pos->SetXSpeed( 0.0 );
@@ -551,9 +553,10 @@
double minright = 1e6;
// Get the data
- uint32_t sample_count=0;
- ModelLaser::Sample* scan = laser->GetSamples( &sample_count );
-
+ //ModelLaser::Sample* scan = laser->GetSamples( &sample_count );
+ const std::vector<ModelLaser::Sample>& scan = laser->GetSamples();
+ uint32_t sample_count = scan.size();
+
for (uint32_t i = 0; i < sample_count; i++)
{
if( verbose ) printf( "%.3f ", scan[i].range );
@@ -715,9 +718,11 @@
// printf( "model %s power pack @%p is charging\n",
// laser->Token(), laser->power_pack );
- if( laser->GetSamples(NULL) == NULL )
- return 0;
+ //if( laser->GetSamples(NULL) == NULL )
+ //return 0;
+ assert( laser->GetSamples().size() > 0 );
+
switch( robot->mode )
{
case MODE_DOCK:
Modified: code/stage/trunk/examples/ctrl/lasernoise.cc
===================================================================
--- code/stage/trunk/examples/ctrl/lasernoise.cc 2010-03-12 04:55:07 UTC
(rev 8586)
+++ code/stage/trunk/examples/ctrl/lasernoise.cc 2010-03-12 08:06:06 UTC
(rev 8587)
@@ -25,11 +25,12 @@
{
// get the data
uint32_t sample_count=0;
- ModelLaser::Sample* scan = mod->GetSamples( &sample_count );
- if( scan )
- for( unsigned int i=0; i<sample_count; i++ )
- scan[i].range *= simple_normal_deviate( 1.0, DEVIATION );
+ const std::vector<ModelLaser::Sample>& scan = mod->GetSamples();
+
+ if( scan.size()>0 )
+ FOR_EACH( it, scan )
+ it->range *= simple_normal_deviate( 1.0, DEVIATION );
return 0; // run again
}
Modified: code/stage/trunk/examples/ctrl/pioneer_flocking.cc
===================================================================
--- code/stage/trunk/examples/ctrl/pioneer_flocking.cc 2010-03-12 04:55:07 UTC
(rev 8586)
+++ code/stage/trunk/examples/ctrl/pioneer_flocking.cc 2010-03-12 08:06:06 UTC
(rev 8587)
@@ -68,12 +68,13 @@
// compute the vector sum of the sonar ranges
double dx=0, dy=0;
+ const std::vector<ModelRanger::Sensor>& sensors = rgr->GetSensors();
+
// use the front-facing sensors only
for( unsigned int i=0; i < 8; i++ )
{
- ModelRanger::Sensor& s = rgr->sensors[i];
- dx += s.range * cos( s.pose.a );
- dy += s.range * sin( s.pose.a );
+ dx += sensors[i].range * cos( sensors[i].pose.a );
+ dy += sensors[i].range * sin( sensors[i].pose.a );
}
if( (dx == 0) || (dy == 0) )
@@ -85,12 +86,12 @@
double turn_speed = EXPAND_WGAIN * resultant_angle;
// if the front is clear, drive forwards
- if( (rgr->sensors[3].range > SAFE_DIST) && // forwards
- (rgr->sensors[4].range > SAFE_DIST) &&
- (rgr->sensors[5].range > SAFE_DIST ) && //
- (rgr->sensors[6].range > SAFE_DIST/2.0) &&
- (rgr->sensors[2].range > SAFE_DIST ) &&
- (rgr->sensors[1].range > SAFE_DIST/2.0) &&
+ if( (sensors[3].range > SAFE_DIST) && // forwards
+ (sensors[4].range > SAFE_DIST) &&
+ (sensors[5].range > SAFE_DIST ) && //
+ (sensors[6].range > SAFE_DIST/2.0) &&
+ (sensors[2].range > SAFE_DIST ) &&
+ (sensors[1].range > SAFE_DIST/2.0) &&
(fabs( resultant_angle ) < SAFE_ANGLE) )
{
forward_speed = VSPEED;
Modified: code/stage/trunk/examples/ctrl/wander.cc
===================================================================
--- code/stage/trunk/examples/ctrl/wander.cc 2010-03-12 04:55:07 UTC (rev
8586)
+++ code/stage/trunk/examples/ctrl/wander.cc 2010-03-12 08:06:06 UTC (rev
8587)
@@ -50,14 +50,14 @@
int LaserUpdate( Model* mod, robot_t* robot )
{
// get the data
- uint32_t sample_count=0;
- ModelLaser::Sample* scan = robot->laser->GetSamples( &sample_count );
- if( ! scan )
+ const std::vector<ModelLaser::Sample>& scan =
robot->laser->GetSamples();
+ uint32_t sample_count = scan.size();
+ if( ! sample_count < 1 )
return 0;
bool obstruction = false;
bool stop = false;
-
+
// find the closest distance to the left and right and check if
// there's anything in front
double minleft = 1e6;
Modified: code/stage/trunk/examples/ctrl/wander_pioneer.cc
===================================================================
--- code/stage/trunk/examples/ctrl/wander_pioneer.cc 2010-03-12 04:55:07 UTC
(rev 8586)
+++ code/stage/trunk/examples/ctrl/wander_pioneer.cc 2010-03-12 08:06:06 UTC
(rev 8587)
@@ -75,12 +75,13 @@
// compute the vector sum of the sonar ranges
double dx=0, dy=0;
+ const std::vector<ModelRanger::Sensor>& sensors = rgr->GetSensors();
+
// use the front-facing sensors only
for( unsigned int i=0; i < 8; i++ )
{
- ModelRanger::Sensor& s = rgr->sensors[i];
- dx += s.range * cos( s.pose.a );
- dy += s.range * sin( s.pose.a );
+ dx += sensors[i].range * cos( sensors[i].pose.a );
+ dy += sensors[i].range * sin( sensors[i].pose.a );
//printf( "sensor %d angle= %.2f\n", s, rgr->sensors[s].pose.a
);
}
@@ -96,12 +97,12 @@
//printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed );
// if the front is clear, drive forwards
- if( (rgr->sensors[3].range > SAFE_DIST) && // forwards
- (rgr->sensors[4].range > SAFE_DIST) &&
- (rgr->sensors[5].range > SAFE_DIST/2.0) && //
- (rgr->sensors[6].range > SAFE_DIST/4.0) &&
- (rgr->sensors[2].range > SAFE_DIST/2.0) &&
- (rgr->sensors[1].range > SAFE_DIST/4.0) &&
+ if( (sensors[3].range > SAFE_DIST) && // forwards
+ (sensors[4].range > SAFE_DIST) &&
+ (sensors[5].range > SAFE_DIST/2.0) && //
+ (sensors[6].range > SAFE_DIST/4.0) &&
+ (sensors[2].range > SAFE_DIST/2.0) &&
+ (sensors[1].range > SAFE_DIST/4.0) &&
(fabs( resultant_angle ) < SAFE_ANGLE) )
{
forward_speed = VSPEED;
@@ -147,29 +148,5 @@
robot->closest_heading_error = robot->closest->geom.a;
}
-// if( (dx == 0) || (dy == 0) )
-// return 0;
-
-// double resultant_angle = atan2( dy, dx );
-// double forward_speed = 0.0;
-// double side_speed = 0.0;
-// double turn_speed = WGAIN * resultant_angle;
-
-// //printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed
);
-
-// // if the front is clear, drive forwards
-// if( (rgr->sensors[3].range > SAFE_DIST) && // forwards
-// (rgr->sensors[4].range > SAFE_DIST) &&
-// (rgr->sensors[5].range > SAFE_DIST/2.0) && //
-// (rgr->sensors[6].range > SAFE_DIST/4.0) &&
-// (rgr->sensors[2].range > SAFE_DIST/2.0) &&
-// (rgr->sensors[1].range > SAFE_DIST/4.0) &&
-// (fabs( resultant_angle ) < SAFE_ANGLE) )
-// {
-// forward_speed = VSPEED;
-// }
-
-// robot->position->SetSpeed( forward_speed, side_speed, turn_speed );
-
return 0;
}
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2010-03-12 04:55:07 UTC (rev 8586)
+++ code/stage/trunk/libstage/model.cc 2010-03-12 08:06:06 UTC (rev 8587)
@@ -184,7 +184,7 @@
}
}
-void Size::Save( Worldfile* wf, int section, const char* keyword )
+void Size::Save( Worldfile* wf, int section, const char* keyword ) const
{
wf->WriteTupleLength( section, keyword, 0, x );
wf->WriteTupleLength( section, keyword, 1, y );
@@ -980,7 +980,7 @@
return NULL;
}
-stg_kg_t Model::GetTotalMass()
+stg_kg_t Model::GetTotalMass() const
{
stg_kg_t sum = mass;
@@ -990,7 +990,7 @@
return sum;
}
-stg_kg_t Model::GetMassOfChildren()
+stg_kg_t Model::GetMassOfChildren() const
{
return( GetTotalMass() - mass);
}
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2010-03-12 04:55:07 UTC (rev
8586)
+++ code/stage/trunk/libstage/model_laser.cc 2010-03-12 08:06:06 UTC (rev
8587)
@@ -125,7 +125,7 @@
SampleConfig();
}
-ModelLaser::Config ModelLaser::GetConfig()
+ModelLaser::Config ModelLaser::GetConfig() const
{
Config cfg;
cfg.sample_count = sample_count;
@@ -239,7 +239,7 @@
Model::Shutdown();
}
-void ModelLaser::Print( char* prefix )
+void ModelLaser::Print( char* prefix ) const
{
Model::Print( prefix );
@@ -254,16 +254,8 @@
puts( " ]" );
}
-
-ModelLaser::Sample* ModelLaser::GetSamples( uint32_t* count )
+const std::vector<ModelLaser::Sample>& ModelLaser::GetSamples() const
{
- // get a C style array from our vector
- if( count ) *count = sample_count;
- return &samples[0];
-}
-
-const std::vector<ModelLaser::Sample>& ModelLaser::GetSamples()
-{
return samples;
}
Modified: code/stage/trunk/libstage/model_position.cc
===================================================================
--- code/stage/trunk/libstage/model_position.cc 2010-03-12 04:55:07 UTC (rev
8586)
+++ code/stage/trunk/libstage/model_position.cc 2010-03-12 08:06:06 UTC (rev
8587)
@@ -88,6 +88,7 @@
Model* parent,
const std::string& type ) :
Model( world, parent, type ),
+ // private
goal(0,0,0,0),
control_mode( CONTROL_VELOCITY ),
drive_mode( DRIVE_DIFFERENTIAL ),
@@ -96,8 +97,9 @@
drand48() *
INTEGRATION_ERROR_MAX_Y - INTEGRATION_ERROR_MAX_Y/2.0,
drand48() *
INTEGRATION_ERROR_MAX_Z - INTEGRATION_ERROR_MAX_Z/2.0,
drand48() *
INTEGRATION_ERROR_MAX_A - INTEGRATION_ERROR_MAX_A/2.0 ),
+ wheelbase( 1.0 ),
+ //public
waypoints(),
- wheelbase( 1.0 ),
wpvis(),
posevis()
{
Modified: code/stage/trunk/libstage/model_ranger.cc
===================================================================
--- code/stage/trunk/libstage/model_ranger.cc 2010-03-12 04:55:07 UTC (rev
8586)
+++ code/stage/trunk/libstage/model_ranger.cc 2010-03-12 08:06:06 UTC (rev
8587)
@@ -276,7 +276,7 @@
Model::Update();
}
-void ModelRanger::Print( char* prefix )
+void ModelRanger::Print( char* prefix ) const
{
Model::Print( prefix );
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2010-03-12 04:55:07 UTC (rev 8586)
+++ code/stage/trunk/libstage/stage.hh 2010-03-12 08:06:06 UTC (rev 8587)
@@ -245,7 +245,7 @@
{/*empty*/}
void Load( Worldfile* wf, int section, const char* keyword );
- void Save( Worldfile* wf, int section, const char* keyword );
+ void Save( Worldfile* wf, int section, const char* keyword ) const;
void Zero()
{ x=y=z=0.0; }
@@ -885,7 +885,7 @@
virtual void Stop(){ paused = true; };
virtual void TogglePause(){ paused ? Start() : Stop(); };
- bool Paused(){ return( paused ); };
+ bool Paused() const { return( paused ); };
/** Force the GUI to redraw the world, even if paused. This
imlementation does nothing, but can be
overridden by
@@ -922,11 +922,9 @@
void LoadBlock( Worldfile* wf, int entity );
void LoadBlockGroup( Worldfile* wf, int entity );
- virtual Model* RecentlySelectedModel(){ return NULL; }
+ virtual Model* RecentlySelectedModel() const { return NULL; }
SuperRegion* AddSuperRegion( const stg_point_int_t& coord );
- //SuperRegion* GetSuperRegion( const stg_point_int_t& coord );
- //SuperRegion* GetSuperRegionCached( const stg_point_int_t& coord);
SuperRegion* GetSuperRegion( int32_t x, int32_t y );
void ExpireSuperRegion( SuperRegion* sr );
@@ -938,10 +936,10 @@
/** convert a distance in meters to a distance in world occupancy
grid pixels */
- int32_t MetersToPixels( stg_meters_t x )
+ int32_t MetersToPixels( stg_meters_t x ) const
{ return (int32_t)floor(x * ppm); };
- stg_point_int_t MetersToPixels( const stg_point_t& pt )
+ stg_point_int_t MetersToPixels( const stg_point_t& pt ) const
{ return stg_point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); };
// dummy implementations to be overloaded by GUI subclasses
@@ -1025,7 +1023,7 @@
/** returns an event queue index number for a model to use for
updates */
- unsigned int GetEventQueue( Model* mod );
+ unsigned int GetEventQueue( Model* mod ) const;
public:
/** returns true when time to quit, false otherwise */
@@ -1035,11 +1033,11 @@
double ppm = DEFAULT_PPM );
virtual ~World();
-
- stg_usec_t SimTimeNow(void);
-
- Worldfile* GetWorldFile(){ return wf; };
-
+
+ stg_usec_t SimTimeNow(void) const { return sim_time; }
+
+ Worldfile* GetWorldFile() { return wf; };
+
virtual bool IsGUI() const { return false; }
virtual void Load( const char* worldfile_path );
@@ -1048,7 +1046,7 @@
virtual bool Save( const char* filename );
virtual bool Update(void);
- bool TestQuit(){ return( quit || quit_all ); }
+ bool TestQuit() const { return( quit || quit_all ); }
void Quit(){ quit = true; }
void QuitAll(){ quit_all = true; }
void CancelQuit(){ quit = false; }
@@ -1058,17 +1056,17 @@
/** Get the resolution in pixels-per-metre of the underlying
discrete raytracing model */
- double Resolution(){ return ppm; };
+ double Resolution() const { return ppm; };
/** Returns a pointer to the model identified by name, or NULL if
nonexistent */
Model* GetModel( const std::string& name ) const;
/** Return the 3D bounding box of the world, in meters */
- const stg_bounds3d_t& GetExtent(){ return extent; };
+ const stg_bounds3d_t& GetExtent() const { return extent; };
/** Return the number of times the world has been updated. */
- uint64_t GetUpdateCount() { return updates; }
+ uint64_t GetUpdateCount() const { return updates; }
/// Register an Option for pickup by the GUI
void RegisterOption( Option* opt );
@@ -2216,10 +2214,10 @@
uint32_t GetId() const { return id; }
/** Get the total mass of a model and it's children recursively */
- stg_kg_t GetTotalMass();
+ stg_kg_t GetTotalMass() const;
/** Get the mass of this model's children recursively. */
- stg_kg_t GetMassOfChildren();
+ stg_kg_t GetMassOfChildren() const;
/** Change a model's parent - experimental*/
int SetParent( Model* newparent);
@@ -2456,7 +2454,7 @@
Bounds range_bounds; ///< min and max ranges
stg_radians_t fov; ///< Field of view, centered about the pose
angle
stg_usec_t interval; ///< Time interval between updates (TODO:
is this used?)
- };
+ };
private:
class Vis : public Visualizer
@@ -2472,12 +2470,12 @@
virtual void Visualize( Model* mod, Camera* cam );
} vis;
- unsigned int sample_count;
- std::vector<Sample> samples;
-
- stg_meters_t range_max;
- stg_radians_t fov;
- uint32_t resolution;
+ unsigned int sample_count;
+ std::vector<Sample> samples;
+
+ stg_meters_t range_max;
+ stg_radians_t fov;
+ uint32_t resolution;
// set up data buffers after the config changes
void SampleConfig();
@@ -2495,16 +2493,13 @@
virtual void Shutdown();
virtual void Update();
virtual void Load();
- virtual void Print( char* prefix );
+ virtual void Print( char* prefix ) const;
- /** returns an array of range & reflectance samples */
- Sample* GetSamples( uint32_t* count );
-
/** returns a const reference to a vector of range and reflectance
samples */
- const std::vector<Sample>& GetSamples();
+ const std::vector<Sample>& GetSamples() const;
/** Get the user-tweakable configuration of the laser */
- Config GetConfig( );
+ Config GetConfig( ) const;
/** Set the user-tweakable configuration of the laser */
void SetConfig( Config& cfg );
@@ -2718,25 +2713,28 @@
};
protected:
-
- virtual void Startup();
- virtual void Shutdown();
- virtual void Update();
- virtual void DataVisualize( Camera* cam );
-
+
+ virtual void Startup();
+ virtual void Shutdown();
+ virtual void Update();
+ virtual void DataVisualize( Camera* cam );
+
public:
- ModelRanger( World* world, Model* parent,
- const std::string& type );
- virtual ~ModelRanger();
-
- virtual void Load();
- virtual void Print( char* prefix );
-
- std::vector<Sensor> sensors;
-
+ ModelRanger( World* world, Model* parent,
+ const
std::string& type );
+ virtual ~ModelRanger();
+
+ virtual void Load();
+ virtual void Print( char* prefix ) const;
+
+ const std::vector<Sensor>& GetSensors() const
+ { return sensors; }
+
private:
- static Option showRangerData;
- static Option showRangerTransducers;
+ std::vector<Sensor> sensors;
+
+ static Option showRangerData;
+ static Option showRangerTransducers;
};
// BLINKENLIGHT MODEL ----------------------------------------------------
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2010-03-12 04:55:07 UTC (rev 8586)
+++ code/stage/trunk/libstage/world.cc 2010-03-12 08:06:06 UTC (rev 8587)
@@ -597,7 +597,7 @@
return false;
}
-unsigned int World::GetEventQueue( Model* mod )
+unsigned int World::GetEventQueue( Model* mod ) const
{
if( worker_threads < 1 )
return 0;
@@ -1053,9 +1053,6 @@
option_table.insert( opt );
}
-stg_usec_t World::SimTimeNow(void)
-{ return sim_time; }
-
void World::Log( Model* mod )
{
LogEntry( sim_time, mod);
Modified: code/stage/trunk/webstage/webstage.cc
===================================================================
--- code/stage/trunk/webstage/webstage.cc 2010-03-12 04:55:07 UTC (rev
8586)
+++ code/stage/trunk/webstage/webstage.cc 2010-03-12 08:06:06 UTC (rev
8587)
@@ -249,25 +249,25 @@
std::vector<double> ranges;
ModelRanger* ranger = (ModelRanger*)mod;
-
- if(ranger){
- for(unsigned int i=0;i<ranger->sensors.size();i++){
- //char str[10];
- //sprintf(str,"size:%d",ranger->sensors.size());
- //puts(str);
- //puts("in the ranger loop");
- websim::Pose pos;
- Pose rpos;
- rpos = ranger->sensors[i].pose;
+
+ const std::vector<ModelRanger::Sensor>& sensors =
ranger->GetSensors();
+
+ FOR_EACH( it, sensors )
+ { //char str[10];
+
//sprintf(str,"size:%d",ranger->sensors.size());
+ //puts(str);
+ //puts("in the ranger loop");
+ websim::Pose pos;
+ Pose rpos;
+ rpos = it->pose;
pos.x = rpos.x;
pos.y = rpos.y;
pos.z = rpos.z;
pos.a = rpos.a;
p.push_back(pos);
- ranges.push_back(ranger->sensors[i].range);
+ ranges.push_back(it->range);
}
- }
WebSim::GetRangerData(name, t, p, ranges, format, response,
xmlparent);
Modified: code/stage/trunk/worlds/benchmark/expand_pioneer.cc
===================================================================
--- code/stage/trunk/worlds/benchmark/expand_pioneer.cc 2010-03-12 04:55:07 UTC
(rev 8586)
+++ code/stage/trunk/worlds/benchmark/expand_pioneer.cc 2010-03-12 08:06:06 UTC
(rev 8587)
@@ -63,9 +63,11 @@
// compute the vector sum of the sonar ranges
double dx=0, dy=0;
- FOR_EACH( it, rgr->sensors )
+ const std::vector<ModelRanger::Sensor>& sensors = rgr->GetSensors();
+
+ FOR_EACH( it, sensors )
{
- ModelRanger::Sensor& s = *it;
+ const ModelRanger::Sensor& s = *it;
dx += s.range * cos( s.pose.a );
dy += s.range * sin( s.pose.a );
@@ -83,12 +85,12 @@
//printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed );
// if the front is clear, drive forwards
- if( (rgr->sensors[3].range > SAFE_DIST) && // forwards
- (rgr->sensors[4].range > SAFE_DIST) &&
- (rgr->sensors[5].range > SAFE_DIST/2.0) && //
- (rgr->sensors[6].range > SAFE_DIST/4.0) &&
- (rgr->sensors[2].range > SAFE_DIST/2.0) &&
- (rgr->sensors[1].range > SAFE_DIST/4.0) &&
+ if( (sensors[3].range > SAFE_DIST) && // forwards
+ (sensors[4].range > SAFE_DIST) &&
+ (sensors[5].range > SAFE_DIST/2.0) && //
+ (sensors[6].range > SAFE_DIST/4.0) &&
+ (sensors[2].range > SAFE_DIST/2.0) &&
+ (sensors[1].range > SAFE_DIST/4.0) &&
(fabs( resultant_angle ) < SAFE_ANGLE) )
{
forward_speed = VSPEED;
Modified: code/stage/trunk/worlds/benchmark/expand_swarm.cc
===================================================================
--- code/stage/trunk/worlds/benchmark/expand_swarm.cc 2010-03-12 04:55:07 UTC
(rev 8586)
+++ code/stage/trunk/worlds/benchmark/expand_swarm.cc 2010-03-12 08:06:06 UTC
(rev 8587)
@@ -60,10 +60,12 @@
{
// compute the vector sum of the sonar ranges
double dx=0, dy=0;
-
- FOR_EACH( it, rgr->sensors )
+
+ const std::vector<ModelRanger::Sensor>& sensors = rgr->GetSensors();
+
+ FOR_EACH( it, sensors )
{
- ModelRanger::Sensor& s = *it;
+ const ModelRanger::Sensor& s = *it;
dx += s.range * cos( s.pose.a );
dy += s.range * sin( s.pose.a );
@@ -81,15 +83,15 @@
//printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed );
// if the front is clear, drive forwards
- if( (rgr->sensors[0].range > SAFE_DIST) &&
+ if( (sensors[0].range > SAFE_DIST) &&
- (rgr->sensors[1].range > SAFE_DIST/1.5) &&
- (rgr->sensors[2].range > SAFE_DIST/3.0) &&
- (rgr->sensors[3].range > SAFE_DIST/5.0) &&
+ (sensors[1].range > SAFE_DIST/1.5) &&
+ (sensors[2].range > SAFE_DIST/3.0) &&
+ (sensors[3].range > SAFE_DIST/5.0) &&
- (rgr->sensors[9].range > SAFE_DIST/5.0) &&
- (rgr->sensors[10].range > SAFE_DIST/3.0) &&
- (rgr->sensors[11].range > SAFE_DIST/1.5) &&
+ (sensors[9].range > SAFE_DIST/5.0) &&
+ (sensors[10].range > SAFE_DIST/3.0) &&
+ (sensors[11].range > SAFE_DIST/1.5) &&
(fabs( resultant_angle ) < SAFE_ANGLE) )
{
forward_speed = VSPEED;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Download Intel® Parallel Studio Eval
Try the new software tools for yourself. Speed compiling, find bugs
proactively, and fine-tune applications for parallel performance.
See why Intel Parallel Studio got high marks during beta.
http://p.sf.net/sfu/intel-sw-dev
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
