Skip to content
Browse files

combined laser and ranger functionality into ranger - still buggy

  • Loading branch information...
1 parent e75c767 commit e45b70c4aa92f53054349fbfe1f81f067a9d29d6 Richard Vaughan committed Jul 20, 2010
Showing with 1,437 additions and 1,158 deletions.
  1. +6 −5 examples/ctrl/CMakeLists.txt
  2. +4 −0 examples/ctrl/astar/astar.h
  3. +3 −2 examples/ctrl/astar/findpath.cpp
  4. +10 −10 examples/ctrl/fasr.cc
  5. +36 −36 examples/ctrl/fasr2.cc
  6. +19 −19 examples/ctrl/fasr_plan.cc
  7. +1 −1 examples/ctrl/lasernoise.cc
  8. +5 −5 examples/ctrl/pioneer_flocking.cc
  9. +25 −14 examples/ctrl/sink.cc
  10. +1 −1 examples/ctrl/source.cc
  11. +9 −9 examples/ctrl/wander.cc
  12. +5 −5 examples/ctrl/wander_pioneer.cc
  13. +2 −2 examples/libstage/stest.cc
  14. +10 −10 libstage/CMakeLists.txt
  15. +1 −1 libstage/ancestor.cc
  16. +31 −31 libstage/block.cc
  17. +5 −5 libstage/blockgroup.cc
  18. +7 −7 libstage/canvas.cc
  19. +1 −1 libstage/canvas.hh
  20. +1 −1 libstage/gl.cc
  21. +1 −1 libstage/logentry.cc
  22. +126 −52 libstage/model.cc
  23. +7 −7 libstage/model_blobfinder.cc
  24. +9 −9 libstage/model_callbacks.cc
  25. +3 −3 libstage/model_draw.cc
  26. +7 −7 libstage/model_fiducial.cc
  27. +13 −18 libstage/model_getset.cc
  28. +3 −3 libstage/model_gripper.cc
  29. +3 −3 libstage/model_laser.cc
  30. +6 −4 libstage/model_load.cc
  31. +2 −2 libstage/model_position.cc
  32. +1 −1 libstage/model_props.cc
  33. +326 −205 libstage/model_ranger.cc
  34. +30 −30 libstage/model_wifi.cc
  35. +32 −32 libstage/powerpack.cc
  36. +2 −2 libstage/puck.cc
  37. +1 −1 libstage/region.cc
  38. +2 −2 libstage/region.hh
  39. +8 −8 libstage/stage.cc
  40. +378 −334 libstage/stage.hh
  41. +0 −1 libstage/typetable.cc
  42. +47 −32 libstage/world.cc
  43. +4 −4 libstage/worldgui.cc
  44. +1 −1 libstageplugin/p_actarray.cc
  45. +1 −1 libstageplugin/p_blobfinder.cc
  46. +6 −6 libstageplugin/p_bumper.cc
  47. +2 −5 libstageplugin/p_driver.cc
  48. +26 −12 libstageplugin/p_driver.h
  49. +6 −6 libstageplugin/p_fiducial.cc
  50. +10 −1 libstageplugin/p_laser.cc
  51. +1 −1 libstageplugin/p_localize.cc
  52. +12 −12 libstageplugin/p_map.cc
  53. +3 −3 libstageplugin/p_position.cc
  54. +3 −3 libstageplugin/p_power.cc
  55. +9 −9 libstageplugin/p_ptz.cc
  56. +3 −3 libstageplugin/p_simulation.cc
  57. +1 −1 libstageplugin/p_sonar.cc
  58. +1 −1 libstageplugin/p_speech.cc
  59. +2 −2 libstageplugin/p_wifi.cc
  60. +1 −1 libstageplugin/stg_time.cc
  61. +4 −4 webstage/webstage.cc
  62. +1 −1 worlds/CMakeLists.txt
  63. +1 −1 worlds/benchmark/expand_pioneer.cc
  64. +1 −1 worlds/benchmark/expand_swarm.cc
  65. +38 −38 worlds/fasr.world
  66. +2 −3 worlds/map.inc
  67. +91 −103 worlds/pioneer.inc
  68. +13 −9 worlds/sick.inc
  69. +5 −4 worlds/simple.world
