Update of /cvsroot/playerstage/code/stage/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv9681/src
Modified Files:
Tag: opengl
Makefile.am block.cc gui_gl.c matrix.c model.cc model_laser.cc
model_load.cc model_position.cc p_driver.cc p_position.cc
raytrace.cc stage_internal.h stest.cc world.cc
Log Message:
laser works. now working on position
Index: stest.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Attic/stest.cc,v
retrieving revision 1.1.2.1
retrieving revision 1.1.2.2
diff -C2 -d -r1.1.2.1 -r1.1.2.2
*** stest.cc 8 Jul 2007 01:44:09 -0000 1.1.2.1
--- stest.cc 13 Jul 2007 05:48:31 -0000 1.1.2.2
***************
*** 47,53 ****
snprintf( lasername, 63, "%s.laser:0", robotname );
- char sonarname[64];
- snprintf( sonarname, 63, "%s.ranger:0", robotname );
-
StgModel* position = stg_world_model_name_lookup( world, robotname );
assert(position);
--- 47,50 ----
***************
*** 55,68 ****
StgModelLaser* laser = (StgModelLaser*)stg_world_model_name_lookup( world,
lasername );
assert(laser);
- //StgModelL* ranger = stg_world_model_name_lookup( world, sonarname );
// subscribe to the laser - starts it collecting data
position->Subscribe();
laser->Subscribe();
- //ranger->Subscribe();
position->Print( "Subscribed to model" );
laser->Print( "Subscribed to model" );
- //ranger->Print( "Subscribed to model" );
printf( "Starting world clock..." ); fflush(stdout);
--- 52,62 ----
Index: model_laser.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Attic/model_laser.cc,v
retrieving revision 1.1.2.4
retrieving revision 1.1.2.5
diff -C2 -d -r1.1.2.4 -r1.1.2.5
*** model_laser.cc 10 Jul 2007 02:18:52 -0000 1.1.2.4
--- model_laser.cc 13 Jul 2007 05:48:31 -0000 1.1.2.5
***************
*** 162,171 ****
StgModel::Update();
! puts( "LASER UPDATE" );
- // no work to do if we're unsubscribed
- if( this->subs < 1 )
- return;
-
stg_laser_config_t* cfg = (stg_laser_config_t*)this->cfg;
assert(cfg);
--- 162,167 ----
StgModel::Update();
! //puts( "LASER UPDATE" );
stg_laser_config_t* cfg = (stg_laser_config_t*)this->cfg;
assert(cfg);
***************
*** 290,307 ****
// start consuming power
this->SetWatts( STG_DEFAULT_LASER_WATTS );
-
- // install the update function
- //stg_model_add_update_callback( mod, laser_update, NULL );
}
void StgModelLaser::Shutdown( void )
{
- StgModel::Shutdown();
PRINT_DEBUG( "laser shutdown" );
- // remove the update function
- //stg_model_remove_update_callback( mod, laser_update );
-
// stop consuming power
this->SetWatts( 0 );
--- 286,296 ----
***************
*** 309,323 ****
// clear the data - this will unrender it too
this->SetData( NULL, 0 );
}
- // void StgModelLaser::Draw( void )
- // {
- // StgModel::Draw();
- // }
-
void StgModelLaser::GuiGenerateData( void )
{
! puts ("GL laser data" );
glNewList( this->dl_data, GL_COMPILE );
--- 298,309 ----
// clear the data - this will unrender it too
this->SetData( NULL, 0 );
+
+ StgModel::Shutdown();
}
void StgModelLaser::GuiGenerateData( void )
{
! //puts ("GL laser data" );
glNewList( this->dl_data, GL_COMPILE );
Index: model.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Attic/model.cc,v
retrieving revision 1.1.2.4
retrieving revision 1.1.2.5
diff -C2 -d -r1.1.2.4 -r1.1.2.5
*** model.cc 10 Jul 2007 02:15:47 -0000 1.1.2.4
--- model.cc 13 Jul 2007 05:48:31 -0000 1.1.2.5
***************
*** 38,42 ****
//extern int _stg_disable_gui;
! extern int dl_debug;
/** @ingroup stage
--- 38,43 ----
//extern int _stg_disable_gui;
!
! extern GList* dl_list;
/** @ingroup stage
***************
*** 168,171 ****
--- 169,178 ----
+ void StgModel::ClearBlocks( void )
+ {
+ stg_block_list_destroy( this->blocks );
+ this->blocks = NULL;
+ }
+
void StgModel::AddBlockRect( double x, double y,
double width, double height )
***************
*** 334,339 ****
this->gui_mask = this->parent ? 0 : STG_DEFAULT_MASK;
this->last_update_ms = -1;
! this->update_interval_ms = 100;
// now we can add the basic square shape
--- 341,349 ----
this->gui_mask = this->parent ? 0 : STG_DEFAULT_MASK;
+ bzero( &this->velocity, sizeof(velocity));
+ this->on_velocity_list = false;
+
this->last_update_ms = -1;
! this->update_interval_ms = 10;
// now we can add the basic square shape
***************
*** 341,344 ****
--- 351,355 ----
this->data = this->cfg = this->cmd = NULL;
+ this->data_len = this->cmd_len = this->cfg_len;
this->subs = 0;
***************
*** 351,354 ****
--- 362,366 ----
this->dl_body = glGenLists( 1 );
this->dl_data = glGenLists( 1 );
+ this->dl_debug = glGenLists( 1 );
// exterimental: creates a menu of models
***************
*** 361,364 ****
--- 373,378 ----
}
+
+
StgModel::~StgModel( void )
{
***************
*** 529,533 ****
if( blocks )
for( GList* it=blocks; it; it=it->next )
! stg_matrix_remove_object( this->world->matrix, it->data );
}
--- 543,547 ----
if( blocks )
for( GList* it=blocks; it; it=it->next )
! stg_matrix_remove_block( this->world->matrix, (stg_block_t*)it->data );
}
***************
*** 541,545 ****
// if this is the first sub, call startup
! if( subs == 1 )
this->Startup();
}
--- 555,559 ----
// if this is the first sub, call startup
! if( this->subs == 1 )
this->Startup();
}
***************
*** 592,613 ****
}
! void StgModel::UpdateTreeIfDue( void )
{
if( this->world->sim_time_ms >=
(this->last_update_ms + this->update_interval_ms) )
this->Update();
-
- LISTMETHOD( this->children, StgModel*, UpdateTreeIfDue );
}
! void StgModel::UpdateTree( void )
! {
! LISTMETHOD( this->children, StgModel*, UpdateTree );
! }
void StgModel::Update( void )
{
//printf( "[%lu] %s update (%d subs)\n",
! // this->world->sim_time, this->token, this->subs );
this->CallCallbacks( &update );
--- 606,634 ----
}
! // void StgModel::UpdateTreeIfDue( void )
! // {
! // if( this->world->sim_time_ms >=
! // (this->last_update_ms + this->update_interval_ms) )
! // this->Update();
!
! // LISTMETHOD( this->children, StgModel*, UpdateTreeIfDue );
! // }
!
! void StgModel::UpdateIfDue( void )
{
if( this->world->sim_time_ms >=
(this->last_update_ms + this->update_interval_ms) )
this->Update();
}
! // void StgModel::UpdateTree( void )
! // {
! // LISTMETHOD( this->children, StgModel*, UpdateTree );
! // }
void StgModel::Update( void )
{
//printf( "[%lu] %s update (%d subs)\n",
! // this->world->sim_time_ms, this->token, this->subs );
this->CallCallbacks( &update );
***************
*** 638,643 ****
void StgModel::Draw( void )
{
! printf( "%s.Draw()\n",
! this->token );
glPushMatrix();
--- 659,664 ----
void StgModel::Draw( void )
{
! // printf( "%s.Draw()\n",
! //this->token );
glPushMatrix();
***************
*** 661,664 ****
--- 682,686 ----
glCallList( this->dl_body );
//glCallList( this->dl_data );
+
// shift up the CS to the top of this model
***************
*** 669,672 ****
--- 691,696 ----
glPopMatrix(); // drop out of local coords
+
+ glCallList( this->dl_debug );
}
***************
*** 796,807 ****
--- 820,852 ----
}
+ bool velocity_is_nonzero( stg_velocity_t* v )
+ {
+ return( v->x || v->y || v->z || v->a );
+ }
+
void StgModel::SetVelocity( stg_velocity_t* vel )
{
assert(vel);
memcpy( &this->velocity, vel, sizeof(stg_velocity_t));
+
+ if( ! this->on_velocity_list && velocity_is_nonzero( & this->velocity ) )
+ {
+ this->world->velocity_list = g_list_prepend(
this->world->velocity_list, this );
+ this->on_velocity_list = true;
+ }
+
+ if( this->on_velocity_list && ! velocity_is_nonzero( &this->velocity ))
+ {
+ this->world->velocity_list = g_list_remove( this->world->velocity_list,
this );
+ this->on_velocity_list = false;
+ }
CallCallbacks( &this->velocity );
}
+ void StgModel::NeedRedraw( void )
+ {
+ this->world->win->dirty = true;
+ }
void StgModel::SetPose( stg_pose_t* pose )
***************
*** 825,828 ****
--- 870,875 ----
// render in the matrix
MapWithChildren( 1 );
+
+ this->NeedRedraw();
}
***************
*** 1038,1049 ****
! /* int lines_raytrace_match( stg_model_t* mod, stg_model_t* hitmod ) */
! /* { */
! /* // Ignore invisible things, myself, my children, and my ancestors. */
! /* if( hitmod->obstacle_return && (!stg_model_is_related(mod,hitmod)) )
*/
! /* return 1; */
! /* return 0; // no match */
! /* } */
--- 1085,1096 ----
! int lines_raytrace_match( StgModel* mod, StgModel* hitmod )
! {
! // Ignore invisible things, myself, my children, and my ancestors.
! if( hitmod->ObstacleReturn() )//&& (!stg_model_is_related(mod,hitmod)) )
! return 1;
! return 0; // no match
! }
***************
*** 1054,1062 ****
// Returns NULL if not collisions.
// This function is useful for writing position devices.
! /* stg_model_t* stg_model_test_collision( stg_model_t* mod, */
! /* //stg_pose_t* pose, */
! /* double* hitx, double* hity ) */
! /* { */
! /* return 0; */
/* stg_model_t* child_hit = NULL; */
--- 1101,1108 ----
// Returns NULL if not collisions.
// This function is useful for writing position devices.
!
! StgModel* StgModel::TestCollision( stg_pose_t* pose,
! double* hitx, double* hity )
! {
/* stg_model_t* child_hit = NULL; */
***************
*** 1078,1149 ****
/* //return NULL; */
! /* // raytrace along all our rectangles. expensive, but most vehicles */
! /* // will just be a single rect, grippers 3 rects, etc. not too bad. */
! /* size_t count=0; */
! /* stg_polygon_t* polys = stg_model_get_polygons(mod, &count); */
! /* // no body? no collision */
! /* if( count < 1 ) */
! /* return NULL; */
! /* // loop over all polygons */
! /* int q; */
! /* for( q=0; q<count; q++ ) */
! /* { */
! /* stg_polygon_t* poly = &polys[q]; */
! /* int point_count = poly->points->len; */
! /* // loop over all points in this polygon */
! /* int p; */
! /* for( p=0; p<point_count; p++ ) */
! /* { */
! /* stg_point_t* pt1 = &g_array_index( poly->points, stg_point_t, p );
*/
! /* stg_point_t* pt2 = &g_array_index( poly->points, stg_point_t, (p+1) %
point_count); */
!
! /* stg_pose_t pp1; */
! /* pp1.x = pt1->x; */
! /* pp1.y = pt1->y; */
! /* pp1.a = 0; */
!
! /* stg_pose_t pp2; */
! /* pp2.x = pt2->x; */
! /* pp2.y = pt2->y; */
! /* pp2.a = 0; */
!
! /* stg_pose_t p1; */
! /* stg_pose_t p2; */
! /* // shift the line points into the global coordinate system */
! /* stg_pose_sum( &p1, &pose, &pp1 ); */
! /* stg_pose_sum( &p2, &pose, &pp2 ); */
! /* //printf( "tracing %.2f %.2f %.2f %.2f\n", p1.x, p1.y, p2.x, p2.y
); */
! /* itl_t* itl = itl_create( p1.x, p1.y, p2.x, p2.y, */
! /* mod->world->matrix, */
! /* PointToPoint ); */
! /* //stg_rtk_fig_t* fig = stg_model_fig_create( mod, "foo", NULL,
STG_LAYER_POSITIONDATA ); */
! /* //stg_rtk_fig_line( fig, p1.x, p1.y, p2.x, p2.y ); */
! /* stg_model_t* hitmod = itl_first_matching( itl, lines_raytrace_match,
mod ); */
! /* if( hitmod ) */
! /* { */
! /* if( hitx ) *hitx = itl->x; // report them */
! /* if( hity ) *hity = itl->y; */
! /* itl_destroy( itl ); */
! /* return hitmod; // we hit this object! stop raytracing */
! /* } */
! /* itl_destroy( itl ); */
! /* } */
! /* } */
! /* return NULL; // done */
! //}
--- 1124,1235 ----
/* //return NULL; */
! // raytrace along all our blocks. expensive, but most vehicles
! // will just be a few blocks, grippers 3 blocks, etc. not too bad.
!
! // no blocks? no hit!
! if( this->blocks == NULL )
! return NULL;
! glNewList( this->dl_debug, GL_COMPILE );
! glPushMatrix();
! glTranslatef( 0,0,1.0 );
! double eps = 0.3;
! push_color_rgb( 0,1,0 );
! glBegin( GL_QUADS );
! glVertex2f( pose->x-eps, pose->y-eps );
! glVertex2f( pose->x+eps, pose->y-eps );
! glVertex2f( pose->x+eps, pose->y+eps );
! glVertex2f( pose->x-eps, pose->y+eps );
! glEnd();
! pop_color();
! // loop over all blocks
! for( GList* it = this->blocks; it; it=it->next )
! {
! stg_block_t* b = (stg_block_t*)it->data;
! // loop over all edges of the block
! for( int p=0; p<b->pt_count; p++ )
! {
! stg_point_t* pt1 = &b->pts[p];
! stg_point_t* pt2 = &b->pts[(p+1) % b->pt_count];
! push_color_rgb( 1,0,0 );
! glBegin( GL_LINES );
! glVertex2f( pt1->x, pt1->y );
! glVertex2f( pt2->x, pt2->y );
! glEnd();
! pop_color();
!
! stg_pose_t pp1;
! pp1.x = pt1->x;
! pp1.y = pt1->y;
! pp1.a = 0;
! stg_pose_t pp2;
! pp2.x = pt2->x;
! pp2.y = pt2->y;
! pp2.a = 0;
! stg_pose_t p1;
! stg_pose_t p2;
! // shift the line points into the global coordinate system
! stg_pose_sum( &p1, pose, &pp1 );
! stg_pose_sum( &p2, pose, &pp2 );
! push_color_rgb( 0,0,1 );
! glBegin( GL_LINES );
! glVertex2f( p1.x, p1.y );
! glVertex2f( p2.x, p2.y );
! glEnd();
! pop_color();
!
! //printf( "tracing %.2f %.2f %.2f %.2f\n", p1.x, p1.y, p2.x, p2.y
);
! double dx = p2.x - p1.x;
! double dy = p2.y - p1.y;
!
! double bearing = atan2( dy, dx );
! double range = hypot( dx, dy );
!
! itl_t* itl = itl_create( p1.x, p1.y, 0,
! //p2.x, p2.y,
! bearing, range,
! world->matrix,
! //PointToPoint );
! PointToBearingRange );
!
! StgModel* hitmod = itl_first_matching( itl, lines_raytrace_match,
this );
+ if( hitmod )
+ printf( "hit model %s\n", hitmod->Token() );
! if( hitmod )
! {
! if( hitx ) *hitx = itl->x; // report them
! if( hity ) *hity = itl->y;
! itl_destroy( itl );
! glPopMatrix();
! //pop_color();
! glEndList();
!
! return hitmod; // we hit this object! stop raytracing
! }
! itl_destroy( itl );
! }
! }
!
! //glEnd();
! glPopMatrix();
! //pop_color();
! glEndList();
!
!
! return NULL; // done
! }
***************
*** 1307,1347 ****
! // int StgModel::UpdatePose( void )
! // {
! // printf( "model %s update pose\n", token );
! // // do nothing yet
! // }
!
! // int stg_model_update_pose( stg_model_t* mod )
! // {
! // PRINT_DEBUG4( "pose update model %d (vel %.2f, %.2f %.2f)",
! // mod->id, mod->velocity.x, mod->velocity.y, mod->velocity.a );
!
! // stg_velocity_t gvel;
! // stg_model_get_global_velocity( mod, &gvel );
! // stg_pose_t gpose;
! // stg_model_get_global_pose( mod, &gpose );
!
! // // convert msec to sec
! // double interval = (double)mod->world->sim_interval / 1000.0;
! // stg_pose_t old_pose;
! // memcpy( &old_pose, &mod->pose, sizeof(old_pose));
! // // compute new pose
! // //gpose.x += gvel.x * interval;
! // //gpose.y += gvel.y * interval;
! // //gpose.a += gvel.a * interval;
! // mod->pose.x += (mod->velocity.x * cos(mod->pose.a) - mod->velocity.y *
sin(mod->pose.a)) * interval;
! // mod->pose.y += (mod->velocity.x * sin(mod->pose.a) + mod->velocity.y *
cos(mod->pose.a)) * interval;
! // mod->pose.a += mod->velocity.a * interval;
! // // check this model and all it's children at the new pose
! // double hitx=0, hity=0, hita=0;
! // stg_model_t* hitthing =
! // //NULL; //stg_model_test_collision( mod, &hitx, &hity );
! // stg_model_test_collision2( mod, &hitx, &hity );
// int stall = 0;
--- 1393,1440 ----
! void StgModel::UpdatePose( void )
! {
! //stg_velocity_t gvel;
! //this->GetGlostg_model_get_global_velocity( mod, &gvel );
! //stg_pose_t gpose;
! //stg_model_get_global_pose( mod, &gpose );
!
! // convert msec to sec
! double interval = (double)world->sim_interval_ms / 1e3;
! //stg_pose_t old_pose;
! //memcpy( &old_pose, &mod->pose, sizeof(old_pose));
! stg_velocity_t v;
! this->GetVelocity( &v );
! stg_pose_t p;
! this->GetPose( &p );
!
! p.x += (v.x * cos(p.a) - v.y * sin(p.a)) * interval;
! p.y += (v.x * sin(p.a) + v.y * cos(p.a)) * interval;
! p.a += v.a * interval;
!
! //this->SetPose( &p );
!
! // convert the new pose to global coords so we can see what it might hit
!
! stg_pose_t gp;
! memcpy( &gp, &p, sizeof(stg_pose_t));
!
! //this->LocalToGlobal( &gp );
!
! // check this model and all it's children at the new pose
! double hitx=0, hity=0, hita=0;
! StgModel* hitthing = this->TestCollision( &gp, &hitx, &hity );
!
! if( hitthing )
! printf( "hit %s at %.2f %/2f\n",
! hitthing->Token(), hitx, hity );
! else
! this->SetPose( &p );
!
// int stall = 0;
***************
*** 1452,1456 ****
// return 0; // ok
! // }
--- 1545,1549 ----
// return 0; // ok
! }
Index: stage_internal.h
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/stage_internal.h,v
retrieving revision 1.58.2.17
retrieving revision 1.58.2.18
diff -C2 -d -r1.58.2.17 -r1.58.2.18
*** stage_internal.h 10 Jul 2007 02:15:47 -0000 1.58.2.17
--- stage_internal.h 13 Jul 2007 05:48:31 -0000 1.58.2.18
***************
*** 241,244 ****
--- 241,247 ----
GList* update_list;
+ /** a list of models that have non-zero velocity, so need their pose
updated */
+ GList* velocity_list;
+
stg_meters_t width; ///< x size of the world
stg_meters_t height; ///< y size of the world
***************
*** 434,437 ****
--- 437,444 ----
stg_block_t* block );
+ /** remove all references to this block from the matrix */
+ void stg_matrix_remove_block( stg_matrix_t* matrix, stg_block_t* block );
+
+
/** specify a line from (x1,y1) to (x2,y2), all in meters
*/
Index: p_position.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/p_position.cc,v
retrieving revision 1.14.4.1
retrieving revision 1.14.4.2
diff -C2 -d -r1.14.4.1 -r1.14.4.2
*** p_position.cc 6 Jan 2007 02:21:28 -0000 1.14.4.1
--- p_position.cc 13 Jul 2007 05:48:31 -0000 1.14.4.2
***************
*** 46,52 ****
//#include "playerclient.h"
- extern "C" {
- int position_init( stg_model_t* mod );
- }
InterfacePosition::InterfacePosition( player_devaddr_t addr,
--- 46,49 ----
***************
*** 55,59 ****
int section )
! : InterfaceModel( addr, driver, cf, section, position_init )
{
//puts( "InterfacePosition constructor" );
--- 52,56 ----
int section )
! : InterfaceModel( addr, driver, cf, section, "position" )
{
//puts( "InterfacePosition constructor" );
***************
*** 64,70 ****
void* data)
{
- stg_position_cmd_t* cmd = (stg_position_cmd_t*)this->mod->cmd;
- stg_position_cfg_t* cfg = (stg_position_cfg_t*)this->mod->cfg;
-
// Is it a new motor command?
if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
--- 61,64 ----
***************
*** 82,86 ****
scmd.a = pcmd->vel.pa;
scmd.mode = STG_POSITION_CONTROL_VELOCITY;
! stg_model_set_cmd( this->mod, &scmd, sizeof(scmd));
}
--- 76,82 ----
scmd.a = pcmd->vel.pa;
scmd.mode = STG_POSITION_CONTROL_VELOCITY;
! this->mod->SetCmd( &scmd, sizeof(scmd) );
!
! return 0;
}
***************
*** 100,104 ****
scmd.a = pcmd->pos.pa;
scmd.mode = STG_POSITION_CONTROL_POSITION;
! stg_model_set_cmd( this->mod, &scmd, sizeof(scmd));
}
--- 96,102 ----
scmd.a = pcmd->pos.pa;
scmd.mode = STG_POSITION_CONTROL_POSITION;
! this->mod->SetCmd( &scmd, sizeof(scmd) );
!
! return 0;
}
***************
*** 118,122 ****
scmd.a = pcmd->angle;
scmd.mode = STG_POSITION_CONTROL_VELOCITY;
! stg_model_set_cmd( this->mod, &scmd, sizeof(scmd));
}
--- 116,122 ----
scmd.a = pcmd->angle;
scmd.mode = STG_POSITION_CONTROL_VELOCITY;
! this->mod->SetCmd( &scmd, sizeof(scmd) );
!
! return 0;
}
***************
*** 129,133 ****
{
stg_geom_t geom;
! stg_model_get_geom( this->mod,&geom );
// fill in the geometry data formatted player-like
--- 129,133 ----
{
stg_geom_t geom;
! this->mod->GetGeom( &geom );
// fill in the geometry data formatted player-like
***************
*** 144,148 ****
PLAYER_POSITION2D_REQ_GET_GEOM,
(void*)&pgeom, sizeof(pgeom), NULL );
! return(0);
}
else
--- 144,148 ----
PLAYER_POSITION2D_REQ_GET_GEOM,
(void*)&pgeom, sizeof(pgeom), NULL );
! return 0;
}
else
***************
*** 160,173 ****
if(hdr->size == 0)
{
PRINT_DEBUG( "resetting odometry" );
stg_pose_t origin;
memset(&origin,0,sizeof(origin));
! stg_model_position_set_odom( this->mod, &origin );
this->driver->Publish( this->addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_RESET_ODOM );
! return(0);
}
else
--- 160,174 ----
if(hdr->size == 0)
{
+ PRINT_WARN( "reset odom not implemented" );
PRINT_DEBUG( "resetting odometry" );
stg_pose_t origin;
memset(&origin,0,sizeof(origin));
! //stg_model_position_set_odom( this->mod, &origin );
this->driver->Publish( this->addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_RESET_ODOM );
! return 0;
}
else
***************
*** 175,179 ****
PRINT_ERR2("config request len is invalid (%d != %d)",
(int)hdr->size, 0);
! return(-1);
}
}
--- 176,180 ----
PRINT_ERR2("config request len is invalid (%d != %d)",
(int)hdr->size, 0);
! return -1;
}
}
***************
*** 185,188 ****
--- 186,191 ----
if(hdr->size == sizeof(player_position2d_set_odom_req_t))
{
+ PRINT_WARN( "set odom not implemented" );
+
player_position2d_set_odom_req_t* req =
(player_position2d_set_odom_req_t*)data;
***************
*** 193,197 ****
pose.a = req->pose.pa;
! stg_model_position_set_odom( this->mod, &pose );
PRINT_DEBUG3( "set odometry to (%.2f,%.2f,%.2f)",
--- 196,200 ----
pose.a = req->pose.pa;
! //stg_model_position_set_odom( this->mod, &pose );
PRINT_DEBUG3( "set odometry to (%.2f,%.2f,%.2f)",
***************
*** 245,248 ****
--- 248,253 ----
if(hdr->size == sizeof(player_position2d_position_mode_req_t))
{
+ PRINT_WARN( "set control mode not implemented") ;
+
player_position2d_position_mode_req_t* req =
(player_position2d_position_mode_req_t*)data;
***************
*** 251,260 ****
// XX should this be in cfg instead?
! cmd->mode = mode;
! model_change( mod, &mod->cmd );
//stg_model_set_property( mod, "position_control", &mode, sizeof(mode));
! PRINT_WARN2( "Put model %s into %s control mode", this->mod->token, mod
? "POSITION" : "VELOCITY" );
this->driver->Publish( this->addr, resp_queue,
--- 256,267 ----
// XX should this be in cfg instead?
! //cmd->mode = mode;
! //model_change( mod, &mod->cmd );
!
!
//stg_model_set_property( mod, "position_control", &mode, sizeof(mode));
! PRINT_WARN2( "Put model %s into %s control mode", this->mod->Token(),
mod ? "POSITION" : "VELOCITY" );
this->driver->Publish( this->addr, resp_queue,
***************
*** 275,279 ****
// Don't know how to handle this message.
! PRINT_WARN2( "stg_position doesn't support msg with type/subtype %d/%d",
hdr->type, hdr->subtype);
return(-1);
--- 282,286 ----
// Don't know how to handle this message.
! PRINT_WARN2( "stg_position doesn't support msg with type %d subtype %d",
hdr->type, hdr->subtype);
return(-1);
***************
*** 285,294 ****
! stg_position_data_t* data = (stg_position_data_t*)this->mod->data;
!
! if( data )
{
- assert(this->mod->data_len == sizeof(stg_position_data_t));
-
//printf( "stage position data: %.2f,%.2f,%.2f\n",
// data->pose.x, data->pose.y, data->pose.a );
--- 292,300 ----
! size_t len=0;
! stg_position_data_t* data = (stg_position_data_t*)this->mod->GetData( &len
);
!
! if( data && ( len == sizeof(stg_position_data_t)))
{
//printf( "stage position data: %.2f,%.2f,%.2f\n",
// data->pose.x, data->pose.y, data->pose.a );
***************
*** 303,314 ****
// speeds
! stg_velocity_t* vel = &this->mod->velocity;
! ppd.vel.px = vel->x;
! ppd.vel.py = vel->y;
! ppd.vel.pa = vel->a;
// etc
! ppd.stall = this->mod->stall;
// publish this data
--- 309,322 ----
// speeds
! stg_velocity_t vel;
! this->mod->GetVelocity( &vel );
! // packing by hand allows for type conversions
! ppd.vel.px = vel.x;
! ppd.vel.py = vel.y;
! ppd.vel.pa = vel.a;
// etc
! ppd.stall = this->mod->Stall();
// publish this data
***************
*** 317,319 ****
--- 325,331 ----
(void*)&ppd, sizeof(ppd), NULL);
}
+ else if( len != 0 )
+ PRINT_ERR3( "Bad position data size for model %s (%d/%d bytes)",
+ this->mod->Token(), len, sizeof(stg_position_data_t));
+
}
Index: world.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Attic/world.cc,v
retrieving revision 1.154.2.3
retrieving revision 1.154.2.4
diff -C2 -d -r1.154.2.3 -r1.154.2.4
*** world.cc 10 Jul 2007 02:15:47 -0000 1.154.2.3
--- world.cc 13 Jul 2007 05:48:31 -0000 1.154.2.4
***************
*** 274,277 ****
--- 274,280 ----
world->wall_last_update = 0;
+ world->update_list = NULL;
+ world->velocity_list = NULL;
+
world->width = width;
world->height = height;
***************
*** 382,398 ****
#endif
! // TEST DEBUGGING
! //glNewList( dl_debug, GL_COMPILE );
! //push_color_rgb( 0,1,0 );
!
! //glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
!
for( GList* it=world->update_list; it; it=it->next )
! ((StgModel*)it->data)->UpdateTreeIfDue();
!
! //pop_color();
! //glEndList(); // dl_debug
!
world->wall_last_update = timenow;
world->sim_time_ms += world->sim_interval_ms;
--- 385,396 ----
#endif
! // update any models that are due to be updated
for( GList* it=world->update_list; it; it=it->next )
! ((StgModel*)it->data)->UpdateIfDue();
+ // update any models with non-zero velocity
+ for( GList* it=world->velocity_list; it; it=it->next )
+ ((StgModel*)it->data)->UpdatePose();
+
world->wall_last_update = timenow;
world->sim_time_ms += world->sim_interval_ms;
Index: gui_gl.c
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Attic/gui_gl.c,v
retrieving revision 1.1.2.27
retrieving revision 1.1.2.28
diff -C2 -d -r1.1.2.27 -r1.1.2.28
*** gui_gl.c 10 Jul 2007 02:15:47 -0000 1.1.2.27
--- gui_gl.c 13 Jul 2007 05:48:31 -0000 1.1.2.28
***************
*** 634,639 ****
}
! glDisable (GL_LIGHTING);
!
glEnable (GL_DEPTH_TEST);
glDepthFunc (GL_LESS);
--- 634,639 ----
}
! glDisable(GL_LIGHTING);
!
glEnable (GL_DEPTH_TEST);
glDepthFunc (GL_LESS);
***************
*** 820,824 ****
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
-
double zclip = hypot(world->width, world->height) * win->scale;
double pixels_width = world->win->canvas->allocation.width;
--- 820,823 ----
***************
*** 919,923 ****
//g_hash_table_foreach( world->models, (GHFunc)model_draw_bbox, NULL);
- //glCallList( dl_debug );
if( win->show_matrix )
--- 918,921 ----
***************
*** 953,971 ****
{
int dl = (int)it->data;
! //printf( "Calling dl %d\n", dl );
! glCallList( (int)it->data );
}
!
! glLineWidth( 2.0 );
! push_color_rgb( 1,0,0 );
! glBegin( GL_LINE_LOOP );
! glVertex3f( 0,0,0 );
! glVertex3f( 1,0,0 );
! glVertex3f( 1,1,0 );
! glVertex3f( 0,1,0 );
! glEnd();
! pop_color();
!
//gl_coord_shift( 0,0,-2,0 );
--- 951,959 ----
{
int dl = (int)it->data;
! printf( "Calling dl %d\n", dl );
! glCallList( dl );
}
! glCallList( dl_debug );
//gl_coord_shift( 0,0,-2,0 );
Index: raytrace.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Attic/raytrace.cc,v
retrieving revision 1.12.2.3
retrieving revision 1.12.2.4
diff -C2 -d -r1.12.2.3 -r1.12.2.4
*** raytrace.cc 10 Jul 2007 02:15:47 -0000 1.12.2.3
--- raytrace.cc 13 Jul 2007 05:48:31 -0000 1.12.2.4
***************
*** 166,171 ****
--- 166,178 ----
StgModel* finder )
{
+ assert(list);
+
+ //printf( "Finder %s is testing..\n", finder->Token() );
+
for( ; list ; list=list->next )
{
+ assert(list->data);
+
+
stg_block_t* block = (stg_block_t*)list->data;
***************
*** 173,176 ****
--- 180,187 ----
StgModel* candidate = block->mod;
+ assert( candidate );
+
+ //printf( "block of %s\n", candidate->Token() );
+
stg_pose_t gpose;
candidate->GetGlobalPose( &gpose );
Index: model_load.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Attic/model_load.cc,v
retrieving revision 1.1.2.4
retrieving revision 1.1.2.5
diff -C2 -d -r1.1.2.4 -r1.1.2.5
*** model_load.cc 10 Jul 2007 02:15:47 -0000 1.1.2.4
--- model_load.cc 13 Jul 2007 05:48:31 -0000 1.1.2.5
***************
*** 121,126 ****
}
! stg_block_list_destroy( this->blocks );
! this->blocks = NULL;
//printf( "found %d rects\n", rect_count );
--- 121,126 ----
}
! this->Map(0);
! this->ClearBlocks();
//printf( "found %d rects\n", rect_count );
***************
*** 144,147 ****
--- 144,148 ----
stg_block_list_scale( this->blocks, &this->geom.size );
+ this->Map(1);
}
}
***************
*** 151,156 ****
int blockcount = wf->ReadInt( this->id, "blocks", -1 );
! stg_block_list_destroy( this->blocks );
! this->blocks = NULL;
//printf( "expecting %d blocks\n", blockcount );
--- 152,157 ----
int blockcount = wf->ReadInt( this->id, "blocks", -1 );
! this->Map(0);
! this->ClearBlocks();
//printf( "expecting %d blocks\n", blockcount );
***************
*** 206,210 ****
stg_block_list_scale( this->blocks, &this->geom.size );
!
}
--- 207,211 ----
stg_block_list_scale( this->blocks, &this->geom.size );
! this->Map(1);
}
Index: p_driver.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/p_driver.cc,v
retrieving revision 1.37.4.5
retrieving revision 1.37.4.6
diff -C2 -d -r1.37.4.5 -r1.37.4.6
*** p_driver.cc 8 Jul 2007 01:44:09 -0000 1.37.4.5
--- p_driver.cc 13 Jul 2007 05:48:31 -0000 1.37.4.6
***************
*** 381,387 ****
break;
! //case PLAYER_POSITION2D_CODE:
! //ifsrc = new InterfacePosition( player_addr, this, cf, section );
! // break;
// case PLAYER_SONAR_CODE:
--- 381,387 ----
break;
! case PLAYER_POSITION2D_CODE:
! ifsrc = new InterfacePosition( player_addr, this, cf, section );
! break;
// case PLAYER_SONAR_CODE:
***************
*** 616,627 ****
Driver::ProcessMessages();
for( int i=0; i<(int)this->devices->len; i++ )
{
Interface* interface = (Interface*)g_ptr_array_index( this->devices, i );
-
assert( interface );
- // Should this be an assertion? - BPG
- //if(!interface)
- //continue;
switch( interface->addr.interf )
--- 616,625 ----
Driver::ProcessMessages();
+ //puts( "STG driver update" );
+
for( int i=0; i<(int)this->devices->len; i++ )
{
Interface* interface = (Interface*)g_ptr_array_index( this->devices, i );
assert( interface );
switch( interface->addr.interf )
Index: block.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Attic/block.cc,v
retrieving revision 1.1.2.4
retrieving revision 1.1.2.5
diff -C2 -d -r1.1.2.4 -r1.1.2.5
*** block.cc 10 Jul 2007 02:15:47 -0000 1.1.2.4
--- block.cc 13 Jul 2007 05:48:31 -0000 1.1.2.5
***************
*** 18,21 ****
--- 18,22 ----
stg_block_t* block = g_new( stg_block_t, 1 );
+ //printf( "BLOCK CREATE %p\n", block );
block->mod = mod;
***************
*** 30,38 ****
}
! /** destroy a block, freeing all memory */
void stg_block_destroy( stg_block_t* block )
{
assert( block->pts );
assert( block );
g_free( block->pts );
--- 31,43 ----
}
! /** destroy a block, freeing all memory, and removing it from it's model's
matrix */
void stg_block_destroy( stg_block_t* block )
{
+ //printf( "BLOCK DESTROY %p\n", block );
assert( block->pts );
assert( block );
+
+ if( block->mod && block->mod->World() && block->mod->World()->matrix )
+ stg_matrix_remove_block( block->mod->World()->matrix, block );
g_free( block->pts );
Index: Makefile.am
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Makefile.am,v
retrieving revision 1.140.2.8
retrieving revision 1.140.2.9
diff -C2 -d -r1.140.2.8 -r1.140.2.9
*** Makefile.am 8 Jul 2007 01:44:08 -0000 1.140.2.8
--- Makefile.am 13 Jul 2007 05:48:30 -0000 1.140.2.9
***************
*** 62,66 ****
# model_fiducial.c
# model_laser.cc
! # model_load.c
# model_ptz.c
# model_ranger.c
--- 62,66 ----
# model_fiducial.c
# model_laser.cc
! # model_load.c
# model_ptz.c
# model_ranger.c
***************
*** 86,94 ****
p_graphics3d.cc \
p_laser.cc \
p_simulation.cc \
stg_time.cc \
stg_time.h
- # p_position.cc
# p_bumper.cc
# p_fiducial.cc
--- 86,94 ----
p_graphics3d.cc \
p_laser.cc \
+ p_position.cc \
p_simulation.cc \
stg_time.cc \
stg_time.h
# p_bumper.cc
# p_fiducial.cc
Index: model_position.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Attic/model_position.cc,v
retrieving revision 1.1.2.1
retrieving revision 1.1.2.2
diff -C2 -d -r1.1.2.1 -r1.1.2.2
*** model_position.cc 8 Jul 2007 01:44:09 -0000 1.1.2.1
--- model_position.cc 13 Jul 2007 05:48:31 -0000 1.1.2.2
***************
*** 292,512 ****
void StgModelPosition::Update( void )
{
! StgModel::Update();
!
! // PRINT_DEBUG1( "[%lu] position update", mod->world->sim_time );
! // stg_position_data_t* data = (stg_position_data_t*)mod->data;
! // stg_position_cmd_t* cmd = (stg_position_cmd_t*)mod->cmd;
! // stg_position_cfg_t* cfg = (stg_position_cfg_t*)mod->cfg;
! // stg_velocity_t* vel = &mod->velocity;
!
! // // stop by default
! // memset( vel, 0, sizeof(stg_velocity_t) );
! // if( mod->subs ) // no driving if noone is subscribed
! // {
! // switch( cmd->mode )
! // {
! // case STG_POSITION_CONTROL_VELOCITY :
! // {
! // PRINT_DEBUG( "velocity control mode" );
! // PRINT_DEBUG4( "model %s command(%.2f %.2f %.2f)",
! // mod->token,
! // mod->cmd.x,
! // mod->cmd.y,
! // mod->cmd.a );
! // switch( cfg->drive_mode )
! // {
! // case STG_POSITION_DRIVE_DIFFERENTIAL:
! // // differential-steering model, like a Pioneer
! // vel->x = cmd->x;
! // vel->y = 0;
! // vel->a = cmd->a;
! // break;
! // case STG_POSITION_DRIVE_OMNI:
! // // direct steering model, like an omnidirectional robot
! // vel->x = cmd->x;
! // vel->y = cmd->y;
! // vel->a = cmd->a;
! // break;
! // case STG_POSITION_DRIVE_CAR:
! // // car like steering model based on speed and turning angle
! // vel->x = cmd->x * cos(cmd->a);
! // vel->y = 0;
! // vel->a = cmd->x * sin(cmd->a)/1.0; // here 1.0 is the wheel
base, this should be a config option
! // break;
! // default:
! // PRINT_ERR1( "unknown steering mode %d", cfg->drive_mode );
! // }
! // } break;
! // case STG_POSITION_CONTROL_POSITION:
! // {
! // PRINT_DEBUG( "position control mode" );
! // double x_error = cmd->x - data->pose.x;
! // double y_error = cmd->y - data->pose.y;
! // double a_error = NORMALIZE( cmd->a - data->pose.a );
! // PRINT_DEBUG3( "errors: %.2f %.2f %.2f\n", x_error, y_error, a_error
);
! // // speed limits for controllers
! // // TODO - have these configurable
! // double max_speed_x = 0.4;
! // double max_speed_y = 0.4;
! // double max_speed_a = 1.0;
! // switch( cfg->drive_mode )
! // {
! // case STG_POSITION_DRIVE_OMNI:
! // {
! // // this is easy - we just reduce the errors in each axis
! // // independently with a proportional controller, speed
! // // limited
! // vel->x = MIN( x_error, max_speed_x );
! // vel->y = MIN( y_error, max_speed_y );
! // vel->a = MIN( a_error, max_speed_a );
! // }
! // break;
! // case STG_POSITION_DRIVE_DIFFERENTIAL:
! // {
! // // axes can not be controlled independently. We have to
! // // turn towards the desired x,y position, drive there,
! // // then turn to face the desired angle. this is a
! // // simple controller that works ok. Could easily be
! // // improved if anyone needs it better. Who really does
! // // position control anyhoo?
! // // start out with no velocity
! // stg_velocity_t calc;
! // memset( &calc, 0, sizeof(calc));
! // double close_enough = 0.02; // fudge factor
! // // if we're at the right spot
! // if( fabs(x_error) < close_enough && fabs(y_error) <
close_enough )
! // {
! // PRINT_DEBUG( "TURNING ON THE SPOT" );
! // // turn on the spot to minimize the error
! // calc.a = MIN( a_error, max_speed_a );
! // calc.a = MAX( a_error, -max_speed_a );
! // }
! // else
! // {
! // PRINT_DEBUG( "TURNING TO FACE THE GOAL POINT" );
! // // turn to face the goal point
! // double goal_angle = atan2( y_error, x_error );
! // double goal_distance = hypot( y_error, x_error );
! // a_error = NORMALIZE( goal_angle - data->pose.a );
! // calc.a = MIN( a_error, max_speed_a );
! // calc.a = MAX( a_error, -max_speed_a );
! // PRINT_DEBUG2( "steer errors: %.2f %.2f \n", a_error,
goal_distance );
! // // if we're pointing about the right direction, move
! // // forward
! // if( fabs(a_error) < M_PI/16 )
! // {
! // PRINT_DEBUG( "DRIVING TOWARDS THE GOAL" );
! // calc.x = MIN( goal_distance, max_speed_x );
! // }
! // }
! // // now set the underlying velocities using the normal
! // // diff-steer model
! // vel->x = calc.x;
! // vel->y = 0;
! // vel->a = calc.a;
! // }
! // break;
! // default:
! // PRINT_ERR1( "unknown steering mode %d", (int)cfg->drive_mode );
! // }
! // }
! // break;
! // default:
! // PRINT_ERR1( "unrecognized position command mode %d", cmd->mode );
! // }
! // // simple model of power consumption
! // // mod->watts = STG_POSITION_WATTS +
! // //fabs(vel->x) * STG_POSITION_WATTS_KGMS * mod->mass +
! // //fabs(vel->y) * STG_POSITION_WATTS_KGMS * mod->mass +
! // //fabs(vel->a) * STG_POSITION_WATTS_KGMS * mod->mass;
! // //PRINT_DEBUG4( "model %s velocity (%.2f %.2f %.2f)",
! // // mod->token,
! // // mod->velocity.x,
! // // mod->velocity.y,
! // // mod->velocity.a );
!
! // // we've poked the velocity
! // model_change( mod, &mod->velocity );
! // }
! // switch( data->localization )
! // {
! // case STG_POSITION_LOCALIZATION_GPS:
! // {
! // // compute our localization pose based on the origin and true pose
! // stg_pose_t gpose;
! // stg_model_get_global_pose( mod, &gpose );
! // data->pose.a = NORMALIZE( gpose.a - data->origin.a );
! // //data->pose.a =0;// NORMALIZE( gpose.a - data->origin.a );
! // double cosa = cos(data->origin.a);
! // double sina = sin(data->origin.a);
! // double dx = gpose.x - data->origin.x;
! // double dy = gpose.y - data->origin.y;
! // data->pose.x = dx * cosa + dy * sina;
! // data->pose.y = dy * cosa - dx * sina;
! // model_change( mod, &mod->data );
! // }
! // break;
! // case STG_POSITION_LOCALIZATION_ODOM:
! // {
! // // integrate our velocities to get an 'odometry' position estimate.
! // double dt = mod->world->sim_interval/1e3;
! // data->pose.a = NORMALIZE( data->pose.a + (vel->a * dt) * (1.0
+data->integration_error.a) );
! // double cosa = cos(data->pose.a);
! // double sina = sin(data->pose.a);
! // double dx = (vel->x * dt) * (1.0 + data->integration_error.x );
! // double dy = (vel->y * dt) * (1.0 + data->integration_error.y );
! // data->pose.x += dx * cosa + dy * sina;
! // data->pose.y -= dy * cosa - dx * sina;
! // model_change( mod, &mod->data );
! // }
! // break;
! // default:
! // PRINT_ERR2( "unknown localization mode %d for model %s\n",
! // data->localization, mod->token );
! // break;
! // }
!
! // model_move_due_to_velocity( mod );
!
! // // we've probably poked the position data
! // //model_change( mod, &mod->pose );
!
}
void StgModelPosition::Startup( void )
{
PRINT_DEBUG( "position startup" );
--- 292,505 ----
void StgModelPosition::Update( void )
{
! PRINT_DEBUG1( "[%lu] position update", this->world->sim_time );
! stg_position_data_t* data = (stg_position_data_t*)this->data;
! stg_position_cmd_t* cmd = (stg_position_cmd_t*)this->cmd;
! stg_position_cfg_t* cfg = (stg_position_cfg_t*)this->cfg;
! // stop by default
! stg_velocity_t vel;
! memset( &vel, 0, sizeof(stg_velocity_t) );
! if( this->subs ) // no driving if noone is subscribed
! {
! switch( cmd->mode )
! {
! case STG_POSITION_CONTROL_VELOCITY :
! {
! PRINT_DEBUG( "velocity control mode" );
! PRINT_DEBUG4( "model %s command(%.2f %.2f %.2f)",
! this->token,
! this->cmd.x,
! this->cmd.y,
! this->cmd.a );
! switch( cfg->drive_mode )
! {
! case STG_POSITION_DRIVE_DIFFERENTIAL:
! // differential-steering model, like a Pioneer
! vel.x = cmd->x;
! vel.y = 0;
! vel.a = cmd->a;
! break;
! case STG_POSITION_DRIVE_OMNI:
! // direct steering model, like an omnidirectional robot
! vel.x = cmd->x;
! vel.y = cmd->y;
! vel.a = cmd->a;
! break;
! case STG_POSITION_DRIVE_CAR:
! // car like steering model based on speed and turning angle
! vel.x = cmd->x * cos(cmd->a);
! vel.y = 0;
! vel.a = cmd->x * sin(cmd->a)/1.0; // here 1.0 is the wheel
base, this should be a config option
! break;
! default:
! PRINT_ERR1( "unknown steering mode %d", cfg->drive_mode );
! }
! } break;
! case STG_POSITION_CONTROL_POSITION:
! {
! PRINT_DEBUG( "position control mode" );
! double x_error = cmd->x - data->pose.x;
! double y_error = cmd->y - data->pose.y;
! double a_error = NORMALIZE( cmd->a - data->pose.a );
! PRINT_DEBUG3( "errors: %.2f %.2f %.2f\n", x_error, y_error, a_error
);
! // speed limits for controllers
! // TODO - have these configurable
! double max_speed_x = 0.4;
! double max_speed_y = 0.4;
! double max_speed_a = 1.0;
! switch( cfg->drive_mode )
! {
! case STG_POSITION_DRIVE_OMNI:
! {
! // this is easy - we just reduce the errors in each axis
! // independently with a proportional controller, speed
! // limited
! vel.x = MIN( x_error, max_speed_x );
! vel.y = MIN( y_error, max_speed_y );
! vel.a = MIN( a_error, max_speed_a );
! }
! break;
! case STG_POSITION_DRIVE_DIFFERENTIAL:
! {
! // axes can not be controlled independently. We have to
! // turn towards the desired x,y position, drive there,
! // then turn to face the desired angle. this is a
! // simple controller that works ok. Could easily be
! // improved if anyone needs it better. Who really does
! // position control anyhoo?
! // start out with no velocity
! stg_velocity_t calc;
! memset( &calc, 0, sizeof(calc));
! double close_enough = 0.02; // fudge factor
! // if we're at the right spot
! if( fabs(x_error) < close_enough && fabs(y_error) <
close_enough )
! {
! PRINT_DEBUG( "TURNING ON THE SPOT" );
! // turn on the spot to minimize the error
! calc.a = MIN( a_error, max_speed_a );
! calc.a = MAX( a_error, -max_speed_a );
! }
! else
! {
! PRINT_DEBUG( "TURNING TO FACE THE GOAL POINT" );
! // turn to face the goal point
! double goal_angle = atan2( y_error, x_error );
! double goal_distance = hypot( y_error, x_error );
! a_error = NORMALIZE( goal_angle - data->pose.a );
! calc.a = MIN( a_error, max_speed_a );
! calc.a = MAX( a_error, -max_speed_a );
! PRINT_DEBUG2( "steer errors: %.2f %.2f \n", a_error,
goal_distance );
! // if we're pointing about the right direction, move
! // forward
! if( fabs(a_error) < M_PI/16 )
! {
! PRINT_DEBUG( "DRIVING TOWARDS THE GOAL" );
! calc.x = MIN( goal_distance, max_speed_x );
! }
! }
! // now set the underlying velocities using the normal
! // diff-steer model
! vel.x = calc.x;
! vel.y = 0;
! vel.a = calc.a;
! }
! break;
! default:
! PRINT_ERR1( "unknown steering mode %d", (int)cfg->drive_mode );
! }
! }
! break;
! default:
! PRINT_ERR1( "unrecognized position command mode %d", cmd->mode );
! }
! // simple model of power consumption
! // this->watts = STG_POSITION_WATTS +
! //fabs(vel->x) * STG_POSITION_WATTS_KGMS * this->mass +
! //fabs(vel->y) * STG_POSITION_WATTS_KGMS * this->mass +
! //fabs(vel->a) * STG_POSITION_WATTS_KGMS * this->mass;
! //PRINT_DEBUG4( "model %s velocity (%.2f %.2f %.2f)",
! // this->token,
! // this->velocity.x,
! // this->velocity.y,
! // this->velocity.a );
! this->SetVelocity( &vel );
! }
! switch( data->localization )
! {
! case STG_POSITION_LOCALIZATION_GPS:
! {
! // compute our localization pose based on the origin and true pose
! stg_pose_t gpose;
! this->GetGlobalPose( &gpose );
! data->pose.a = NORMALIZE( gpose.a - data->origin.a );
! //data->pose.a =0;// NORMALIZE( gpose.a - data->origin.a );
! double cosa = cos(data->origin.a);
! double sina = sin(data->origin.a);
! double dx = gpose.x - data->origin.x;
! double dy = gpose.y - data->origin.y;
! data->pose.x = dx * cosa + dy * sina;
! data->pose.y = dy * cosa - dx * sina;
!
! }
! break;
! case STG_POSITION_LOCALIZATION_ODOM:
! {
! // integrate our velocities to get an 'odometry' position estimate.
! double dt = this->world->sim_interval_ms/1e3;
! data->pose.a = NORMALIZE( data->pose.a + (vel.a * dt) * (1.0
+data->integration_error.a) );
! double cosa = cos(data->pose.a);
! double sina = sin(data->pose.a);
! double dx = (vel.x * dt) * (1.0 + data->integration_error.x );
! double dy = (vel.y * dt) * (1.0 + data->integration_error.y );
! data->pose.x += dx * cosa + dy * sina;
! data->pose.y -= dy * cosa - dx * sina;
!
! }
! break;
! default:
! PRINT_ERR2( "unknown localization mode %d for model %s\n",
! data->localization, this->token );
! break;
! }
!
! StgModel::Update();
}
void StgModelPosition::Startup( void )
{
+ StgModel::Startup();
+
PRINT_DEBUG( "position startup" );
***************
*** 530,533 ****
--- 523,528 ----
this->SetWatts( 0 );
+
+ StgModel::Shutdown();
}
Index: matrix.c
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/matrix.c,v
retrieving revision 1.22.4.6
retrieving revision 1.22.4.7
diff -C2 -d -r1.22.4.6 -r1.22.4.7
*** matrix.c 10 Jul 2007 02:15:47 -0000 1.22.4.6
--- matrix.c 13 Jul 2007 05:48:31 -0000 1.22.4.7
***************
*** 50,57 ****
--- 50,73 ----
}
+ #include "model.hh"
+ GLvoid glPrint( const char *fmt, ... );
+
void stg_cell_render( stg_cell_t* cell )
{
double dx = cell->size/2.0;
glRectf( cell->x-dx , cell->y-dx, cell->x+dx, cell->y+dx );
+
+ /* int offset = 0; */
+
+ /* if( cell->data ) */
+ /* { */
+
+ /* int offset = 0; */
+ /* for( GSList* it = (GSList*)cell->data; it; it=it->next ) */
+ /* { */
+ /* glRasterPos2f( cell->x, cell->y + 0.02*offset++ ); */
+ /* glPrint( "%s", ((stg_block_t*)it->data)->mod->Token()); */
+ /* } */
+ /* } */
}
***************
*** 171,174 ****
--- 187,195 ----
}
+ void stg_matrix_remove_block( stg_matrix_t* matrix, stg_block_t* block )
+ {
+ stg_matrix_remove_object( matrix, (void*)block );
+ }
+
// remove <object> from the matrix
void stg_matrix_remove_object( stg_matrix_t* matrix, void* object )
***************
*** 284,296 ****
// debug rendering
//if( _render_matrix_deltas && ! cell->fig )
! stg_cell_render( cell );
}
// now the cell small enough, we add the object here
! cell->data = (GSList*)g_slist_prepend( (GSList*)cell->data, object );
// debug rendering
//if( _render_matrix_deltas && ! cell->fig )
! stg_cell_render( cell );
// add this object the hash table
--- 305,317 ----
// debug rendering
//if( _render_matrix_deltas && ! cell->fig )
! //stg_cell_render( cell );
}
// now the cell small enough, we add the object here
! cell->data = g_slist_prepend( (GSList*)cell->data, object );
// debug rendering
//if( _render_matrix_deltas && ! cell->fig )
! //stg_cell_render( cell );
// add this object the hash table
***************
*** 416,419 ****
--- 437,442 ----
stg_block_t* block )
{
+ //printf( "matrix block %p\n", block );
+
int p;
for( p=0; p<block->pt_count; p++ ) // for
-------------------------------------------------------------------------
This SF.net email is sponsored by DB2 Express
Download DB2 Express C - the FREE version of DB2 express and take
control of your XML. No limits. Just data. Click to get it now.
http://sourceforge.net/powerbar/db2/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
