Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

working on fiducial support

  • Loading branch information...
commit 6cab8019a490b4a23ceca16d886de7f9aaa995f0 1 parent 48e3af7
Richard Vaughan authored
Showing with 174 additions and 10 deletions.
  1. +162 −9 avonstage/avonstage.cc
  2. +12 −1 libstage/CMakeLists.txt
View
171 avonstage/avonstage.cc
@@ -140,7 +140,7 @@ int GetModelGeom( Stg::Model* mod, av_geom_t* g )
}
-int RangerData( Stg::Model* mod, av_data_t* data )
+int RangerData( Stg::Model* mod, av_msg_t* data )
{
assert(mod);
assert(data);
@@ -150,7 +150,7 @@ int RangerData( Stg::Model* mod, av_data_t* data )
non-reentrant! but fast and simple code. */
static av_ranger_data_t rd;
bzero(&rd, sizeof(rd));
- bzero(data, sizeof(av_data_t));
+ bzero(data, sizeof(av_msg_t));
data->time = GetTime(mod);
data->type = AV_MODEL_RANGER;
@@ -164,6 +164,8 @@ int RangerData( Stg::Model* mod, av_data_t* data )
assert( rd.transducer_count <= AV_RANGER_TRANSDUCERS_MAX );
+ rd.time = data->time;
+
for( unsigned int c=0; c<rd.transducer_count; c++ )
{
av_ranger_transducer_data_t& t = rd.transducers[c];
@@ -199,7 +201,7 @@ int RangerData( Stg::Model* mod, av_data_t* data )
return 0; //ok
}
-int RangerCmd( Stg::Model* mod, av_cmd_t* data )
+int RangerCmd( Stg::Model* mod, av_msg_t* data )
{
assert(mod);
assert(data);
@@ -207,7 +209,7 @@ int RangerCmd( Stg::Model* mod, av_cmd_t* data )
return 0; //ok
}
-int RangerCfgSet( Stg::Model* mod, av_cfg_t* data )
+int RangerCfgSet( Stg::Model* mod, av_msg_t* data )
{
assert(mod);
assert(data);
@@ -215,14 +217,156 @@ int RangerCfgSet( Stg::Model* mod, av_cfg_t* data )
return 0; //ok
}
-int RangerCfgGet( Stg::Model* mod, av_cfg_t* data )
+
+int RangerCfgGet( Stg::Model* mod, av_msg_t* data )
{
assert(mod);
assert(data);
- puts( "ranger getcfg does nothing" );
+
+ static av_ranger_cfg_t cfg;
+ bzero(&cfg,sizeof(cfg));
+
+ data->time = GetTime(mod);
+ data->type = AV_MODEL_RANGER;
+ data->data = (const void*)&cfg;
+
+ cfg.time = data->time;
+
+ Stg::ModelRanger* r = dynamic_cast<Stg::ModelRanger*>(mod);
+
+ const std::vector<Stg::ModelRanger::Sensor>& sensors = r->GetSensors();
+
+ cfg.transducer_count = sensors.size();
+
+ assert( cfg.transducer_count <= AV_RANGER_TRANSDUCERS_MAX );
+
+
+ for( unsigned int c=0; c<cfg.transducer_count; c++ )
+ {
+ // bearing
+ cfg.transducers[c].fov[0].min = -sensors[c].fov/2.0;
+ cfg.transducers[c].fov[0].max = sensors[c].fov/2.0;
+
+ //azimuth
+ cfg.transducers[c].fov[1].min = 0.0;
+ cfg.transducers[c].fov[1].max = 0.0;
+
+ // range
+ cfg.transducers[c].fov[2].min = sensors[c].range.min;
+ cfg.transducers[c].fov[2].min = sensors[c].range.max;
+
+ // pose 6dof
+ cfg.transducers[c].geom.pose[0] = sensors[c].pose.x;
+ cfg.transducers[c].geom.pose[1] = sensors[c].pose.y;
+ cfg.transducers[c].geom.pose[2] = sensors[c].pose.z;
+ cfg.transducers[c].geom.pose[3] = 0.0;
+ cfg.transducers[c].geom.pose[4] = 0.0;
+ cfg.transducers[c].geom.pose[5] = sensors[c].pose.a;
+
+ // extent 3dof
+ cfg.transducers[c].geom.extent[0] = sensors[c].size.x;
+ cfg.transducers[c].geom.extent[1] = sensors[c].size.y;
+ cfg.transducers[c].geom.extent[2] = sensors[c].size.z;
+ }
+
return 0; //ok
}
+
+int FiducialData( Stg::Model* mod, av_msg_t* data )
+{
+ assert(mod);
+ assert(data);
+
+ /* 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->type = AV_MODEL_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( int 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] = sf[i].pose_rel.x;
+ fd.fiducials[i].geom.pose[1] = 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].pose_rel.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 FiducialCmd( Stg::Model* mod, av_msg_t* data )
+{
+ assert(mod);
+ assert(data);
+ puts( "fiducial command does nothing" );
+ return 0; //ok
+}
+
+int FiducialCfgSet( Stg::Model* mod, av_msg_t* data )
+{
+ assert(mod);
+ assert(data);
+ puts( "fiducial setcfg does nothing" );
+ return 0; //ok
+}
+
+
+int FiducialCfgGet( 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->type = AV_MODEL_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
+}
+
+
int RegisterModel( Stg::Model* mod, void* dummy )
{
//printf( "[AvonStage] registering %s\n", mod->Token() );
@@ -231,10 +375,12 @@ int RegisterModel( Stg::Model* mod, void* dummy )
std::string str = mod->GetModelType();
- if( str == "position" )
- type = AV_MODEL_POSITION2D;
- else if( str == "ranger" )
+ // if( str == "position" )
+ //type = AV_MODEL_POSITION2D;
+ if( str == "ranger" )
type = AV_MODEL_RANGER;
+ else if( str == "fiducial" )
+ type = AV_MODEL_FIDUCIAL;
Stg::Model* parent = mod->Parent();
const char* parent_name = parent ? parent->Token() : NULL;
@@ -338,6 +484,13 @@ int main( int argc, char* argv[] )
(av_cmd_set_t)RangerCmd,
(av_cfg_set_t)RangerCfgSet,
(av_cfg_get_t)RangerCfgGet );
+
+ av_install_typed_callbacks( AV_MODEL_FIDUCIAL,
+ (av_data_get_t)FiducialData,
+ (av_cmd_set_t)FiducialCmd,
+ (av_cfg_set_t)FiducialCfgSet,
+ (av_cfg_get_t)FiducialCfgGet );
+
// arguments at index [optindex] and later are not options, so they
// must be world file names
View
13 libstage/CMakeLists.txt
@@ -64,7 +64,18 @@ set_target_properties( stage PROPERTIES
set( stagebinarySrcs main.cc )
set_source_files_properties( ${stagebinarySrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
-add_executable( stagebinary ${stagebinarySrcs} )
+# TODO: build an app bundle on OS X
+#set(MACOSX_BUNDLE_INFO_STRING "${PROJECT_NAME}")
+#set(MACOSX_BUNDLE_GUI_IDENTIFIER "org.playerstage.stage")
+#set(MACOSX_BUNDLE_LONG_VERSION_STRING "${PROJECT_NAME} Version ${VERSION}")
+#set(MACOSX_BUNDLE_BUNDLE_NAME ${PROJECT_NAME})
+#set(MACOSX_BUNDLE_SHORT_VERSION_STRING ${VERSION})
+#set(MACOSX_BUNDLE_BUNDLE_VERSION ${VERSION})
+#set(MACOSX_BUNDLE_COPYRIGHT "Copyright 2010. All Rights Reserved.")
+
+# add_executable( stagebinary MACOSX_BUNDLE ${stagebinarySrcs} )
+
+add_executable( stagebinary MACOSX_BUNDLE ${stagebinarySrcs} )
# Apple seems to have trouble when libstage and stagebinary are linked against FLTK
# Newer Linux distributions won't allow stagebinary to inherit libstage's links to fltk, so we need
Please sign in to comment.
Something went wrong with that request. Please try again.