Revision: 7181 http://playerstage.svn.sourceforge.net/playerstage/?rev=7181&view=rev Author: rtv Date: 2008-11-28 01:21:44 +0000 (Fri, 28 Nov 2008)
Log Message: ----------- moved waypoints to position model only, and added coord system viz Modified Paths: -------------- code/stage/trunk/examples/ctrl/fasr.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_ranger.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 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/examples/ctrl/fasr.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -27,7 +27,6 @@ }; - typedef struct { StgModelPosition* pos; @@ -77,6 +76,21 @@ robot->sink = mod->GetWorld()->GetModel( "sink" ); assert(robot->sink); + + const int waypoint_count = 100; + Waypoint* waypoints = new Waypoint[waypoint_count]; + + for( int i=0; i<waypoint_count; i++ ) + { + waypoints[i].pose.x = i* 0.1; + waypoints[i].pose.y = drand48() * 4.0; + waypoints[i].pose.z = 0; + waypoints[i].pose.a = normalize( i/10.0 ); + waypoints[i].color = stg_color_pack( 0,0,1,0 ); + } + + robot->pos->SetWaypoints( waypoints, waypoint_count ); + return 0; //ok } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/canvas.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -762,7 +762,7 @@ DrawBoundingBoxes(); // TODO - finish this properly - LISTMETHOD( models_sorted, StgModel*, DrawWaypoints ); + //LISTMETHOD( models_sorted, StgModel*, DrawWaypoints ); // MOTION BLUR if( showBlur ) Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/gl.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -9,11 +9,18 @@ } // transform the current coordinate frame by the given pose -void Stg::gl_pose_shift( stg_pose_t* pose ) +void Stg::gl_pose_shift( const stg_pose_t &pose ) { - gl_coord_shift( pose->x, pose->y, pose->z, pose->a ); + gl_coord_shift( pose.x, pose.y, pose.z, pose.a ); } +void Stg::gl_pose_inverse_shift( const stg_pose_t &pose ) +{ + gl_coord_shift( 0,0,0, -pose.a ); + gl_coord_shift( -pose.x, -pose.y, -pose.z, 0 ); +} + + // TODO - this could be faster, but we don't draw a lot of text void Stg::gl_draw_string( float x, float y, float z, const char *str ) { @@ -45,7 +52,21 @@ glEnd(); } +void Stg::gl_draw_vector( double x, double y, double z ) +{ + glBegin( GL_LINES ); + glVertex3f( 0,0,0 ); + glVertex3f( x,y,z ); + glEnd(); +} +void Stg::gl_draw_origin( double len ) +{ + gl_draw_vector( len,0,0 ); + gl_draw_vector( 0,len,0 ); + gl_draw_vector( 0,0,len ); +} + void Stg::gl_draw_grid( stg_bounds3d_t vol ) { glBegin(GL_LINES); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/model.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -194,9 +194,7 @@ wf_entity(0), has_default_block( true ), map_caches_are_invalid( true ), - thread_safe( false ), - waypoints( NULL ), - waypoint_count( 0 ) + thread_safe( false ) { assert( modelsbyid ); assert( world ); @@ -581,23 +579,6 @@ return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); } -// stg_point3_t StgModel::LocalToGlobal( stg_point3_t point ) -// { -// stg_pose_t pose; -// pose.x = point.x; -// pose.y = point.y; -// pose.z = point.z; -// pose.a = 0; - -// pose = LocalToGlobal( pose ); - -// point.x = pose.x; -// point.y = pose.y; -// point.z = pose.z; - -// return point; -// } - void StgModel::MapWithChildren() { UnMap(); @@ -617,33 +598,6 @@ ((StgModel*)it->data)->UnMapWithChildren(); } -// given an input point array in model local coordinates, return -// an array with the same points in global coordinates. caller must -// delete[] the points. -// stg_point_t* StgModel::LocalToGlobal( double scalex, -// double scaley, -// stg_point_t pts[], -// uint32_t pt_count ) -// { -// stg_point_t* glob = new stg_point_t[pt_count]; - -// stg_pose_t global_pose = GetGlobalPose(); - -// for( int p=0; p<pt_count; p++ ) -// { -// stg_pose_t local( pts[p].x * scalex, -// pts[p].y * scaley, -// 0, 0 ); -// stg_pose_t global = pose_sum( global_pose, local ); - -// glob[p].x = global.x; -// glob[p].y = global.y; -// } - -// return glob; -// } - - void StgModel::Map() { //PRINT_DEBUG1( "%s.Map()", token ); @@ -773,7 +727,7 @@ glRotatef( rtod(pose.a), 0,0,1 ); - gl_pose_shift( &geom.pose ); + gl_pose_shift( geom.pose ); double dx = geom.size.x / 2.0 * 1.6; double dy = geom.size.y / 2.0 * 1.6; @@ -804,8 +758,8 @@ stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i ); glPushMatrix(); - gl_pose_shift( &checkpoint->pose ); - gl_pose_shift( &geom.pose ); + gl_pose_shift( checkpoint->pose ); + gl_pose_shift( geom.pose ); stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); PushColor( r, g, b, 0.1 ); @@ -926,7 +880,7 @@ void StgModel::DrawBlocks( ) { - gl_pose_shift( &geom.pose ); + gl_pose_shift( geom.pose ); blockgroup.CallDisplayList( this ); } @@ -940,7 +894,7 @@ void StgModel::DrawBoundingBox() { - gl_pose_shift( &geom.pose ); + gl_pose_shift( geom.pose ); PushColor( color ); @@ -985,7 +939,7 @@ if( parent ) glTranslatef( 0,0, parent->geom.size.z ); - gl_pose_shift( &pose ); + gl_pose_shift( pose ); } void StgModel::PopCoords() @@ -1677,36 +1631,3 @@ { blockgroup.UnMap(); } - - -/** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ -Waypoint* StgModel::SetWaypoints( Waypoint* wps, uint32_t count ) -{ - Waypoint* replaced = waypoints; - - waypoints = wps; - waypoint_count = count; - - return replaced; -} - - -void StgModel::DrawWaypoints() -{ - if( waypoints && waypoint_count ) - { - //PushLocalCoords(); - - PushColor( color ); - glPointSize( 3 ); - - // puts( "drawing wps" ); - - for( unsigned int i=0; i < waypoint_count; i++ ) - waypoints[i].Draw(); - - //PopCoords(); - // restore a sensible drawing color - PopColor(); - } -} Modified: code/stage/trunk/libstage/model_blinkenlight.cc =================================================================== --- code/stage/trunk/libstage/model_blinkenlight.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/model_blinkenlight.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -56,7 +56,11 @@ StgModelBlinkenlight::StgModelBlinkenlight( StgWorld* world, StgModel* parent ) - : StgModel( world, parent, MODEL_TYPE_BLINKENLIGHT ) + : StgModel( world, parent, MODEL_TYPE_BLINKENLIGHT ), + dutycycle( 1.0 ), + enabled( true ), + period( 1000 ), + on( true ) { PRINT_DEBUG2( "Constructing StgModelBlinkenlight %d (%s)\n", id, typestr ); @@ -73,10 +77,6 @@ geom.size.z = 0.02; this->SetGeom( geom ); - this->dutycycle = 1.0; - this->enabled = true; - this->period = 1000; - this->on = true; this->Startup(); @@ -108,17 +108,10 @@ void StgModelBlinkenlight::DataVisualize( Camera* cam ) { + // TODO XX if( on && showBlinkenData ) { - glPushMatrix(); - // move into this model's local coordinate frame - gl_pose_shift( &this->pose ); - gl_pose_shift( &this->geom.pose ); - glLineWidth( 3 ); - // TODO //LISTMETHOD( this->blocks, StgBlock*, Draw ); - glLineWidth( 1 ); - glPopMatrix(); // drop out of local coords } } Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/model_laser.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -309,6 +309,8 @@ glPushMatrix(); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + // we only regenerate the list if there's new data if( 1 ) // data_dirty ) // TODO - hmm, why doesn't this work? { Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/model_position.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -40,13 +40,13 @@ position ( # position properties - drive "" - localization "" + drive "diff" + localization "gps" localization_origin [ <defaults to model's start pose> ] # odometry error model parameters, # only used if localization is set to "odom" - odom_error [0.03 0.03 0.05] + odom_error [0.03 0.03 0.00 0.05] # model properties ) @@ -84,7 +84,9 @@ StgModelPosition::StgModelPosition( StgWorld* world, StgModel* parent ) - : StgModel( world, parent, MODEL_TYPE_POSITION ) + : StgModel( world, parent, MODEL_TYPE_POSITION ), + waypoints( NULL ), + waypoint_count( 0 ) { PRINT_DEBUG2( "Constructing StgModelPosition %d (%s)\n", id, typestr ); @@ -178,14 +180,13 @@ { est_origin.x = wf->ReadTupleLength( wf_entity, "localization_origin", 0, est_origin.x ); est_origin.y = wf->ReadTupleLength( wf_entity, "localization_origin", 1, est_origin.y ); - est_origin.a = wf->ReadTupleAngle( wf_entity, "localization_origin", 2, est_origin.a ); + est_origin.z = wf->ReadTupleLength( wf_entity, "localization_origin", 2, est_origin.z ); + est_origin.a = wf->ReadTupleAngle( wf_entity, "localization_origin", 3, est_origin.a ); // compute our localization pose based on the origin and true pose stg_pose_t gpose = this->GetGlobalPose(); est_pose.a = normalize( gpose.a - est_origin.a ); - //double cosa = cos(est_pose.a); - //double sina = sin(est_pose.a); double cosa = cos(est_origin.a); double sina = sin(est_origin.a); double dx = gpose.x - est_origin.x; @@ -204,8 +205,10 @@ wf->ReadTupleLength( wf_entity, "odom_error", 0, integration_error.x ); integration_error.y = wf->ReadTupleLength( wf_entity, "odom_error", 1, integration_error.y ); + integration_error.z = + wf->ReadTupleLength( wf_entity, "odom_error", 2, integration_error.z ); integration_error.a - = wf->ReadTupleAngle( wf_entity, "odom_error", 2, integration_error.a ); + = wf->ReadTupleAngle( wf_entity, "odom_error", 3, integration_error.a ); } // choose a localization model @@ -400,7 +403,6 @@ stg_pose_t gpose = this->GetGlobalPose(); est_pose.a = normalize( gpose.a - est_origin.a ); - //est_pose.a =0;// normalize( gpose.a - est_origin.a ); double cosa = cos(est_origin.a); double sina = sin(est_origin.a); double dx = gpose.x - est_origin.x; @@ -425,7 +427,6 @@ est_pose.x += dx * cosa + dy * sina; est_pose.y -= dy * cosa - dx * sina; - } break; @@ -448,8 +449,6 @@ PRINT_DEBUG( "position startup" ); this->SetWatts( STG_POSITION_WATTS ); - - //stg_model_position_odom_reset( mod ); } void StgModelPosition::Shutdown( void ) @@ -544,3 +543,74 @@ est_origin.a = da; } +/** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ +Waypoint* StgModelPosition::SetWaypoints( Waypoint* wps, uint32_t count ) +{ + Waypoint* replaced = waypoints; + + waypoints = wps; + waypoint_count = count; + + return replaced; +} + +void StgModelPosition::DataVisualize( Camera* cam ) +{ + if( waypoints && waypoint_count ) + DrawWaypoints(); + + // vizualize my estimated pose + glPushMatrix(); + + // back into global coords + gl_pose_inverse_shift( GetGlobalPose() ); + + gl_pose_shift( est_origin ); + PushColor( 1,0,0,1 ); // origin in red + gl_draw_origin( 0.5 ); + PopColor(); + + gl_pose_shift( est_pose ); + PushColor( 0,1,0,1 ); // pose in green + gl_draw_origin( 0.5 ); + PopColor(); + + gl_pose_shift( geom.pose ); + PushColor( 0,0,1,1 ); // offset in blue + gl_draw_origin( 0.5 ); + PopColor(); + + double r,g,b,a; + stg_color_unpack( color, &r, &g, &b, &a ); + PushColor( r, g, b, 0.5 ); + + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + blockgroup.DrawFootPrint(); + PopColor(); + + glPopMatrix(); +} + +void StgModelPosition::DrawWaypoints() +{ + glPointSize( 3 ); + + // TRUE + glPushMatrix(); + PushColor( color ); + + gl_pose_inverse_shift( pose ); + gl_pose_shift( est_origin ); + + for( unsigned int i=0; i < waypoint_count; i++ ) + waypoints[i].Draw(); + + PopColor(); + glPopMatrix(); + + /* printf( "pose [%.2f %.2f %.2f %.2f] est_pose [%.2f %.2f %.2f %.2f] est_origin [%.2f %.2f %.2f %.2f]\n", + pose.x, pose.y, pose.z, pose.a, + est_pose.x, est_pose.y, est_pose.z, est_pose.a, + est_origin.x, est_origin.y, est_origin.z, est_origin.a ); + */ +} Modified: code/stage/trunk/libstage/model_ranger.cc =================================================================== --- code/stage/trunk/libstage/model_ranger.cc 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/model_ranger.cc 2008-11-28 01:21:44 UTC (rev 7181) @@ -330,6 +330,7 @@ if( showRangerTransducers ) { + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); PushColor( 0,0,0,1 ); for( unsigned int s=0; s<sensor_count; s++ ) @@ -384,6 +385,7 @@ } // draw the filled triangles in transparent pale green + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); glDepthMask( GL_FALSE ); PushColor( 0, 1, 0, 0.1 ); glVertexPointer( 3, GL_FLOAT, 0, pts ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/libstage/stage.hh 2008-11-28 01:21:44 UTC (rev 7181) @@ -615,7 +615,8 @@ void RegisterModels(); void gl_draw_grid( stg_bounds3d_t vol ); - void gl_pose_shift( stg_pose_t* pose ); + void gl_pose_shift( const stg_pose_t &pose ); + void gl_pose_inverse_shift( const stg_pose_t &pose ); void gl_coord_shift( double x, double y, double z, double a ); class StgFlag @@ -632,6 +633,8 @@ void gl_draw_string( float x, float y, float z, const char *string); void gl_speech_bubble( float x, float y, float z, const char* str ); void gl_draw_octagon( float w, float h, float m ); + void gl_draw_vector( double x, double y, double z ); + void gl_draw_origin( double len ); typedef int(*stg_model_callback_t)(StgModel* mod, void* user ); typedef int(*stg_cell_callback_t)(Cell* cell, void* user ); @@ -1303,10 +1306,6 @@ GMutex* access_mutex; - Waypoint* waypoints; - uint32_t waypoint_count; - void DrawWaypoints(); - public: void Lock() { @@ -1562,9 +1561,6 @@ bool IsRelated( StgModel* mod2 ); - /** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ - Waypoint* SetWaypoints( Waypoint* wps, uint32_t count ); - /** get the pose of a model in the global CS */ stg_pose_t GetGlobalPose(); @@ -2464,12 +2460,19 @@ /// %StgModelPosition class class StgModelPosition : public StgModel { + friend class StgCanvas; + private: stg_pose_t goal; //< the current velocity or pose to reach, depending on the value of control_mode stg_position_control_mode_t control_mode; stg_position_drive_mode_t drive_mode; stg_position_localization_mode_t localization_mode; ///< global or local mode stg_velocity_t integration_error; ///< errors to apply in simple odometry model + + Waypoint* waypoints; + uint32_t waypoint_count; + void DrawWaypoints(); + public: static const char* typestr; // constructor @@ -2482,7 +2485,12 @@ virtual void Shutdown(); virtual void Update(); virtual void Load(); + + virtual void DataVisualize( Camera* cam ); + /** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ + Waypoint* SetWaypoints( Waypoint* wps, uint32_t count ); + /** Set the current pose estimate.*/ void SetOdom( stg_pose_t odom ); Modified: code/stage/trunk/worlds/fasr.world =================================================================== --- code/stage/trunk/worlds/fasr.world 2008-11-27 16:37:06 UTC (rev 7180) +++ code/stage/trunk/worlds/fasr.world 2008-11-28 01:21:44 UTC (rev 7181) @@ -7,8 +7,8 @@ include "sick.inc" interval_sim 100 # simulation timestep in milliseconds -interval_real 0 # real-time interval between simulation updates in milliseconds -paused 0 +interval_real 50 # real-time interval between simulation updates in milliseconds +paused 1 resolution 0.02 @@ -29,7 +29,7 @@ pcam_angle [ 70.000 0 ] pcam_on 0 - show_data 0 + show_data 1 show_flags 1 interval 50 @@ -64,29 +64,32 @@ ( sicklaser( samples 32 range_max 5 laser_return 2 ) ctrl "fasr" + + localization "odom" ) autorob( pose [5.099 4.804 0 -73.937] ) -autorob( pose [6.471 5.304 0 14.941] ) -autorob( pose [5.937 4.858 0 -147.503] ) -autorob( pose [7.574 6.269 0 -111.715] ) -autorob( pose [5.664 5.938 0 -51.799] ) -autorob( pose [7.016 6.428 0 -128.279] ) -autorob( pose [5.911 4.040 0 -97.047] ) -autorob( pose [4.909 6.097 0 -44.366] ) -autorob( pose [6.898 4.775 0 -117.576] ) -autorob( pose [7.012 5.706 0 129.497] ) +#autorob( pose [5.099 4.804 0 -73.937] localization_origin [5.099 4.804 0 -73.937] ) +#autorob( pose [6.471 5.304 0 14.941] ) +#autorob( pose [5.937 4.858 0 -147.503] ) +#autorob( pose [7.574 6.269 0 -111.715] ) +#autorob( pose [5.664 5.938 0 -51.799] ) +#autorob( pose [7.016 6.428 0 -128.279] ) +#autorob( pose [5.911 4.040 0 -97.047] ) +#autorob( pose [4.909 6.097 0 -44.366] ) +#autorob( pose [6.898 4.775 0 -117.576] ) +#autorob( pose [7.012 5.706 0 129.497] ) -autorob( pose [6.616 6.893 0 170.743] ) -autorob( pose [6.451 4.189 0 -61.453] ) -autorob( pose [5.098 6.788 0 -61.295] ) -autorob( pose [4.374 5.163 0 -90.417] ) -autorob( pose [4.999 4.230 0 -42.157] ) -autorob( pose [4.331 4.217 0 -95.000] ) -autorob( pose [5.440 5.317 0 -26.545] ) -autorob( pose [7.518 6.973 0 163.239] ) -autorob( pose [7.559 4.764 0 -139.066] ) -autorob( pose [4.839 3.595 0 -179.567] ) +#autorob( pose [6.616 6.893 0 170.743] ) +#autorob( pose [6.451 4.189 0 -61.453] ) +#autorob( pose [5.098 6.788 0 -61.295] ) +#autorob( pose [4.374 5.163 0 -90.417] ) +#autorob( pose [4.999 4.230 0 -42.157] ) +#autorob( pose [4.331 4.217 0 -95.000] ) +#autorob( pose [5.440 5.317 0 -26.545] ) +#autorob( pose [7.518 6.973 0 163.239] ) +#autorob( pose [7.559 4.764 0 -139.066] ) +#autorob( pose [4.839 3.595 0 -179.567] ) #autorob( pose [7.122 4.175 0 -31.440] ) #autorob( pose [6.203 6.963 0 2.937] ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------- This SF.Net email is sponsored by the Moblin Your Move Developer's challenge Build the coolest Linux based applications with Moblin SDK & win great prizes Grand prize is a trip for two to an Open Source event anywhere in the world http://moblin-contest.org/redirect.php?banner_id=100&url=/ _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit