C++ orcaice::Context类代码示例

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_()
{
}

    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();

}

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

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 ) :
    SubsystemThread( context.tracer(), context.status(), "MainThread" ),
    context_(context)
{
    setMaxHeartbeatInterval( 20.0 );
}