C++ PolyDriverList类代码示例

bool AnalogServer::attachAll(const PolyDriverList &analog2attach)
{
    yTrace();
    if (analog2attach.size() != 1)
    {
        std::cerr<<"AnalogServer: cannot attach more than one device\n";
        return false;
    }

    yarp::dev::PolyDriver * Idevice2attach=analog2attach[0]->poly;

    if (Idevice2attach->isValid())
    {
        Idevice2attach->view(analogSensor_p);
    }

    if(NULL == analogSensor_p)
    {
        yError() << "AnalogServer: subdevice passed to attach method is invalid!!!";
        return false;
    }
    attach(analogSensor_p);
    start();

    return true;
}

bool JoypadControlServer::attachAll(const PolyDriverList& p)
{
    if (p.size() != 1)
    {
        yError("JoypadControlServer: cannot attach more than one device");
        return false;
    }

    yarp::dev::PolyDriver* Idevice2attach = p[0]->poly;
    if(p[0]->key == "IJoypadController")
    {
        yInfo() << "JoypadControlServer: Good name Dude!";
    }
    else
    {
        yInfo() << "JoypadControlServer: Bad name Dude!!";
    }

    if (!Idevice2attach->isValid())
    {
        yError() << "JoypadControlServer: Device " << p[0]->key << " to attach to is not valid ... cannot proceed";
        return false;
    }

    Idevice2attach->view(m_device);
    if(!attach(m_device))
        return false;

    PeriodicThread::setPeriod(m_period);
    if (!PeriodicThread::start())
        return false;

    openPorts();
    return true;
}

bool BatteryWrapper::attachAll(const PolyDriverList &battery2attach)
{
    if (battery2attach.size() != 1)
    {
        yError("BatteryWrapper: cannot attach more than one device");
        return false;
    }

    yarp::dev::PolyDriver * Idevice2attach = battery2attach[0]->poly;

    if (Idevice2attach->isValid())
    {
        Idevice2attach->view(battery_p);
    }

    if(NULL == battery_p)
    {
        yError("BatteryWrapper: subdevice passed to attach method is invalid");
        return false;
    }
    attach(battery_p);
    RateThread::setRate(_rate);
    RateThread::start();

    return true;
}

bool AnalogWrapper::attachAll(const PolyDriverList &analog2attach)
{
    //check if we already instantiated a subdevice previously
    if (ownDevices)
        return false;

    if (analog2attach.size() != 1)
    {
        yError("AnalogWrapper: cannot attach more than one device");
        return false;
    }

    yarp::dev::PolyDriver * Idevice2attach=analog2attach[0]->poly;

    if (Idevice2attach->isValid())
    {
        Idevice2attach->view(analogSensor_p);
    }

    if(YARP_NULLPTR == analogSensor_p)
    {
        yError("AnalogWrapper: subdevice passed to attach method is invalid");
        return false;
    }
    attach(analogSensor_p);
    RateThread::setRate(_rate);
    RateThread::start();

    return true;
}

bool yarp::dev::ServerInertial::attachAll(const PolyDriverList &imuToAttachTo)
{
    if (imuToAttachTo.size() != 1)
    {
        yError("ServerInertial: cannot attach more than one device");
        return false;
    }

    return attach(imuToAttachTo[0]->poly);
}

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 Rangefinder2DWrapper::attachAll(const PolyDriverList &device2attach)
{
    if (device2attach.size() != 1)
    {
        yError("Rangefinder2DWrapper: cannot attach more than one device");
        return false;
    }

    yarp::dev::PolyDriver * Idevice2attach = device2attach[0]->poly;

    if (Idevice2attach->isValid())
    {
        Idevice2attach->view(sens_p);
        Idevice2attach->view(iTimed);
    }

    if (NULL == sens_p)
    {
        yError("Rangefinder2DWrapper: subdevice passed to attach method is invalid");
        return false;
    }
    attach(sens_p);

    if(!sens_p->getDistanceRange(minDistance, maxDistance))
    {
        yError() << "Laser device does not provide min & max distance range.";
        return false;
    }

    if(!sens_p->getScanLimits(minAngle, maxAngle))
    {
        yError() << "Laser device does not provide min & max angle scan range.";
        return false;
    }

    if (!sens_p->getHorizontalResolution(resolution))
    {
        yError() << "Laser device does not provide horizontal resolution ";
        return false;
    }

    RateThread::setRate(_rate);
    RateThread::start();

    return true;
}

bool VirtualAnalogWrapper::attachAll(const PolyDriverList &polylist)
{
    mMutex.wait();

    for (int p=0; p<polylist.size(); ++p)
    {
        std::string key=polylist[p]->key.c_str();

        // find appropriate entry in list of subdevices and attach
        for (unsigned int k=0; k<mSubdevices.size(); ++k)
        {    
            if (mSubdevices[k].getKey()==key)
            {
                if (!mSubdevices[k].attach(polylist[p]->poly,key))
                {
                    mMutex.post();
                    return false;
                }
            }
        }
    }

    //check if all devices are attached to the driver
    for (unsigned int k=0; k<mSubdevices.size(); ++k)
    {
        if (!mSubdevices[k].isAttached())
        {
            mMutex.post();
            return false;
        }
   }

    mMutex.post();

    Thread::start();

    return true;
}

/**
  * IWrapper and IMultipleWrapper interfaces
  */
bool RGBDSensorWrapper::attachAll(const PolyDriverList &device2attach)
{
    // First implementation only accepts devices with both the interfaces Framegrabber and IDepthSensor,
    // on a second version maybe two different devices could be accepted, one for each interface.
    // Yet to be defined which and how parameters shall be used in this case ... using the name of the
    // interface to view could be a good initial guess.
    if (device2attach.size() != 1)
    {
        yError("RGBDSensorWrapper: cannot attach more than one device");
        return false;
    }

    yarp::dev::PolyDriver * Idevice2attach = device2attach[0]->poly;
    if(device2attach[0]->key == "IRGBDSensor")
    {
        yInfo() << "RGBDSensorWrapper: Good name Dude!";
    }
    else
    {
        yInfo() << "RGBDSensorWrapper: Bad name Dude!!";
    }

    if (!Idevice2attach->isValid())
    {
        yError() << "RGBDSensorWrapper: Device " << device2attach[0]->key << " to attach to is not valid ... cannot proceed";
        return false;
    }

    Idevice2attach->view(sensor_p);
    if(!attach(sensor_p))
        return false;

    RateThread::setRate(rate);
    RateThread::start();

    return true;
}

bool HapticDeviceWrapper::attachAll(const PolyDriverList &p)
{
    if (p.size()!=1)
    {
        yError("*** Haptic Device Wrapper: cannot attach more than one device");
        return false;
    }

    PolyDriver *dev=p[0]->poly;
    if (dev->isValid())
        dev->view(device);

    if (device==NULL)
    {
        yError("*** Haptic Device Wrapper: invalid device");
        return false;
    }

    start();
    if (verbosity>0)
        yInfo("*** Haptic Device Wrapper: started");

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