Revision: 7334
http://playerstage.svn.sourceforge.net/playerstage/?rev=7334&view=rev
Author: rtv
Date: 2009-02-15 23:44:46 +0000 (Sun, 15 Feb 2009)
Log Message:
-----------
cleaning up gripper and adding visualization
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/libstage/gl.cc
code/stage/trunk/libstage/model_gripper.cc
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/worlds/fasr.world
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2009-02-13 18:55:58 UTC (rev
7333)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2009-02-15 23:44:46 UTC (rev
7334)
@@ -115,7 +115,7 @@
void Dock()
{
// close the grippers so they can be pushed into the charger
- ModelGripper::data_t gripper_data = gripper->GetData();
+ ModelGripper::config_t gripper_data = gripper->GetConfig();
if( gripper_data.paddles != ModelGripper::PADDLE_CLOSED )
gripper->CommandClose();
@@ -183,7 +183,7 @@
pos->SetXSpeed( 0.0 );
// once we have backed off a bit, open and lower the gripper
- ModelGripper::data_t gripper_data = gripper->GetData();
+ ModelGripper::config_t gripper_data = gripper->GetConfig();
if( charger_range > gripper_distance )
{
if( gripper_data.paddles != ModelGripper::PADDLE_OPEN )
@@ -284,7 +284,7 @@
{
if( verbose ) puts( "Cruise" );
- ModelGripper::data_t gdata = gripper->GetData();
+ ModelGripper::config_t gdata = gripper->GetConfig();
//avoidcount = 0;
pos->SetXSpeed( cruisespeed );
@@ -303,7 +303,7 @@
if( y < 0 ) y = 0;
double a_goal =
- dtor( ( pos->GetFlagCount() || gdata.stack_count ) ?
have[y][x] : need[y][x] );
+ dtor( ( pos->GetFlagCount() || gdata.gripped ) ?
have[y][x] : need[y][x] );
// if we are low on juice - find the direction to the
recharger instead
if( Hungry() )
@@ -481,8 +481,8 @@
static int GripperUpdate( ModelGripper* grip, Robot* robot )
{
- ModelGripper::data_t gdata = grip->GetData();
-
+ ModelGripper::config_t gdata = grip->GetConfig();
+
printf( "BREAKBEAMS %s %s\n",
gdata.beam[0] ? gdata.beam[0]->Token() :
"<null>",
gdata.beam[1] ? gdata.beam[1]->Token() :
"<null>" );
Modified: code/stage/trunk/libstage/gl.cc
===================================================================
--- code/stage/trunk/libstage/gl.cc 2009-02-13 18:55:58 UTC (rev 7333)
+++ code/stage/trunk/libstage/gl.cc 2009-02-15 23:44:46 UTC (rev 7334)
@@ -50,6 +50,11 @@
glEnd();
}
+void Stg::Gl::draw_centered_rect( float x, float y, float dx, float dy )
+{
+ glRectf( x-0.5*dx, y-0.5*dy, x+0.5*dx, y+0.5*dy );
+}
+
void Stg::Gl::draw_vector( double x, double y, double z )
{
glBegin( GL_LINES );
Modified: code/stage/trunk/libstage/model_gripper.cc
===================================================================
--- code/stage/trunk/libstage/model_gripper.cc 2009-02-13 18:55:58 UTC (rev
7333)
+++ code/stage/trunk/libstage/model_gripper.cc 2009-02-15 23:44:46 UTC (rev
7334)
@@ -27,10 +27,12 @@
gripper
(
# gripper properties
- <none>
+ paddle_size [ 0.66 0.1 0.4 ]
+ paddle_state [ "open" "down" ]
+ autosnatch 0
# model properties
- size [0.12 0.28]
+ size [ 0.2 0.3 0.2 ]
)
@endverbatim
@@ -38,6 +40,12 @@
@par Details
+- autosnatch < 0 or 1>\n
+ iff 1, gripper will close automatically when break beams are broken
+- paddle_size [ <float x> <float y < float z> ]\n
+ Gripper paddle size as a proportion of the model's body size (0.0 to 1.0)
+- paddle_state [ <string open/close> <string up/down> ]\n
+ Gripper arms state, either "open" or "closed", and lift state, either "up"
or "down"
*/
@@ -47,6 +55,9 @@
#include "worldfile.hh"
using namespace Stg;
+#include "option.hh"
+Option ModelGripper::showData( "Gripper data", "show_gripper_data", "", true,
NULL );
+
// TODO - simulate energy use when moving grippers
ModelGripper::ModelGripper( World* world,
@@ -66,8 +77,7 @@
cfg.lift_position = 0.0;
cfg.paddles_stalled = false;
cfg.autosnatch = false;
- cfg.grip_stack = NULL;
- cfg.grip_stack_size = 1;
+ cfg.gripped = NULL;
// place the break beam sensors at 1/4 and 3/4 the length of the paddle
cfg.break_beam_inset[0] = 3.0/4.0 * cfg.paddle_size.x;
@@ -88,10 +98,10 @@
geom.size.y = 0.3;
geom.size.z = 0.2;
SetGeom( geom );
-
- //Startup();
PositionPaddles();
+
+ registerOption( &showData );
}
ModelGripper::~ModelGripper()
@@ -108,8 +118,8 @@
cfg.paddle_size.y = wf->ReadTupleFloat( wf_entity, "paddle_size", 1,
cfg.paddle_size.y );
cfg.paddle_size.z = wf->ReadTupleFloat( wf_entity, "paddle_size", 2,
cfg.paddle_size.z );
- const char* paddles = wf->ReadTupleString( wf_entity, "paddles", 0, NULL );
- const char* lift = wf->ReadTupleString( wf_entity, "paddles", 1, NULL );
+ const char* paddles = wf->ReadTupleString( wf_entity, "paddle_state", 0,
NULL );
+ const char* lift = wf->ReadTupleString( wf_entity, "paddle_state", 1,
NULL );
if( paddles && strcmp( paddles, "closed" ) == 0 )
{
@@ -135,8 +145,6 @@
cfg.lift = LIFT_DOWN;
}
- cfg.grip_stack_size = wf->ReadInt( wf_entity , "stack_size",
cfg.grip_stack_size );
-
FixBlocks();
// do this at the end to ensure that the blocks are resize correctly
@@ -149,14 +157,10 @@
wf->WriteTupleFloat( wf_entity, "paddle_size", 0, cfg.paddle_size.x );
wf->WriteTupleFloat( wf_entity, "paddle_size", 1, cfg.paddle_size.y );
- wf->WriteTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z );
-
- wf->WriteInt( wf_entity , "stack_size", cfg.grip_stack_size );
-}
+ wf->WriteTupleFloat( wf_entity, "paddle_size", 2, cfg.paddle_size.z );
-void ModelGripper::DataVisualize( Camera* cam )
-{
- /* do nothing */
+ wf->WriteTupleString( wf_entity, "paddle_state", 0, (cfg.paddles ==
PADDLE_CLOSED ) ? "closed" : "open" );
+ wf->WriteTupleString( wf_entity, "paddle_state", 1, (cfg.lift == LIFT_UP ) ?
"up" : "down" );
}
void ModelGripper::FixBlocks()
@@ -259,16 +263,15 @@
cfg.paddles = PADDLE_OPEN; // change state
}
- // drop the thing at the head of the stack
- if( cfg.grip_stack &&
+
+ // drop the thing we're carrying
+ if( cfg.gripped &&
(cfg.paddle_position == 0.0 || cfg.paddle_position <
cfg.close_limit ))
{
- Model* head = (Model*)cfg.grip_stack->data;
- cfg.grip_stack = g_list_remove( cfg.grip_stack, head );
-
// move it to the new location
- head->SetParent( NULL );
- head->SetPose( this->GetGlobalPose() );
+ cfg.gripped->SetParent( NULL );
+ cfg.gripped->SetPose( this->GetGlobalPose() );
+ cfg.gripped = NULL;
cfg.close_limit = 1.0;
}
@@ -327,37 +330,8 @@
Model::Update();
}
-ModelGripper::data_t ModelGripper::GetData()
-{
- data_t data;
-
- data.paddles = cfg.paddles;
- data.paddle_position = cfg.paddle_position;
- data.lift = cfg.lift;
- data.lift_position = cfg.lift_position;
- data.beam[0] = cfg.beam[0];
- data.beam[1] = cfg.beam[1];
- data.contact[0] = cfg.contact[0];
- data.contact[1] = cfg.contact[1];
-
- if( cfg.grip_stack )
- data.stack_count = g_list_length( cfg.grip_stack );
- else
- data.stack_count = 0;
-
- data.paddles_stalled = cfg.paddles_stalled;
- return data;
-}
-
-
-void ModelGripper::SetConfig( config_t & cfg )
-{
- this->cfg = cfg;
-}
-
-
static bool gripper_raytrace_match( Model* hit,
Model* finder,
const void* dummy )
@@ -473,8 +447,7 @@
if( !hit )
hit = cfg.contact[1];
- if( cfg.grip_stack_size > 0 &&
- (g_list_length( cfg.grip_stack ) <
cfg.grip_stack_size) )
+ if( cfg.gripped == NULL ) // if we're not carrying
something already
{
// get the global pose of the gripper for
calculations of the gripped object position
// and move it to be right between the paddles
@@ -489,7 +462,7 @@
hit->SetParent( this );
hit->SetPose( Pose(0,0, -1.0 * geom.size.z
,0) );
- cfg.grip_stack = g_list_prepend(
cfg.grip_stack, hit );
+ cfg.gripped = hit;
// // calculate how far closed we can
get the paddles now
double puckw = hitgeom.size.y;
@@ -500,189 +473,76 @@
}
}
-// int gripper_render_data( stg_model_t* mod, void* userp )
-// {
-// //puts( "gripper render data" );
-// // only draw if someone is using the gripper
-// if( mod->subs < 1 )
-// return 0;
-
-// stg_rtk_fig_t* fig = stg_model_get_fig( mod, "gripper_data_fig" );
+void ModelGripper::DataVisualize( Camera* cam )
+{
+ // only draw if someone is using the gripper
+ if( subs < 1 )
+ return;
-// if( ! fig )
-// {
-// fig = stg_model_fig_create( mod, "gripper_data_fig", "top",
STG_LAYER_GRIPPERDATA );
-// //stg_rtk_fig_color_rgb32( fig, gripper_col );
-// stg_rtk_fig_color_rgb32( fig, 0xFF0000 ); // red
-
-// }
-// else
-// stg_rtk_fig_clear( fig );
+ if( ! showData )
+ return;
-// //printf( "SUBS %d\n", mod->subs );
-
-// stg_gripper_data_t* data = (stg_gripper_data_t*)mod->data;
-// assert(data);
-
-// stg_gripper_config_t *cfg = (stg_gripper_config_t*)mod->cfg;
-// assert(cfg);
+ // outline the sensor lights in black
+ PushColor( 0,0,0,1.0 ); // black
+ glTranslatef( 0,0, geom.size.z * cfg.paddle_size.z );
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
-// stg_geom_t *geom = &mod->geom;
-
-// //stg_rtk_fig_rectangle( gui->data, 0,0,0, geom.size.x, geom.size.y, 0 );
-
-// // different x location for each beam
-// double ibbx = (geom->size.x - cfg->inner_break_beam_inset *
geom->size.x) - geom->size.x/2.0;
-// double obbx = (geom->size.x - cfg->outer_break_beam_inset *
geom->size.x) - geom->size.x/2.0;
-
-// // common y position
-// double bby =
-// (1.0-data->paddle_position) *
((geom->size.y/2.0)-(geom->size.y*cfg->paddle_size.y));
-
-// // size of the paddle indicator lights
-// double led_dx = cfg->paddle_size.y * 0.5 * geom->size.y;
-
-
-// if( data->inner_break_beam )
-// {
-// stg_rtk_fig_rectangle( fig, ibbx, bby+led_dx, 0, led_dx, led_dx, 1 );
-// stg_rtk_fig_rectangle( fig, ibbx, -bby-led_dx, 0, led_dx, led_dx, 1 );
-// }
-// else
-// {
-// stg_rtk_fig_line( fig, ibbx, bby, ibbx, -bby );
-// stg_rtk_fig_rectangle( fig, ibbx, bby+led_dx, 0, led_dx, led_dx, 0 );
-// stg_rtk_fig_rectangle( fig, ibbx, -bby-led_dx, 0, led_dx, led_dx, 0 );
-// }
-
-// if( data->outer_break_beam )
-// {
-// stg_rtk_fig_rectangle( fig, obbx, bby+led_dx, 0, led_dx, led_dx, 1 );
-// stg_rtk_fig_rectangle( fig, obbx, -bby-led_dx, 0, led_dx, led_dx, 1 );
-// }
-// else
-// {
-// stg_rtk_fig_line( fig, obbx, bby, obbx, -bby );
-// stg_rtk_fig_rectangle( fig, obbx, bby+led_dx, 0, led_dx, led_dx, 0 );
-// stg_rtk_fig_rectangle( fig, obbx, -bby-led_dx, 0, led_dx, led_dx, 0 );
-// }
-
-// // draw the contact indicators
-// stg_rtk_fig_rectangle( fig,
-// ((1.0 - cfg->paddle_size.x/2.0) * geom->size.x) -
geom->size.x/2.0,
-// (1.0 - cfg->paddle_position) *
((geom->size.y/2.0)-(geom->size.y*cfg->paddle_size.y)),
-// 0.0,
-// cfg->paddle_size.x * geom->size.x,
-// cfg->paddle_size.y/6.0 * geom->size.y,
-// data->paddle_contacts[0] );
-
-// stg_rtk_fig_rectangle( fig,
-// ((1.0 - cfg->paddle_size.x/2.0) * geom->size.x) -
geom->size.x/2.0,
-// (1.0 - cfg->paddle_position) *
-((geom->size.y/2.0)-(geom->size.y*cfg->paddle_size.y)),
-// 0.0,
-// cfg->paddle_size.x * geom->size.x,
-// cfg->paddle_size.y/6.0 * geom->size.y,
-// data->paddle_contacts[1] );
-
-// //stg_rtk_fig_color_rgb32( fig, gripper_col );
+ // different x location for each beam
+ double ibbx = (geom.size.x - cfg.break_beam_inset[0] * geom.size.x) -
geom.size.x/2.0;
+ double obbx = (geom.size.x - cfg.break_beam_inset[1] * geom.size.x) -
geom.size.x/2.0;
+
+ // common y position
+ double invp = 1.0 - cfg.paddle_position;
+ double bby =
+ invp * ((geom.size.y/2.0)-(geom.size.y*cfg.paddle_size.y));
+
+ // // size of the paddle indicator lights
+ double led_dx = cfg.paddle_size.y * 0.5 * geom.size.y;
+
+ // paddle break beams
+ Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx, led_dx );
+ Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx, led_dx );
+ Gl::draw_centered_rect( obbx, bby+led_dx, led_dx, led_dx );
+ Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx, led_dx );
+
+ // paddle contacts
+ double cx = ((1.0 - cfg.paddle_size.x/2.0) * geom.size.x) -
geom.size.x/2.0;
+ double cy = (geom.size.y/2.0)-(geom.size.y * 0.8 * cfg.paddle_size.y);
+ double plen = cfg.paddle_size.x * geom.size.x;
+ double pwidth = 0.4 * cfg.paddle_size.y * geom.size.y;
+
+ Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth );
+ Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth );
+
+ // if the gripper detects anything, fill the lights in with yellow
+ if( cfg.beam[0] || cfg.beam[1] || cfg.contact[0] || cfg.contact[1] )
+ {
+ PushColor( 1,1,0,1.0 ); // yellow
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+
+ if( cfg.contact[0] )
+ Gl::draw_centered_rect( cx, invp * +cy, plen, pwidth );
+
+ if( cfg.contact[1] )
+ Gl::draw_centered_rect( cx, invp * -cy, plen, pwidth );
+
+ if( cfg.beam[0] )
+ {
+ Gl::draw_centered_rect( ibbx, bby+led_dx, led_dx,
led_dx );
+ Gl::draw_centered_rect( ibbx, -bby-led_dx, led_dx,
led_dx );
+ }
+
+ if( cfg.beam[1] )
+ {
+ Gl::draw_centered_rect( obbx, bby+led_dx, led_dx,
led_dx );
+ Gl::draw_centered_rect( obbx, -bby-led_dx, led_dx,
led_dx );
+ }
-// return 0;
-// }
+ PopColor(); // yellow
+ }
+
+ PopColor(); // black
+ }
-// int gripper_render_cfg( stg_model_t* mod, void* user )
-// {
-// puts( "gripper render cfg" );
-// stg_rtk_fig_t* fig = stg_model_get_fig( mod, "gripper_cfg_fig" );
-
-// if( ! fig )
-// {
-// fig = stg_model_fig_create( mod, "gripper_cfg_fig", "top",
-// STG_LAYER_GRIPPERCONFIG );
-
-// stg_rtk_fig_color_rgb32( fig, stg_lookup_color( STG_GRIPPER_CFG_COLOR
));
-// }
-// else
-// stg_rtk_fig_clear( fig );
-
-// stg_geom_t geom;
-// stg_model_get_geom( mod, &geom );
-
-// // get the config and make sure it's the right size
-// stg_gripper_config_t* cfg = (stg_gripper_config_t*)mod->cfg;
-// assert( mod->cfg_len == sizeof(stg_gripper_config_t) );
-
-// // different x location for each beam
-// double ibbx = (cfg->inner_break_beam_inset) * geom.size.x -
geom.size.x/2.0;
-// double obbx = (cfg->outer_break_beam_inset) * geom.size.x -
geom.size.x/2.0;
-
-// // common y position
-// double bby =
-// (1.0-cfg->paddle_position) *
((geom.size.y/2.0)-(geom.size.y*cfg->paddle_size.y));
-
-// // draw the position of the break beam sensors
-// stg_rtk_fig_rectangle( fig, ibbx, bby, 0, 0.01, 0.01, 0 );
-// stg_rtk_fig_rectangle( fig, obbx, bby, 0, 0.01, 0.01, 0 );
-
-// return 0; //ok
-// }
-
-
-// int gripper_startup( stg_model_t* mod )
-// {
-// PRINT_DEBUG( "gripper startup" );
-// stg_model_set_watts( mod, STG_GRIPPER_WATTS );
-// return 0; // ok
-// }
-
-// int gripper_shutdown( stg_model_t* mod )
-// {
-// PRINT_DEBUG( "gripper shutdown" );
-// stg_model_set_watts( mod, 0 );
-
-// // unrender the break beams & lights
-// stg_model_fig_clear( mod, "gripper_data_fig" );
-// return 0; // ok
-// }
-
-// void stg_print_gripper_config( stg_gripper_config_t* cfg )
-// {
-// char* pstate;
-// switch( cfg->paddles )
-// {
-// case STG_GRIPPER_PADDLE_OPEN: pstate = "OPEN"; break;
-// case STG_GRIPPER_PADDLE_CLOSED: pstate = "CLOSED"; break;
-// case STG_GRIPPER_PADDLE_OPENING: pstate = "OPENING"; break;
-// case STG_GRIPPER_PADDLE_CLOSING: pstate = "CLOSING"; break;
-// default: pstate = "*unknown*";
-// }
-
-// char* lstate;
-// switch( cfg->lift )
-// {
-// case STG_GRIPPER_LIFT_UP: lstate = "UP"; break;
-// case STG_GRIPPER_LIFT_DOWN: lstate = "DOWN"; break;
-// case STG_GRIPPER_LIFT_DOWNING: lstate = "DOWNING"; break;
-// case STG_GRIPPER_LIFT_UPPING: lstate = "UPPING"; break;
-// default: lstate = "*unknown*";
-// }
-
-// printf("gripper state: paddles(%s)[%.2f] lift(%s)[%.2f] stall(%s)\n",
-// pstate, cfg->paddle_position, lstate, cfg->lift_position,
-// cfg->paddles_stalled ? "true" : "false" );
-// }
-
-
-// int gripper_unrender_data( stg_model_t* mod, void* userp )
-// {
-// stg_model_fig_clear( mod, "gripper_data_fig" );
-// return 1; // callback just runs one time
-// }
-
-// int gripper_unrender_cfg( stg_model_t* mod, void* userp )
-// {
-// stg_model_fig_clear( mod, "gripper_cfg_fig" );
-// return 1; // callback just runs one time
-// }
-
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2009-02-13 18:55:58 UTC (rev
7333)
+++ code/stage/trunk/libstage/model_laser.cc 2009-02-15 23:44:46 UTC (rev
7334)
@@ -24,7 +24,6 @@
static const bool DEFAULT_FILLED = true;
static const stg_watts_t DEFAULT_WATTS = 17.5;
static const Size DEFAULT_SIZE( 0.15, 0.15, 0.2 );
-static const stg_meters_t DEFAULT_MINRANGE = 0.0;
static const stg_meters_t DEFAULT_MAXRANGE = 8.0;
static const stg_radians_t DEFAULT_FOV = M_PI;
static const unsigned int DEFAULT_SAMPLES = 180;
@@ -35,6 +34,8 @@
//TODO make instance attempt to register an option (as customvisualizations do)
Option ModelLaser::showLaserData( "Laser scans", "show_laser", "", true, NULL
);
Option ModelLaser::showLaserStrikes( "Laser strikes", "show_laser_strikes",
"", false, NULL );
+Option ModelLaser::showLaserFov( "Laser FOV", "show_laser_fov", "", false,
NULL );
+Option ModelLaser::showLaserBeams( "Laser beams", "show_laser_beams", "",
false, NULL );
/**
@ingroup model
@@ -52,7 +53,6 @@
(
# laser properties
samples 180
- range_min 0.0
range_max 8.0
fov 3.14159
resolution 1
@@ -67,8 +67,6 @@
- samples <int>\n
the number of laser samples per scan
-- range_min <float>\n
- the minimum range reported by the scanner, in meters. The scanner will
detect objects closer than this, but report their range as the minimum.
- range_max <float>\n
the maximum range reported by the scanner, in meters. The scanner will not
detect objects beyond this range.
- fov <float>\n
@@ -84,7 +82,6 @@
data_dirty( true ),
samples( NULL ), // don't allocate sample buffer memory until
Update() is called
sample_count( DEFAULT_SAMPLES ),
- range_min( DEFAULT_MINRANGE ),
range_max( DEFAULT_MAXRANGE ),
fov( DEFAULT_FOV ),
resolution( DEFAULT_RESOLUTION )
@@ -113,6 +110,8 @@
registerOption( &showLaserData );
registerOption( &showLaserStrikes );
+ registerOption( &showLaserFov );
+ registerOption( &showLaserBeams );
}
@@ -128,7 +127,6 @@
void ModelLaser::Load( void )
{
sample_count = wf->ReadInt( wf_entity, "samples", sample_count );
- range_min = wf->ReadLength( wf_entity, "range_min", range_min);
range_max = wf->ReadLength( wf_entity, "range_max", range_max );
fov = wf->ReadAngle( wf_entity, "fov", fov );
resolution = wf->ReadInt( wf_entity, "resolution", resolution );
@@ -149,7 +147,6 @@
{
stg_laser_cfg_t cfg;
cfg.sample_count = sample_count;
- cfg.range_bounds.min = range_min;
cfg.range_bounds.max = range_max;
cfg.fov = fov;
cfg.resolution = resolution;
@@ -159,7 +156,6 @@
void ModelLaser::SetConfig( stg_laser_cfg_t cfg )
{
- range_min = cfg.range_bounds.min;
range_max = cfg.range_bounds.max;
fov = cfg.fov;
resolution = cfg.resolution;
@@ -309,8 +305,8 @@
{
if( ! (samples && sample_count) )
return;
-
- if ( ! (showLaserData || showLaserStrikes) )
+
+ if ( ! (showLaserData || showLaserStrikes || showLaserFov || showLaserBeams
) )
return;
glPushMatrix();
@@ -379,7 +375,43 @@
glDrawArrays( GL_POINTS, 0, sample_count+1 );
PopColor();
}
+
+ if( showLaserFov )
+ {
+ for( unsigned int s=0; s<sample_count; s++ )
+ {
+ double ray_angle = (s * (fov /
(sample_count-1))) - fov/2.0;
+ pts[2*s+2] = (float)(range_max *
cos(ray_angle) );
+ pts[2*s+3] = (float)(range_max *
sin(ray_angle) );
+ }
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+ PushColor( 0, 0, 1, 0.5 );
+ glDrawArrays( GL_POLYGON, 0, sample_count+1 );
+ PopColor();
+ // glPolygonMode(
GL_FRONT_AND_BACK, GL_LINE );
+
+ }
+ if( showLaserBeams )
+ {
+ PushColor( 0, 0, 1, 0.5 );
+ glBegin( GL_LINES );
+
+ for( unsigned int s=0; s<sample_count; s++ )
+ {
+
+ glVertex2f( 0,0 );
+ double ray_angle = (s * (fov /
(sample_count-1))) - fov/2.0;
+ glVertex2f( samples[s].range *
cos(ray_angle),
+
samples[s].range * sin(ray_angle) );
+
+ }
+ glEnd();
+ PopColor();
+ }
+
+
glDepthMask( GL_TRUE );
glEndList();
} // end if ( data_dirty )
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-02-13 18:55:58 UTC (rev 7333)
+++ code/stage/trunk/libstage/stage.hh 2009-02-15 23:44:46 UTC (rev 7334)
@@ -466,6 +466,8 @@
void draw_octagon( float w, float h, float m );
void draw_vector( double x, double y, double z );
void draw_origin( double len );
+ /** Draws a rectangle with center at x,y, with sides of length dx,dy */
+ void draw_centered_rect( float x, float y, float dx, float dy );
}
@@ -2233,7 +2235,8 @@
// LASER MODEL --------------------------------------------------------
-
+
+ // TODO - move these into the class definition, like the gripper
/** laser sample packet
*/
typedef struct
@@ -2261,12 +2264,14 @@
stg_laser_sample_t* samples;
uint32_t sample_count;
- stg_meters_t range_min, range_max;
+ stg_meters_t range_max;
stg_radians_t fov;
uint32_t resolution;
static Option showLaserData;
static Option showLaserStrikes;
+ static Option showLaserFov;
+ static Option showLaserBeams;
public:
static const char* typestr;
@@ -2304,6 +2309,14 @@
{
public:
+ // class Viz : public CustomVisualizer
+// {
+
+
+// };
+
+// static Viz viz;
+
enum paddle_state_t {
PADDLE_OPEN = 0, // default state
PADDLE_CLOSED,
@@ -2326,7 +2339,7 @@
CMD_DOWN
};
-
+
/** gripper configuration
*/
struct config_t
@@ -2339,12 +2352,11 @@
double paddle_position; ///< 0.0 = full open, 1.0 full closed
double lift_position; ///< 0.0 = full down, 1.0 full up
+ Model* gripped;
+
bool paddles_stalled; // true iff some solid object stopped
// the paddles closing or opening
- GList*grip_stack; ///< stack of items gripped
- int grip_stack_size; ///< maximum number of objects in stack,
or -1 for unlimited
-
double close_limit; ///< How far the gripper can close. If <
1.0, the gripper has its mouth full.
bool autosnatch; ///< if true, cycle the gripper through
open-close-up-down automatically
@@ -2353,28 +2365,7 @@
Model* beam[2]; ///< points to a model detected by the beams
Model* contact[2]; ///< pointers to a model detected by the contacts
};
-
-
- /** gripper data packet
- */
- struct data_t
- {
- paddle_state_t paddles;
- lift_state_t lift;
-
- double paddle_position; ///< 0.0 = full open, 1.0 full closed
- double lift_position; ///< 0.0 = full down, 1.0 full up
-
-
- stg_bool_t paddles_stalled; // true iff some solid object stopped
- // the paddles closing or opening
-
- Model* beam[2]; ///< points to a model detected by the beams
- Model* contact[2]; ///< pointers to a model detected by the contacts
-
- int stack_count; ///< number of objects in stack
- };
-
+
private:
virtual void Update();
virtual void DataVisualize( Camera* cam );
@@ -2390,6 +2381,8 @@
Block* paddle_left;
Block* paddle_right;
+ static Option showData;
+
public:
static const char* typestr;
static const Size size;
@@ -2403,13 +2396,11 @@
virtual void Load();
virtual void Save();
- void SetConfig( config_t & newcfg );
-
- /** Returns the static state of the gripper */
+ /** Configure the gripper */
+ void SetConfig( config_t & newcfg ){ this->cfg = cfg; FixBlocks(); }
+
+ /** Returns the state of the gripper .*/
config_t GetConfig(){ return cfg; };
-
- /** Returns the dynamic state of the gripper */
- data_t GetData();
/** Set the current activity of the gripper. */
void SetCommand( cmd_t cmd ) { this->cmd = cmd; }
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2009-02-13 18:55:58 UTC (rev 7333)
+++ code/stage/trunk/worlds/fasr.world 2009-02-15 23:44:46 UTC (rev 7334)
@@ -22,9 +22,9 @@
(
size [ 1379.000 856.000 ]
- center [ 2.468 4.122 ]
+ center [ 0.783 5.056 ]
rotate [ 0 0 ]
- scale 91.508
+ scale 355.764
pcam_loc [ 0 -4.000 2.000 ]
pcam_angle [ 70.000 0 ]
@@ -72,7 +72,7 @@
define autorob pioneer2dx
(
- sicklaser( samples 32 range_max 5 laser_return 2 watts 30 )
+ sicklaser( samples 180 range_max 5 laser_return 2 watts 30 )
ctrl "fasr"
joules 100000
joules_capacity 400000
@@ -151,7 +151,7 @@
puck( pose [ 0.549 2.367 0 0 ] )
puck( pose [ 0.162 2.983 0 0 ] )
puck( pose [ 1.067 3.367 0 0 ] )
-puck( pose [ 1.577 3.488 0 0 ] )
+puck( pose [ 0.320 4.907 0 0 ] )
autorob( pose [5.418 7.478 0 -163.478] joules 300000 )
autorob( pose [7.574 6.269 0 -111.715] joules 100000 )
@@ -165,7 +165,7 @@
autorob( pose [6.451 4.189 0 -61.453] joules 200000 )
autorob( pose [5.060 6.868 0 -61.295] joules 300000 )
-autorob( pose [4.127 5.388 0 -147.713] joules 400000 )
+autorob( pose [1.095 5.276 0 -147.713] joules 400000 )
autorob( pose [4.911 4.552 0 -125.236] joules 100000 )
autorob( pose [3.985 6.474 0 -158.025] joules 200000 )
autorob( pose [5.440 5.317 0 -26.545] joules 300000 )
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Open Source Business Conference (OSBC), March 24-25, 2009, San Francisco, CA
-OSBC tackles the biggest issue in open source: Open Sourcing the Enterprise
-Strategies to boost innovation and cut costs with open source participation
-Receive a $600 discount off the registration fee with the source code: SFAD
http://p.sf.net/sfu/XcvMzF8H
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
