Update of /cvsroot/playerstage/code/stage/libstage
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv30083
Modified Files:
Tag: opengl
canvas.cc stest.cc
Log Message:
swarm demo now works
Index: stest.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/stest.cc,v
retrieving revision 1.1.2.14
retrieving revision 1.1.2.15
diff -C2 -d -r1.1.2.14 -r1.1.2.15
*** stest.cc 19 Nov 2007 07:40:41 -0000 1.1.2.14
--- stest.cc 19 Nov 2007 08:18:44 -0000 1.1.2.15
***************
*** 31,36 ****
} robot_t;
! const int POPSIZE = 10;
int main( int argc, char* argv[] )
--- 31,40 ----
} robot_t;
! const int POPSIZE = 100;
+ #define VSPEED 0.4 // meters per second
+ #define WGAIN 1.0 // turn speed gain
+ #define SAFE_DIST 0.35 // meters
+ #define SAFE_ANGLE 0.4 // radians
int main( int argc, char* argv[] )
***************
*** 79,93 ****
robots[i].obs = false;
! sprintf( namebuf, "MyWorld.position:%d", i );
robots[i].position = (StgModelPosition*)world.GetModel( namebuf );
assert(robots[i].position);
robots[i].position->Subscribe();
! sprintf( namebuf, "MyWorld.position:%d.laser:0", i );
! robots[i].laser = (StgModelLaser*)world.GetModel( namebuf );
! assert(robots[i].laser);
! robots[i].laser->Subscribe();
! sprintf( namebuf, "MyWorld.position:%d.ranger:0", i );
robots[i].ranger = (StgModelRanger*)world.GetModel( namebuf );
assert(robots[i].ranger);
--- 83,100 ----
robots[i].obs = false;
! char* base = "r";
! sprintf( namebuf, "%s%02d", base, i );
! printf( "finding robot \"%s\"\n", namebuf );
robots[i].position = (StgModelPosition*)world.GetModel( namebuf );
assert(robots[i].position);
robots[i].position->Subscribe();
! //sprintf( namebuf, "MyWorld.position:%d.laser:0", i );
! //robots[i].laser = (StgModelLaser*)world.GetModel( namebuf );
! //assert(robots[i].laser);
! //robots[i].laser->Subscribe();
! //sprintf( namebuf, "MyWorld.position:%d.ranger:0", i );
! sprintf( namebuf, "%s%02d.ranger:0", base, i );
robots[i].ranger = (StgModelRanger*)world.GetModel( namebuf );
assert(robots[i].ranger);
***************
*** 108,111 ****
--- 115,166 ----
while( world.RealTimeUpdate() )
// while( world.Update() )
+ for( int i=0; i<POPSIZE; i++ )
+ {
+
+ StgModelRanger* rgr = robots[i].ranger;
+
+ // compute the vector sum of the sonar ranges
+ double dx=0, dy=0;
+
+ int num_ranges = rgr->sensor_count;//spp[i]->GetCount();
+ for( int s=0; s<num_ranges; s++ )
+ {
+ //player_pose3d_t spose = spp[i]->GetPose(s);
+ double srange = rgr->sensors[s].range; //spp[i]->GetScan(s);
+
+ dx += srange * cos( rgr->sensors[s].pose.a );
+ dy += srange * sin( rgr->sensors[s].pose.a );
+ }
+
+ double resultant_angle = atan2( dy, dx );
+ //double resultant_magnitude = hypot( dy, dx );
+
+ double forward_speed = 0.0;
+ double side_speed = 0.0;
+ double turn_speed = WGAIN * resultant_angle;
+
+ int forward = num_ranges/2 -1 ;
+ // if the front is clear, drive forwards
+ if( (rgr->sensors[forward-1].range > SAFE_DIST) &&
+ (rgr->sensors[forward ].range > SAFE_DIST) &&
+ (rgr->sensors[forward+1].range > SAFE_DIST) &&
+ (fabs( resultant_angle ) < SAFE_ANGLE) )
+ {
+ forward_speed = VSPEED;
+ }
+
+ // send a command to the robot
+ stg_velocity_t vel;
+ vel.x = forward_speed;
+ vel.y = side_speed;
+ vel.z = 0;
+ vel.a = turn_speed;
+
+ robots[i].position->Do( forward_speed, side_speed, turn_speed );
+
+
+ //ppp[i]->SetSpeed( forward_speed, side_speed, turn_speed );
+ }
+
{
Index: canvas.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/canvas.cc,v
retrieving revision 1.1.2.1
retrieving revision 1.1.2.2
diff -C2 -d -r1.1.2.1 -r1.1.2.2
*** canvas.cc 19 Nov 2007 07:42:10 -0000 1.1.2.1
--- canvas.cc 19 Nov 2007 08:18:44 -0000 1.1.2.2
***************
*** 34,38 ****
rotating = false;
! showflags = STG_SHOW_CLOCK | STG_SHOW_DATA | STG_SHOW_BLOCKS |
STG_SHOW_GRID;
// start the timer that causes regular redraws
--- 34,39 ----
rotating = false;
! //showflags = STG_SHOW_CLOCK | STG_SHOW_DATA | STG_SHOW_BLOCKS |
STG_SHOW_GRID;
! showflags = STG_SHOW_CLOCK | STG_SHOW_BLOCKS | STG_SHOW_GRID;
// start the timer that causes regular redraws
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2005.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
