Skip to content

Commit

Permalink
cleaning up worldfile loading
Browse files Browse the repository at this point in the history
  • Loading branch information
rtv committed Jul 6, 2008
1 parent 88919f9 commit 328cf81
Show file tree
Hide file tree
Showing 13 changed files with 259 additions and 258 deletions.
3 changes: 2 additions & 1 deletion libstage/canvas.cc
View file
Expand Up @@ -157,7 +157,8 @@ StgModel* StgCanvas::getModel( int x, int y )
glEnable(GL_BLEND);
glClearColor ( 0.7, 0.7, 0.8, 1.0);

Screenshot();
// useful for debugging the picker
//Screenshot();

return mod;
}
Expand Down
14 changes: 7 additions & 7 deletions libstage/model.cc
View file
Expand Up @@ -141,9 +141,9 @@ static const stg_color_t BUBBLE_TEXT = 0xFF000000; // black

// static members
uint32_t StgModel::count = 0;

GHashTable* StgModel::modelsbyid = g_hash_table_new( NULL, NULL );


// constructor
StgModel::StgModel( StgWorld* world,
StgModel* parent,
Expand Down Expand Up @@ -203,21 +203,20 @@ StgModel::StgModel( StgWorld* world,
this->blob_return = DEFAULT_BLOBRETURN;
this->laser_return = DEFAULT_LASERRETURN;
this->gripper_return = DEFAULT_GRIPPERRETURN;
this->fiducial_return = 0;
this->fiducial_key = 0;

this->boundary = DEFAULT_BOUNDARY;
this->color = DEFAULT_COLOR;
this->map_resolution = DEFAULT_MAP_RESOLUTION; // meters

this->gui_nose = DEFAULT_NOSE;
this->gui_grid = DEFAULT_GRID;
this->gui_outline = DEFAULT_OUTLINE;
this->gui_mask = this->parent ? 0 : DEFAULT_MASK;

this->fiducial_return = 0;
this->fiducial_key = 0;

this->callbacks = g_hash_table_new( g_int_hash, g_int_equal );

this->flag_list = NULL;

this->blinkenlights = g_ptr_array_new();

bzero( &this->velocity, sizeof(velocity));
Expand Down Expand Up @@ -740,7 +739,8 @@ void StgModel::DrawSelected()
stg_pose_t gpose = GetGlobalPose();

char buf[64];
snprintf( buf, 63, "%s [%.2f,%.2f,%.2f]", token, gpose.x, gpose.y, rtod(gpose.a) );
snprintf( buf, 63, "%s [%.2f %.2f %.2f %.2f]",
token, gpose.x, gpose.y, gpose.z, rtod(gpose.a) );

PushColor( 0,0,0,1 ); // text color black
gl_draw_string( 0.5,0.5,0.5, buf );
Expand Down
77 changes: 27 additions & 50 deletions libstage/model_load.cc
View file
Expand Up @@ -46,17 +46,8 @@ void StgModel::Load()
stg_geom_t geom = GetGeom();
geom.pose.x = wf->ReadTupleLength(wf_entity, "origin", 0, geom.pose.x );
geom.pose.y = wf->ReadTupleLength(wf_entity, "origin", 1, geom.pose.y );
geom.pose.a = wf->ReadTupleAngle(wf_entity, "origin", 2, geom.pose.a );
this->SetGeom( geom );
}

if( wf->PropertyExists( wf_entity, "origin4" ) )
{
stg_geom_t geom = GetGeom();
geom.pose.x = wf->ReadTupleLength(wf_entity, "origin4", 0, geom.pose.x );
geom.pose.y = wf->ReadTupleLength(wf_entity, "origin4", 1, geom.pose.y );
geom.pose.z = wf->ReadTupleLength(wf_entity, "origin4", 2, geom.pose.z );
geom.pose.a = wf->ReadTupleAngle(wf_entity, "origin4", 3, geom.pose.a );
geom.pose.z = wf->ReadTupleLength(wf_entity, "origin", 2, geom.pose.z );
geom.pose.a = wf->ReadTupleAngle(wf_entity, "origin", 3, geom.pose.a );
this->SetGeom( geom );
}

Expand All @@ -65,15 +56,7 @@ void StgModel::Load()
stg_geom_t geom = GetGeom();
geom.size.x = wf->ReadTupleLength(wf_entity, "size", 0, geom.size.x );
geom.size.y = wf->ReadTupleLength(wf_entity, "size", 1, geom.size.y );
this->SetGeom( geom );
}

