Skip to content

Commit a09d6ca

Browse files
richmattesrichmattes
committed
Fixed PRINT_DEBUG statements, added some temporary debugging printfs
1 parent 6e3c9e4 commit a09d6ca

18 files changed

+71
-38
lines changed

‎libstage/blockgroup.cc

+2-2
Original file line numberDiff line numberDiff line change
@@ -255,7 +255,7 @@ void BlockGroup::LoadBlock( Model* mod, Worldfile* wf, int entity )
255255

256256
void BlockGroup::LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile* wf )
257257
{
258-
PRINT_DEBUG1( "attempting to load bitmap \"%s\n", bitmapfile );
258+
PRINT_DEBUG1( "attempting to load bitmap \"%s\n", bitmapfile.c_str() );
259259

260260
std::string full;
261261

@@ -268,7 +268,7 @@ void BlockGroup::LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfil
268268
free( workaround_const );
269269
}
270270

271-
PRINT_DEBUG1( "attempting to load image %s", full );
271+
PRINT_DEBUG1( "attempting to load image %s", full.c_str() );
272272

273273
std::vector<rotrect_t> rects;
274274
unsigned int width, height;

‎libstage/model_actuator.cc

+2-2
Original file line numberDiff line numberDiff line change
@@ -185,7 +185,7 @@ void ModelActuator::Load( void )
185185

