MainThread::MainThread( const orcaice::Context& context ) :
SubsystemThread( context.tracer(), context.status(), "MainThread" ),
context_(context)
{
// this subsystem will initialise and exit, but the component will continue running.
setSubsystemType( gbxutilacfr::SubsystemEarlyExit );
}
void readFromProperties( orcaice::Context context, VfhAlgorithmConfig &c )
{
Ice::PropertiesPtr prop = context.properties();
std::string prefix = context.tag();
prefix += ".Config.Vfh.";
c.cellSize = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"CellSize", 0.1 );
c.gridWidthInCells = orcaice::getPropertyAsIntWithDefault( prop, prefix+"GridWidthInCells", 61 );
c.sectorAngle = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"SectorAngle", 5.0 )*M_PI/180.0;
// c.robotRadius = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"RobotRadius", 0.3 );
c.safetyDist0ms = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"SafetyDist0ms", 0.1 );
c.safetyDist1ms = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"SafetyDist1ms", 0.1 );
c.maxSpeed = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"MaxSpeed", 0.2 );
c.maxSpeedNarrowOpening = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"MaxSpeedNarrowOpening", 0.2 );
c.maxSpeedWideOpening = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"MaxSpeedWideOpening", 0.2 );
c.maxAcceleration = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"MaxAcceleration", 0.2 );
c.maxTurnrate0ms = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"MaxTurnrate0ms", 40.0 )*M_PI/180.0;
c.maxTurnrate1ms = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"MaxTurnrate1ms", 40.0 )*M_PI/180.0;
c.absoluteMaxTurnrate = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"AbsoluteMaxTurnrate", 60.0 )*M_PI/180.0;
c.minTurnRadiusSafetyFactor = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"MinTurnRadiusSafetyFactor", 1.10 );
c.freeSpaceCutoff0ms = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"FreeSpaceCutoff0ms", 2e6 );
c.freeSpaceCutoff1ms = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"FreeSpaceCutoff1ms", 2e6 );
c.obsCutoff0ms = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"ObsCutoff0ms", 2e6 );
c.obsCutoff1ms = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"ObsCutoff1ms", 2e6 );
c.weightDesiredDir = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"WeightDesiredDir", 5.0 );
c.weightCurrentDir = orcaice::getPropertyAsDoubleWithDefault( prop, prefix+"WeightCurrentDir", 3.0 );
}
MainThread::MainThread( const orcaice::Context &context )
: orcaice::SubsystemThread( context.tracer(), context.status(), "MainThread" )
, context_(context)
, descr_(new orca::ImageDescription())
, config_()
{
}
MainThread::MainThread( const orcaice::Context &context )
: SubsystemThread( context.tracer(), context.status(), "MainThread" ),
context_(context)
{
// this subsystem is designed to terminate early (before the component itself)
// we have to let Status know so it can use this info in summarizing the component's
// state.
setSubsystemType( gbxutilacfr::SubsystemEarlyExit );
}
void
TestThread::initialise()
{
context_.tracer().info( "TestThread::initialise()" );
try
{
throw string("Error in initialise()");
}
catch (...)
{
orcaice::catchExceptions( context_.tracer(), "initialising" );
}
}
void
loadElementsFromConfigFile( orcaqgemv::GuiElementModel &guiElementModel,
const orcaice::Context &context )
{
// create a map of types
const string typePrefix = context.tag() + ".Config.Element.Type";
std::map<string,string> typeMap = context.properties()->getPropertiesForPrefix(typePrefix);
// create a map of descriptions
const string descriptionPrefix = context.tag() + ".Config.Element.Description";
std::map<string,string> descriptionMap = context.properties()->getPropertiesForPrefix(descriptionPrefix);
// debug output
for ( map<string,string>::iterator it=typeMap.begin(); it!=typeMap.end(); ++it )
{
stringstream ss;
ss << "it->first: " << it->first << endl;
ss << "it->second: " << it->second << endl;
context.tracer().debug( ss.str(), 5 );
}
// create elements based on Type entries
for ( map<string,string>::iterator it = typeMap.begin(); it != typeMap.end(); ++it )
{
// extract the key: e.g. for Config.Element.TypeXX the key is XX
QString elementType( it->second.c_str() );
string key = it->first.substr( typePrefix.size() );
stringstream ss; ss << "Extracted key is: " << key;
context.tracer().debug( ss.str(), 5 );
// find the corresponding entry in the descriptionMap
map<string,string>::iterator descriptionMapIt = descriptionMap.find( context.tag() + ".Config.Element.Description" + key );
if ( descriptionMapIt==descriptionMap.end() )
{
ss.str(""); ss << "'Description' entry with key " << key << " expected. Check your config file!";
context.tracer().warning( ss.str() );
continue;
}
// found an entry
ss.str(""); ss << "Found Description entry: " << descriptionMapIt->second;
context.tracer().debug( ss.str(), 5 );
//
// assemble all pieces to create the GUI element
//
QString elementDescription( descriptionMapIt->second.c_str() );
QString uniqueId = extractUniqueId( elementDescription );
QString platformName = extractPlatformName( elementDescription );
guiElementModel.createGuiElement( elementType, elementDescription, platformName, uniqueId );
}
}
void
MultiCameraReader::readFromFile( std::ifstream *file,
const std::string &format,
orcaice::Context context,
orca::MultiCameraDescriptionPtr &obj )
{
descr_ = new orca::MultiCameraDescription();
if ( format=="ice" )
{
orcalog::IceReadHelper helper( context.communicator(), file );
ice_readMultiCameraDescription( helper.stream_, obj );
helper.read();
}
else if ( format=="jpeg" || format=="bmp" || format=="avi")
{
#ifndef OPENCV_FOUND
std::stringstream infostring;
infostring << "Images can only be replayed in '" << format << "' format if you have OpenCV.";
context.tracer().info( infostring.str() );
context.tracer().info( "Please have a look at the documentation for installing OpenCV." );
std::stringstream errorstring;
errorstring << "Logger: '"<< format <<"' format not supported because OpenCV is not installed.";
throw orcalog::FormatNotSupportedException( ERROR_INFO, errorstring.str() );
#endif
// Populate the description with data from the file
std::string line;
std::getline( *file, line );
std::stringstream ss( line );
fromLogString( ss, descr_ );
// Point the descriptor pointer to our copy of it
obj = descr_;
}
else
{
stringstream ss;
ss << "can't handle format: " << format;
throw orcalog::FormatNotSupportedException( ERROR_INFO, ss.str() );
}
// Now that we know the size, initialise the data storage
initDataStorage();
}
void
TestThread::finalise()
{
context_.tracer().info( "TestThread::finalise()" );
try {
throw gbxutilacfr::Exception( ERROR_INFO, "Error in finalise()" );
}
catch ( ... ) {
// orcaice::catchExceptions( context_.tracer(), "finalising" );
// orcaice::catchExceptionsWithStatus( "finalising", health() );
orcaice::catchExceptionsWithStatus( "finalising", health(), gbxutilacfr::SubsystemWarning );
// orcaice::catchExceptionsWithStatusAndSleep( "finalising", health(), gbxutilacfr::SubsystemWarning );
}
context_.shutdown();
}
HwThread::HwThread( const orcaice::Context &context ) :
SubsystemThread( context.tracer(), context.status(), "HwThread" ),
context_(context)
{
setMaxHeartbeatInterval( 10.0 );
//
// Read settings
//
Ice::PropertiesPtr prop = context_.properties();
std::string prefix = context_.tag() + ".Config.";
isMotionEnabled_ = (bool)orcaice::getPropertyAsIntWithDefault( prop, prefix+"EnableMotion", 1 );
// Dynamically load the library and find the factory
std::string driverLibName =
orcaice::getPropertyWithDefault( prop, prefix+"DriverLib", "libHydroBicyclePlayerClient.so" );
context_.tracer().debug( "HwThread: Loading driver library "+driverLibName, 4 );
// The factory which creates the driver
std::auto_ptr<hydrointerfaces::BicycleFactory> driverFactory;
try {
driverLib_.reset( new hydrodll::DynamicallyLoadedLibrary(driverLibName) );
driverFactory.reset(
hydrodll::dynamicallyLoadClass<hydrointerfaces::BicycleFactory,BicycleDriverFactoryMakerFunc>
( *driverLib_, "createBicycleDriverFactory" ) );
}
catch (hydrodll::DynamicLoadException &e)
{
context_.tracer().error( e.what() );
throw;
}
// create the driver
try {
context_.tracer().info( "HwThread: Creating driver..." );
driver_.reset( driverFactory->createDriver( context_.toHydroContext() ) );
}
catch ( ... )
{
stringstream ss;
ss << "HwThread: Caught unknown exception while creating driver";
context_.tracer().error( ss.str() );
throw;
}
}
NetworkThread::NetworkThread( Display* display, const orcaice::Context& context ) :
SafeThread( context.tracer() ),
events_(new hydroiceutil::EventQueue),
display_(display),
context_(context)
{
// install event optimizer
hydroiceutil::EventQueueOptimizerPtr opt = new TeleopEventQueueOptimizer;
events_->setOptimizer( opt );
}
void
TestThread::work()
{
context_.tracer().info( "TestThread::work()" );
try
{
try
{
throw string("Error in work()");
}
catch ( const string& e )
{
context_.tracer().info( "caught string the first time. Rethrowing ..." );
throw;
}
}
catch (...)
{
orcaice::catchExceptions( context_.tracer(), "working" );
// orcaice::catchExceptions( context_.tracer(), string(__func__) );
}
}
ReplayConductor::ReplayConductor( orcalog::MasterFileReader &masterFileReader,
std::vector<orcalog::Replayer*> &replayers,
const IceUtil::Time &beginTime,
double replayRate,
const orcaice::Context &context )
: SubsystemThread( context.tracer(), context.status() ),
isPlaying_(false),
isPlayingOrAboutToStart_(false),
masterFileReader_(masterFileReader),
replayers_(replayers),
context_(context)
{
clock_.setReplayRate(replayRate);
bool cursorValid = masterFileReader_.getCursorTime( firstItemSec_, firstItemUsec_ );
assert( cursorValid );
// Fast-forward if non-zero beginTime
if ( beginTime > orcalog::iceUtilTime(0,0) )
{
fastForward( beginTime );
}
}
MainThread::MainThread( const orcaice::Context & context ) :
SafeThread(context.tracer()),
context_(context)
{
//
// Read settings
//
string prefix = context_.tag() + ".Config.";
update_interval = orcaice::getPropertyAsDoubleWithDefault( context_.properties(), prefix+"UpdateInterval", 0.1 );
// based on the config parameter, create the right driver
string driverName = orcaice::getPropertyWithDefault( context_.properties(),
prefix+"Driver", "static" );
if ( driverName == "static" )
{
std::string driverPrefix = prefix + "Static.";
orca::Frame2d pose;
orcaobj::setInit(pose);
pose = orcaobj::getPropertyAsFrame2dWithDefault( context_.properties(),
driverPrefix+"Pose", pose );
driver_ = new StaticDriver( pose );
}
else if ( driverName == "stage" )
{
#ifdef HAVE_STAGE_DRIVER
context_.tracer().debug( "loading Player-Client driver",3);
std::string driverPrefix = prefix + "Stage.";
std::string playerHost = orcaice::getPropertyWithDefault( context_.properties(),
driverPrefix+"Host", "localhost" );
int playerPort = orcaice::getPropertyAsIntWithDefault( context_.properties(),
driverPrefix+"Port", 6665 );
std::string playerId = orcaice::getPropertyWithDefault( context_.properties(),
driverPrefix+"Id", "model1" );
driver_ = new StageDriver( playerHost.c_str(), playerPort, playerId.c_str() );
#else
// unrecoverable error
context_.shutdown();
throw gbxutilacfr::Exception( ERROR_INFO, "Can't instantiate driver 'stage' because it was not built!" );
#endif
}
context_.tracer().debug("driver instantiated",5);
}
IceGrid::QueryPrx
getDefaultQuery( const orcaice::Context& context )
{
Ice::CommunicatorPtr ic = context.communicator();
assert( ic );
Ice::ObjectPrx locatorPrx = ic->getDefaultLocator();
Ice::Identity locatorId = locatorPrx->ice_getIdentity();
Ice::Identity queryId;
queryId.category = locatorId.category;
queryId.name = "Query";
Ice::ObjectPrx objPrx = ic->stringToProxy( ic->identityToString( queryId ) );
IceGrid::QueryPrx queryPrx;
try {
// objPrx->ice_ping();
// string address = orcacm::connectionToRemoteAddress( queryPrx->ice_getConnection()->toString() );
// std::ostringstream os;
// os<<"Registry ping successful: "<<data.address;
// context.tracer().debug( os.str() );
queryPrx = IceGrid::QueryPrx::checkedCast( objPrx );
}
catch ( const Ice::Exception& e ) {
// what do we do?
ostringstream os;
os << "(while looking for IceGrid Query interface) :"<<e.what();
context.tracer().warning( os.str() );
}
return queryPrx;
}
void
MultiCameraWriter::logToFile( std::ofstream *file, const std::string &format, orcaice::Context context, const orca::MultiCameraDescriptionPtr &obj)
{
if ( format=="ice" ){
orcalog::IceWriteHelper helper( context.communicator() );
ice_writeMultiCameraDescription( helper.stream_, obj );
helper.write( file );
}
else if ( format=="jpeg" || format == "bmp" || format=="avi") {
//Log the MultiCamera Description to an ascii file
(*file) << toLogString(obj) << endl;
}
else{
stringstream ss;
ss << "can't handle format: " << format;
throw orcalog::FormatNotSupportedException( ERROR_INFO, ss.str() );
}
}
Ice::ObjectPrx
getComponentAdmin( const orcaice::Context& context, const orca::FQComponentName& fqName )
{
orca::FQComponentName resolvedFqname = orcaice::resolveLocalPlatform( context, fqName );
Ice::CommunicatorPtr ic = context.communicator();
assert( ic );
Ice::LocatorPrx locatorPrx = ic->getDefaultLocator();
Ice::Identity adminId = toAdminIdentity( resolvedFqname );
Ice::ObjectPrx adminPrx;
try {
adminPrx = locatorPrx->findObjectById( adminId );
}
catch ( const Ice::Exception& ) {
// what do we do?
}
return adminPrx;
}
TestThread::TestThread( Config config, const orcaice::Context &context ) :
orcaice::SubsystemThread( context.tracer(), context.status() ),
config_( config ),
context_(context)
{
}
MainThread::MainThread( const orcaice::Context& context ) :
SafeThread(context.tracer()),
context_(context)
{
}
InputThread::InputThread( Network* network, const orcaice::Context& context ) :
SafeThread( context.tracer() ),
network_(network),
context_(context)
{
}
MainThread::MainThread( const orcaice::Context &context ) :
orcaice::SubsystemThread( context.tracer(), context.status(), "MainThread" ),
context_(context)
{
}
MainThread::MainThread( const orcaice::Context& context ) :
SubsystemThread( context.tracer(), context.status(), "MainThread" ),
context_(context)
{
setMaxHeartbeatInterval( 20.0 );
}
void
MultiCameraReader::readFromFile( std::ifstream *file,
const std::string &format,
orcaice::Context context,
orca::MultiCameraDataPtr &obj )
{
if ( format=="ice" )
{
orcalog::IceReadHelper helper( context.communicator(), file );
ice_readMultiCameraData( helper.stream_, obj );
helper.read();
}
else if ( format=="jpeg" || format == "bmp" )
{
#ifndef OPENCV_FOUND
std::stringstream infostring;
infostring << "Images can only be replayed in '"<< format <<"' format if you have OpenCV.";
context.tracer().info( infostring.str() );
context.tracer().info( "Please have a look at the documentation for installing OpenCV." );
std::stringstream errorstring;
errorstring << "Logger: '"<< format << "' format not supported because OpenCV is not installed.";
throw orcalog::FormatNotSupportedException( ERROR_INFO, errorstring.str() );
#endif
// Pull the next line from the data log file
std::string line;
std::getline( *file, line );
// Fill the MultiCameraDataObject with the data
std::stringstream ss( line );
fromLogString( ss, data_ );
// Load the correct images using the filenames provided
loadImageData( ss, data_);
// Point the descriptor pointer to our copy
obj = data_;
}
else if ( format=="avi" )
{
#ifndef OPENCV_FOUND
context.tracer().info( "Data can only be replayed in 'avi' format if you have OpenCV." );
context.tracer().info( "Please have a look at the documentation for installing OpenCV." );
throw orcalog::FormatNotSupportedException( ERROR_INFO, "Logger: 'avi' format not supported because OpenCV is not installed." );
#endif
// Pull the next line from the data log file
std::string line;
std::getline( *file, line );
// Fill the MultiCameraDataObject with the data
std::stringstream ss( line );
fromLogString( ss, data_ );
// Load the correct images using the filenames provided
loadVideoData( ss, data_ );
// Point the descriptor pointer to our copy
obj = data_;
}
else
{
stringstream ss;
ss << "can't handle format: " << format;
throw orcalog::FormatNotSupportedException( ERROR_INFO, ss.str() );
}
}
void
MultiCameraWriter::logToFile( std::ofstream *file, const std::string &format, orcaice::Context context, const orca::MultiCameraDataPtr &obj)
{
if ( format == "ice" ){
orcalog::IceWriteHelper helper( context.communicator() );
ice_writeMultiCameraData( helper.stream_, obj );
helper.write( file );
}
else if ( format=="jpeg" ){
// write object meta data into a ascii file
(*file) << toLogString(obj);
// For each image in MultiCameraDatPtr, write it to disk
for (unsigned int i = 0; i < obj->cameraDataVector.size(); ++i)
{
// generate image filename (different file for each image)
std::stringstream filename;
filename << ".//" << directoryPrefix_ << "//" << "cam" << i << "_image"
<< std::setw(5) << std::setfill('0') << dataCounter_ << ".jpeg";
std::string outputname;
outputname = filename.str();
// Write the outputted filename to the logfile as well
(*file) << " " << outputname;
// the images are saved as individual jpegs
writeCameraDataAsFile(obj, outputname, i);
}
// Make sure we put in a newline
(*file) << endl;
}
else if ( format=="bmp" ){
// write object meta data into a ascii file
(*file) << toLogString(obj);
// For each image in MultiCameraDatPtr, write it to disk
for (unsigned int i = 0; i < obj->cameraDataVector.size(); ++i)
{
// generate image filename (different file for each image)
std::stringstream filename;
filename << ".//" << directoryPrefix_ << "//" << "cam" << i << "_image"
<< std::setw(5) << std::setfill('0') << dataCounter_ << ".bmp";
std::string outputname;
outputname = filename.str();
// Write the outputted filename to the logfile as well
(*file) << " " <<outputname;
// the images are saved as individual bitmaps
writeCameraDataAsFile(obj, outputname, i);
}
// Make sure we put in a newline
(*file) << endl;
}
else if (format =="avi") {
// write object meta data into a ascii file
(*file) << toLogString(obj);
// For each image in MultiCameraDatPtr, write it to disk
for (unsigned int i = 0; i < obj->cameraDataVector.size(); ++i)
{
// generate image filename. This will be the same for each video,
// but is important for the logplayer to know what the filename is.
// Also allows for future enhancements to split the video file into
// manageable pieces
std::stringstream filename;
filename << ".//" << directoryPrefix_ << "//" << "cam" << i << "_avi_video.avi";
std::string outputname = filename.str();
// Write the outputted filename to the logfile as well
(*file) << " " << outputname;
// the images are saved as uncompressed video into separate files
writeCameraDataAsVideo(obj, i);
}
// Make sure we put in a newline
(*file) << endl;
}
else{
stringstream ss;
ss << "can't handle format: " << format;
throw orcalog::FormatNotSupportedException( ERROR_INFO, ss.str() );
}
// Increment output image number
dataCounter_++;
}
