Revision: 7591
http://playerstage.svn.sourceforge.net/playerstage/?rev=7591&view=rev
Author: rtv
Date: 2009-04-09 22:51:12 +0000 (Thu, 09 Apr 2009)
Log Message:
-----------
bugfix
Modified Paths:
--------------
code/stage/trunk/examples/ctrl/fasr.cc
code/stage/trunk/libstage/CMakeLists.txt
code/stage/trunk/libstage/canvas.cc
code/stage/trunk/libstage/main.cc
code/stage/trunk/libstage/model_position.cc
code/stage/trunk/libstage/stage.hh
Modified: code/stage/trunk/examples/ctrl/fasr.cc
===================================================================
--- code/stage/trunk/examples/ctrl/fasr.cc 2009-04-09 18:16:27 UTC (rev
7590)
+++ code/stage/trunk/examples/ctrl/fasr.cc 2009-04-09 22:51:12 UTC (rev
7591)
@@ -504,7 +504,7 @@
{
if( strcmp( mod->Token(), "r0" ) == 0 )
{
- const unsigned int dw = 61, dh = 31;
+ const unsigned int dw = 64, dh = 32;
uint8_t* data = new uint8_t[dw*dh*2];
memset( data, 0, sizeof(uint8_t) * dw * dh );
Modified: code/stage/trunk/libstage/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstage/CMakeLists.txt 2009-04-09 18:16:27 UTC (rev
7590)
+++ code/stage/trunk/libstage/CMakeLists.txt 2009-04-09 22:51:12 UTC (rev
7591)
@@ -4,7 +4,6 @@
include_directories(${PROJECT_BINARY_DIR})
set( stageSrcs
- vis_strip.cc
ancestor.cc
block.cc
blockgroup.cc
@@ -35,6 +34,7 @@
stage.hh
texture_manager.cc
typetable.cc
+ vis_strip.cc
waypoint.cc
world.cc
worldfile.cc
Modified: code/stage/trunk/libstage/canvas.cc
===================================================================
--- code/stage/trunk/libstage/canvas.cc 2009-04-09 18:16:27 UTC (rev 7590)
+++ code/stage/trunk/libstage/canvas.cc 2009-04-09 22:51:12 UTC (rev 7591)
@@ -574,13 +574,13 @@
char str[64];
PushColor( 0.15, 0.15, 0.15, 1.0 ); // pale gray
- for( double i = floor(bounds.x.min); i < bounds.x.max; i++)
+ for( double i = ceil(bounds.x.min); i < bounds.x.max; i++)
{
snprintf( str, 16, "%d", (int)i );
Gl::draw_string( i, 0, 0.00, str );
}
- for( double i = floor(bounds.y.min); i < bounds.y.max; i++)
+ for( double i = ceil(bounds.y.min); i < bounds.y.max; i++)
{
snprintf( str, 16, "%d", (int)i );
Gl::draw_string( 0, i, 0.00, str );
@@ -610,7 +610,25 @@
glDisable(GL_TEXTURE_2D);
glEnable(GL_BLEND);
-
+
+// glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+// glTranslatef( 0,0,0.1 );
+// glColor3f( 0, 1.0, 0 );
+
+// glBegin( GL_LINES );
+// for( float x=bounds.x.min; x<=bounds.x.max; x++ )
+// {
+// glVertex2f( ceil(x), bounds.y.min );
+// glVertex2f( ceil(x), bounds.y.max );
+// }
+
+// for( float y=bounds.y.min; y<=bounds.y.max; y++ )
+// {
+// glVertex2f( bounds.x.min, ceil(y) );
+// glVertex2f( bounds.x.max, ceil(y) );
+// }
+// glEnd();
+
glDisable(GL_POLYGON_OFFSET_FILL );
}
Modified: code/stage/trunk/libstage/main.cc
===================================================================
--- code/stage/trunk/libstage/main.cc 2009-04-09 18:16:27 UTC (rev 7590)
+++ code/stage/trunk/libstage/main.cc 2009-04-09 22:51:12 UTC (rev 7591)
@@ -14,12 +14,12 @@
"USAGE: stage [options] [<worldfile>]\n"
"Available [options] are:\n"
" --clock : print simulation time peridically on standard output\n"
- " -c : print simulation time peridically on standard output\n"
+ " -c : short for --clock\n"
" --gui : run without a GUI\n"
- " -g : run without a GUI\n"
- " --help : print this message.\n"
- " -h : print this message.\n"
- " -? : print this message.\n"
+ " -g : short for --gui\n"
+ " --help : print this message\n"
+ " -h : short for --help\n"
+ " -? : short for --help\n"
" If <worldfile> is not specified, Stage starts with a file selector dialog";
/* options descriptor */
Modified: code/stage/trunk/libstage/model_position.cc
===================================================================
--- code/stage/trunk/libstage/model_position.cc 2009-04-09 18:16:27 UTC (rev
7590)
+++ code/stage/trunk/libstage/model_position.cc 2009-04-09 22:51:12 UTC (rev
7591)
@@ -19,7 +19,6 @@
//#define DEBUG
#include "stage.hh"
-#include "option.hh"
#include "worldfile.hh"
using namespace Stg;
@@ -86,14 +85,13 @@
const stg_position_localization_mode_t POSITION_LOCALIZATION_DEFAULT =
STG_POSITION_LOCALIZATION_GPS;
const stg_position_drive_mode_t POSITION_DRIVE_DEFAULT =
STG_POSITION_DRIVE_DIFFERENTIAL;
-Option ModelPosition::showCoords( "Position Coordinates", "show_coords", "",
false, NULL );
-Option ModelPosition::showWaypoints( "Position Waypoints", "show_waypoints",
"", true, NULL );
-
ModelPosition::ModelPosition( World* world,
Model* parent )
: Model( world, parent, MODEL_TYPE_POSITION ),
waypoints( NULL ),
- waypoint_count( 0 )
+ waypoint_count( 0 ),
+ wpvis(),
+ posevis()
{
PRINT_DEBUG2( "Constructing ModelPosition %d (%s)\n",
id, typestr );
@@ -132,8 +130,8 @@
drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_A -
STG_POSITION_INTEGRATION_ERROR_MAX_A/2.0;
- RegisterOption( &showCoords );
- RegisterOption( &showWaypoints );
+ AddVisualizer( &wpvis, true );
+ AddVisualizer( &posevis, false );
}
@@ -238,10 +236,6 @@
PRINT_ERR1( "no localization mode string specified for model \"%s\"",
this->token );
}
-
- // TODO
- // we've probably poked the localization data, so we must let people know
- //model_change( mod, &mod->data );
}
void ModelPosition::Update( void )
@@ -252,7 +246,7 @@
Velocity vel;
memset( &vel, 0, sizeof(Velocity) );
- if( this->subs ) // no driving if noone is subscribed
+ if( this->subs ) // no driving if noone is subscribed
{
switch( control_mode )
{
@@ -265,7 +259,6 @@
this->goal.y,
this->goal.a );
-
switch( drive_mode )
{
case STG_POSITION_DRIVE_DIFFERENTIAL:
@@ -334,8 +327,6 @@
// start out with no velocity
Velocity calc;
- memset( &calc, 0, sizeof(calc));
-
double close_enough = 0.02; // fudge factor
// if we're at the right spot
@@ -388,16 +379,16 @@
// simple model of power consumption
watts = STG_POSITION_WATTS +
- fabs(vel.x) * STG_POSITION_WATTS_KGMS * mass +
- fabs(vel.y) * STG_POSITION_WATTS_KGMS * mass +
- fabs(vel.a) * STG_POSITION_WATTS_KGMS * mass;
+ fabs(vel.x) * STG_POSITION_WATTS_KGMS * mass +
+ fabs(vel.y) * STG_POSITION_WATTS_KGMS * mass +
+ fabs(vel.a) * STG_POSITION_WATTS_KGMS * mass;
//PRINT_DEBUG4( "model %s velocity (%.2f %.2f %.2f)",
// this->token,
// this->velocity.x,
// this->velocity.y,
// this->velocity.a );
-
+
this->SetVelocity( vel );
}
@@ -421,21 +412,21 @@
case STG_POSITION_LOCALIZATION_ODOM:
{
- // integrate our velocities to get an 'odometry' position estimate.
- double dt = this->world->GetSimInterval()/1e6;
-
- est_pose.a = normalize( est_pose.a + (vel.a * dt) * (1.0
+integration_error.a) );
-
- double cosa = cos(est_pose.a);
- double sina = sin(est_pose.a);
- double dx = (vel.x * dt) * (1.0 + integration_error.x );
- double dy = (vel.y * dt) * (1.0 + integration_error.y );
-
- est_pose.x += dx * cosa + dy * sina;
- est_pose.y -= dy * cosa - dx * sina;
+ // integrate our velocities to get an 'odometry' position
estimate.
+ double dt = this->world->GetSimInterval()/1e6;
+
+ est_pose.a = normalize( est_pose.a + (vel.a * dt) * (1.0
+integration_error.a) );
+
+ double cosa = cos(est_pose.a);
+ double sina = sin(est_pose.a);
+ double dx = (vel.x * dt) * (1.0 + integration_error.x );
+ double dy = (vel.y * dt) * (1.0 + integration_error.y );
+
+ est_pose.x += dx * cosa + dy * sina;
+ est_pose.y -= dy * cosa - dx * sina;
}
break;
-
+
default:
PRINT_ERR2( "unknown localization mode %d for model %s\n",
localization_mode, this->token );
@@ -582,98 +573,108 @@
return replaced;
}
-void ModelPosition::DataVisualize( Camera* cam )
+ModelPosition::PoseVis::PoseVis()
+ : Visualizer( "Position coordinates", "show_position_coords" )
+{}
+
+void ModelPosition::PoseVis::Visualize( Model* mod, Camera* cam )
{
- if( showWaypoints && waypoints && waypoint_count )
- DrawWaypoints();
+ ModelPosition* pos = dynamic_cast<ModelPosition*>(mod);
- if( showCoords )
- {
- // vizualize my estimated pose
- glPushMatrix();
+ // 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 );
-
- glEnable (GL_LINE_STIPPLE);
- glLineStipple (3, 0xAAAA);
-
- PushColor( 1,0,0,0.5 );
- glBegin( GL_LINE_STRIP );
- glVertex2f( 0,0 );
- glVertex2f( est_pose.x, 0 );
- glVertex2f( est_pose.x, est_pose.y );
- glEnd();
+ // back into global coords
+ Gl::pose_inverse_shift( pos->GetGlobalPose() );
+
+ Gl::pose_shift( pos->est_origin );
+ pos->PushColor( 1,0,0,1 ); // origin in red
+ Gl::draw_origin( 0.5 );
- glDisable(GL_LINE_STIPPLE);
+ glEnable (GL_LINE_STIPPLE);
+ glLineStipple (3, 0xAAAA);
+
+ pos->PushColor( 1,0,0,0.5 );
+ glBegin( GL_LINE_STRIP );
+ glVertex2f( 0,0 );
+ glVertex2f( pos->est_pose.x, 0 );
+ glVertex2f( pos->est_pose.x, pos->est_pose.y );
+ glEnd();
+
+ glDisable(GL_LINE_STIPPLE);
+
+ char label[64];
+ snprintf( label, 64, "x:%.3f", pos->est_pose.x );
+ Gl::draw_string( pos->est_pose.x / 2.0, -0.5, 0, label );
+
+ snprintf( label, 64, "y:%.3f", pos->est_pose.y );
+ Gl::draw_string( pos->est_pose.x + 0.5 , pos->est_pose.y / 2.0, 0, (const
char*)label );
+
+ pos->PopColor();
+
+ Gl::pose_shift( pos->est_pose );
+ pos->PushColor( 0,1,0,1 ); // pose in green
+ Gl::draw_origin( 0.5 );
+ pos->PopColor();
+
+ Gl::pose_shift( pos->geom.pose );
+ pos->PushColor( 0,0,1,1 ); // offset in blue
+ Gl::draw_origin( 0.5 );
+ pos->PopColor();
+
+ double r,g,b,a;
+ stg_color_unpack( pos->color, &r, &g, &b, &a );
+ pos->PushColor( r, g, b, 0.5 );
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+ pos->blockgroup.DrawFootPrint( pos->geom );
+ pos->PopColor();
+
+ glPopMatrix();
+}
- char label[64];
- snprintf( label, 64, "x:%.3f", est_pose.x );
- Gl::draw_string( est_pose.x / 2.0, -0.5, 0, label );
- snprintf( label, 64, "y:%.3f", est_pose.y );
- Gl::draw_string( est_pose.x + 0.5 , est_pose.y / 2.0, 0, (const
char*)label );
-
- PopColor();
+ModelPosition::WaypointVis::WaypointVis()
+ : Visualizer( "Position waypoints", "show_position_waypoints" )
+{}
- Gl::pose_shift( est_pose );
- PushColor( 0,1,0,1 ); // pose in green
- Gl::draw_origin( 0.5 );
- PopColor();
+void ModelPosition::WaypointVis::Visualize( Model* mod, Camera* cam )
+{
+ ModelPosition* pos = dynamic_cast<ModelPosition*>(mod);
- 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( geom );
- PopColor();
+ Waypoint* waypoints = pos->waypoints;
+ unsigned int waypoint_count = pos->waypoint_count;
- glPopMatrix();
- }
+ if( (waypoints == NULL) || (waypoint_count < 1) )
+ return;
- // inherit more viz
- Model::DataVisualize( cam );
-}
-
-void ModelPosition::DrawWaypoints()
-{
glPointSize( 5 );
glPushMatrix();
- PushColor( color );
+ pos->PushColor( pos->color );
- Gl::pose_inverse_shift( pose );
- Gl::pose_shift( est_origin );
+ Gl::pose_inverse_shift( pos->pose );
+ Gl::pose_shift( pos->est_origin );
glTranslatef( 0,0,0.02 );
-
+
// draw waypoints
for( unsigned int i=0; i < waypoint_count; i++ )
- waypoints[i].Draw();
-
+ waypoints[i].Draw();
+
// draw lines connecting the waypoints
if( waypoint_count > 1 )
{
glBegin( GL_LINES );
for( unsigned int i=1; i < waypoint_count; i++ )
- {
- glVertex2f( waypoints[i].pose.x, waypoints[i].pose.y );
- glVertex2f( waypoints[i-1].pose.x, waypoints[i-1].pose.y );
- }
-
+ {
+ glVertex2f( waypoints[i].pose.x, waypoints[i].pose.y
);
+ glVertex2f( waypoints[i-1].pose.x,
waypoints[i-1].pose.y );
+ }
+
glEnd();
}
- PopColor();
+ pos->PopColor();
glPopMatrix();
}
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-04-09 18:16:27 UTC (rev 7590)
+++ code/stage/trunk/libstage/stage.hh 2009-04-09 22:51:12 UTC (rev 7591)
@@ -373,7 +373,6 @@
Bounds z;
} stg_bounds3d_t;
-
/** Define a three-dimensional bounding box, initialized to zero */
typedef struct
{
@@ -568,7 +567,6 @@
// MACROS ------------------------------------------------------
// Some useful macros
-
/** Look up the color in the X11 database. (i.e. transform color
name to color value). If the color is not found in the
database, a bright red color (0xF00) will be returned instead.
@@ -2768,12 +2766,7 @@
Waypoint* waypoints;
uint32_t waypoint_count;
- void DrawWaypoints();
- private:
- static Option showCoords;
- static Option showWaypoints;
-
public:
static const char* typestr;
// constructor
@@ -2787,11 +2780,29 @@
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 );
+ class WaypointVis : public Visualizer
+ {
+ public:
+ WaypointVis();
+ virtual ~WaypointVis( void ){}
+ virtual void Visualize( Model* mod, Camera* cam );
+ };
+
+ WaypointVis wpvis;
+
+ class PoseVis : public Visualizer
+ {
+ public:
+ PoseVis();
+ virtual ~PoseVis( void ){}
+ virtual void Visualize( Model* mod, Camera* cam );
+ };
+
+ PoseVis posevis;
+
/** Set the current pose estimate.*/
void SetOdom( Pose odom );
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:
High Quality Requirements in a Collaborative Environment.
Download a free trial of Rational Requirements Composer Now!
http://p.sf.net/sfu/www-ibm-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
