Menu
▾
▴
[Playerstage-commit] SF.net SVN: playerstage:[7952] code/stage/trunk/worlds/benchmark
|
From: <rt...@us...> - 2009-07-03 23:56:16
|
Revision: 7952
http://playerstage.svn.sourceforge.net/playerstage/?rev=7952&view=rev
Author: rtv
Date: 2009-07-03 23:56:10 +0000 (Fri, 03 Jul 2009)
Log Message:
-----------
fixed demos
Modified Paths:
--------------
code/stage/trunk/worlds/benchmark/CMakeLists.txt
code/stage/trunk/worlds/benchmark/cave.world
code/stage/trunk/worlds/benchmark/hospital.world
Added Paths:
-----------
code/stage/trunk/worlds/benchmark/expand_pioneer.cc
code/stage/trunk/worlds/benchmark/expand_swarm.cc
Removed Paths:
-------------
code/stage/trunk/worlds/benchmark/expand.cc
Modified: code/stage/trunk/worlds/benchmark/CMakeLists.txt
===================================================================
--- code/stage/trunk/worlds/benchmark/CMakeLists.txt 2009-07-03 22:05:37 UTC (rev 7951)
+++ code/stage/trunk/worlds/benchmark/CMakeLists.txt 2009-07-03 23:56:10 UTC (rev 7952)
@@ -1,6 +1,13 @@
-SET( expandSrcs expand.cc )
-ADD_LIBRARY( expand MODULE ${expandSrcs} )
-TARGET_LINK_LIBRARIES( expand stage )
-set_source_files_properties( ${expandSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
-SET_TARGET_PROPERTIES( expand PROPERTIES PREFIX "" )
-INSTALL( TARGETS expand DESTINATION lib)
+SET( expand_swarmSrcs expand_swarm.cc )
+ADD_LIBRARY( expand_swarm MODULE ${expand_swarmSrcs} )
+TARGET_LINK_LIBRARIES( expand_swarm stage )
+set_source_files_properties( ${expand_swarmSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
+SET_TARGET_PROPERTIES( expand_swarm PROPERTIES PREFIX "" )
+
+SET( expand_pioneerSrcs expand_pioneer.cc )
+ADD_LIBRARY( expand_pioneer MODULE ${expand_pioneerSrcs} )
+TARGET_LINK_LIBRARIES( expand_pioneer stage )
+set_source_files_properties( ${expand_pioneerSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
+SET_TARGET_PROPERTIES( expand_pioneer PROPERTIES PREFIX "" )
+
+INSTALL( TARGETS expand_swarm expand_pioneer DESTINATION lib)
Modified: code/stage/trunk/worlds/benchmark/cave.world
===================================================================
--- code/stage/trunk/worlds/benchmark/cave.world 2009-07-03 22:05:37 UTC (rev 7951)
+++ code/stage/trunk/worlds/benchmark/cave.world 2009-07-03 23:56:10 UTC (rev 7952)
@@ -32,15 +32,13 @@
define rob pioneer2dx
#define rob fancypioneer2dx # alternative with more complex polygons
(
- origin [ 0 0 0 0 ]
+ # origin [ 0 0 0 0 ]
- sicklaser( samples 180 )
+ # sicklaser( samples 180 )
# alternative laser with more complex polygons
#fancysicklaser( samples 180 )
- ranger( pose [ 0 0 -0.050 0 ] )
-
- ctrl "expand"
+ ctrl "expand_pioneer"
)
define redrob rob( color "red" )
Deleted: code/stage/trunk/worlds/benchmark/expand.cc
===================================================================
--- code/stage/trunk/worlds/benchmark/expand.cc 2009-07-03 22:05:37 UTC (rev 7951)
+++ code/stage/trunk/worlds/benchmark/expand.cc 2009-07-03 23:56:10 UTC (rev 7952)
@@ -1,98 +0,0 @@
-/////////////////////////////////
-// File: stest.c
-// Desc: Stage library test program
-// Created: 2004.9.15
-// Author: Richard Vaughan <va...@sf...>
-// CVS: $Id: stest.cc,v 1.3 2008-02-01 03:11:02 rtv Exp $
-// License: GPL
-/////////////////////////////////
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include "stage.hh"
-using namespace Stg;
-
-typedef struct
-{
- ModelLaser* laser;
- ModelPosition* position;
- ModelRanger* ranger;
-} robot_t;
-
-#define VSPEED 0.4 // meters per second
-#define WGAIN 1.5 // turn speed gain
-#define SAFE_DIST 0.6 // meters
-#define SAFE_ANGLE 1 // radians
-
-// forward declare
-int RangerUpdate( ModelRanger* mod, robot_t* robot );
-
-// Stage calls this when the model starts up
-extern "C" int Init( Model* mod )
-{
- robot_t* robot = new robot_t;
- robot->position = (ModelPosition*)mod;
-
- // subscribe to the ranger, which we use for navigating
- robot->ranger = (ModelRanger*)mod->GetModel( "ranger:0" );
- assert( robot->ranger );
- robot->ranger->Subscribe();
-
- // ask Stage to call into our ranger update function
- robot->ranger->AddUpdateCallback( (stg_model_callback_t)RangerUpdate, robot );
-
- // subscribe to the laser, though we don't use it for navigating
- //robot->laser = (ModelLaser*)mod->GetModel( "laser:0" );
- //assert( robot->laser );
- //robot->laser->Subscribe();
-
- return 0; //ok
-}
-
-int RangerUpdate( ModelRanger* rgr, robot_t* robot )
-{
- // compute the vector sum of the sonar ranges
- double dx=0, dy=0;
-
- for( std::vector<ModelRanger::Sensor>::iterator it = rgr->sensors.begin();
- it != rgr->sensors.end();
- ++it )
- {
- ModelRanger::Sensor& s = *it;
- dx += s.range * cos( s.pose.a );
- dy += s.range * sin( s.pose.a );
-
- //printf( "sensor %d angle= %.2f\n", s, rgr->sensors[s].pose.a );
- }
-
- if( (dx == 0) || (dy == 0) )
- return 0;
-
- 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;
-
- //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) &&
- (rgr->sensors[1].range > SAFE_DIST/2.0) &&
- (rgr->sensors[2].range > SAFE_DIST/5.0) &&
- (rgr->sensors[15].range > SAFE_DIST/2.0) &&
- (rgr->sensors[14].range > SAFE_DIST/5.0) &&
- (fabs( resultant_angle ) < SAFE_ANGLE) )
- {
- forward_speed = VSPEED;
- }
-
- robot->position->SetSpeed( forward_speed, side_speed, turn_speed );
-
- return 0;
-}
-
Added: code/stage/trunk/worlds/benchmark/expand_pioneer.cc
===================================================================
--- code/stage/trunk/worlds/benchmark/expand_pioneer.cc (rev 0)
+++ code/stage/trunk/worlds/benchmark/expand_pioneer.cc 2009-07-03 23:56:10 UTC (rev 7952)
@@ -0,0 +1,96 @@
+/////////////////////////////////
+// File: stest.c
+// Desc: Stage library test program
+// Created: 2004.9.15
+// Author: Richard Vaughan <va...@sf...>
+// CVS: $Id: stest.cc,v 1.3 2008-02-01 03:11:02 rtv Exp $
+// License: GPL
+/////////////////////////////////
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "stage.hh"
+using namespace Stg;
+
+typedef struct
+{
+ ModelLaser* laser;
+ ModelPosition* position;
+ ModelRanger* ranger;
+} robot_t;
+
+
+const double VSPEED = 0.2; // meters per second
+const double WGAIN = 1.0; // turn speed gain
+const double SAFE_DIST = 0.5; // meters
+const double SAFE_ANGLE = 1.0; // radians
+
+
+// forward declare
+int RangerUpdate( ModelRanger* mod, robot_t* robot );
+
+// Stage calls this when the model starts up
+extern "C" int Init( Model* mod )
+{
+ robot_t* robot = new robot_t;
+ robot->position = (ModelPosition*)mod;
+
+ // subscribe to the ranger, which we use for navigating
+ robot->ranger = (ModelRanger*)mod->GetModel( "ranger:0" );
+ assert( robot->ranger );
+ robot->ranger->Subscribe();
+
+ // ask Stage to call into our ranger update function
+ robot->ranger->AddUpdateCallback( (stg_model_callback_t)RangerUpdate, robot );
+
+ // subscribe to the laser, though we don't use it for navigating
+ //robot->laser = (ModelLaser*)mod->GetModel( "laser:0" );
+ //assert( robot->laser );
+ //robot->laser->Subscribe();
+
+ return 0; //ok
+}
+
+int RangerUpdate( ModelRanger* rgr, robot_t* robot )
+{
+ // compute the vector sum of the sonar ranges
+ double dx=0, dy=0;
+
+ FOR_EACH( it, rgr->sensors )
+ {
+ ModelRanger::Sensor& s = *it;
+ dx += s.range * cos( s.pose.a );
+ dy += s.range * sin( s.pose.a );
+
+ //printf( "sensor %d angle= %.2f\n", s, rgr->sensors[s].pose.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/5.0) &&
+ (rgr->sensors[2].range > SAFE_DIST/2.0) &&
+ (rgr->sensors[1].range > SAFE_DIST/5.0) &&
+ (fabs( resultant_angle ) < SAFE_ANGLE) )
+ {
+ forward_speed = VSPEED;
+ }
+
+ robot->position->SetSpeed( forward_speed, side_speed, turn_speed );
+
+ return 0;
+}
+
Added: code/stage/trunk/worlds/benchmark/expand_swarm.cc
===================================================================
--- code/stage/trunk/worlds/benchmark/expand_swarm.cc (rev 0)
+++ code/stage/trunk/worlds/benchmark/expand_swarm.cc 2009-07-03 23:56:10 UTC (rev 7952)
@@ -0,0 +1,98 @@
+/////////////////////////////////
+// File: stest.c
+// Desc: Stage library test program
+// Created: 2004.9.15
+// Author: Richard Vaughan <va...@sf...>
+// CVS: $Id: stest.cc,v 1.3 2008-02-01 03:11:02 rtv Exp $
+// License: GPL
+/////////////////////////////////
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "stage.hh"
+using namespace Stg;
+
+typedef struct
+{
+ ModelLaser* laser;
+ ModelPosition* position;
+ ModelRanger* ranger;
+} robot_t;
+
+// swarmbot
+const double VSPEED = 0.3; // meters per second
+const double WGAIN = 1.0; // turn speed gain
+const double SAFE_DIST = 0.5; // meters
+const double SAFE_ANGLE = 0.5; // radians
+
+// forward declare
+int RangerUpdate( ModelRanger* mod, robot_t* robot );
+
+// Stage calls this when the model starts up
+extern "C" int Init( Model* mod )
+{
+ robot_t* robot = new robot_t;
+ robot->position = (ModelPosition*)mod;
+
+ // subscribe to the ranger, which we use for navigating
+ robot->ranger = (ModelRanger*)mod->GetModel( "ranger:0" );
+ assert( robot->ranger );
+ robot->ranger->Subscribe();
+
+ // ask Stage to call into our ranger update function
+ robot->ranger->AddUpdateCallback( (stg_model_callback_t)RangerUpdate, robot );
+
+ // subscribe to the laser, though we don't use it for navigating
+ //robot->laser = (ModelLaser*)mod->GetModel( "laser:0" );
+ //assert( robot->laser );
+ //robot->laser->Subscribe();
+
+ return 0; //ok
+}
+
+int RangerUpdate( ModelRanger* rgr, robot_t* robot )
+{
+ // compute the vector sum of the sonar ranges
+ double dx=0, dy=0;
+
+ FOR_EACH( it, rgr->sensors )
+ {
+ ModelRanger::Sensor& s = *it;
+ dx += s.range * cos( s.pose.a );
+ dy += s.range * sin( s.pose.a );
+
+ //printf( "sensor %d angle= %.2f\n", s, rgr->sensors[s].pose.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[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) &&
+
+ (rgr->sensors[9].range > SAFE_DIST/5.0) &&
+ (rgr->sensors[10].range > SAFE_DIST/3.0) &&
+ (rgr->sensors[11].range > SAFE_DIST/1.4) &&
+ (fabs( resultant_angle ) < SAFE_ANGLE) )
+ {
+ forward_speed = VSPEED;
+ }
+
+ robot->position->SetSpeed( forward_speed, side_speed, turn_speed );
+
+ return 0;
+}
+
Modified: code/stage/trunk/worlds/benchmark/hospital.world
===================================================================
--- code/stage/trunk/worlds/benchmark/hospital.world 2009-07-03 22:05:37 UTC (rev 7951)
+++ code/stage/trunk/worlds/benchmark/hospital.world 2009-07-03 23:56:10 UTC (rev 7952)
@@ -38,7 +38,7 @@
define swarmbot position
(
size [0.100 0.100 0.100]
- color "red"
+ color "random"
ranger( pose [ 0 0 -0.010 0 ]
@@ -57,7 +57,7 @@
spose[11] [0 0 330]
sview [ 0 2 30] )
- ctrl "expand"
+ ctrl "expand_swarm"
)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|