avonstage.cc

Go to the documentation of this file.

#include <getopt.h>

extern "C" {
#include <avon.h>
}

#include "stage.hh"
#include "config.h"

const char* AVONSTAGE_VERSION = "1.0.0";

const char* USAGE = 
  "USAGE:  stage [options] <worldfile1> [worldfile2 ... worldfileN]\n"
  "Available [options] are:\n"
  "  --clock        : print simulation time peridically on standard output\n"
  "  -c             : equivalent to --clock\n"
  "  --gui          : run without a GUI\n"
  "  -g             : equivalent to --gui\n"
  "  --help         : print this message\n"
  "  --args \"str\" : define an argument string to be passed to all controllers\n"
  "  -a \"str\"     : equivalent to --args \"str\"\n"
  "  --host \"str\" : set the http server host name (default: \"localhost\")\n"
  " --port num      : set the http sever port number (default: 8000)\n" 
  " --verbose       : provide lots of informative output\n"
  " -v              : equivalent to --verbose\n"
  "  -?             : equivalent to --help\n";

/* options descriptor */
static struct option longopts[] = {
        { "gui",  optional_argument,   NULL,  'g' },
        { "clock",  optional_argument,   NULL,  'c' },
        { "help",  optional_argument,   NULL,  'h' },
        { "args",  required_argument,   NULL,  'a' },
        { "verbose", no_argument, NULL, 'v' },
        { "port",  required_argument,   NULL,  'p' },
        { "host",  required_argument,   NULL,  'h' },
        { "rootdir",  required_argument,   NULL,  'r' },
        { NULL, 0, NULL, 0 }
};



uint64_t GetTimeWorld( Stg::World* world )
{
  Stg::usec_t stgtime = world->SimTimeNow();  
  return static_cast<uint64_t>(stgtime);
}

uint64_t GetTime( Stg::Model* mod )
{
  assert(mod);  
  return GetTimeWorld( mod->GetWorld() );
}

int GetModelPVA( Stg::Model* mod, av_pva_t* pva  )
{
  assert(mod);
  assert(pva);

  bzero(pva,sizeof(av_pva_t));
  
  pva->time = GetTime(mod);  
  
  Stg::Pose sp = mod->GetPose(); 

  pva->p[0] = sp.x;
  pva->p[1] = sp.y;
  pva->p[2] = sp.z;
  pva->p[3] = 0;
  pva->p[4] = 0;
  pva->p[5] = sp.a;
  
  Stg::Velocity sv = mod->GetVelocity();                
  
  pva->v[0] = sv.x;
  pva->v[1] = sv.y;
  pva->v[2] = sv.z;
  pva->v[3] = 0;
  pva->v[4] = 0;
  pva->v[5] = sv.a;
  
  return 0; // ok
}


int SetModelPVA(  Stg::Model* mod, av_pva_t* p  )
{
  assert(mod);
  assert(p);
  
  mod->SetPose( Stg::Pose( p->p[0], // x
                                                                        p->p[1], // y
                                                                        p->p[2], // z
                                                                        p->p[5] )); // a
  
  mod->SetVelocity( Stg::Velocity( p->v[0], // x
                                                                                          p->v[1], // y
                                                                                          p->v[2], // z
                                                                                          p->v[5] )); // a  
  return 0; // ok
}


int SetModelGeom(  Stg::Model* mod, av_geom_t* g )
{       
  assert(mod);
  assert(g);
  
  mod->SetGeom( Stg::Geom( Stg::Pose(g->pose[0], 
                                                                                                                                                 g->pose[1], 
                                                                                                                                                 g->pose[2],
                                                                                                                                                 g->pose[5] ),
                                                                                                         Stg::Size(g->extent[0], 
                                                                                                                                                 g->extent[1], 
                                                                                                                                                 g->extent[2] )));
        
  // force GUI update to see the change if Stage was paused
  mod->Redraw();
  
  return 0; // ok
}

int GetModelGeom(  Stg::Model* mod, av_geom_t* g )
{       
  assert(mod);
  assert(g);
  
  bzero(g, sizeof(av_geom_t));

  g->time = GetTime(mod);  
  
  Stg::Geom ext(mod->GetGeom());        
  g->pose[0] = ext.pose.x;
  g->pose[1] = ext.pose.y;
  g->pose[2] = ext.pose.a;
  g->extent[0] = ext.size.x;
  g->extent[1] = ext.size.y;
  g->extent[2] = ext.size.z;

  return 0; // ok
}


