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