if( wf->PropertyExists( wf_entity, "size3" ) )
{
stg_geom_t geom = GetGeom();
geom.size.x = wf->ReadTupleLength(wf_entity, "size3", 0, geom.size.x );
geom.size.y = wf->ReadTupleLength(wf_entity, "size3", 1, geom.size.y );
geom.size.z = wf->ReadTupleLength(wf_entity, "size3", 2, geom.size.z );
geom.size.z = wf->ReadTupleLength(wf_entity, "size", 2, geom.size.z );
this->SetGeom( geom );
}

Expand All @@ -82,19 +65,8 @@ void StgModel::Load()
stg_pose_t pose = GetPose();
pose.x = wf->ReadTupleLength(wf_entity, "pose", 0, pose.x );
pose.y = wf->ReadTupleLength(wf_entity, "pose", 1, pose.y );
pose.z = 0.0;
pose.a = wf->ReadTupleAngle(wf_entity, "pose", 2, pose.a );
this->SetPose( pose );
}

if( wf->PropertyExists( wf_entity, "pose4" ))
{
stg_pose_t pose = GetPose();
pose.x = wf->ReadTupleLength(wf_entity, "pose4", 0, pose.x );
pose.y = wf->ReadTupleLength(wf_entity, "pose4", 1, pose.y );
pose.z = wf->ReadTupleLength(wf_entity, "pose4", 2, pose.z );
pose.a = wf->ReadTupleAngle( wf_entity, "pose4", 3, pose.a );

pose.z = wf->ReadTupleLength(wf_entity, "pose", 2, pose.z );
pose.a = wf->ReadTupleAngle(wf_entity, "pose", 3, pose.a );
this->SetPose( pose );
}

Expand All @@ -103,22 +75,14 @@ void StgModel::Load()
stg_velocity_t vel = GetVelocity();
vel.x = wf->ReadTupleLength(wf_entity, "velocity", 0, vel.x );
vel.y = wf->ReadTupleLength(wf_entity, "velocity", 1, vel.y );
vel.z = wf->ReadTupleLength(wf_entity, "velocity", 2, vel.z );
vel.a = wf->ReadTupleAngle(wf_entity, "velocity", 3, vel.a );
this->SetVelocity( vel );

if( vel.x || vel.y || vel.z || vel.a )
world->StartUpdatingModel( this );
}

if( wf->PropertyExists( wf_entity, "velocity4" ))
{
stg_velocity_t vel = GetVelocity();
vel.x = wf->ReadTupleLength(wf_entity, "velocity4", 0, vel.x );
vel.y = wf->ReadTupleLength(wf_entity, "velocity4", 1, vel.y );
vel.z = wf->ReadTupleLength(wf_entity, "velocity4", 2, vel.z );
vel.a = wf->ReadTupleAngle(wf_entity, "velocity4", 3, vel.a );
this->SetVelocity( vel );
}

if( wf->PropertyExists( wf_entity, "boundary" ))
{
Expand Down Expand Up @@ -376,15 +340,28 @@ void StgModel::Save( void )
this->pose.y,
this->pose.a );

// right now we only save poses
wf->WriteTupleLength( wf_entity, "pose", 0, this->pose.x);
wf->WriteTupleLength( wf_entity, "pose", 1, this->pose.y);
wf->WriteTupleAngle( wf_entity, "pose", 2, this->pose.a);
if( wf->PropertyExists( wf_entity, "pose" ) )
{
wf->WriteTupleLength( wf_entity, "pose", 0, this->pose.x);
wf->WriteTupleLength( wf_entity, "pose", 1, this->pose.y);
wf->WriteTupleLength( wf_entity, "pose", 2, this->pose.z);
wf->WriteTupleAngle( wf_entity, "pose", 3, this->pose.a);
}

if( wf->PropertyExists( wf_entity, "size" ) )
{
wf->WriteTupleLength( wf_entity, "size", 0, this->geom.size.x);
wf->WriteTupleLength( wf_entity, "size", 1, this->geom.size.y);
wf->WriteTupleLength( wf_entity, "size", 2, this->geom.size.z);
}