int RangerData( Stg::Model* mod, av_msg_t* data )
{
  assert(mod);
  assert(data);
  
        if( ! mod->HasSubscribers() )
                mod->Subscribe();

  /* Will set the data pointer in arg data to point to this static
          struct, thus avoiding dynamic memory allocation. This is deeply
          non-reentrant! but fast and simple code. */
  static av_ranger_data_t rd;
  bzero(&rd, sizeof(rd));  
  bzero(data, sizeof(av_msg_t));
  
  data->time = GetTime(mod);  
  data->interface = AV_INTERFACE_RANGER;
  data->data = (const void*)&rd;  
  
  Stg::ModelRanger* r = dynamic_cast<Stg::ModelRanger*>(mod);
  
  const std::vector<Stg::ModelRanger::Sensor>& sensors = r->GetSensors();
  
  rd.transducer_count = sensors.size();
  
  assert( rd.transducer_count <= AV_RANGER_TRANSDUCERS_MAX );
  
  rd.time = data->time;

          
  return 0; //ok
}


int RangerSet( Stg::Model* mod, av_msg_t* data )
{
  assert(mod);
  assert(data);  
  puts( "ranger set does nothing" );  
  return 0; //ok
}

int RangerGet( Stg::Model* mod, av_msg_t* data )
{
  assert(mod);
  assert(data);  

  static av_ranger_t rgr;
  bzero(&rgr,sizeof(rgr));

  data->time = GetTime(mod);  
  data->interface = AV_INTERFACE_RANGER;
  data->data = (const void*)&rgr;  
 
  rgr.time = data->time;
  
  Stg::ModelRanger* r = dynamic_cast<Stg::ModelRanger*>(mod);
  
  const std::vector<Stg::ModelRanger::Sensor>& sensors = r->GetSensors();
  
  rgr.transducer_count = sensors.size();
  
  assert( rgr.transducer_count <= AV_RANGER_TRANSDUCERS_MAX );
  

  for( unsigned int c=0; c<rgr.transducer_count; c++ )
         {              
                // bearing
                rgr.transducers[c].fov[0].min = -sensors[c].fov/2.0;
                rgr.transducers[c].fov[0].max =  sensors[c].fov/2.0;
                
                //azimuth
                rgr.transducers[c].fov[1].min = 0.0;
                rgr.transducers[c].fov[1].max = 0.0;
                
                // range
                rgr.transducers[c].fov[2].min = sensors[c].range.min;
                rgr.transducers[c].fov[2].min = sensors[c].range.max;
                
                // pose 6dof
                rgr.transducers[c].geom.pose[0] = sensors[c].pose.x;
                rgr.transducers[c].geom.pose[1] = sensors[c].pose.y;
                rgr.transducers[c].geom.pose[2] = sensors[c].pose.z;
                rgr.transducers[c].geom.pose[3] = 0.0;
                rgr.transducers[c].geom.pose[4] = 0.0;
                rgr.transducers[c].geom.pose[5] = sensors[c].pose.a;

                // extent 3dof
                rgr.transducers[c].geom.extent[0] = sensors[c].size.x;
                rgr.transducers[c].geom.extent[1] = sensors[c].size.y;
                rgr.transducers[c].geom.extent[2] = sensors[c].size.z;

                
                av_ranger_transducer_data_t& t =        rgr.transducers[c];
                const Stg::ModelRanger::Sensor& s = sensors[c];
                
                t.pose[0] = s.pose.x;
                t.pose[1] = s.pose.y;
                t.pose[2] = s.pose.z;
                t.pose[3] = 0.0;
                t.pose[4] = 0.0;
                t.pose[5] = s.pose.a;           
                
                const std::vector<Stg::meters_t>& ranges = s.ranges;
                const std::vector<double>& intensities = s.intensities;
                
                assert( ranges.size() == intensities.size() );
                
                t.sample_count = ranges.size(); 
                
                const double fov_max = s.fov / 2.0;
                const double fov_min = -fov_max;
                const double delta = (fov_max - fov_min) / (double)t.sample_count;
                
                for( unsigned int r=0; r<t.sample_count; r++ )
                  {
                         t.samples[r][AV_SAMPLE_BEARING] = fov_min + r*delta;
                         t.samples[r][AV_SAMPLE_AZIMUTH] = 0.0; // linear scanner
                         t.samples[r][AV_SAMPLE_RANGE] = ranges[r];
                         t.samples[r][AV_SAMPLE_INTENSITY] = intensities[r];
                  }
         }
  return 0; //ok
}


