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
