Revision: 6809
http://playerstage.svn.sourceforge.net/playerstage/?rev=6809&view=rev
Author: rtv
Date: 2008-07-07 21:29:12 -0700 (Mon, 07 Jul 2008)
Log Message:
-----------
moving things around
Added Paths:
-----------
code/stage/trunk/worlds/benchmark/benchmark.cc
Copied: code/stage/trunk/worlds/benchmark/benchmark.cc (from rev 6802,
code/stage/trunk/examples/ctrl/expand.cc)
===================================================================
--- code/stage/trunk/worlds/benchmark/benchmark.cc
(rev 0)
+++ code/stage/trunk/worlds/benchmark/benchmark.cc 2008-07-08 04:29:12 UTC
(rev 6809)
@@ -0,0 +1,121 @@
+/////////////////////////////////
+// File: stest.c
+// Desc: Stage library test program
+// Created: 2004.9.15
+// Author: Richard Vaughan <[EMAIL PROTECTED]>
+// 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
+{
+ StgModelLaser* laser;
+ StgModelPosition* position;
+ StgModelRanger* ranger;
+} robot_t;
+
+#define VSPEED 0.5 // 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( StgModel* mod, robot_t* robot );
+
+// Stage calls this when the model starts up
+extern "C" int Init( StgModel* mod )
+{
+ robot_t* robot = new robot_t;
+ robot->position = (StgModelPosition*)mod;
+
+ // subscribe to the ranger, which we use for navigating
+ robot->ranger = (StgModelRanger*)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 = (StgModelLaser*)mod->GetModel( "laser:0" );
+ //assert( robot->laser );
+ //robot->laser->Subscribe();
+
+ return 0; //ok
+}
+
+int RangerUpdate( StgModel* mod, robot_t* robot )
+{
+ StgModelRanger* rgr = robot->ranger;
+
+ if( rgr->samples == NULL )
+ return 0;
+
+ // 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 );
+
+ //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 );
+
+ int forward = 0 ;
+ // if the front is clear, drive forwards
+ if( (rgr->samples[0] > SAFE_DIST) &&
+ (rgr->samples[1] > SAFE_DIST/2.0) &&
+ (rgr->samples[2] > SAFE_DIST/5.0) &&
+ (rgr->samples[15] > SAFE_DIST/2.0) &&
+ (rgr->samples[14] > SAFE_DIST/5.0) &&
+ (fabs( resultant_angle ) < SAFE_ANGLE) )
+ {
+ forward_speed = VSPEED;
+ }
+
+ //printf( "forward angle = %.2f\n", rgr->sensors[forward].pose.a );
+
+ // // send a command to the robot
+ // stg_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;
+ //stg_blobfinder_blob_t* blobs = robots[i].blobfinder->GetBlobs( &bcount );
+
+ //printf( "robot %s sees %u blobs\n", robots[i].blobfinder->Token(), bcount
);
+
+ robot->position->SetSpeed( forward_speed, side_speed, turn_speed );
+
+ return 0;
+}
+
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
Sponsored by: SourceForge.net Community Choice Awards: VOTE NOW!
Studies have shown that voting for your favorite open source project,
along with a healthy diet, reduces your potential for chronic lameness
and boredom. Vote Now at http://www.sourceforge.net/community/cca08
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