int FiducialData( Stg::Model* mod, av_msg_t* data )
{
  assert(mod);
  assert(data);
  
        if( ! mod->HasSubscribers() )
                mod->Subscribe();

  /* Will set the data pointer in arg data to point to this static
          struct, thus avoiding dynamic memory allocation. This is deeply
          non-reentrant! but fast and simple code. */
  static av_fiducial_data_t fd;
  bzero(&fd, sizeof(fd));  
  bzero(data, sizeof(av_msg_t));
  
  data->time = GetTime(mod);  
  data->interface = AV_INTERFACE_FIDUCIAL;
  data->data = (const void*)&fd;  
  
  Stg::ModelFiducial* fm = dynamic_cast<Stg::ModelFiducial*>(mod);

        const std::vector<Stg::ModelFiducial::Fiducial>& sf = fm->GetFiducials();

        fd.fiducial_count = sf.size();
        
        printf( "FIDUCIALS %u\n", fd.fiducial_count );

        for( size_t i=0; i<fd.fiducial_count; i++ )
                {
                        fd.fiducials[i].pose[0] = sf[i].bearing;
                        fd.fiducials[i].pose[1] = 0.0; // no azimuth in Stage
                        fd.fiducials[i].pose[2] = sf[i].range;
                        
                        fd.fiducials[i].geom.pose[0] = 0.0; // sf[i].pose_rel.x;
                        fd.fiducials[i].geom.pose[1] = 0.0; // sf[i].pose_rel.y;
                        fd.fiducials[i].geom.pose[2] = 0.0;
                        fd.fiducials[i].geom.pose[3] = 0.0; 
                        fd.fiducials[i].geom.pose[4] = 0.0; 
                        fd.fiducials[i].geom.pose[5] = sf[i].geom.a;
                        
                        fd.fiducials[i].geom.extent[0] = sf[i].geom.x;
                        fd.fiducials[i].geom.extent[1] = sf[i].geom.y;
                        fd.fiducials[i].geom.extent[2] = sf[i].geom.z;
                }

        return 0;
}


int FiducialSet( Stg::Model* mod, av_msg_t* data )
{
  assert(mod);
  assert(data);  
  puts( "fiducial setcfg does nothing" );  
  return 0; //ok
}


int FiducialGet( Stg::Model* mod, av_msg_t* msg )
{
        assert(mod);
        assert(msg);
        
        static av_fiducial_cfg_t cfg;
        bzero(&cfg,sizeof(cfg));
        
  msg->time = GetTime(mod);  
  msg->interface = AV_INTERFACE_FIDUCIAL;
  msg->data = (const void*)&cfg;  

        //      cfg.time = msg->time;
        
        Stg::ModelFiducial* fid = dynamic_cast<Stg::ModelFiducial*>(mod);
  
        // bearing
        cfg.fov[0].min = -fid->fov/2.0;
        cfg.fov[0].max = fid->fov/2.0;

        // azimuth
        cfg.fov[1].min = 0;
        cfg.fov[1].max = 0;
        
        // range
        cfg.fov[2].min = fid->min_range;
        cfg.fov[2].max = fid->max_range_anon;

        return 0; // ok
}


class Reg
{
public:
        std::string prototype;
        av_interface_t interface;
        av_prop_get_t getter;
        av_prop_set_t setter;
        
        Reg( const std::string& prototype, 
                         av_interface_t interface,
                         av_prop_get_t getter,
                         av_prop_set_t setter ) :
                prototype(prototype), 
                interface(interface),
                getter(getter),
                setter(setter)
        {}
};

std::pair<std::string,av_interface_t> proto_iface_pair_t;

