Permalink
Browse files

Merge pull request #9 from chmp/master

New method World::Run, reimplemented stest
  • Loading branch information...
2 parents bc131bf + 2bb8584 commit 7de05ad18838c055cb0c6af731b8da3d25da9f84 @rtv committed Dec 2, 2011
Showing with 195 additions and 139 deletions.
  1. +161 −134 examples/libstage/stest.cc
  2. +2 −5 libstage/main.cc
  3. +8 −0 libstage/stage.hh
  4. +24 −0 libstage/world.cc
View
@@ -11,151 +11,178 @@
#include <stdlib.h>
#include <string.h>
-#include "stage.hh"
-using namespace Stg;
-
-#include "config.h"
+#include <sstream>
+#include <iostream>
+#include <cmath>
-double speed = 0.400;
-double turnrate = M_PI/3.0;
+#include "stage.hh"
-typedef struct
+class Robot
{
- ModelLaser* laser;
- ModelPosition* position;
- ModelRanger* ranger;
- ModelFiducial* fiducial;
- ModelBlobfinder* blobfinder;
-} robot_t;
-
-#define VSPEED 0.4 // meters per second
-#define WGAIN 1.0 // turn speed gain
-#define SAFE_DIST 0.1 // meters
-#define SAFE_ANGLE 0.3 // radians
+public:
+ Stg::ModelPosition* position;
+ Stg::ModelRanger* ranger;
+};
-int main( int argc, char* argv[] )
-{
- printf( "%s %s benchmarker\n", PACKAGE, VERSION );
-
- if( argc < 3 )
+class Logic
+{
+public:
+ static int Callback(Stg::World* world, void* userarg)
{
- puts( "Usage: stest <worldfile> <number of robots>" );
- exit(0);
+ Logic* lg = reinterpret_cast<Logic*>(userarg);
+
+ lg->Tick(world);
+
+ // never remove this call-back
+ return 0;
}
-
- const int POPSIZE = atoi(argv[2] );
-
- // initialize libstage
- Stg::Init( &argc, &argv );
- //StgWorld world;
- StgWorldGui world(800, 700, "Stage Benchmark Program");
-
- world.Load( argv[1] );
-
- char namebuf[256];
- robot_t* robots = new robot_t[POPSIZE];
-
- for( int i=0; i<POPSIZE; i++ )
+
+ Logic(unsigned int popsize)
+ :
+ population_size(popsize),
+ robots(new Robot[population_size])
{
- char* base = "r";
- sprintf( namebuf, "%s%d", base, i );
- robots[i].position = (ModelPosition*)world.GetModel( namebuf );
- assert(robots[i].position);
- robots[i].position->Subscribe();
-
-// robots[i].laser = (ModelLaser*)
-// robots[i].position->GetUnsubscribedModelOfType( "laser" );
-// assert(robots[i].laser);
-// robots[i].laser->Subscribe();
-
-// robots[i].fiducial = (ModelFiducial*)
-// robots[i].position->GetUnsubscribedModelOfType( "fiducial" );
-// assert(robots[i].fiducial);
-// robots[i].fiducial->Subscribe();
-
- robots[i].ranger = (ModelRanger*)
- robots[i].position->GetUnsubscribedModelOfType( "ranger" );
- assert(robots[i].ranger);
- robots[i].ranger->Subscribe();
-// robots[i].blobfinder = (ModelBlobfinder*)
-// robots[i].position->GetUnsubscribedModelOfType( "blobfinder" );
-// assert(robots[i].blobfinder);
-// robots[i].blobfinder->Subscribe();
}
-
- // start the clock
- //world.Start();
- //puts( "done" );
-
- while( ! world.TestQuit() )
- if( world.RealTimeUpdate() )
- // if( world.Update() )
- for( int i=0; i<POPSIZE; i++ )
- {
-
- //continue;
-
- ModelRanger* rgr = robots[i].ranger;
-
- if( rgr->samples == NULL )
- continue;
-
- // compute the vector sum of the sonar ranges
- double dx=0, dy=0;
-
- for( unsigned int s=0; s< rgr->sensor_count; s++ )
- {
- double srange = rgr->samples[s];
-
- dx += srange * cos( rgr->sensors[s].pose.a );
- dy += srange * sin( rgr->sensors[s].pose.a );
- }
-
- if( dy == 0 )
- continue;
-
- if( dx == 0 )
- continue;
-
- assert( dy != 0 );
- assert( dx != 0 );
-
- double resultant_angle = atan2( dy, dx );
- double forward_speed = 0.0;
- double side_speed = 0.0;
- double turn_speed = WGAIN * resultant_angle;
-
- int forward = rgr->sensor_count/2 -1 ;
- // if the front is clear, drive forwards
- if( (rgr->samples[forward-1] > SAFE_DIST/5.0) &&
- (rgr->samples[forward ] > SAFE_DIST) &&
- (rgr->samples[forward+1] > SAFE_DIST/5.0) &&
- (fabs( resultant_angle ) < SAFE_ANGLE) )
- {
- forward_speed = VSPEED;
- }
-
-// // send a command to the robot
-// velocity_t vel;
-// bzero(&vel,sizeof(vel));
-// vel.x = forward_speed;
-// vel.y = side_speed;
-// vel.z = 0;
-// vel.a = turn_speed;
-
- //printf( "robot %s [%.2f %.2f %.2f %.2f]\n",
- //robots[i].position->Token(), vel.x, vel.y, vel.z, vel.a );
-
- // uint32_t bcount=0;
- //blobfinder_blob_t* blobs = robots[i].blobfinder->GetBlobs( &bcount );
+
+ void connect(Stg::World* world)
+ {
+ // connect the first population_size robots to this controller
+ for(unsigned int idx = 0; idx < population_size; idx++)
+ {
+ // the robots' models are named r0 .. r1999
+ std::stringstream name;
+ name << "r" << idx;
+
+ // get the robot's model and subscribe to it
+ Stg::ModelPosition* posmod = reinterpret_cast<Stg::ModelPosition*>(
+ world->GetModel(name.str())
+ );
+ assert(posmod != 0);
+
+ robots[idx].position = posmod;
+ robots[idx].position->Subscribe();
+
+ // get the robot's ranger model and subscribe to it
+ Stg::ModelRanger* rngmod = reinterpret_cast<Stg::ModelRanger*>(
+ robots[idx].position->GetChild( "ranger:0" )
+ );
+ assert(rngmod != 0);
+
+ robots[idx].ranger = rngmod;
+ robots[idx].ranger->Subscribe();
+ }
+
+ // register with the world
+ world->AddUpdateCallback(Logic::Callback, reinterpret_cast<void*>(this));
+ }
+
+ ~Logic()
+ {
+ delete[] robots;
+ }
+
- //printf( "robot %s sees %u blobs\n", robots[i].blobfinder->Token(), bcount );
+ void Tick(Stg::World*)
+ {
+ // the controllers parameters
+ const double vspeed = 0.4; // meters per second
+ const double wgain = 1.0; // turn speed gain
+ const double safe_dist = 0.1; // meters
+ const double safe_angle = 0.3; // radians
+
+ // each robot has a group of ir sensors
+ // each sensor takes one sample
+ // the forward sensor is the middle sensor
+ for(unsigned int idx = 0; idx < population_size; idx++)
+ {
+ Stg::ModelRanger* rgr = robots[idx].ranger;
+
+ // compute the vector sum of the sonar ranges
+ double dx=0, dy=0;
+
+ // the range model has multiple sensors
+ typedef std::vector<Stg::ModelRanger::Sensor>::const_iterator sensor_iterator;
+ const std::vector<Stg::ModelRanger::Sensor> sensors = rgr->GetSensors();
+
+ for(sensor_iterator sensor = sensors.begin(); sensor != sensors.end(); sensor++)
+ {
+ // each sensor takes a single sample (as specified in the .world)
+ const double srange = (*sensor).ranges[0];
+ const double angle = (*sensor).pose.a;
+
+ dx += srange * std::cos(angle);
+ dy += srange * std::sin(angle);
+ }
+
+ if(dx == 0)
+ continue;
+
+ if(dy == 0)
+ continue;
+
+ // calculate the angle towards the farthest obstacle
+ const double resultant_angle = std::atan2( dy, dx );
+
+ // check whether the front is clear
+ const unsigned int forward_idx = sensors.size() / 2u - 1u;
+
+ const double forwardm_range = sensors[forward_idx - 1].ranges[0];
+ const double forward_range = sensors[forward_idx + 0].ranges[0];
+ const double forwardp_range = sensors[forward_idx + 1].ranges[0];
+
+ bool front_clear = (
+ (forwardm_range > safe_dist / 5.0) &&
+ (forward_range > safe_dist) &&
+ (forwardp_range > safe_dist / 5.0) &&
+ (std::abs(resultant_angle) < safe_angle)
+ );
+
+ // turn the sensor input into movement commands
+
+ // move forwards if the front is clear
+ const double forward_speed = front_clear ? vspeed : 0.0;
+ // do not strafe
+ const double side_speed = 0.0;
+
+ // turn towards the farthest obstacle
+ const double turn_speed = wgain * resultant_angle;
+
+ // finally, relay the commands to the robot
+ robots[idx].position->SetSpeed( forward_speed, side_speed, turn_speed );
+ }
+ }
+
+protected:
+ unsigned int population_size;
+ Robot* robots;
+};
- robots[i].position->SetSpeed( forward_speed, side_speed, turn_speed );
+int main( int argc, char* argv[] )
+{
+ // check and handle the argumets
+ if( argc < 3 )
+ {
+ puts( "Usage: stest <worldfile> <number of robots>" );
+ exit(0);
}
-
+
+ const int popsize = atoi(argv[2] );
- delete[] robots;
- exit( 0 );
+ // initialize libstage
+ Stg::Init( &argc, &argv );
+
+ // create the world
+ //Stg::World world;
+ Stg::WorldGui world(800, 700, "Stage Benchmark Program");
+ world.Load( argv[1] );
+
+ // create the logic and connect it to the world
+ Logic logic(popsize);
+ logic.connect(&world);
+
+ // and then run the simulation
+ world.Run();
+
+ return 0;
}
View
@@ -96,11 +96,8 @@ int main( int argc, char* argv[] )
optindex++;
}
- if( usegui )
- Fl::run();
- else
- while( ! World::UpdateAll() );
-
+ World::Run();
+
puts( "\n[Stage: done]" );
return EXIT_SUCCESS;
View
@@ -1100,6 +1100,14 @@ namespace Stg
/** returns true when time to quit, false otherwise */
static bool UpdateAll();
+ /** run all worlds.
+ * If only non-gui worlds were created, UpdateAll() is
+ * repeatedly called.
+ * To simulate a gui world only a single gui world may
+ * have been created. This world is then simulated.
+ */
+ static void Run();
+
World( const std::string& name = "MyWorld",
double ppm = DEFAULT_PPM );
View
@@ -204,6 +204,30 @@ void World::DestroySuperRegion( SuperRegion* sr )
delete sr;
}
+void World::Run()
+{
+ // first check wheter there is a single gui world
+ bool found_gui = false;
+ FOR_EACH( world_it, world_set )
+ {
+ found_gui |= (*world_it)->IsGUI();
+ }
+ if(found_gui && (world_set.size() != 1))
+ {
+ PRINT_WARN( "When using the GUI only a single world can be simulated." );
+ exit(-1);
+ }
+
+ if(found_gui)
+ {
+ Fl::run();
+ }
+ else
+ {
+ while(!UpdateAll());
+ }
+}
+
bool World::UpdateAll()
{
bool quit( true );

0 comments on commit 7de05ad

Please sign in to comment.