View
11 examples/ctrl/CMakeLists.txt
@@ -1,11 +1,11 @@
SET( PLUGINS
- fasr
+# fasr
sink
source
wander
- wander_pioneer
- pioneer_flocking
+# wander_pioneer
+# pioneer_flocking
rasterize
dynamic
)
@@ -22,10 +22,11 @@ foreach( PLUGIN ${PLUGINS} )
ADD_LIBRARY( ${PLUGIN} MODULE ${PLUGIN}.cc )
endforeach( PLUGIN )
-ADD_LIBRARY( fasr2 MODULE fasr2.cc astar/findpath.cpp )
+#ADD_LIBRARY( fasr2 MODULE fasr2.cc astar/findpath.cpp )
+#ADD_LIBRARY( energy MODULE energy.cc astar/findpath.cpp )
# add fasr2 to the list of plugins
-SET( PLUGINS ${PLUGINS} fasr2 )
+#SET( PLUGINS ${PLUGINS} fasr2 energy)
set_source_files_properties( ${PLUGINS} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
View
4 examples/ctrl/astar/astar.h
@@ -1,4 +1,7 @@
+namespace ast
+{
+
class point_t
{
public:
@@ -14,3 +17,4 @@ bool astar( uint8_t* map,
const point_t goal,
std::vector<point_t>& path );
+}
View
5 examples/ctrl/astar/findpath.cpp
@@ -17,7 +17,7 @@
#define DEBUG_LIST_LENGTHS_ONLY 0
using namespace std;
-
+//using namespace astar;
// map helper functions
@@ -181,8 +181,9 @@ float MapSearchNode::GetCost( MapSearchNode &successor )
// Main
#include "astar.h"
+using namespace ast;
-bool astar( uint8_t* map,
+bool ast::astar( uint8_t* map,
uint32_t width,
uint32_t height,
const point_t start,
View
20 examples/ctrl/fasr.cc
@@ -93,25 +93,25 @@ class Robot
// pos->GetUnusedModelOfType( "laser" );
// PositionUpdate() checks to see if we reached source or sink
- pos->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)PositionUpdate, this );
+ pos->AddCallback( Model::CB_UPDATE, (model_callback_t)PositionUpdate, this );
pos->Subscribe();
// LaserUpdate() controls the robot, by reading from laser and
// writing to position
- laser->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)LaserUpdate, this );
+ laser->AddCallback( Model::CB_UPDATE, (model_callback_t)LaserUpdate, this );
laser->Subscribe();
- fiducial->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)FiducialUpdate, this );
+ fiducial->AddCallback( Model::CB_UPDATE, (model_callback_t)FiducialUpdate, this );
fiducial->Subscribe();
- //gripper->AddUpdateCallback( (stg_model_callback_t)GripperUpdate, this );
+ //gripper->AddUpdateCallback( (model_callback_t)GripperUpdate, this );
//gripper->Subscribe();
- //blobfinder->AddUpdateCallback( (stg_model_callback_t)BlobFinderUpdate, this );
+ //blobfinder->AddUpdateCallback( (model_callback_t)BlobFinderUpdate, this );
//blobfinder->Subscribe();
- //pos->AddFlagIncrCallback( (stg_model_callback_t)FlagIncr, NULL );
- //pos->AddFlagDecrCallback( (stg_model_callback_t)FlagDecr, NULL );
+ //pos->AddFlagIncrCallback( (model_callback_t)FlagIncr, NULL );
+ //pos->AddFlagDecrCallback( (model_callback_t)FlagDecr, NULL );
}
void Dock()
@@ -174,9 +174,9 @@ class Robot
void UnDock()
{
- //const stg_meters_t gripper_distance = 0.2;
- const stg_meters_t back_off_distance = 0.3;
- const stg_meters_t back_off_speed = -0.05;
+ //const meters_t gripper_distance = 0.2;
+ const meters_t back_off_distance = 0.3;
+ const meters_t back_off_speed = -0.05;
// back up a bit
if( charger_range < back_off_distance )
View
72 examples/ctrl/fasr2.cc
@@ -19,18 +19,18 @@ static const unsigned int PAYLOAD = 1;
static unsigned int
-MetersToCell( stg_meters_t m, stg_meters_t size_m, unsigned int size_c )
+MetersToCell( meters_t m, meters_t size_m, unsigned int size_c )
{
m += size_m / 2.0; // shift to local coords
m /= size_m / (float)size_c; // scale
return (unsigned int)floor(m); // quantize
}
-static stg_meters_t
-CellToMeters( unsigned int c, stg_meters_t size_m, unsigned int size_c )
+static meters_t
+CellToMeters( unsigned int c, meters_t size_m, unsigned int size_c )
{
- stg_meters_t cell_size = size_m/(float)size_c;
- stg_meters_t m = c * cell_size; // scale
+ meters_t cell_size = size_m/(float)size_c;
+ meters_t m = c * cell_size; // scale
m -= size_m / 2.0; // shift to local coords
return m + cell_size/2.0; // offset to cell center
@@ -115,7 +115,7 @@ class Robot
FOR_EACH( it, nodes ){ (*it)->Draw(); }
}
- bool GoodDirection( const Pose& pose, stg_meters_t range, stg_radians_t& heading_result )
+ bool GoodDirection( const Pose& pose, meters_t range, radians_t& heading_result )
{
// find the node with the lowest value within range of the given
// pose and return the absolute heading from pose to that node
@@ -143,7 +143,7 @@ class Robot
if( best_node == NULL )
{
- printf( "FASR warning: no nodes in range" );
+ fprintf( stderr, "FASR warning: no nodes in range" );
return false;
}
@@ -217,7 +217,7 @@ class Robot
MODE_QUEUE
} mode;
- stg_radians_t docked_angle;
+ radians_t docked_angle;
static pthread_mutex_t planner_mutex;
@@ -290,7 +290,7 @@ class Robot
EnableLaser(true);
// we access all other data from the position callback
- pos->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)UpdateCallback, this );
+ pos->AddCallback( Model::CB_UPDATE, (model_callback_t)UpdateCallback, this );
pos->Subscribe();
pos->AddVisualizer( &graphvis, true );
@@ -368,7 +368,7 @@ class Robot
static std::map< std::pair<uint64_t,uint64_t>, Graph*> plancache;
- uint64_t Pt64( const point_t& pt)
+ uint64_t Pt64( const ast::point_t& pt)
{
// quantize the position a bit to reduce planning frequency
uint64_t x = pt.x / 1;
@@ -377,13 +377,13 @@ class Robot
return (x<<32) + y;
}
- void CachePlan( const point_t& start, const point_t& goal, Graph* graph )
+ void CachePlan( const ast::point_t& start, const ast::point_t& goal, Graph* graph )
{
std::pair<uint64_t,uint64_t> key( Pt64(start),Pt64(goal));
plancache[key] = graph;
}
- Graph* LookupPlan( const point_t& start, const point_t& goal )
+ Graph* LookupPlan( const ast::point_t& start, const ast::point_t& goal )
{
std::pair<uint64_t,uint64_t> key(Pt64(start),Pt64(goal));
return plancache[key];
@@ -401,9 +401,9 @@ class Robot
Pose pose = pos->GetPose();
Geom g = map_model->GetGeom();
- point_t start( MetersToCell(pose.x, g.size.x, map_width),
+ ast::point_t start( MetersToCell(pose.x, g.size.x, map_width),
MetersToCell(pose.y, g.size.y, map_height) );
- point_t goal( MetersToCell(sp.x, g.size.x, map_width),
+ ast::point_t goal( MetersToCell(sp.x, g.size.x, map_width),
MetersToCell(sp.y, g.size.y, map_height) );
//printf( "searching from (%.2f, %.2f) [%d, %d]\n", pose.x, pose.y, start.x, start.y );
@@ -427,14 +427,14 @@ class Robot
if( ! graphp ) // no plan cached
{
misses++;
-
- std::vector<point_t> path;
- bool result = astar( map_data,
- map_width,
- map_height,
- start,
- goal,
- path );
+
+ std::vector<ast::point_t> path;
+ bool result = ast::astar( map_data,
+ map_width,
+ map_height,
+ start,
+ goal,
+ path );
// if( ! result )
//printf( "FASR warning: plan failed to find path from (%.2f,%.2f) to (%.2f,%.2f)\n",
@@ -447,7 +447,7 @@ class Robot
Node* last_node = NULL;
- for( std::vector<point_t>::const_reverse_iterator rit = path.rbegin();
+ for( std::vector<ast::point_t>::const_reverse_iterator rit = path.rbegin();
rit != path.rend();
++rit )
{
@@ -481,7 +481,7 @@ class Robot
void Dock()
{
- const stg_meters_t creep_distance = 0.5;
+ const meters_t creep_distance = 0.5;
if( charger_ahoy )
{
@@ -542,10 +542,10 @@ class Robot
void UnDock()
{
- const stg_meters_t back_off_distance = 0.2;
- const stg_meters_t back_off_speed = -0.02;
- const stg_radians_t undock_rotate_speed = 0.3;
- const stg_meters_t wait_distance = 0.2;
+ const meters_t back_off_distance = 0.2;
+ const meters_t back_off_speed = -0.02;
+ const radians_t undock_rotate_speed = 0.3;
+ const meters_t wait_distance = 0.2;
const unsigned int BACK_SENSOR_FIRST = 10;
const unsigned int BACK_SENSOR_LAST = 13;
@@ -772,12 +772,12 @@ class Robot
seconds = timenow;
// report the task participation
- printf( "time: %d sec\n", seconds );
+ // printf( "time: %d sec\n", seconds );
- unsigned int sz = tasks.size();
- for( unsigned int i=0; i<sz; ++i )
- printf( "\t task[%u] %3u (%s)\n",
- i, tasks[i].participants, tasks[i].source->Token() );
+// unsigned int sz = tasks.size();
+// for( unsigned int i=0; i<sz; ++i )
+// printf( "\t task[%u] %3u (%s)\n",
+// i, tasks[i].participants, tasks[i].source->Token() );
}
}
@@ -834,7 +834,7 @@ class Robot
Pose sourcepose = goal->GetPose();
Geom sourcegeom = goal->GetGeom();
- stg_meters_t dest_dist = hypot( sourcepose.x-pose.x, sourcepose.y-pose.y );
+ meters_t dest_dist = hypot( sourcepose.x-pose.x, sourcepose.y-pose.y );
// when we get close enough, we start waiting
if( dest_dist < sourcegeom.size.x/2.0 ) // nearby
@@ -966,8 +966,8 @@ void Robot::Node::Draw() const
// STATIC VARS
pthread_mutex_t Robot::planner_mutex = PTHREAD_MUTEX_INITIALIZER;
-const unsigned int Robot::map_width( 50 );
-const unsigned int Robot::map_height( 50 );
+const unsigned int Robot::map_width( 32 );
+const unsigned int Robot::map_height( 32 );
uint8_t* Robot::map_data( NULL );
Model* Robot::map_model( NULL );
View
38 examples/ctrl/fasr_plan.cc
@@ -271,23 +271,23 @@ class Robot
assert( sink );
// PositionUpdate() checks to see if we reached source or sink
- pos->AddUpdateCallback( (stg_model_callback_t)PositionUpdate, this );
+ pos->AddUpdateCallback( (model_callback_t)PositionUpdate, this );
pos->Subscribe();
// LaserUpdate() controls the robot, by reading from laser and
// writing to position
- laser->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, this );
+ laser->AddUpdateCallback( (model_callback_t)LaserUpdate, this );
laser->Subscribe();
- fiducial->AddUpdateCallback( (stg_model_callback_t)FiducialUpdate, this );
+ fiducial->AddUpdateCallback( (model_callback_t)FiducialUpdate, this );
fiducial->Subscribe();
- //gripper->AddUpdateCallback( (stg_model_callback_t)GripperUpdate, this );
+ //gripper->AddUpdateCallback( (model_callback_t)GripperUpdate, this );
gripper->Subscribe();
if( blobfinder ) // optional
{
- blobfinder->AddUpdateCallback( (stg_model_callback_t)BlobFinderUpdate, this );
+ blobfinder->AddUpdateCallback( (model_callback_t)BlobFinderUpdate, this );
blobfinder->Subscribe();
}
@@ -363,9 +363,9 @@ class Robot
void UnDock()
{
- const stg_meters_t gripper_distance = 0.2;
- const stg_meters_t back_off_distance = 0.3;
- const stg_meters_t back_off_speed = -0.05;
+ const meters_t gripper_distance = 0.2;
+ const meters_t back_off_distance = 0.3;
+ const meters_t back_off_speed = -0.05;
// back up a bit
if( charger_range < back_off_distance )
@@ -402,7 +402,7 @@ class Robot
// Get the data
uint32_t sample_count=0;
- stg_laser_sample_t* scan = laser->GetSamples( &sample_count );
+ laser_sample_t* scan = laser->GetSamples( &sample_count );
for (uint32_t i = 0; i < sample_count; i++)
{
@@ -523,7 +523,7 @@ class Robot
typedef struct
{
- stg_laser_sample_t* scan;
+ laser_sample_t* scan;
Pose pose;
} scan_record_t;
@@ -535,7 +535,7 @@ class Robot
// laser->Token(), laser->power_pack );
uint32_t sample_count=0;
- stg_laser_sample_t* scan = laser->GetSamples( &sample_count );
+ laser_sample_t* scan = laser->GetSamples( &sample_count );
if( scan == NULL )
return 0;
@@ -544,8 +544,8 @@ class Robot
sr->pose = laser->GetGlobalPose();
// deep copy the samples
- sr->scan = new stg_laser_sample_t[sample_count];
- memcpy( sr->scan, scan, sample_count * sizeof(stg_laser_sample_t));
+ sr->scan = new laser_sample_t[sample_count];
+ memcpy( sr->scan, scan, sample_count * sizeof(laser_sample_t));
// add the copy to the list
g_queue_push_tail( robot->laser_history, sr );
@@ -567,12 +567,12 @@ class Robot
int scan_points_count = 0;
- stg_laser_cfg_t cfg = laser->GetConfig();
+ laser_cfg_t cfg = laser->GetConfig();
for( int i = 0; i < robot->laser_history->length; i++ )
{
scan_record_t* scr = (scan_record_t*)g_queue_peek_nth( robot->laser_history, i );
- stg_laser_sample_t* ascan = scr->scan;
+ laser_sample_t* ascan = scr->scan;
Pose apose = scr->pose;
double amin = -cfg.fov / 2.0;
@@ -612,7 +612,7 @@ class Robot
// wps[j].pose.y = pts[2*j+1];
// wps[j].pose.z = 0;
// wps[j].pose.a = 0;
-// wps[j].color = stg_color_pack( 0,1,0,0 );
+// wps[j].color = color_pack( 0,1,0,0 );
// }
// Waypoint* oldp = ((ModelPosition*)robot->pos->GetWorld()->GetModel( "dummy" ))->SetWaypoints( wps, scan_points_count );
@@ -733,7 +733,7 @@ class Robot
//printf( "WAYPOINT_COUNT %d\n", plan->waypoint_count );
Waypoint* wps = NULL;
- stg_color_t col = pos->GetColor();
+ color_t col = pos->GetColor();
if( plan->waypoint_count > 0 )
{
@@ -816,7 +816,7 @@ class Robot
for( unsigned int i = 0; i < mod->fiducial_count; i++ )
{
- stg_fiducial_t* f = &mod->fiducials[i];
+ fiducial_t* f = &mod->fiducials[i];
//printf( "fiducial %d is %d at %.2f m %.2f radians\n",
// i, f->id, f->range, f->bearing );
@@ -840,7 +840,7 @@ class Robot
static int BlobFinderUpdate( ModelBlobfinder* blobmod, Robot* robot )
{
unsigned int blob_count = 0;
- stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &blob_count );
+ blobfinder_blob_t* blobs = blobmod->GetBlobs( &blob_count );
if( blobs && (blob_count>0) )
{
View
2 examples/ctrl/lasernoise.cc
@@ -39,7 +39,7 @@ int LaserUpdate( ModelLaser* mod, void* dummy )
// the model that gets called just after the sensor update is done.
extern "C" int Init( Model* mod )
{
- mod->AddUpdateCallback( (stg_model_callback_t)LaserUpdate, NULL );
+ mod->AddUpdateCallback( (model_callback_t)LaserUpdate, NULL );
// add this so we can see the effects immediately, without needing
// anyone else to subscribe to the laser
View
10 examples/ctrl/pioneer_flocking.cc
@@ -21,9 +21,9 @@ typedef struct
ModelFiducial* fiducial;
ModelFiducial::Fiducial* closest;
- stg_radians_t closest_bearing;
- stg_meters_t closest_range;
- stg_radians_t closest_heading_error;
+ radians_t closest_bearing;
+ meters_t closest_range;
+ radians_t closest_heading_error;
} robot_t;
@@ -50,11 +50,11 @@ extern "C" int Init( Model* mod )
assert( robot->ranger );
// ask Stage to call into our ranger update function
- robot->ranger->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)RangerUpdate, robot );
+ robot->ranger->AddCallback( Model::CB_UPDATE, (model_callback_t)RangerUpdate, robot );
robot->fiducial = (ModelFiducial*)mod->GetUnusedModelOfType( "fiducial" ) ;
assert( robot->fiducial );
- robot->fiducial->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)FiducialUpdate, robot );
+ robot->fiducial->AddCallback( Model::CB_UPDATE, (model_callback_t)FiducialUpdate, robot );
robot->fiducial->Subscribe();
robot->ranger->Subscribe();
View
39 examples/ctrl/sink.cc
@@ -1,26 +1,37 @@
#include "stage.hh"
using namespace Stg;
-const int INTERVAL = 6000;
+unsigned int total = 0;
+
+// inspect the laser data and decide what to do
+int Delivery( Model* mod, void* dummy )
+{
+ (void)dummy;
+
+ total++;
+
+ printf( "%.2f %d %.2f %.2f\n",
+ mod->GetWorld()->SimTimeNow() / 1e6,
+ //mod->GetFlagCount(),
+ total,
+ // dynamic_cast<WorldGui*>(mod->GetWorld())->EnergyString().c_str() );
+
+ Stg::PowerPack::global_input / 1e3,
+ Stg::PowerPack::global_dissipated / 1e3 );
+
+
+ return 0; // run again
+}
+
-int Update( Model* mod, void* dummy );
// Stage calls this when the model starts up
extern "C" int Init( Model* mod )
{
- mod->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)Update, NULL );
+ // mod->AddCallback( Model::CB_UPDATE, (model_callback_t)Update, NULL );
+
+ mod->AddCallback( Model::CB_FLAGINCR, (model_callback_t)Delivery, NULL );
mod->Subscribe();
return 0; //ok
}
-// inspect the laser data and decide what to do
-int Update( Model* mod, void* dummy )
-{
- if( mod->GetWorld()->GetUpdateCount() % INTERVAL == 0 )
- // get rid of all the flags
- while( mod->GetFlagCount() )
- mod->PopFlag();
-
- return 0; // run again
-}
-
View
2 examples/ctrl/source.cc
@@ -51,7 +51,7 @@ extern "C" int Init( Model* mod, CtrlArgs* args )
info->interval = atoi( words[1].c_str() );
info->capacity = atoi( words[2].c_str() );
- mod->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)Update, info );
+ mod->AddCallback( Model::CB_UPDATE, (model_callback_t)Update, info );
mod->Subscribe();
return 0; //ok
}
View
18 examples/ctrl/wander.cc
@@ -5,18 +5,18 @@ const double cruisespeed = 0.4;
const double avoidspeed = 0.05;
const double avoidturn = 0.5;
const double minfrontdistance = 1.0; // 0.6
-const bool verbose = false;
+const bool verbose = true;
const double stopdist = 0.3;
const int avoidduration = 10;
typedef struct
{
ModelPosition* pos;
- ModelLaser* laser;
+ ModelRanger* ranger;
int avoidcount, randcount;
} robot_t;
-int LaserUpdate( Model* mod, robot_t* robot );
+int RangerUpdate( Model* mod, robot_t* robot );
int PositionUpdate( Model* mod, robot_t* robot );
// Stage calls this when the model starts up
@@ -36,21 +36,21 @@ extern "C" int Init( Model* mod, CtrlArgs* args )
robot->randcount = 0;
robot->pos = (ModelPosition*)mod;
- robot->laser = (ModelLaser*)mod->GetChild( "laser:0" );
- robot->laser->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)LaserUpdate, robot );
+ robot->ranger = (ModelRanger*)mod->GetChild( "ranger:0" );
+ robot->ranger->AddCallback( Model::CB_UPDATE, (model_callback_t)RangerUpdate, robot );
- robot->laser->Subscribe(); // starts the laser updates
+ robot->ranger->Subscribe(); // starts the ranger updates
robot->pos->Subscribe(); // starts the position updates
return 0; //ok
}
-// inspect the laser data and decide what to do
-int LaserUpdate( Model* mod, robot_t* robot )
+// inspect the ranger data and decide what to do
+int RangerUpdate( Model* mod, robot_t* robot )
{
// get the data
- const std::vector<ModelLaser::Sample>& scan = robot->laser->GetSamples();
+ const std::vector<ModelRanger::Sensor::Sample>& scan = robot->ranger->GetSensors()[0].samples;
uint32_t sample_count = scan.size();
if( ! sample_count < 1 )
return 0;
View
10 examples/ctrl/wander_pioneer.cc
@@ -22,9 +22,9 @@ typedef struct
ModelFiducial* fiducial;
ModelFiducial::Fiducial* closest;
- stg_radians_t closest_bearing;
- stg_meters_t closest_range;
- stg_radians_t closest_heading_error;
+ radians_t closest_bearing;
+ meters_t closest_range;
+ radians_t closest_heading_error;
} robot_t;
@@ -50,12 +50,12 @@ extern "C" int Init( Model* mod )
robot->ranger = (ModelRanger*)mod->GetUnusedModelOfType( "ranger" );
assert( robot->ranger );
// ask Stage to call into our ranger update function whenever the ranger is updated
- robot->ranger->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)RangerUpdate, robot );
+ robot->ranger->AddCallback( Model::CB_UPDATE, (model_callback_t)RangerUpdate, robot );
robot->fiducial = (ModelFiducial*)mod->GetUnusedModelOfType( "fiducial" ) ;
assert( robot->fiducial );
// ask Stage to call into our fiducial update function whenever the fiducial is updated
- robot->fiducial->AddCallback( Model::CB_UPDATE, (stg_model_callback_t)FiducialUpdate, robot );
+ robot->fiducial->AddCallback( Model::CB_UPDATE, (model_callback_t)FiducialUpdate, robot );
// subscribe to the laser, though we don't use it for navigating
//robot->laser = (ModelLaser*)mod->GetModel( "laser:0" );
View
4 examples/libstage/stest.cc
@@ -137,7 +137,7 @@ int main( int argc, char* argv[] )
}
// // send a command to the robot
-// stg_velocity_t vel;
+// velocity_t vel;
// bzero(&vel,sizeof(vel));
// vel.x = forward_speed;
// vel.y = side_speed;
@@ -148,7 +148,7 @@ int main( int argc, char* argv[] )
//robots[i].position->Token(), vel.x, vel.y, vel.z, vel.a );
// uint32_t bcount=0;
- //stg_blobfinder_blob_t* blobs = robots[i].blobfinder->GetBlobs( &bcount );
+ //blobfinder_blob_t* blobs = robots[i].blobfinder->GetBlobs( &bcount );
//printf( "robot %s sees %u blobs\n", robots[i].blobfinder->Token(), bcount );
View
20 libstage/CMakeLists.txt
@@ -22,7 +22,6 @@ set( stageSrcs
model_fiducial.cc
model_getset.cc
model_gripper.cc
- model_laser.cc
model_lightindicator.cc
model_load.cc
model_position.cc
@@ -36,28 +35,29 @@ set( stageSrcs
typetable.cc
world.cc
worldfile.cc
- canvas.cc
- options_dlg.cc
- options_dlg.hh
- vis_strip.cc
- worldgui.cc
- ancestor.cc
+ canvas.cc
+ options_dlg.cc
+ options_dlg.hh
+ vis_strip.cc
+ worldgui.cc
+ ancestor.cc
)
+# model_laser.cc
+
+
set_source_files_properties( ${stageSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
add_library(stage SHARED ${stageSrcs})
target_link_libraries( stage
${LTDL_LIB}
-# ${OPENGL_LIBRARIES} # is it safe to remove these permanently?
-# ${FLTK_LIBS2}
)
# causes the shared library to have a version number
set_target_properties( stage PROPERTIES
VERSION ${VERSION}
- LINK_FLAGS "${FLTK_LDFLAGS}"
+ LINK_FLAGS "${FLTK_LDFLAGS}"
)
set( stagebinarySrcs main.cc )
View
2 libstage/ancestor.cc
@@ -63,7 +63,7 @@ Pose Ancestor::GetGlobalPose()
return pose;
}
-void Ancestor::ForEachDescendant( stg_model_callback_t func, void* arg )
+void Ancestor::ForEachDescendant( model_callback_t func, void* arg )
{
FOR_EACH( it, children )
{
View
62 libstage/block.cc
@@ -4,17 +4,17 @@
using namespace Stg;
using std::vector;
-static void canonicalize_winding(vector<stg_point_t>& pts);
+static void canonicalize_winding(vector<point_t>& pts);
/** Create a new block. A model's body is a list of these
blocks. The point data is copied, so pts can safely be freed
after calling this.*/
Block::Block( Model* mod,
- stg_point_t* pts,
+ point_t* pts,
size_t pt_count,
- stg_meters_t zmin,
- stg_meters_t zmax,
+ meters_t zmin,
+ meters_t zmax,
Color color,
bool inherit_color,
bool wheel ) :
@@ -214,7 +214,7 @@ void Block::UnMap()
mapped = false;
}
-void Block::RemoveFromCellArray( CellPtrVec *cells )
+inline void Block::RemoveFromCellArray( CellPtrVec *cells )
{
FOR_EACH( it, *cells )
{
@@ -301,12 +301,12 @@ void Block::SwitchToTestedCells()
mapped = true;
}
-inline stg_point_t Block::BlockPointToModelMeters( const stg_point_t& bpt )
+inline point_t Block::BlockPointToModelMeters( const point_t& bpt )
{
Size bgsize = mod->blockgroup.GetSize();
- stg_point3_t bgoffset = mod->blockgroup.GetOffset();
+ point3_t bgoffset = mod->blockgroup.GetOffset();
- return stg_point_t( (bpt.x - bgoffset.x) * (mod->geom.size.x/bgsize.x),
+ return point_t( (bpt.x - bgoffset.x) * (mod->geom.size.x/bgsize.x),
(bpt.y - bgoffset.y) * (mod->geom.size.y/bgsize.y));
}
@@ -341,7 +341,7 @@ void Block::GenerateCandidateCells()
Pose gpose = mod->GetGlobalPose();
gpose.z += mod->geom.pose.z;
double scalez = mod->geom.size.z / mod->blockgroup.GetSize().z;
- stg_meters_t z = gpose.z - mod->blockgroup.GetOffset().z;
+ meters_t z = gpose.z - mod->blockgroup.GetOffset().z;
// store the block's absolute z bounds at this rendering
global_z.min = (scalez * local_z.min) + z;
@@ -360,17 +360,17 @@ void swap( int& a, int& b )
void Block::Rasterize( uint8_t* data,
unsigned int width,
unsigned int height,
- stg_meters_t cellwidth,
- stg_meters_t cellheight )
+ meters_t cellwidth,
+ meters_t cellheight )
{
//printf( "rasterize block %p : w: %u h: %u scale %.2f %.2f offset %.2f %.2f\n",
// this, width, height, scalex, scaley, offsetx, offsety );
for( unsigned int i=0; i<pt_count; i++ )
{
// convert points from local to model coords
- stg_point_t mpt1 = BlockPointToModelMeters( pts[i] );
- stg_point_t mpt2 = BlockPointToModelMeters( pts[(i+1)%pt_count] );
+ point_t mpt1 = BlockPointToModelMeters( pts[i] );
+ point_t mpt2 = BlockPointToModelMeters( pts[(i+1)%pt_count] );
// record for debug visualization
mod->rastervis.AddPoint( mpt1.x, mpt1.y );
@@ -382,9 +382,9 @@ void Block::Rasterize( uint8_t* data,
mpt2.y += mod->geom.size.y/2.0;
// convert from meters to cells
- stg_point_int_t a( floor( mpt1.x / cellwidth ),
+ point_int_t a( floor( mpt1.x / cellwidth ),
floor( mpt1.y / cellheight ));
- stg_point_int_t b( floor( mpt2.x / cellwidth ),
+ point_int_t b( floor( mpt2.x / cellwidth ),
floor( mpt2.y / cellheight ) );
bool steep = abs( b.y-a.y ) > abs( b.x-a.x );
@@ -523,42 +523,42 @@ void Block::Load( Worldfile* wf, int entity )
static
/// util; puts angle into [0, 2pi)
-void positivize(stg_radians_t& angle)
+void positivize(radians_t& angle)
{
while (angle < 0) angle += 2 * M_PI;
}
static
/// util; puts angle into -pi/2, pi/2
-void pi_ize(stg_radians_t& angle)
+void pi_ize(radians_t& angle)
{
while (angle < -M_PI) angle += 2 * M_PI;
while (M_PI < angle) angle -= 2 * M_PI;
}
-typedef stg_point_t V2;
+typedef point_t V2;
static
/// util; How much was v1 rotated to get to v2?
-stg_radians_t angle_change(V2 v1, V2 v2)
+radians_t angle_change(V2 v1, V2 v2)
{
- stg_radians_t a1 = atan2(v1.y, v1.x);
+ radians_t a1 = atan2(v1.y, v1.x);
positivize(a1);
- stg_radians_t a2 = atan2(v2.y, v2.x);
+ radians_t a2 = atan2(v2.y, v2.x);
positivize(a2);
- stg_radians_t angle_change = a2 - a1;
+ radians_t angle_change = a2 - a1;
pi_ize(angle_change);
return angle_change;
}
static
/// util; find vectors between adjacent points, pts[next] - pts[cur]
-vector<stg_point_t> find_vectors(vector<stg_point_t> const& pts)
+vector<point_t> find_vectors(vector<point_t> const& pts)
{
- vector<stg_point_t> vs;
+ vector<point_t> vs;
assert(2 <= pts.size());
for (unsigned i = 0, n = pts.size(); i < n; ++i)
{
@@ -572,9 +572,9 @@ vector<stg_point_t> find_vectors(vector<stg_point_t> const& pts)
static
/// util; finds sum of angle changes, from each vertex to the
/// next one (in current ordering), wrapping around.
-stg_radians_t angles_sum(vector<stg_point_t> const& vs)
+radians_t angles_sum(vector<point_t> const& vs)
{
- stg_radians_t angle_sum = 0;
+ radians_t angle_sum = 0;
for (unsigned i = 0, n = vs.size(); i < n; ++i)
{
unsigned j = (i + 1) % n;
@@ -585,11 +585,11 @@ stg_radians_t angles_sum(vector<stg_point_t> const& vs)
static
/// Util
-bool is_canonical_winding(vector<stg_point_t> const& ps)
+bool is_canonical_winding(vector<point_t> const& ps)
{
- // reuse stg_point_t as vector
- vector<stg_point_t> vs = find_vectors(ps);
- stg_radians_t sum = angles_sum(vs);
+ // reuse point_t as vector
+ vector<point_t> vs = find_vectors(ps);
+ radians_t sum = angles_sum(vs);
bool bCanon = 0 < sum;
return bCanon;
@@ -602,7 +602,7 @@ static
// Note that a simple line that doubles back on itself has an
// angle sum of 0, but that's intrinsic to a line - its winding could
// be either way.
-void canonicalize_winding(vector<stg_point_t>& ps)
+void canonicalize_winding(vector<point_t>& ps)
{
if (not is_canonical_winding(ps))
{
View
10 libstage/blockgroup.cc
@@ -270,10 +270,10 @@ void BlockGroup::LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfil
PRINT_DEBUG1( "attempting to load image %s", full );
- stg_rotrect_t* rects = NULL;
+ rotrect_t* rects = NULL;
unsigned int rect_count = 0;
unsigned int width, height;
- if( stg_rotrects_from_image_file( full,
+ if( rotrects_from_image_file( full,
&rects,
&rect_count,
&width, &height ) )
@@ -293,7 +293,7 @@ void BlockGroup::LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfil
for( unsigned int r=0; r<rect_count; r++ )
{
- stg_point_t pts[4];
+ point_t pts[4];
double x = rects[r].pose.x;
double y = rects[r].pose.y;
@@ -326,8 +326,8 @@ void BlockGroup::LoadBitmap( Model* mod, const std::string& bitmapfile, Worldfil
void BlockGroup::Rasterize( uint8_t* data,
unsigned int width,
unsigned int height,
- stg_meters_t cellwidth,
- stg_meters_t cellheight )
+ meters_t cellwidth,
+ meters_t cellheight )
{
FOR_EACH( it, blocks )
(*it)->Rasterize( data, width, height, cellwidth, cellheight );
View
14 libstage/canvas.cc
@@ -594,7 +594,7 @@ void Canvas::RemoveModel( Model* mod )
void Canvas::DrawGlobalGrid()
{
- stg_bounds3d_t bounds = world->GetExtent();
+ bounds3d_t bounds = world->GetExtent();
/* printf( "bounds [%.2f %.2f] [%.2f %.2f] [%.2f %.2f]\n",
bounds.x.min, bounds.x.max,
@@ -671,7 +671,7 @@ void Canvas::DrawGlobalGrid()
//draw the floor without any grid ( for robot's perspective camera model )
void Canvas::DrawFloor()
{
- stg_bounds3d_t bounds = world->GetExtent();
+ bounds3d_t bounds = world->GetExtent();
glEnable(GL_POLYGON_OFFSET_FILL);
glPolygonOffset(2.0, 2.0);
@@ -744,19 +744,19 @@ void Canvas::resetCamera()
class DistFuncObj
{
- stg_meters_t x, y;
- DistFuncObj( stg_meters_t x, stg_meters_t y )
+ meters_t x, y;
+ DistFuncObj( meters_t x, meters_t y )
: x(x), y(y) {}
bool operator()(const Model* a, const Model* b ) const
{
Pose a_pose = a->GetGlobalPose();
Pose b_pose = b->GetGlobalPose();
- stg_meters_t a_dist = hypot( y - a_pose.y,
+ meters_t a_dist = hypot( y - a_pose.y,
x - a_pose.x );
- stg_meters_t b_dist = hypot( y - b_pose.y,
+ meters_t b_dist = hypot( y - b_pose.y,
x - b_pose.x );
return ( a_dist < b_dist );
@@ -1264,7 +1264,7 @@ void Canvas::draw()
}
else
{
- stg_bounds3d_t extent = world->GetExtent();
+ bounds3d_t extent = world->GetExtent();
camera.SetProjection( w(), h(), extent.y.min, extent.y.max );
current_camera = &camera;
}
View
2 libstage/canvas.hh
@@ -67,7 +67,7 @@ namespace Stg
Model* last_selection; ///< the most recently selected model
///(even if it is now unselected).
- stg_msec_t interval; // window refresh interval in ms
+ msec_t interval; // window refresh interval in ms
void RecordRay( double x1, double y1, double x2, double y2 );
void DrawRays();
View
2 libstage/gl.cc
@@ -134,7 +134,7 @@ void Stg::Gl::draw_origin( double len )
draw_vector( 0,0,len );
}
-void Stg::Gl::draw_grid( stg_bounds3d_t vol )
+void Stg::Gl::draw_grid( bounds3d_t vol )
{
glBegin(GL_LINES);
View
2 libstage/logentry.cc
@@ -4,7 +4,7 @@ using namespace Stg;
// static data members
std::vector<LogEntry> LogEntry::log;
-LogEntry::LogEntry( stg_usec_t timestamp, Model* mod ) :
+LogEntry::LogEntry( usec_t timestamp, Model* mod ) :
timestamp( timestamp ),
mod( mod ),
pose( mod->GetPose() )
View
178 libstage/model.cc
@@ -35,9 +35,9 @@
fiducial_return 0
fiducial_key 0
obstacle_return 1
- ranger_return 1
+ ranger_return 1.0
blob_return 1
- laser_return LaserVisible
+ ranger_return 1.0
gripper_return 0
# GUI properties
@@ -112,15 +112,15 @@
- obstacle_return <int>\n if 1, this model can collide with other
models that have this property set
- - ranger_return <int>\n if 1, this model can be detected by ranger
- sensors
-
- blob_return <int>\n if 1, this model can be detected in the
blob_finder (depending on its color)
- - laser_return <int>\n if 0, this model is not detected by laser
- sensors. if 1, the model shows up in a laser sensor with normal
- (0) reflectance. If 2, it shows up with high (1) reflectance.
+ - ranger_return <float>\n This model is detected with this
+ reflectance value by ranger_model sensors. If negative, this model
+ is invisible to ranger sensors, and does not block propogation of
+ range-sensing rays. This models an idealized reflectance sensor,
+ and replaces the normal/bright reflectance of deprecated laser
+ model. Defaults to 1.0.
- gripper_return <int>\n iff 1, this model can be gripped by a
gripper and can be pushed around by collisions with anything that
@@ -166,9 +166,74 @@ using namespace Stg;
uint32_t Model::count(0);
uint32_t Model::trail_length(50);
uint64_t Model::trail_interval(5);
-std::map<stg_id_t,Model*> Model::modelsbyid;
+std::map<id_t,Model*> Model::modelsbyid;
std::map<std::string, creator_t> Model::name_map;
+void Bounds::Load( Worldfile* wf, const int section, const char* keyword )
+{
+ if( CProperty* prop = wf->GetProperty( section, keyword ) )
+ {
+ if( prop->values.size() != 2 )
+ {
+ puts( "" ); // newline
+ PRINT_ERR1( "Loading 1D bounds. Need a vector of length 2: found %d.\n",
+ (int)prop->values.size() );
+ exit(-1);
+ }
+
+ min = atof( wf->GetPropertyValue( prop, 0 )) * wf->unit_length;
+ max = atof( wf->GetPropertyValue( prop, 1 )) * wf->unit_length;
+ }
+}
+
+bool Color::Load( Worldfile* wf, const int section )
+{
+ if( wf->PropertyExists( section, "color" ))
+ {
+ const std::string& colorstr = wf->ReadString( section, "color", "" );
+ if( colorstr != "" )
+ {
+ if( colorstr == "random" )
+ {
+ r = drand48();
+ g = drand48();
+ b = drand48();
+ a = 1.0;
+ }
+ else
+ {
+ Color c = Color( colorstr );
+ r = c.r;
+ g = c.g;
+ b = c.b;
+ a = c.a;
+ }
+ }
+ return true;
+ }
+
+ if( wf->PropertyExists( section, "color_rgba" ))
+ {
+ if (wf->GetProperty(section,"color_rgba")->values.size() < 4)
+ {
+ PRINT_ERR1( "color_rgba requires 4 values, found %d\n",
+ (int)wf->GetProperty(section,"color_rgba")->values.size() );
+ exit(-1);
+ }
+ else
+ {
+ r = wf->ReadTupleFloat( section, "color_rgba", 0, r );
+ g = wf->ReadTupleFloat( section, "color_rgba", 1, g );
+ b = wf->ReadTupleFloat( section, "color_rgba", 2, b );
+ a = wf->ReadTupleFloat( section, "color_rgba", 3, a );
+ }
+
+ return true;
+ }
+
+ return false;
+}
+
void Size::Load( Worldfile* wf, const int section, const char* keyword )
{
if( CProperty* prop = wf->GetProperty( section, keyword ) )
@@ -228,9 +293,8 @@ Model::Visibility::Visibility() :
fiducial_key( 0 ),
fiducial_return( 0 ),
gripper_return( false ),
- laser_return( LaserVisible ),
obstacle_return( true ),
- ranger_return( true )
+ ranger_return( 1.0 )
{ /* nothing to do */ }
//static const members
@@ -242,11 +306,21 @@ void Model::Visibility::Load( Worldfile* wf, int wf_entity )
fiducial_key = wf->ReadInt( wf_entity, "fiducial_key", fiducial_key);
fiducial_return = wf->ReadInt( wf_entity, "fiducial_return", fiducial_return);
gripper_return = wf->ReadInt( wf_entity, "gripper_return", gripper_return);
- laser_return = (stg_laser_return_t)wf->ReadInt( wf_entity, "laser_return", laser_return);
obstacle_return = wf->ReadInt( wf_entity, "obstacle_return", obstacle_return);
- ranger_return = wf->ReadInt( wf_entity, "ranger_return", ranger_return);
+ ranger_return = wf->ReadFloat( wf_entity, "ranger_return", ranger_return);
}
+void Model::Visibility::Save( Worldfile* wf, int wf_entity )
+{
+ wf->WriteInt( wf_entity, "blob_return", blob_return);
+ wf->WriteInt( wf_entity, "fiducial_key", fiducial_key);
+ wf->WriteInt( wf_entity, "fiducial_return", fiducial_return);
+ wf->WriteInt( wf_entity, "gripper_return", gripper_return);
+ wf->WriteInt( wf_entity, "obstacle_return", obstacle_return);
+ wf->WriteFloat( wf_entity, "ranger_return", ranger_return);
+}
+
+
Model::GuiState::GuiState() :
grid( false ),
move( false ),
@@ -280,9 +354,9 @@ Model::Model( World* world,
geom(),
has_default_block( true ),
id( Model::count++ ),
- interval((stg_usec_t)1e5), // 100msec
- interval_energy((stg_usec_t)1e5), // 100msec
- interval_pose((stg_usec_t)1e5), // 100msec
+ interval((usec_t)1e5), // 100msec
+ interval_energy((usec_t)1e5), // 100msec
+ interval_pose((usec_t)1e5), // 100msec
last_update(0),
log_state(false),
map_resolution(0.1),
@@ -430,15 +504,15 @@ void Model::LoadBlock( Worldfile* wf, int entity )
}
-Block* Model::AddBlockRect( stg_meters_t x,
- stg_meters_t y,
- stg_meters_t dx,
- stg_meters_t dy,
- stg_meters_t dz )
+Block* Model::AddBlockRect( meters_t x,
+ meters_t y,
+ meters_t dx,
+ meters_t dy,
+ meters_t dz )
{
UnMap();
- stg_point_t pts[4];
+ point_t pts[4];
pts[0].x = x;
pts[0].y = y;
pts[1].x = x + dx;
@@ -462,8 +536,8 @@ Block* Model::AddBlockRect( stg_meters_t x,
RaytraceResult Model::Raytrace( const Pose &pose,
- const stg_meters_t range,
- const stg_ray_test_func_t func,
+ const meters_t range,
+ const ray_test_func_t func,
const void* arg,
const bool ztest )
{
@@ -475,9 +549,9 @@ RaytraceResult Model::Raytrace( const Pose &pose,
ztest );
}
-RaytraceResult Model::Raytrace( const stg_radians_t bearing,
- const stg_meters_t range,
- const stg_ray_test_func_t func,
+RaytraceResult Model::Raytrace( const radians_t bearing,
+ const meters_t range,
+ const ray_test_func_t func,
const void* arg,
const bool ztest )
{
@@ -490,10 +564,10 @@ RaytraceResult Model::Raytrace( const stg_radians_t bearing,
}
-void Model::Raytrace( const stg_radians_t bearing,
- const stg_meters_t range,
- const stg_radians_t fov,
- const stg_ray_test_func_t func,
+void Model::Raytrace( const radians_t bearing,
+ const meters_t range,
+ const radians_t fov,
+ const ray_test_func_t func,
const void* arg,
RaytraceResult* samples,
const uint32_t sample_count,
@@ -575,22 +649,22 @@ bool Model::IsRelated( const Model* that ) const
return candidate->IsDescendent( that );
}
-stg_point_t Model::LocalToGlobal( const stg_point_t& pt) const
+point_t Model::LocalToGlobal( const point_t& pt) const
{
const Pose gpose = LocalToGlobal( Pose( pt.x, pt.y, 0, 0 ) );
- return stg_point_t( gpose.x, gpose.y );
+ return point_t( gpose.x, gpose.y );
}
-void Model::LocalToPixels( const std::vector<stg_point_t>& local,
- std::vector<stg_point_int_t>& global) const
+void Model::LocalToPixels( const std::vector<point_t>& local,
+ std::vector<point_int_t>& global) const
{
const Pose gpose = GetGlobalPose() + geom.pose;
FOR_EACH( it, local )
{
Pose ptpose = gpose + Pose( it->x, it->y, 0, 0 );
- global.push_back( stg_point_int_t( (int32_t)floor( ptpose.x * world->ppm) ,
+ global.push_back( point_int_t( (int32_t)floor( ptpose.x * world->ppm) ,
(int32_t)floor( ptpose.y * world->ppm) ));
}
}
@@ -738,9 +812,9 @@ void Model::Update( void )
}
-stg_meters_t Model::ModelHeight() const
+meters_t Model::ModelHeight() const
{
- stg_meters_t m_child = 0; //max size of any child
+ meters_t m_child = 0; //max size of any child
FOR_EACH( it, children )
m_child = std::max( m_child, (*it)->ModelHeight() );
@@ -764,8 +838,8 @@ void Model::AddToPose( const Pose& pose )
this->AddToPose( pose.x, pose.y, pose.z, pose.a );
}
-void Model::PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax,
- stg_meters_t ymin, stg_meters_t ymax )
+void Model::PlaceInFreeSpace( meters_t xmin, meters_t xmax,
+ meters_t ymin, meters_t ymax )
{
while( TestCollisionTree() )
SetPose( Pose::Random( xmin,xmax, ymin, ymax ));
@@ -830,8 +904,8 @@ void Model::UpdateCharge()
//printf( " toucher %s can take up to %.2f wats\n",
// toucher->Token(), toucher->watts_take );
- const stg_watts_t rate = std::min( watts_give, toucher->watts_take );
- const stg_joules_t amount = rate * interval_energy * 1e-6;
+ const watts_t rate = std::min( watts_give, toucher->watts_take );
+ const joules_t amount = rate * interval_energy * 1e-6;
//printf ( "moving %.2f joules from %s to %s\n",
// amount, token, toucher->token );
@@ -975,17 +1049,17 @@ Model* Model::GetUnusedModelOfType( const std::string& type )
return NULL;
}
-stg_kg_t Model::GetTotalMass() const
+kg_t Model::GetTotalMass() const
{
- stg_kg_t sum = mass;
+ kg_t sum = mass;
FOR_EACH( it, children )
sum += (*it)->GetTotalMass();
return sum;
}
-stg_kg_t Model::GetMassOfChildren() const
+kg_t Model::GetMassOfChildren() const
{
return( GetTotalMass() - mass);
}
@@ -1029,8 +1103,8 @@ void Model::RegisterOption( Option* opt )
void Model::Rasterize( uint8_t* data,
unsigned int width,
unsigned int height,
- stg_meters_t cellwidth,
- stg_meters_t cellheight )
+ meters_t cellwidth,
+ meters_t cellheight )
{
rastervis.ClearPts();
blockgroup.Rasterize( data, width, height, cellwidth, cellheight );
@@ -1098,7 +1172,7 @@ void Model::RasterVis::Visualize( Model* mod, Camera* cam )
FOR_EACH( it, pts )
{
- stg_point_t& pt = *it;
+ point_t& pt = *it;
glVertex2f( pt.x, pt.y );
char buf[128];
@@ -1163,8 +1237,8 @@ void Model::RasterVis::Visualize( Model* mod, Camera* cam )
void Model::RasterVis::SetData( uint8_t* data,
const unsigned int width,
const unsigned int height,
- const stg_meters_t cellwidth,
- const stg_meters_t cellheight )
+ const meters_t cellwidth,
+ const meters_t cellheight )
{
// copy the raster for test visualization
if( this->data )
@@ -1180,9 +1254,9 @@ void Model::RasterVis::SetData( uint8_t* data,
}
-void Model::RasterVis::AddPoint( const stg_meters_t x, const stg_meters_t y )
+void Model::RasterVis::AddPoint( const meters_t x, const meters_t y )
{
- pts.push_back( stg_point_t( x, y ) );
+ pts.push_back( point_t( x, y ) );
}
void Model::RasterVis::ClearPts()
View
14 libstage/model_blobfinder.cc
@@ -20,10 +20,10 @@
#include "worldfile.hh"
using namespace Stg;
-static const stg_watts_t DEFAULT_BLOBFINDERWATTS = 2.0;
-static const stg_meters_t DEFAULT_BLOBFINDERRANGE = 12.0;
-static const stg_radians_t DEFAULT_BLOBFINDERFOV = M_PI/3.0;
-static const stg_radians_t DEFAULT_BLOBFINDERPAN = 0.0;
+static const watts_t DEFAULT_BLOBFINDERWATTS = 2.0;
+static const meters_t DEFAULT_BLOBFINDERRANGE = 12.0;
+static const radians_t DEFAULT_BLOBFINDERFOV = M_PI/3.0;
+static const radians_t DEFAULT_BLOBFINDERPAN = 0.0;
static const unsigned int DEFAULT_BLOBFINDERINTERVAL_MS = 100;
static const unsigned int DEFAULT_BLOBFINDERRESOLUTION = 1;
static const unsigned int DEFAULT_BLOBFINDERSCANWIDTH = 80;
@@ -232,7 +232,7 @@ void ModelBlobfinder::Update( void )
double robotHeight = 0.6; // meters
// find the average range to the blob;
- stg_meters_t range = 0;
+ meters_t range = 0;
for( unsigned int t=right; t<=left; t++ )
range += samples[t].range;
range /= left-right + 1;
@@ -372,8 +372,8 @@ void ModelBlobfinder::Vis::Visualize( Model* mod, Camera* cam )
for( unsigned int s=0; s<bf->blobs.size(); s++ )
{
Blob* b = &bf->blobs[s];
- //stg_blobfinder_blob_t* b =
- //&g_array_index( blobs, stg_blobfinder_blob_t, s);
+ //blobfinder_blob_t* b =
+ //&g_array_index( blobs, blobfinder_blob_t, s);
bf->PushColor( b->color );
glRectf( b->left, b->top, b->right, b->bottom );
View
18 libstage/model_callbacks.cc
@@ -3,19 +3,19 @@ using namespace Stg;
using namespace std;
void Model::AddCallback( callback_type_t type,
- stg_model_callback_t cb,
+ model_callback_t cb,
void* user )
{
- //callbacks[address].insert( stg_cb_t( cb, user ));
- callbacks[type].insert( stg_cb_t( cb, user ));
+ //callbacks[address].insert( cb_t( cb, user ));
+ callbacks[type].insert( cb_t( cb, user ));
}
int Model::RemoveCallback( callback_type_t type,
- stg_model_callback_t callback )
+ model_callback_t callback )
{
- set<stg_cb_t>& callset = callbacks[type];
- callset.erase( stg_cb_t( callback, NULL) );
+ set<cb_t>& callset = callbacks[type];
+ callset.erase( cb_t( callback, NULL) );
// return the number of callbacks remaining for this address. Useful
// for detecting when there are none.
@@ -26,13 +26,13 @@ int Model::RemoveCallback( callback_type_t type,
int Model::CallCallbacks( callback_type_t type )
{
// maintain a list of callbacks that should be cancelled
- vector<stg_cb_t> doomed;
+ vector<cb_t> doomed;
- set<stg_cb_t>& callset = callbacks[type];
+ set<cb_t>& callset = callbacks[type];
FOR_EACH( it, callset )
{
- const stg_cb_t& cba = *it;
+ const cb_t& cba = *it;
if( (cba.callback)( this, cba.arg ) )
doomed.push_back( cba );
}
View
6 libstage/model_draw.cc
@@ -563,8 +563,8 @@ void Model::DrawFlagList( void )
// for( unsigned int i=0; i<blinkenlights->len; i++ )
// {
-// stg_blinkenlight_t* b =
-// (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
+// blinkenlight_t* b =
+// (blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
// assert(b);
// glTranslatef( b->pose.x, b->pose.y, b->pose.z );
@@ -635,7 +635,7 @@ void Model::DrawGrid( void )
{
PushLocalCoords();
- stg_bounds3d_t vol;
+ bounds3d_t vol;
vol.x.min = -geom.size.x/2.0;
vol.x.max = geom.size.x/2.0;
vol.y.min = -geom.size.y/2.0;
View
14 libstage/model_fiducial.cc
@@ -175,16 +175,16 @@ void ModelFiducial::AddModelIfVisible( Model* him )
// return this as a hit.
//printf( "range %.2f\n", range );
-
+
RaytraceResult ray( Raytrace( dtheta,
- max_range_anon, // TODOscan only as far as the object
- fiducial_raytrace_match,
- NULL,
- true ) );
+ max_range_anon, // TODOscan only as far as the object
+ fiducial_raytrace_match,
+ NULL,
+ true ) );
// TODO
- //if( ignore_zloc && ray.mod == NULL ) // i.e. we didn't hit anything *else*
- //ray.mod = him; // so he was just at the wrong height
+ if( ignore_zloc && ray.mod == NULL ) // i.e. we didn't hit anything *else*
+ ray.mod = him; // so he was just at the wrong height
//printf( "ray hit %s and was seeking LOS to %s\n",
// ray.mod ? ray.mod->Token() : "null",
View
31 libstage/model_getset.cc
@@ -23,17 +23,17 @@ void Model::SetColor( Color val )
NeedRedraw();
}
-void Model::SetMass( stg_kg_t val )
+void Model::SetMass( kg_t val )
{
mass = val;
}
-void Model::SetStall( stg_bool_t val )
+void Model::SetStall( bool val )
{
stall = val;
}
-void Model::SetGripperReturn( int val )
+void Model::SetGripperReturn( bool val )
{
vis.gripper_return = val;
}
@@ -55,57 +55,52 @@ void Model::SetFiducialKey( int val )
vis.fiducial_key = val;
}
-void Model::SetLaserReturn( stg_laser_return_t val )
-{
- vis.laser_return = val;
-}
-
-void Model::SetObstacleReturn( int val )
+void Model::SetObstacleReturn( bool val )
{
vis.obstacle_return = val;
}
-void Model::SetBlobReturn( int val )
+void Model::SetBlobReturn( bool val )
{
vis.blob_return = val;
}
-void Model::SetRangerReturn( int val )
+void Model::SetRangerReturn( float val )
{
vis.ranger_return = val;
}
-void Model::SetBoundary( int val )
+void Model::SetBoundary( bool val )
{
boundary = val;
}
-void Model::SetGuiNose( int val )
+void Model::SetGuiNose( bool val )
{
gui.nose = val;
}
-void Model::SetGuiMove( int val )
+void Model::SetGuiMove( bool val )
{
gui.move = val;
}
-void Model::SetGuiGrid( int val )
+void Model::SetGuiGrid( bool val )
{
gui.grid = val;
}
-void Model::SetGuiOutline( int val )
+void Model::SetGuiOutline( bool val )
{
gui.outline = val;
}
-void Model::SetWatts( stg_watts_t val )
+void Model::SetWatts( watts_t val )
{
watts = val;
}
-void Model::SetMapResolution( stg_meters_t val )
+void Model::SetMapResolution( meters_t val )
{
map_resolution = val;
}
View
6 libstage/model_gripper.cc
@@ -449,9 +449,9 @@ void ModelGripper::UpdateContacts()
Geom hitgeom = hit->GetGeom();
//Pose hitgpose = hit->GetGlobalPose();
- // stg_pose_t pose = {0,0,0};
- // stg_model_local_to_global( lhit, &pose );
- // stg_model_global_to_local( mod, &pose );
+ // pose_t pose = {0,0,0};
+ // model_local_to_global( lhit, &pose );
+ // model_global_to_local( mod, &pose );
// // grab the model we hit - very simple grip model for now
hit->SetParent( this );
View
6 libstage/model_laser.cc
@@ -20,10 +20,10 @@ using namespace Stg;
// DEFAULT PARAMETERS FOR LASER MODEL
static const bool DEFAULT_FILLED = true;
-static const stg_watts_t DEFAULT_WATTS = 17.5;
+static const watts_t DEFAULT_WATTS = 17.5;
static const Size DEFAULT_SIZE( 0.15, 0.15, 0.2 );
-static const stg_meters_t DEFAULT_MAXRANGE = 8.0;
-static const stg_radians_t DEFAULT_FOV = M_PI;
+static const meters_t DEFAULT_MAXRANGE = 8.0;
+static const radians_t DEFAULT_FOV = M_PI;
static const unsigned int DEFAULT_SAMPLES = 180;
static const unsigned int DEFAULT_RESOLUTION = 1;
static const char* DEFAULT_COLOR = "blue";
View
10 libstage/model_load.cc
@@ -30,7 +30,7 @@ void Model::Load()
if( !power_pack )
power_pack = new PowerPack( this );
- stg_joules_t j = wf->ReadFloat( wf_entity, "joules",
+ joules_t j = wf->ReadFloat( wf_entity, "joules",
power_pack->GetStored() ) ;
/* assume that the store is full, so the capacity is the same as
@@ -53,7 +53,7 @@ void Model::Load()
if( !power_pack )
power_pack = new PowerPack( this );
- stg_joules_t j = 1000.0 * wf->ReadFloat( wf_entity, "kjoules",
+ joules_t j = 1000.0 * wf->ReadFloat( wf_entity, "kjoules",
power_pack->GetStored() ) ;
/* assume that the store is full, so the capacity is the same as
@@ -195,7 +195,7 @@ void Model::Load()
this->stack_children =
wf->ReadInt( wf_entity, "stack_children", this->stack_children );
- stg_kg_t m = wf->ReadFloat(wf_entity, "mass", this->mass );
+ kg_t m = wf->ReadFloat(wf_entity, "mass", this->mass );
if( m != this->mass )
SetMass( m );
@@ -301,6 +301,8 @@ void Model::Save( void )
wf->WriteTupleAngle( wf_entity, "origin", 3, geom.pose.a);
}
+ vis.Save( wf, wf_entity );
+
// call any type-specific save callbacks
CallCallbacks( CB_SAVE );
@@ -339,7 +341,7 @@ void Model::LoadControllerModule( const char* lib )
{
//printf( "]" );
- stg_model_callback_t initfunc = (stg_model_callback_t)lt_dlsym( handle, "Init" );
+ model_callback_t initfunc = (model_callback_t)lt_dlsym( handle, "Init" );
if( initfunc == NULL )
{
printf( "Libtool error: %s. Something is wrong with your plugin. Quitting\n",
View
4 libstage/model_position.cc
@@ -656,7 +656,7 @@ ModelPosition::Waypoint::Waypoint( const Pose& pose, Color color )
{
}
-ModelPosition::Waypoint::Waypoint( stg_meters_t x, stg_meters_t y, stg_meters_t z, stg_radians_t a, Color color )
+ModelPosition::Waypoint::Waypoint( meters_t x, meters_t y, meters_t z, radians_t a, Color color )
: pose(x,y,z,a), color(color)
{
}
@@ -683,7 +683,7 @@ void ModelPosition::Waypoint::Draw() const
glVertex3f( pose.x, pose.y, pose.z );
glEnd();
- stg_meters_t quiver_length = 0.15;
+ meters_t quiver_length = 0.15;
double dx = cos(pose.a) * quiver_length;
double dy = sin(pose.a) * quiver_length;
View
2 libstage/model_props.cc
@@ -47,7 +47,7 @@ int Model::SetProperty( const char* key,
}
if( MATCH( key, MP_LASER_RETURN ) )
{
- this->SetLaserReturn( *(stg_laser_return_t*)data );
+ this->SetLaserReturn( *(laser_return_t*)data );
return 0;
}
if( MATCH( key, MP_OBSTACLE_RETURN ) )
View
531 libstage/model_ranger.cc
@@ -71,7 +71,7 @@
- [range_min range_max fov]
- minimum range and maximum range in meters, field of view angle in degrees. Currently fov has no effect on the sensor model, other than being shown in the confgiuration graphic for the ranger device.
- sview[\<transducer index\>] [float float float]
- - per-transducer version of the sview property. Overrides the common setting.
+s - per-transducer version of the sview property. Overrides the common setting.
*/
@@ -82,67 +82,57 @@
#include "option.hh"
using namespace Stg;
-static const stg_watts_t RANGER_WATTSPERSENSOR = 0.2;
-static const Size RANGER_SIZE( 0.4, 0.4, 0.05 );
-static const Size RANGER_TRANSDUCER_SIZE( 0.01, 0.04, 0.04 );
-static const stg_meters_t RANGER_RANGEMAX = 5.0;
-static const stg_meters_t RANGER_RANGEMIN = 0.0;
-static const unsigned int RANGER_RAYCOUNT = 3;
-static const unsigned int RANGER_SENSORCOUNT = 16;
+static const watts_t RANGER_WATTSPERSENSOR = 0.2;
+static const Size RANGER_SIZE( 0.15, 0.15, 0.2 ); // SICK LMS size
-static const char RANGER_COLOR[] = "gray75";
-static const char RANGER_CONFIG_COLOR[] = "gray90";
-static const char RANGER_GEOM_COLOR[] = "orange";
+//static const Color RANGER_COLOR( 0,0,1 );
+static const Color RANGER_CONFIG_COLOR( 0,0,0.5 );
+//static const Color RANGER_GEOM_COLOR( 1,0,1 );
// static members
-Option ModelRanger::showRangerData( "Ranger ranges", "show_ranger", "", true, NULL );
-Option ModelRanger::showRangerTransducers( "Ranger transducers", "show_ranger_transducers", "", false, NULL );
+Option ModelRanger::Vis::showData( "Ranger ranges", "show_ranger", "", true, NULL );
+Option ModelRanger::Vis::showTransducers( "Ranger transducers", "show_ranger_transducers", "", false, NULL );
+Option ModelRanger::Vis::showArea( "Ranger scans", "show_ranger", "", true, NULL );
+Option ModelRanger::Vis::showStrikes( "Ranger strikes", "show_ranger_strikes", "", false, NULL );
+Option ModelRanger::Vis::showFov( "Ranger FOV", "show_ranger_fov", "", false, NULL );
+Option ModelRanger::Vis::showBeams( "Ranger beams", "show_ranger_beams", "", false, NULL );
+
ModelRanger::ModelRanger( World* world,
- Model* parent,
- const std::string& type )
- : Model( world, parent, type )
+ Model* parent,
+ const std::string& type )
+ : Model( world, parent, type ),
+ vis( world )
{
PRINT_DEBUG2( "Constructing ModelRanger %d (%s)\n",
- id, type );
+ id, type );
// Set up sensible defaults
// assert that Update() is reentrant for this derived model
thread_safe = true;
- this->SetColor( Color( RANGER_CONFIG_COLOR ) );
+ this->SetColor( RANGER_CONFIG_COLOR );
// remove the polygon: ranger has no body
this->ClearBlocks();
- this->SetGeom( Geom( Pose(), RANGER_SIZE ));
-
- // spread the transducers around the ranger's body
- double offset = std::min(geom.size.x, geom.size.y) / 2.0;
-
- sensors.resize( RANGER_SENSORCOUNT );
-
- // create default ranger config
- for( unsigned int c(0); c<sensors.size(); c++ )
- {
- sensors[c].pose.a = (2.0*M_PI)/sensors.size() * c;
- sensors[c].pose.x = offset * cos( sensors[c].pose.a );
- sensors[c].pose.y = offset * sin( sensors[c].pose.a );
- sensors[c].pose.z = geom.size.z / 2.0; // half way up
-
- sensors[c].size = RANGER_TRANSDUCER_SIZE;
-
- sensors[c].bounds_range.min = RANGER_RANGEMIN;
- sensors[c].bounds_range.max = RANGER_RANGEMAX;;
- sensors[c].fov = (2.0*M_PI)/(sensors.size()+1);
- sensors[c].ray_count = RANGER_RAYCOUNT;
-
- sensors[c].range = 0; // invalid range
- }
+ this->SetGeom( Geom( Pose(), RANGER_SIZE ));
- RegisterOption( &showRangerData );
- RegisterOption( &showRangerTransducers );
+ // sensors.resize( RANGER_SENSORCOUNT );
+
+// for( unsigned int c(0); c<sensors.size(); c++ )
+// {
+// // change a few of the defaults now we know how many sensors
+// // there are and where they are located
+// // (spread the transducers around the ranger's body)
+// Sensor::Cfg& cfg = sensors[c].cfg;
+// cfg.pose.a = (2.0*M_PI)/sensors.size() * c;
+// cfg.pose.z = geom.size.z / 2.0; // half way up
+// cfg.fov = (2.0*M_PI)/(sensors.size()+1);
+// }
+
+ AddVisualizer( &vis, true );
}
ModelRanger::~ModelRanger()
@@ -168,198 +158,329 @@ void ModelRanger::Shutdown( void )
Model::Shutdown();
}
+void ModelRanger::LoadSensor( Worldfile* wf, int entity )
+{
+ //static int c=0;
+ // printf( "ranger %s loading sensor %d\n", token.c_str(), c++ );
+
+ Sensor s;
+ s.cfg.pose.Load( wf, entity, "pose" );
+ s.cfg.size.Load( wf, entity, "size" );
+ s.cfg.range.Load( wf, entity, "range" );
+ s.cfg.col.Load( wf, entity );
+ s.cfg.fov = wf->ReadAngle( entity, "fov", s.cfg.fov );
+ s.cfg.sample_count = wf->ReadInt( entity, "samples", s.cfg.sample_count );
+ sensors.push_back(s);
+}
+
void ModelRanger::Load( void )
{
Model::Load();
-
- if( wf->PropertyExists( wf_entity, "scount" ) )
- {
- PRINT_DEBUG( "Loading ranger array" );
-
- // Load the geometry of a ranger array
- int sensor_count = wf->ReadInt( wf_entity, "scount", 0);
- assert( sensor_count > 0 );
-
- char key[256];
-
- sensors.resize( sensor_count );
-
- Size common_size;
- common_size.x = wf->ReadTupleLength( wf_entity, "ssize", 0, RANGER_SIZE.x );
- common_size.y = wf->ReadTupleLength( wf_entity, "ssize", 1, RANGER_SIZE.y );
-
- double common_min = wf->ReadTupleLength( wf_entity, "sview", 0, RANGER_RANGEMIN );
- double common_max = wf->