int RegisterModel( Stg::Model* mod, void* dummy )
{       
        // expensive to test this here! XX todo optmize this for large pops
        if( mod->TokenStr() == "_ground_model" )
                return 0;
        
        static std::map<std::string,Reg> type_table;
        
        if( type_table.size() == 0 ) // first call only
                {
                        type_table[ "model" ] = 
                                Reg( "generic", AV_INTERFACE_GENERIC,NULL,NULL );
                        
                        type_table[ "position" ] = 
                                Reg( "position2d", AV_INTERFACE_GENERIC,NULL,NULL );
                        
                        type_table[ "ranger" ] = 
                                Reg( "ranger", AV_INTERFACE_RANGER, RangerGet, RangerSet );
                        
                        type_table[ "fiducial" ] = 
                                Reg( "fiducial", AV_INTERFACE_FIDUCIA, FiducualGet, FiducualSet );
                }
        
  printf( "[AvonStage] registering %s\n", mod->Token() );
                
        // look up the model type in the table

        const std::map<std::string,proto_iface_pair_t>::iterator it = 
                type_table.find( mod->GetModelType() );

        if( it != type_table.end() ) // if we found it in the table
                {                       
                        Stg::Model* parent = mod->Parent();
                        const char* parent_name = parent ? parent->Token() : NULL;
                        
                        av_register_object( mod->Token(),
                                                                                                        parent_name,  
                                                                                                        it->prototype.c_str(), 
                                                                                                        it->interface, 
                                                                                                        it->getter,
                                                                                                        it->setter,
                                                                                                        dynamic_cast<void*>(mod) );
                        return 0; // ok
                }
        
        // else
        printf( "Avonstage error: model type \"%s\" not found.\n", 
                                        mod->GetModelType().c_str() );
        return 1; //fail
}

int main( int argc, char* argv[] )
{
  // initialize libstage - call this first
  Stg::Init( &argc, &argv );

  printf( "%s %s ", PROJECT, VERSION );
  
  int ch=0, optindex=0;
  bool usegui = true;
  bool showclock = false;
  
  std::string host = "localhost";
  std::string rootdir = ".";
  unsigned short port = AV_DEFAULT_PORT;
  bool verbose = false;

  while ((ch = getopt_long(argc, argv, "cvrgh?p?", longopts, &optindex)) != -1)
         {
                switch( ch )
                  {
                  case 0: // long option given
                         printf( "option %s given\n", longopts[optindex].name );
                         break;

                         // stage options
                  case 'a':
                         Stg::World::ctrlargs = std::string(optarg);
                         break;
                  case 'c': 
                         showclock = true;
                         printf( "[Clock enabled]" );
                         break;
                  case 'g': 
                         usegui = false;
                         printf( "[GUI disabled]" );
                         break;
                         
                         // avon options
                  case 'p':
                         port = atoi(optarg);
                         break;
                  case 'h':
                         host = std::string(optarg);
                         break;
                         //               case 'f':
                         //                      fedfilename = optarg;
                         //                      usefedfile = true;
                         //                      break;
                  case 'r':
                         rootdir = std::string(optarg);
                         break;
                  case 'v':
                         verbose = true;
                         break;
                         // help options
                  case '?':  
                         puts( USAGE );
                         exit(0);
                         break;
                  default:
                         printf("unhandled option %c\n", ch );
                         puts( USAGE );
                         //exit(0);
                  }
         }
  
  const char* worldfilename = argv[optind];

  if( worldfilename == NULL )
         {      
                puts( "[AvonStage] no worldfile specified on command line. Quit.\n" );
                exit(-1);
         }

  puts("");// end the first start-up line
  
  printf( "[AvonStage] host %s:%u world %s\n",
                         host.c_str(), 
                         port, 
                         worldfilename );
  
        char version[1024];
        snprintf( version, 1024, "%s (%s-%s)",
                                                AVONSTAGE_VERSION,
                                                PROJECT,
                                                VERSION );

  // avon
  av_init( host.c_str(), port, rootdir.c_str(), verbose, "AvonStage", version );

  
  // arguments at index [optindex] and later are not options, so they
  // must be world file names
  
  
  Stg::World* world = ( usegui ? 
                                                                                                new Stg::WorldGui( 400, 300, worldfilename ) : 
                                                                                                new Stg::World( worldfilename ) );
        
  world->Load( worldfilename );
  world->ShowClock( showclock );
        
        // now we have a world object, we can install a clock callback
        av_install_clock_callbacks( (av_clock_get_t)GetTimeWorld, world );
        
  // start the http server
  av_startup();
        
  // register all models here  
  world->ForEachDescendant( RegisterModel, NULL );
 
  if( ! world->paused ) 
         world->Start();
    
  while( 1 )
         {
                // TODO - loop properly

                Fl::check(); 
                av_check();      

                usleep(100); // TODO - loop sensibly here
         }
  
  puts( "\n[AvonStage: done]" );
  
  return EXIT_SUCCESS;
}