wf->WriteTupleLength( wf_entity, "pose4", 0, this->pose.x);
wf->WriteTupleLength( wf_entity, "pose4", 1, this->pose.y);
wf->WriteTupleLength( wf_entity, "pose4", 2, this->pose.z);
wf->WriteTupleAngle( wf_entity, "pose4", 3, this->pose.a);
if( wf->PropertyExists( wf_entity, "origin" ) )
{
wf->WriteTupleLength( wf_entity, "origin", 0, this->geom.pose.x);
wf->WriteTupleLength( wf_entity, "origin", 1, this->geom.pose.y);
wf->WriteTupleLength( wf_entity, "origin", 2, this->geom.pose.z);
wf->WriteTupleAngle( wf_entity, "origin", 3, this->geom.pose.a);
}

// call any type-specific save callbacks
this->CallCallbacks( &this->save_hook );
Expand Down
64 changes: 48 additions & 16 deletions libstage/worldfile.cc
View file
Expand Up @@ -1500,9 +1500,15 @@ void Worldfile::WriteInt(int entity, const char *name, int value)
// Write a float
void Worldfile::WriteFloat(int entity, const char *name, double value)
{
char default_str[64];
snprintf(default_str, sizeof(default_str), "%.3f", value);
WriteString(entity, name, default_str);
if( fabs(value) < 0.001 ) // nearly 0
WriteString(entity, name, "0" ); // compact zeros make the file
// more readable
else
{
char default_str[64];
snprintf(default_str, sizeof(default_str), "%.3f", value);
WriteString(entity, name, default_str);
}
}


Expand Down Expand Up @@ -1531,9 +1537,17 @@ double Worldfile::ReadLength(int entity, const char *name, double value)
// Write a length (includes units conversion)
void Worldfile::WriteLength(int entity, const char *name, double value)
{
char default_str[64];
snprintf(default_str, sizeof(default_str), "%.3f", value / this->unit_length);
WriteString(entity, name, default_str);
value /= this->unit_length;

if( fabs(value) < 0.001 ) // nearly 0
WriteString(entity, name, "0" ); // compact zeros make the file
// more readable
else
{
char default_str[64];
snprintf(default_str, sizeof(default_str), "%.3f", value );
WriteString(entity, name, default_str);
}
}


Expand Down Expand Up @@ -1687,9 +1701,14 @@ double Worldfile::ReadTupleFloat(int entity, const char *name,
void Worldfile::WriteTupleFloat(int entity, const char *name,
int index, double value)
{
char default_str[64];
snprintf(default_str, sizeof(default_str), "%.3f", value);
WriteTupleString(entity, name, index, default_str);
if( fabs(value) < 0.001 ) // nearly 0
WriteTupleString(entity, name, index, "0" );
else
{
char default_str[64];
snprintf(default_str, sizeof(default_str), "%.3f", value);
WriteTupleString(entity, name, index, default_str);
}
}


Expand All @@ -1710,12 +1729,18 @@ double Worldfile::ReadTupleLength(int entity, const char *name,
void Worldfile::WriteTupleLength(int entity, const char *name,
int index, double value)
{
char default_str[64];
snprintf(default_str, sizeof(default_str), "%.3f", value / this->unit_length);
WriteTupleString(entity, name, index, default_str);
value /= this->unit_length;

if( fabs(value) < 0.001 ) // nearly 0
WriteTupleString(entity, name, index, "0" );
else
{
char default_str[64];
snprintf(default_str, sizeof(default_str), "%.3f", value );
WriteTupleString(entity, name, index, default_str);
}
}


///////////////////////////////////////////////////////////////////////////
// Read an angle from a tuple (includes unit conversion)
double Worldfile::ReadTupleAngle(int entity, const char *name,
Expand All @@ -1733,9 +1758,16 @@ double Worldfile::ReadTupleAngle(int entity, const char *name,
void Worldfile::WriteTupleAngle(int entity, const char *name,
int index, double value)
{
char default_str[64];
snprintf(default_str, sizeof(default_str), "%.3f", value / this->unit_angle);
WriteTupleString(entity, name, index, default_str);
value /= this->unit_angle;

if( fabs(value) < 0.001 ) // nearly 0
WriteTupleString(entity, name, index, "0" );
else
{
char default_str[64];
snprintf(default_str, sizeof(default_str), "%.3f", value );
WriteTupleString(entity, name, index, default_str);
}
}


Expand Down
2 changes: 1 addition & 1 deletion libstage/worldgui.cc
View file
Expand Up @@ -324,7 +324,7 @@ std::string StgWorldGui::ClockString()


if( paused == true )
status_stream << " --PAUSED--";
status_stream << " [ PAUSED ]";


return status_stream.str();
Expand Down

0 comments on commit 328cf81

Please sign in to comment.