Revision: 6761
http://playerstage.svn.sourceforge.net/playerstage/?rev=6761&view=rev
Author: rtv
Date: 2008-07-04 14:53:04 -0700 (Fri, 04 Jul 2008)
Log Message:
-----------
fixed vertexarray bug
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/swarmbenchmark.cc
code/stage/trunk/libstage/canvas.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_camera.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/model_ranger.cc
code/stage/trunk/worlds/everything.world
code/stage/trunk/worlds/pioneer.inc
code/stage/trunk/worlds/swarmbenchmark/cave.world
Modified: code/stage/trunk/examples/ctrl/swarmbenchmark.cc
===================================================================
--- code/stage/trunk/examples/ctrl/swarmbenchmark.cc 2008-07-04 19:55:31 UTC
(rev 6760)
+++ code/stage/trunk/examples/ctrl/swarmbenchmark.cc 2008-07-04 21:53:04 UTC
(rev 6761)
@@ -14,10 +14,6 @@
#include "stage.hh"
using namespace Stg;
-static double minfrontdistance = 0.750;
-static double speed = 0.400;
-static double turnrate = M_PI/3.0;
-
typedef struct
{
StgModelLaser* laser;
@@ -25,10 +21,10 @@
StgModelRanger* ranger;
} robot_t;
-#define VSPEED 0.4 // meters per second
-#define WGAIN 1.0 // turn speed gain
+#define VSPEED 0.5 // meters per second
+#define WGAIN 1.5 // turn speed gain
#define SAFE_DIST 0.6 // meters
-#define SAFE_ANGLE 0.25 // radians
+#define SAFE_ANGLE 1 // radians
// forward declare
int RangerUpdate( StgModel* mod, robot_t* robot );
@@ -48,8 +44,8 @@
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 = (StgModelLaser*)mod->GetModel( "laser:0" );
+ //assert( robot->laser );
//robot->laser->Subscribe();
return 0; //ok
@@ -71,6 +67,8 @@
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) )
@@ -84,15 +82,21 @@
double side_speed = 0.0;
double turn_speed = WGAIN * resultant_angle;
- int forward = rgr->sensor_count/2 -1 ;
+ //printf( "resultant %.2f turn_speed %.2f\n", resultant_angle, turn_speed );
+
+ int forward = 0 ;
// 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) &&
+ 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;
Modified: code/stage/trunk/libstage/canvas.cc
===================================================================
--- code/stage/trunk/libstage/canvas.cc 2008-07-04 19:55:31 UTC (rev 6760)
+++ code/stage/trunk/libstage/canvas.cc 2008-07-04 21:53:04 UTC (rev 6761)
@@ -874,8 +874,8 @@
glHint( GL_LINE_SMOOTH_HINT, GL_FASTEST );
glDepthMask( GL_TRUE );
glEnable( GL_TEXTURE_2D );
+ glEnableClientState( GL_VERTEX_ARRAY );
-
//TODO find a better home for loading textures
if( loaded_texture == false ) {
std::string fullpath = world->fileMan->fullPath( "stall.png" );
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2008-07-04 19:55:31 UTC (rev 6760)
+++ code/stage/trunk/libstage/model.cc 2008-07-04 21:53:04 UTC (rev 6761)
@@ -1181,7 +1181,7 @@
PushLocalCoords();
DataVisualize(); // virtual function overridden by most model types
- // shift to top of this model
+ // shift to top of this model - why is this not necessary?
glTranslatef( 0,0, geom.size.z );
// and draw the children
Modified: code/stage/trunk/libstage/model_camera.cc
===================================================================
--- code/stage/trunk/libstage/model_camera.cc 2008-07-04 19:55:31 UTC (rev
6760)
+++ code/stage/trunk/libstage/model_camera.cc 2008-07-04 21:53:04 UTC (rev
6761)
@@ -329,7 +329,7 @@
}
- glEnableClientState( GL_VERTEX_ARRAY );
+ //glEnableClientState( GL_VERTEX_ARRAY );
glEnableClientState( GL_COLOR_ARRAY );
glVertexPointer( 3, GL_FLOAT, 0, _camera_quads );
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2008-07-04 19:55:31 UTC (rev
6760)
+++ code/stage/trunk/libstage/model_laser.cc 2008-07-04 21:53:04 UTC (rev
6761)
@@ -318,9 +318,18 @@
data_dirty = false;
glNewList( data_dl, GL_COMPILE );
-
+ //glEnableClientState( GL_VERTEX_ARRAY );
glTranslatef( 0,0, geom.size.z/2.0 ); // shoot the laser beam out at the
right height
+ PushColor( 0, 0, 1, 0.5 );
+
+ glPointSize( 4.0 );
+
+ // DEBUG - draw the origin of the laser beams
+ glBegin( GL_POINTS );
+ glVertex2f( 0,0 );
+ glEnd();
+
// pack the laser hit points into a vertex array for fast rendering
static float* pts = NULL;
pts = (float*)g_realloc( pts, 2 * (sample_count+1) * sizeof(float));
@@ -328,10 +337,7 @@
pts[0] = 0.0;
pts[1] = 0.0;
- PushColor( 0, 0, 1, 0.5 );
- glPointSize( 4.0 );
-
glVertexPointer( 2, GL_FLOAT, 0, pts );
for( unsigned int s=0; s<sample_count; s++ )
@@ -379,7 +385,7 @@
glDepthMask( GL_TRUE );
-
+ //glDisableClientState( GL_VERTEX_ARRAY );
glEndList();
}
Modified: code/stage/trunk/libstage/model_ranger.cc
===================================================================
--- code/stage/trunk/libstage/model_ranger.cc 2008-07-04 19:55:31 UTC (rev
6760)
+++ code/stage/trunk/libstage/model_ranger.cc 2008-07-04 21:53:04 UTC (rev
6761)
@@ -80,9 +80,12 @@
#include <math.h>
static const stg_watts_t DEFAULT_RANGER_WATTSPERSENSOR = 0.2;
-static const stg_meters_t DEFAULT_RANGER_SIZEX = 0.01;
-static const stg_meters_t DEFAULT_RANGER_SIZEY = 0.04;
-static const stg_meters_t DEFAULT_RANGER_SIZEZ = 0.04;
+static const stg_meters_t DEFAULT_RANGER_SIZEX = 0.4;
+static const stg_meters_t DEFAULT_RANGER_SIZEY = 0.4;
+static const stg_meters_t DEFAULT_RANGER_SIZEZ = 0.05;
+static const stg_meters_t DEFAULT_RANGER_TRANSDUCER_SIZEX = 0.01;
+static const stg_meters_t DEFAULT_RANGER_TRANSDUCER_SIZEY = 0.04;
+static const stg_meters_t DEFAULT_RANGER_TRANSDUCER_SIZEZ = 0.04;
static const stg_meters_t DEFAULT_RANGER_RANGEMAX = 5.0;
static const stg_meters_t DEFAULT_RANGER_RANGEMIN = 0.0;
static const unsigned int DEFAULT_RANGER_RAYCOUNT = 3;
@@ -113,12 +116,16 @@
stg_geom_t geom;
memset( &geom, 0, sizeof(geom)); // no size
+ geom.size.x = DEFAULT_RANGER_SIZEX;
+ geom.size.y = DEFAULT_RANGER_SIZEY;
+ geom.size.z = DEFAULT_RANGER_SIZEZ;
this->SetGeom( geom );
samples = NULL;
sensor_count = DEFAULT_RANGER_SENSORCOUNT;
sensors = new stg_ranger_sensor_t[sensor_count];
+ // spread the transducers around the ranger's body
double offset = MIN(geom.size.x, geom.size.y) / 2.0;
// create default ranger config
@@ -127,11 +134,11 @@
sensors[c].pose.a = (2.0*M_PI)/sensor_count * c;
sensors[c].pose.x = offset * cos( sensors[c].pose.a );
sensors[c].pose.y = offset * sin( sensors[c].pose.a );
- sensors[c].pose.z = 0;//geom.size.z / 2.0; // half way up
+ sensors[c].pose.z = geom.size.z / 2.0; // half way up
- sensors[c].size.x = DEFAULT_RANGER_SIZEX;
- sensors[c].size.y = DEFAULT_RANGER_SIZEY;
- sensors[c].size.z = DEFAULT_RANGER_SIZEZ;
+ sensors[c].size.x = DEFAULT_RANGER_TRANSDUCER_SIZEX;
+ sensors[c].size.y = DEFAULT_RANGER_TRANSDUCER_SIZEY;
+ sensors[c].size.z = DEFAULT_RANGER_TRANSDUCER_SIZEZ;
sensors[c].bounds_range.min = DEFAULT_RANGER_RANGEMIN;
sensors[c].bounds_range.max = DEFAULT_RANGER_RANGEMAX;;
@@ -356,11 +363,6 @@
{
stg_ranger_sensor_t* rngr = &sensors[s];
- //double dx = rngr->size.x/2.0;
- //double dy = rngr->size.y/2.0;
- //double dz = rngr->size.z/2.0;
-
-
// sensor FOV
double sidelen = samples[s];
double da = rngr->fov/2.0;
@@ -384,12 +386,13 @@
glDepthMask( GL_FALSE );
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
PushColor( 0, 1, 0, 0.1 ); // transparent pale green
- glEnableClientState( GL_VERTEX_ARRAY );
+ //glEnableClientState( GL_VERTEX_ARRAY );
glVertexPointer( 3, GL_FLOAT, 0, pts );
glDrawArrays( GL_TRIANGLES, 0, 3 * sensor_count );
// restore state
glDepthMask( GL_TRUE );
+ //glDisableClientState( GL_VERTEX_ARRAY );
PopColor();
}
Modified: code/stage/trunk/worlds/everything.world
===================================================================
--- code/stage/trunk/worlds/everything.world 2008-07-04 19:55:31 UTC (rev
6760)
+++ code/stage/trunk/worlds/everything.world 2008-07-04 21:53:04 UTC (rev
6761)
@@ -18,8 +18,8 @@
window(
size [ 1059.000 541.000 ]
- center [-3.707 -1.447]
- scale 25.877 # pixels per meter
+ center [-6.333 2.011]
+ scale 80.217 # pixels per meter
)
floorplan(
@@ -47,44 +47,44 @@
puck( pose [-9.114 2.467 -105.501 ] color "red" )
puck( pose [-9.045 2.624 -37.717 ] color "purple" )
-puck( pose [-8.959 2.752 -461.643 ] color "orange" )
+puck( pose [-8.959 2.752 -101.643 ] color "orange" )
+ranger( alwayson 1 )
+
+ sicklaser( pose [0.030 0.000 0.000 ] alwayson 1 )
+
+
# extend the pioneer2dx definition from pioneer.inc
#
define trickedoutpioneer pioneer2dx
(
- sicklaser( alwayson 1
- pose [0.030 0.000 0.000 ]
- fiducial( range_max 8 range_max_id 5 )
+ ranger( alwayson 1 )
- blobfinder( alwayson 1
+ sicklaser( pose [0.030 0.000 0.000 ] alwayson 1 )
+
+ fiducial( range_max 8 range_max_id 5 )
+
+ blobfinder(
channel_count 6
channels [ "red" "blue" "green" "cyan" "yellow" "magenta" ]
+ alwayson 1
)
- )
+
+ #camera( alwayson 1 )
-
fiducial_return 17
gripper_return 0
localization "gps"
localization_origin [ 0 0 0 ]
-
-# bumper( bcount 3
-# blength 0.2
-# bpose[0] [0 -0.165 90]
-# bpose[1] [0 0.165 -90]
-# bpose[2] [-0.26 0 0]
-# blength[2] 0.1 # set the length of a single bumper
-# )
)
trickedoutpioneer
(
name "robot1"
- pose [-10.071 3.186 -722.333]
+ pose [-10.071 3.186 -2.333]
#gripper( pose [0.23 0.000 0.000] color "gray" )
#speech()
@@ -105,21 +105,21 @@
fiducial_return 18
color "blue"
name "p2"
- pose [-6.009 2.481 -194.220]
+ pose [-6.009 2.481 165.780]
)
trickedoutpioneer
(
color "green"
name "p3"
- pose [-6.492 2.156 -199.781]
+ pose [-6.492 2.156 160.219]
)
trickedoutpioneer
(
color "cyan"
name "p4"
- pose [-7.641 3.989 -487.216]
+ pose [-7.641 3.989 -127.216]
)
trickedoutpioneer
@@ -165,5 +165,5 @@
roomba(
name "roomba1"
- pose [-9 0.5 0]
+ pose [-9.000 0.500 0.000]
)
Modified: code/stage/trunk/worlds/pioneer.inc
===================================================================
--- code/stage/trunk/worlds/pioneer.inc 2008-07-04 19:55:31 UTC (rev 6760)
+++ code/stage/trunk/worlds/pioneer.inc 2008-07-04 21:53:04 UTC (rev 6761)
@@ -52,7 +52,7 @@
# use the sonar array defined above with a small vertical offset to
# drop the sensors into the robot body
- p2dx_sonar( pose4 [0.04 0 -0.03 0] )
+ #p2dx_sonar( pose4 [0.04 0 -0.03 0] )
# differential steering model
drive "diff"
Modified: code/stage/trunk/worlds/swarmbenchmark/cave.world
===================================================================
--- code/stage/trunk/worlds/swarmbenchmark/cave.world 2008-07-04 19:55:31 UTC
(rev 6760)
+++ code/stage/trunk/worlds/swarmbenchmark/cave.world 2008-07-04 21:53:04 UTC
(rev 6761)
@@ -8,17 +8,17 @@
resolution 0.02 # resolution of the underlying raytrace mode
interval_sim 100 # simulation timestep in milliseconds
-interval_real 10 # real-time interval between simulation updates in
milliseconds
+interval_real 100 # real-time interval between simulation updates in
milliseconds
paused 1
# configure the GUI window
window
(
- size [ 1000.000 1000.000 ]
center [-0.932 -0.088]
rotate [ -61.500 -50.000 ]
scale 51.828
+ interval 10
)
floorplan
@@ -32,10 +32,11 @@
define rob fancypioneer2dx
(
fancysicklaser( pose [ 0.040 0.000 0.000 ] samples 180 )
+ ranger( pose4 [ 0 0 -0.05 0 ] )
+
ctrl "swarmbenchmark"
)
-
define redrob rob( color "red" )
define greenrob rob( color "green" )
define magentarob rob( color "magenta" )
@@ -58,7 +59,7 @@
redrob( pose [-4.393 4.938 114.269] )
redrob( pose [-6.277 4.252 -97.494] )
redrob( pose [-5.202 5.566 -89.743] )
-
+#
bluerob( pose [-7.014 6.409 -103.088] )
bluerob( pose [-6.139 5.649 -29.509] )
bluerob( pose [-6.315 6.277 -35.740] )
@@ -69,7 +70,7 @@
bluerob( pose [-6.810 4.727 -41.884] )
bluerob( pose [-7.415 7.485 -14.803] )
bluerob( pose [-6.722 7.120 -70.950] )
-
+#
greenrob( pose [-2.686 6.660 -20.602] )
greenrob( pose [-3.274 5.174 63.058] )
greenrob( pose [-2.477 6.075 -93.720] )
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
