Пример #1
0
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 );
}
Пример #2
0
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 );
}
Пример #3
0
MainThread::MainThread( const orcaice::Context &context ) 
: orcaice::SubsystemThread( context.tracer(), context.status(), "MainThread" )
, context_(context)
, descr_(new orca::ImageDescription())
, config_()
{
}
Пример #4
0
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 );
}
Пример #5
0
void 
TestThread::initialise() 
{
    context_.tracer().info( "TestThread::initialise()" );
    try
    {
        throw string("Error in initialise()");
    }
    catch (...)
    {
        orcaice::catchExceptions( context_.tracer(), "initialising" );
    }
}
Пример #6
0
    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 );
        }

    }
Пример #7
0
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();

}
Пример #8
0
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();
}
Пример #9
0
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;
    }  
}
Пример #10
0
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 );
}
Пример #11
0
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__) );
    }
}
Пример #12
0
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 );
    }
}
Пример #13
0
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);
}
Пример #14
0
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;
}
Пример #15
0
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() );
    }

}
Пример #16
0
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;
}
Пример #17
0
TestThread::TestThread( Config config, const orcaice::Context &context ) :
    orcaice::SubsystemThread( context.tracer(), context.status() ),
    config_( config ),
    context_(context)
{
}
Пример #18
0
MainThread::MainThread( const orcaice::Context& context ) :
    SafeThread(context.tracer()),
    context_(context)
{
}
Пример #19
0
InputThread::InputThread( Network* network, const orcaice::Context& context ) :
    SafeThread( context.tracer() ),
    network_(network),
    context_(context)
{
}
Пример #20
0
MainThread::MainThread( const orcaice::Context &context ) :
    orcaice::SubsystemThread( context.tracer(), context.status(), "MainThread" ),
    context_(context)
{
}
Пример #21
0
MainThread::MainThread( const orcaice::Context& context ) :
    SubsystemThread( context.tracer(), context.status(), "MainThread" ),
    context_(context)
{
    setMaxHeartbeatInterval( 20.0 );
}
Пример #22
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() );
    }
}
Пример #23
0
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_++;
}