Skip to content
Browse files

Merge branch 'upstream'

Conflicts:
	worlds/simple.world
  • Loading branch information...
2 parents 154e022 + a09d6ca commit 459bbc82cfc70bb9b6b15e8cd25e541e1b6763ec @jpgr87 committed
View
4 libstage/blockgroup.cc
@@ -258,7 +258,7 @@ void BlockGroup::LoadBlock( Model* mod, Worldfile* wf, int entity )
void BlockGroup::LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfile* wf )
{
- PRINT_DEBUG1( "attempting to load bitmap \"%s\n", bitmapfile );
+ PRINT_DEBUG1( "attempting to load bitmap \"%s\n", bitmapfile.c_str() );
std::string full;
@@ -271,7 +271,7 @@ void BlockGroup::LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfil
free( workaround_const );
}
- PRINT_DEBUG1( "attempting to load image %s", full );
+ PRINT_DEBUG1( "attempting to load image %s", full.c_str() );
std::vector<rotrect_t> rects;
unsigned int width, height;
View
4 libstage/model_actuator.cc
@@ -185,7 +185,7 @@ void ModelActuator::Load( void )
void ModelActuator::Update( void )
{
- PRINT_DEBUG1( "[%lu] actuator update", 0 );
+ PRINT_DEBUG1( "[%d] actuator update", 0 );
// stop by default
double velocity = 0;
@@ -226,7 +226,7 @@ void ModelActuator::Update( void )
{
PRINT_DEBUG( "actuator velocity control mode" );
PRINT_DEBUG2( "model %s command(%.2f)",
- this->token,
+ this->token.c_str(),
this->goal);
if ((pos <= min_position && goal < 0) || (pos >= max_position && goal > 0))
velocity = 0;
View
2 libstage/model_blinkenlight.cc
@@ -67,7 +67,7 @@ ModelBlinkenlight::ModelBlinkenlight( World* world,
on( true )
{
PRINT_DEBUG2( "Constructing ModelBlinkenlight %d (%s)\n",
- id, typestr );
+ id, type.c_str() );
// Set up sensible defaults
View
2 libstage/model_blobfinder.cc
@@ -93,7 +93,7 @@ blobfinder
scan_width( DEFAULT_BLOBFINDERSCANWIDTH )
{
PRINT_DEBUG2( "Constructing ModelBlobfinder %d (%s)\n",
- id, typestr );
+ id, type.c_str() );
ClearBlocks();
AddVisualizer( &this->vis, true );
View
2 libstage/model_camera.cc
@@ -103,7 +103,7 @@ ModelCamera::ModelCamera( World* world,
_pitch_offset( 0.0 )
{
PRINT_DEBUG2( "Constructing ModelCamera %d (%s)\n",
- id, typestr );
+ id, type.c_str() );
WorldGui* world_gui = dynamic_cast< WorldGui* >( world );
View
6 libstage/model_load.cc
@@ -20,7 +20,7 @@ void Model::Load()
assert( wf );
assert( wf_entity );
- PRINT_DEBUG1( "Model \"%s\" loading...", token );
+ PRINT_DEBUG1( "Model \"%s\" loading...", token.c_str() );
// choose the thread to run in, if thread_safe > 0
event_queue_num = wf->ReadInt( wf_entity, "event_queue", event_queue_num );
@@ -272,7 +272,7 @@ void Model::Save( void )
assert( wf_entity );
PRINT_DEBUG4( "saving model %s pose %.2f %.2f %.2f",
- token,
+ token.c_str(),
pose.x,
pose.y,
pose.a );
@@ -309,7 +309,7 @@ void Model::Save( void )
// call any type-specific save callbacks
CallCallbacks( CB_SAVE );
- PRINT_DEBUG1( "Model \"%s\" saving complete.", token );
+ PRINT_DEBUG1( "Model \"%s\" saving complete.", token.c_str() );
}
View
6 libstage/model_position.cc
@@ -104,7 +104,7 @@ ModelPosition::ModelPosition( World* world,
posevis()
{
PRINT_DEBUG2( "Constructing ModelPosition %d (%s)\n",
- id, typestr );
+ id, type.c_str() );
// assert that Update() is reentrant for this derived model
thread_safe = false;
@@ -217,7 +217,7 @@ void ModelPosition::Load( void )
void ModelPosition::Update( void )
{
- PRINT_DEBUG1( "[%lu] position update", this->world->sim_time );
+ PRINT_DEBUG1( "[%lu] position update", this->world->SimTimeNow() );
// stop by default
Velocity vel(0,0,0,0);
@@ -230,7 +230,7 @@ void ModelPosition::Update( void )
{
PRINT_DEBUG( "velocity control mode" );
PRINT_DEBUG4( "model %s command(%.2f %.2f %.2f)",
- this->token,
+ this->token.c_str(),
this->goal.x,
this->goal.y,
this->goal.a );
View
2 libstage/model_ranger.cc
@@ -91,7 +91,7 @@ ModelRanger::ModelRanger( World* world,
vis( world )
{
PRINT_DEBUG2( "Constructing ModelRanger %d (%s)\n",
- id, type );
+ id, type.c_str() );
// Set up sensible defaults
View
2 libstage/stage.hh
@@ -1,4 +1,4 @@
-
+//#define DEBUG 1
#ifndef STG_H
#define STG_H
/*
View
2 libstage/world.cc
@@ -190,7 +190,7 @@ World::World( const std::string& name,
World::~World( void )
{
- PRINT_DEBUG2( "destroying world %d %s", id, token.c_str() );
+ PRINT_DEBUG2( "destroying world %d %s", next_id, token.c_str() );
if( ground ) delete ground;
if( wf ) delete wf;
World::world_set.erase( this );
View
4 libstage/worldgui.cc
@@ -255,7 +255,7 @@ void WorldGui::Show()
void WorldGui::Load( const std::string& filename )
{
- PRINT_DEBUG1( "%s.Load()", token );
+ PRINT_DEBUG1( "%s.Load()", token.c_str() );
// needs to happen before StgWorld load, or we segfault with GL calls on some graphics cards
Fl::check();
@@ -311,7 +311,7 @@ void WorldGui::UnLoad()
bool WorldGui::Save( const char* filename )
{
- PRINT_DEBUG1( "%s.Save()", token );
+ PRINT_DEBUG1( "%s.Save()", token.c_str() );
// worldgui exclusive properties live in the top-level section
const int world_section = 0;
View
37 libstageplugin/p_driver.cc
@@ -499,20 +499,26 @@ Interface* StgDriver::LookupDevice( player_devaddr_t addr )
// subscribe to a device
int StgDriver::Subscribe(QueuePointer &queue,player_devaddr_t addr)
{
+ printf("StgDriver::Subscribe\n");
if( addr.interf == PLAYER_SIMULATION_CODE )
- return 0; // ok
+ {
+ printf("StgDriver::Subscribe: Something's subscribing to the simulation\n");
+ return 1; // ok
+ }
Interface* device = this->LookupDevice( addr );
if( device )
{
- device->Subscribe();
+ device->Subscribe();
device->Subscribe(queue);
- return Driver::Subscribe(addr);
+ puts("Found device");
+ int result = this->Driver::Subscribe(addr);
+ return result;
}
puts( "failed to find a device" );
- return 1; // error
+ return -1; // error
}
@@ -520,18 +526,18 @@ int StgDriver::Subscribe(QueuePointer &queue,player_devaddr_t addr)
int StgDriver::Unsubscribe(QueuePointer &queue,player_devaddr_t addr)
{
if( addr.interf == PLAYER_SIMULATION_CODE )
- return 0; // ok
+ return 1; // ok
Interface* device = this->LookupDevice( addr );
if( device )
{
- device->Unsubscribe();
+ device->Unsubscribe();
device->Unsubscribe(queue);
return Driver::Unsubscribe(addr);
}
else
- return 1; // error
+ return -1; // error
}
StgDriver::~StgDriver()
@@ -565,6 +571,7 @@ StgDriver::ProcessMessage(QueuePointer &resp_queue,
player_msghdr * hdr,
void * data)
{
+ printf("StgDriver::ProcessMessage\n");
// find the right interface to handle this config
Interface* in = this->LookupDevice( hdr->addr );
if( in )
@@ -583,8 +590,8 @@ StgDriver::ProcessMessage(QueuePointer &resp_queue,
void StgDriver::Update(void)
{
- Driver::ProcessMessages();
-
+ //printf("StgDriver::Update\n");
+ ProcessMessages();
FOR_EACH( it, this->devices )
{
Interface* interface = *it;
@@ -618,5 +625,15 @@ void StgDriver::Update(void)
}
}
}
-
+/*
+void StgDriver::Main(void)
+{
+ for(;;)
+ {
+ pthread_testcancel();
+ ProcessMessages();
+ this->UpdateDrivers();
+ }
+}
+*/
View
2 libstageplugin/p_driver.h
@@ -37,6 +37,8 @@ class StgDriver : public Driver
/// check for new commands and configs
virtual void Update();
+// virtual void Main();
+
/// all player devices share the same Stage world (for now)
static Stg::World* world;
static bool usegui;
View
6 libstageplugin/p_position.cc
@@ -175,9 +175,9 @@ int InterfacePosition::ProcessMessage(QueuePointer &resp_queue,
mod->est_pose.a = req->pose.pa;
PRINT_DEBUG3( "set odometry to (%.2f,%.2f,%.2f)",
- pose.x,
- pose.y,
- pose.a );
+ req->pose.px,
+ req->pose.py,
+ req->pose.pa );
this->driver->Publish( this->addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
View
3 libstageplugin/p_simulation.cc
@@ -190,7 +190,7 @@ int InterfaceSimulation::ProcessMessage(QueuePointer &resp_queue,
reply.pose.px = pose.x;
reply.pose.py = pose.y;
reply.pose.pa = pose.a;
-
+
this->driver->Publish( this->addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SIMULATION_REQ_GET_POSE2D,
@@ -209,6 +209,7 @@ int InterfaceSimulation::ProcessMessage(QueuePointer &resp_queue,
PLAYER_SIMULATION_REQ_SET_POSE2D,
this->addr))
{
+ printf("p_simulation: Got a request for 2D pose\n");
player_simulation_pose2d_req_t* req =
(player_simulation_pose2d_req_t*)data;
View
8 libstageplugin/stg_time.cc
@@ -48,7 +48,7 @@ StTime::~StTime()
// Get the simulator time
int StTime::GetTime(struct timeval* time)
{
- PRINT_DEBUG( "get time" );
+// PRINT_DEBUG( "get time" );
assert( this->driver );
@@ -58,14 +58,14 @@ int StTime::GetTime(struct timeval* time)
time->tv_sec = (int)floor(usec/million);
time->tv_usec = (int)rint(fmod(usec,million) * million);
- PRINT_DEBUG2( "time now %ld sec %ld usec", time->tv_sec, time->tv_usec );
+// PRINT_DEBUG2( "time now %ld sec %ld usec", time->tv_sec, time->tv_usec );
return 0;
}
int StTime::GetTimeDouble(double* time)
{
- PRINT_DEBUG( "get time (double)" );
+// PRINT_DEBUG( "get time (double)" );
assert( this->driver );
@@ -73,7 +73,7 @@ int StTime::GetTimeDouble(double* time)
*time = world->SimTimeNow() / million;
- PRINT_DEBUG1( "time now %f sec ", *time);
+// PRINT_DEBUG1( "time now %f sec ", *time);
return 0;
}
View
12 worlds/simple.cfg
@@ -14,7 +14,7 @@ driver
plugin "stageplugin"
# load the named file into the simulator
- worldfile "simple.world"
+ worldfile "simple.world"
)
# Create a Stage driver and attach position2d and laser interfaces
@@ -26,3 +26,13 @@ driver
model "r0"
)
+#driver
+#(
+# name "fakelocalize"
+## provides [ "localize:0" ]
+# provides [ "localize:0" "position2d:1" ]
+# requires [ "simulation:0" ]
+# model "r0"
+# request_timeout 2.0
+## alwayson 1
+#)
View
4 worlds/simple.world
@@ -15,6 +15,8 @@ resolution 0.02
threads 1
+
+
# configure the GUI window
window
(
@@ -38,6 +40,7 @@ floorplan
pioneer2dx
(
+ debug 9
# can refer to the robot by this name
name "r0"
pose [ -6.432 -5.895 0 45.000 ]
@@ -49,6 +52,7 @@ pioneer2dx
# you probably should comment this out when using simple.cfg with Player
ctrl "wander"
watts 10
+
# report error-free position in world coordinates
localization "gps"
localization_origin [ 0 0 0 0 ]

0 comments on commit 459bbc8

Please sign in to comment.
Something went wrong with that request. Please try again.