C++ PolyDriverList::push方法代码示例

bool floatingBaseEstimator::attachAllControlBoard(const PolyDriverList& p)
{    
    PolyDriverList controlBoardList;
    for(size_t devIdx = 0; devIdx < (size_t) p.size(); devIdx++)
    {
        IEncoders * pEncs = 0;
        if( p[devIdx]->poly->view(pEncs) )
        {
            controlBoardList.push(const_cast<PolyDriverDescriptor&>(*p[devIdx]));
        }
    }

    // Attach the controlBoardList to the controlBoardRemapper
    bool ok = remappedControlBoardInterfaces.multwrap->attachAll(controlBoardList);

    if( !ok )
    {
        yError() << " floatingBaseEstimator::attachAll in attachAll of the remappedControlBoard";
        return false;
    }

    return true;
}

bool RemoteControlBoardRemapper::open(Searchable& config)
{
    Property prop;
    prop.fromString(config.toString().c_str());

    std::string localPortPrefix;
    std::vector<std::string> remoteControlBoardsPorts;

    // Check if the required parameters  are found
    if( prop.check("localPortPrefix") && prop.find("localPortPrefix").isString() )
    {
        localPortPrefix = prop.find("localPortPrefix").asString().c_str();
    }
    else
    {
        yError() <<"RemoteControlBoardRemapper: Error parsing parameters: \"localPortPrefix\" should be a string.";
        return false;
    }

    Bottle *remoteControlBoards=prop.find("remoteControlBoards").asList();
    if(remoteControlBoards==0)
    {
        yError() <<"RemoteControlBoardRemapper: Error parsing parameters: \"remoteControlBoards\" should be followed by a list.";
        return false;
    }

    remoteControlBoardsPorts.resize(remoteControlBoards->size());
    for(int ax=0; ax < remoteControlBoards->size(); ax++)
    {
        remoteControlBoardsPorts[ax] = remoteControlBoards->get(ax).asString().c_str();
    }

    // Load the REMOTE_CONTROLBOARD_OPTIONS, containg any additional option to pass to the remote control boards
    Property remoteControlBoardsOptions;

    Bottle & optionsGroupBot = prop.findGroup("REMOTE_CONTROLBOARD_OPTIONS");
    if( !(optionsGroupBot.isNull()) )
    {
        remoteControlBoardsOptions.fromString(optionsGroupBot.toString());
    }

    // Parameters loaded, open all the remote controlboards

    m_remoteControlBoardDevices.resize(remoteControlBoardsPorts.size(),0);

    PolyDriverList remoteControlBoardsList;

    for(size_t ctrlBrd=0; ctrlBrd < remoteControlBoardsPorts.size(); ctrlBrd++ )
    {
        std::string remote = remoteControlBoardsPorts[ctrlBrd];
        // Note: as local parameter we use localPortPrefix+remoteOfTheReportControlBoard
        std::string local = localPortPrefix+remote;

        Property options = remoteControlBoardsOptions;
        options.put("device", "remote_controlboard");
        options.put("local", local);
        options.put("remote", remote);

        m_remoteControlBoardDevices[ctrlBrd] = new PolyDriver();

        bool ok = m_remoteControlBoardDevices[ctrlBrd]->open(options);

        if( !ok || !(m_remoteControlBoardDevices[ctrlBrd]->isValid()) )
        {
            yError() << "RemoteControlBoardRemapper: error opening remote_controlboard with remote \"" << remote << "\", opening the device failed.";
            closeAllRemoteControlBoards();
            return false;
        }

        // We use the remote name of the remote_controlboard as the key for it, in absense of anything better
        remoteControlBoardsList.push((m_remoteControlBoardDevices[ctrlBrd]),remote.c_str());
    }

    // Device opened, now we open the ControlBoardRemapper and then we call attachAll
    bool ok = ControlBoardRemapper::open(prop);

    if( !ok )
    {
        yError() << "RemoteControlBoardRemapper: error opening the controlboardremapper device, opening the device failed.";
        ControlBoardRemapper::close();
        closeAllRemoteControlBoards();
        return false;
    }

    // If open went ok, we now call attachAll
    ok = ControlBoardRemapper::attachAll(remoteControlBoardsList);

    if( !ok )
    {
        yError() << "RemoteControlBoardRemapper: error calling attachAll in the controlboardremapper device, opening the device failed.";
        ControlBoardRemapper::close();
        closeAllRemoteControlBoards();
        return false;
    }

    // All went ok, return true
    // TODO: close devices that are not actually used by the remapper
    return true;
}