186186
void ModelActuator::Update( void )
187187
{
188-
PRINT_DEBUG1( "[%lu] actuator update", 0 );
188+
PRINT_DEBUG1( "[%d] actuator update", 0 );
189189

190190
// stop by default
191191
double velocity = 0;
@@ -223,7 +223,7 @@ void ModelActuator::Update( void )
223223
{
224224
PRINT_DEBUG( "actuator velocity control mode" );
225225
PRINT_DEBUG2( "model %s command(%.2f)",
226-
this->token,
226+
this->token.c_str(),
227227
this->goal);
228228
if ((pos <= min_position && goal < 0) || (pos >= max_position && goal > 0))
229229
velocity = 0;

‎libstage/model_blinkenlight.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -67,7 +67,7 @@ ModelBlinkenlight::ModelBlinkenlight( World* world,
6767
on( true )
6868
{
6969
PRINT_DEBUG2( "Constructing ModelBlinkenlight %d (%s)\n",
70-
id, typestr );
70+
id, type.c_str() );
7171

7272
// Set up sensible defaults
7373

‎libstage/model_blobfinder.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -93,7 +93,7 @@ blobfinder
9393
scan_width( DEFAULT_BLOBFINDERSCANWIDTH )
9494
{
9595
PRINT_DEBUG2( "Constructing ModelBlobfinder %d (%s)\n",
96-
id, typestr );
96+
id, type.c_str() );
9797
ClearBlocks();
9898

9999
AddVisualizer( &this->vis, true );

‎libstage/model_camera.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -103,7 +103,7 @@ ModelCamera::ModelCamera( World* world,
103103
_pitch_offset( 0.0 )
104104
{
105105
PRINT_DEBUG2( "Constructing ModelCamera %d (%s)\n",
106-
id, typestr );
106+
id, type.c_str() );
107107

108108
WorldGui* world_gui = dynamic_cast< WorldGui* >( world );
109109

‎libstage/model_load.cc

+3-3
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ void Model::Load()
2020
assert( wf );
2121
assert( wf_entity );
2222

23-
PRINT_DEBUG1( "Model \"%s\" loading...", token );
23+
PRINT_DEBUG1( "Model \"%s\" loading...", token.c_str() );
2424

2525
// choose the thread to run in, if thread_safe > 0
2626
event_queue_num = wf->ReadInt( wf_entity, "event_queue", event_queue_num );
@@ -272,7 +272,7 @@ void Model::Save( void )
272272
assert( wf_entity );
273273

274274
PRINT_DEBUG4( "saving model %s pose %.2f %.2f %.2f",
275-
token,
275+
token.c_str(),
276276
pose.x,
277277
pose.y,
278278
pose.a );
@@ -309,7 +309,7 @@ void Model::Save( void )
309309
// call any type-specific save callbacks
310310
CallCallbacks( CB_SAVE );
311311

312-
PRINT_DEBUG1( "Model \"%s\" saving complete.", token );
312+
PRINT_DEBUG1( "Model \"%s\" saving complete.", token.c_str() );
313313
}
314314

315315

‎libstage/model_position.cc

+3-3
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,7 @@ ModelPosition::ModelPosition( World* world,
104104
posevis()
105105
{
106106
PRINT_DEBUG2( "Constructing ModelPosition %d (%s)\n",
107-
id, typestr );
107+
id, type.c_str() );
108108

109109
// assert that Update() is reentrant for this derived model
110110
thread_safe = false;
@@ -217,7 +217,7 @@ void ModelPosition::Load( void )
217217

218218
void ModelPosition::Update( void )
219219
{
220-
PRINT_DEBUG1( "[%lu] position update", this->world->sim_time );
220+
PRINT_DEBUG1( "[%lu] position update", this->world->SimTimeNow() );
221221

222222
// stop by default
223223
Velocity vel(0,0,0,0);
@@ -230,7 +230,7 @@ void ModelPosition::Update( void )
230230
{
231231
PRINT_DEBUG( "velocity control mode" );
232232
PRINT_DEBUG4( "model %s command(%.2f %.2f %.2f)",
233-
this->token,
233+
this->token.c_str(),
234234
this->goal.x,
235235
this->goal.y,
236236
this->goal.a );

‎libstage/model_ranger.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -91,7 +91,7 @@ ModelRanger::ModelRanger( World* world,
9191
vis( world )
9292
{
9393
PRINT_DEBUG2( "Constructing ModelRanger %d (%s)\n",
94-
id, type );
94+
id, type.c_str() );
9595

9696
// Set up sensible defaults
9797

‎libstage/stage.hh

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
1+
//#define DEBUG 1
22
#ifndef STG_H
33
#define STG_H
44
/*

‎libstage/world.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -190,7 +190,7 @@ World::World( const std::string& name,
190190

191191
World::~World( void )
192192
{
193-
PRINT_DEBUG2( "destroying world %d %s", id, token.c_str() );
193+
PRINT_DEBUG2( "destroying world %d %s", next_id, token.c_str() );
194194
if( ground ) delete ground;
195195
if( wf ) delete wf;
196196
World::world_set.erase( this );

‎libstage/worldgui.cc

+2-2
Original file line numberDiff line numberDiff line change
@@ -255,7 +255,7 @@ void WorldGui::Show()
255255

256256
void WorldGui::Load( const std::string& filename )
257257
{
258-
PRINT_DEBUG1( "%s.Load()", token );
258+
PRINT_DEBUG1( "%s.Load()", token.c_str() );
259259

260260
// needs to happen before StgWorld load, or we segfault with GL calls on some graphics cards
261261
Fl::check();
@@ -311,7 +311,7 @@ void WorldGui::UnLoad()
311311

312312
bool WorldGui::Save( const char* filename )
313313
{
314-
PRINT_DEBUG1( "%s.Save()", token );
314+
PRINT_DEBUG1( "%s.Save()", token.c_str() );
315315

316316
// worldgui exclusive properties live in the top-level section
317317
const int world_section = 0;

‎libstageplugin/p_driver.cc

+27-10
Original file line numberDiff line numberDiff line change
@@ -499,39 +499,45 @@ Interface* StgDriver::LookupDevice( player_devaddr_t addr )
499499
// subscribe to a device
500500
int StgDriver::Subscribe(QueuePointer &queue,player_devaddr_t addr)
501501
{
502+
printf("StgDriver::Subscribe\n");
502503
if( addr.interf == PLAYER_SIMULATION_CODE )
503-
return 0; // ok
504+
{
505+
printf("StgDriver::Subscribe: Something's subscribing to the simulation\n");
506+
return 1; // ok
507+
}
504508

505509
Interface* device = this->LookupDevice( addr );
506510

507511
if( device )
508512
{
509-
device->Subscribe();
513+
device->Subscribe();
510514
device->Subscribe(queue);
511-
return Driver::Subscribe(addr);
515+
puts("Found device");
516+
int result = this->Driver::Subscribe(addr);
517+
return result;
512518
}
513519

514520
puts( "failed to find a device" );
515-
return 1; // error
521+
return -1; // error
516522
}
517523

518524

519525
// unsubscribe to a device
520526
int StgDriver::Unsubscribe(QueuePointer &queue,player_devaddr_t addr)
521527
{
522528
if( addr.interf == PLAYER_SIMULATION_CODE )
523-
return 0; // ok
529+
return 1; // ok
524530

525531
Interface* device = this->LookupDevice( addr );
526532

527533
if( device )
528534
{
529-
device->Unsubscribe();
535+
device->Unsubscribe();
530536
device->Unsubscribe(queue);
531537
return Driver::Unsubscribe(addr);
532538
}
533539
else
534-
return 1; // error
540+
return -1; // error
535541
}
536542

537543
StgDriver::~StgDriver()
@@ -565,6 +571,7 @@ StgDriver::ProcessMessage(QueuePointer &resp_queue,
565571
player_msghdr * hdr,
566572
void * data)
567573
{
574+
printf("StgDriver::ProcessMessage\n");
568575
// find the right interface to handle this config
569576
Interface* in = this->LookupDevice( hdr->addr );
570577
if( in )
@@ -583,8 +590,8 @@ StgDriver::ProcessMessage(QueuePointer &resp_queue,
583590

584591
void StgDriver::Update(void)
585592
{
586-
Driver::ProcessMessages();
587-
593+
//printf("StgDriver::Update\n");
594+
ProcessMessages();
588595
FOR_EACH( it, this->devices )
589596
{
590597
Interface* interface = *it;
@@ -619,5 +626,15 @@ void StgDriver::Update(void)
619626
}
620627
}
621628
}
622-
629+
/*
630+
void StgDriver::Main(void)
631+
{
632+
for(;;)
633+
{
634+
pthread_testcancel();
635+
ProcessMessages();
636+
this->UpdateDrivers();
637+
}
638+
}
639+
*/
623640

‎libstageplugin/p_driver.h

+2
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,8 @@ class StgDriver : public Driver
3737
/// check for new commands and configs
3838
virtual void Update();
3939

40+
// virtual void Main();
41+
4042
/// all player devices share the same Stage world (for now)
4143
static Stg::World* world;
4244
static bool usegui;

‎libstageplugin/p_position.cc

+3-3
Original file line numberDiff line numberDiff line change
@@ -175,9 +175,9 @@ int InterfacePosition::ProcessMessage(QueuePointer &resp_queue,
175175
mod->est_pose.a = req->pose.pa;
176176

177177
PRINT_DEBUG3( "set odometry to (%.2f,%.2f,%.2f)",
178-
pose.x,
179-
pose.y,
180-
pose.a );
178+
req->pose.px,
179+
req->pose.py,
180+
req->pose.pa );
181181

182182
this->driver->Publish( this->addr, resp_queue,
183183
PLAYER_MSGTYPE_RESP_ACK,

‎libstageplugin/p_simulation.cc

+2-1
Original file line numberDiff line numberDiff line change
@@ -188,7 +188,7 @@ int InterfaceSimulation::ProcessMessage(QueuePointer &resp_queue,
188188
reply.pose.px = pose.x;
189189
reply.pose.py = pose.y;
190190
reply.pose.pa = pose.a;
191-
191+
192192
this->driver->Publish( this->addr, resp_queue,
193193
PLAYER_MSGTYPE_RESP_ACK,
194194
PLAYER_SIMULATION_REQ_GET_POSE2D,
@@ -207,6 +207,7 @@ int InterfaceSimulation::ProcessMessage(QueuePointer &resp_queue,
207207
PLAYER_SIMULATION_REQ_SET_POSE2D,
208208
this->addr))
209209
{
210+
printf("p_simulation: Got a request for 2D pose\n");
210211
player_simulation_pose2d_req_t* req =
211212
(player_simulation_pose2d_req_t*)data;
212213

‎libstageplugin/stg_time.cc

+4-4
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ StTime::~StTime()
4848
// Get the simulator time
4949
int StTime::GetTime(struct timeval* time)
5050
{
51-
PRINT_DEBUG( "get time" );
51+
// PRINT_DEBUG( "get time" );
5252

5353
assert( this->driver );
5454

@@ -58,22 +58,22 @@ int StTime::GetTime(struct timeval* time)
5858
time->tv_sec = (int)floor(usec/million);
5959
time->tv_usec = (int)rint(fmod(usec,million) * million);
6060

61-
PRINT_DEBUG2( "time now %ld sec %ld usec", time->tv_sec, time->tv_usec );
61+
// PRINT_DEBUG2( "time now %ld sec %ld usec", time->tv_sec, time->tv_usec );
6262

6363
return 0;
6464
}
6565

6666
int StTime::GetTimeDouble(double* time)
6767
{
68-
PRINT_DEBUG( "get time (double)" );
68+
// PRINT_DEBUG( "get time (double)" );
6969

7070
assert( this->driver );
7171

7272
World* world = driver->world;
7373

7474
*time = world->SimTimeNow() / million;
7575

76-
PRINT_DEBUG1( "time now %f sec ", *time);
76+
// PRINT_DEBUG1( "time now %f sec ", *time);
7777

7878
return 0;
7979
}

‎worlds/simple.cfg

+11-1
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ driver
1414
plugin "stageplugin"
1515

1616
# load the named file into the simulator
17-
worldfile "simple.world"
17+
worldfile "simple.world"
1818
)
1919

2020
# Create a Stage driver and attach position2d and laser interfaces
@@ -26,3 +26,13 @@ driver
2626
model "r0"
2727
)
2828

29+
#driver
30+
#(
31+
# name "fakelocalize"
32+
## provides [ "localize:0" ]
33+
# provides [ "localize:0" "position2d:1" ]
34+
# requires [ "simulation:0" ]
35+
# model "r0"
36+
# request_timeout 2.0
37+
## alwayson 1
38+
#)

‎worlds/simple.world

+4-1
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,8 @@ resolution 0.02
1515

1616
threads 1
1717

18+
19+
1820
# configure the GUI window
1921
window
2022
(
@@ -38,6 +40,7 @@ floorplan
3840

3941
pioneer2dx
4042
(
43+
debug 9
4144
# can refer to the robot by this name
4245
name "r0"
4346
pose [ -6.432 -5.895 0 45.000 ]
@@ -47,7 +50,7 @@ pioneer2dx
4750

4851
# demonstrate a plugin controller, implemented in examples/ctrl/wander.cc
4952
# you probably should comment this out when using simple.cfg with Player
50-
ctrl "wander"
53+
# ctrl "wander"
5154

5255
# report error-free position in world coordinates
5356
localization "gps"

0 commit comments

Comments
 (0)