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_++; }