bool Rangefinder2DWrapper::open(yarp::os::Searchable &config)
{
    Property params;
    params.fromString(config.toString().c_str());

    if (!config.check("period"))
    {
        yError() << "Rangefinder2DWrapper: missing 'period' parameter. Check you configuration file\n";
        return false;
    }
    else
        _rate = config.find("period").asInt();

    if (!config.check("name"))
    {
        yError() << "Rangefinder2DWrapper: missing 'name' parameter. Check you configuration file; it must be like:";
        yError() << "   name:         full name of the port, like /robotName/deviceId/sensorType:o";
        return false;
    }
    else
    {
        streamingPortName  = config.find("name").asString().c_str();
        rpcPortName = streamingPortName + "/rpc:i";
        setId("Rangefinder2DWrapper");
    }

    if(!initialize_YARP(config) )
    {
        yError() << sensorId << "Error initializing YARP ports";
        return false;
    }

    checkROSParams(config);

    // call ROS node/topic initilization, if needed
    if (!initialize_ROS())
    {
        return false;
    }

    if(config.check("subdevice"))
    {
        Property       p;
        PolyDriverList driverlist;
        p.fromString(config.toString(), false);
        p.put("device", config.find("subdevice").asString());

        if(!driver.open(p) || !driver.isValid())
        {
            yError() << "RangeFinder2DWrapper: failed to open subdevice.. check params";
            return false;
        }

        driverlist.push(&driver, "1");
        if(!attachAll(driverlist))
        {
            yError() << "RangeFinder2DWrapper: failed to open subdevice.. check params";
            return false;
        }
        isDeviceOwned = true;
    }
    return true;
}

bool TwoCanBusThreeWrappers::configure(ResourceFinder &rf) {

    if(rf.check("help")) {
        printf("TwoCanBusThreeWrappers options:\n");
        printf("\t--help (this help)\t--from [file.ini]\t--context [path]\n");
        CD_DEBUG_NO_HEADER("%s\n",rf.toString().c_str());
        return false;
    }

    //-- /dev/can0 --
    Bottle devCan0 = rf.findGroup("devCan0");
    CD_DEBUG("%s\n",devCan0.toString().c_str());
    Property optionsDevCan0;
    optionsDevCan0.fromString(devCan0.toString());
    deviceDevCan0.open(optionsDevCan0);
    if (!deviceDevCan0.isValid()) {
        CD_ERROR("deviceDevCan0 instantiation not worked.\n");
        return false;
    }

    //-- /dev/can1 --
    Bottle devCan1 = rf.findGroup("devCan1");
    CD_DEBUG("%s\n",devCan1.toString().c_str());
    Property optionsDevCan1;
    optionsDevCan1.fromString(devCan1.toString());
    deviceDevCan1.open(optionsDevCan1);
    if (!deviceDevCan1.isValid()) {
        CD_ERROR("deviceDevCan1 instantiation not worked.\n");
        return false;
    }

    //-- wrapper0 --
    Bottle wrapper0 = rf.findGroup("wrapper0");
    CD_DEBUG("%s\n",wrapper0.toString().c_str());
    Property optionsWrapper0;
    optionsWrapper0.fromString(wrapper0.toString());
    deviceWrapper0.open(optionsWrapper0);
    if (!deviceWrapper0.isValid()) {
        CD_ERROR("deviceWrapper0 instantiation not worked.\n");
        return false;
    }

    //-- wrapper1 --
    Bottle wrapper1 = rf.findGroup("wrapper1");
    CD_DEBUG("%s\n",wrapper1.toString().c_str());
    Property optionsWrapper1;
    optionsWrapper1.fromString(wrapper1.toString());
    deviceWrapper1.open(optionsWrapper1);
    if (!deviceWrapper1.isValid()) {
        CD_ERROR("deviceWrapper1 instantiation not worked.\n");
        return false;
    }

    //-- wrapper2 --
    Bottle wrapper2 = rf.findGroup("wrapper2");
    CD_DEBUG("%s\n",wrapper2.toString().c_str());
    Property optionsWrapper2;
    optionsWrapper2.fromString(wrapper2.toString());
    deviceWrapper2.open(optionsWrapper2);
    if (!deviceWrapper2.isValid()) {
        CD_ERROR("deviceWrapper2 instantiation not worked.\n");
        return false;
    }

    IMultipleWrapper *iWrapper0, *iWrapper1, *iWrapper2;

    deviceWrapper0.view(iWrapper0);
    deviceWrapper1.view(iWrapper1);
    deviceWrapper2.view(iWrapper2);

    PolyDriverList list;
    list.push(&deviceDevCan0, "devCan0");
    list.push(&deviceDevCan1, "devCan1");
    iWrapper0->attachAll(list);
    iWrapper1->attachAll(list);
    iWrapper2->attachAll(list);

    return true;
}