C++ Searchable::find方法代码示例

bool RGBDSensorWrapper::openAndAttachSubDevice(Searchable& prop)
{
    Property p;
    subDeviceOwned = new PolyDriver;
    p.fromString(prop.toString().c_str());

    p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
    p.unput("device");
    p.put("device",prop.find("subdevice").asString());  // subdevice was already checked before

    // if error occour during open, quit here.
    yDebug("opening IRGBDSensor subdevice\n");
    subDeviceOwned->open(p);

    if (!subDeviceOwned->isValid())
    {
        yError("opening controlBoardWrapper2 subdevice... FAILED\n");
        return false;
    }
    isSubdeviceOwned = true;
    if(!attach(subDeviceOwned))
        return false;

    RateThread::setRate(rate);
    RateThread::start();
    return true;
}

bool realsense2Driver::open(Searchable& config)
{
    std::vector<RGBDSensorParamParser::RGBDParam*> params;
    for (auto& p:params_map)
    {
        params.push_back(&(p.second));
    }

    m_verbose = config.check("verbose");
    if (config.check("stereoMode")) {
        m_stereoMode = config.find("stereoMode").asBool();
    }

    if (!m_paramParser->parseParam(config, params))
    {
        yError()<<"realsense2Driver: failed to parse the parameters";
        return false;
    }

    if (!initializeRealsenseDevice())
    {
        yError()<<"realsense2Driver: failed to initialize the realsense device";
        return false;
    }

    // setting Parameters
    if (!setParams())
    {
        return false;
    }

    return true;
}

bool JoypadControlServer::openAndAttachSubDevice(Searchable& prop)
{
    Property p;

    m_subDeviceOwned = new PolyDriver;

    p.fromString(prop.toString().c_str());
    p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
    p.unput("device");
    p.put("device",prop.find("subdevice").asString());  // subdevice was already checked before

    // if error occour during open, quit here.
    m_subDeviceOwned->open(p);

    if (!m_subDeviceOwned->isValid())
    {
        yError("JoypadControlServer: opening subdevice... FAILED\n");
        return false;
    }
    m_isSubdeviceOwned = true;
    if(!attach(m_subDeviceOwned))
        return false;

    if(!m_parser.configure(m_device) )
    {
        yError() << "JoypadControlServer: error configuring interfaces for parsers";
        return false;
    }

    openPorts();
    PeriodicThread::setPeriod(m_period);
    return PeriodicThread::start();
}

bool HapticDeviceWrapper::open(Searchable &config)
{
    portStemName=config.check("name",
                              Value(HAPTICDEVICE_WRAPPER_DEFAULT_NAME)).asString().c_str();
    verbosity=config.check("verbosity",Value(0)).asInt();
    int period=config.check("period",
                            Value(HAPTICDEVICE_WRAPPER_DEFAULT_PERIOD)).asInt();
    setRate(period);

    if (config.check("subdevice"))
    {
        Property p(config.toString().c_str());
        p.setMonitor(config.getMonitor(),"subdevice");
        p.unput("device");
        p.put("device",config.find("subdevice").asString());

        if (driver.open(p))
        {
            IHapticDevice *d;
            driver.view(d);
            attach(d);
        }
        else
        {
            yError("*** Haptic Device Wrapper: failed to open the driver!");
            return false;
        }
    }

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

    return true;
}

Value ReachManager::GetValueFromConfig(Searchable& config, string valueName)
{
    if(!config.check(valueName.c_str()))
    {
        cout << "ERROR with config file : couldn't find value : \"" << valueName << "\"." << endl;
        return false;
    }
    return config.find(valueName.c_str());
}

bool HapticDeviceClient::open(Searchable &config)
{
    if (!config.check("remote"))
    {
        yError("*** Haptic Device Client: \"remote\" option missing, failed to open!");
        return false;
    }

    if (!config.check("local"))
    {
        yError("*** Haptic Device Client: \"local\" option missing, failed to open!");
        return false;
    }

    string remote=config.find("remote").asString().c_str();
    string local=config.find("local").asString().c_str();
    verbosity=config.check("verbosity",Value(0)).asInt();

    statePort.open((local+"/state:i").c_str());
    feedbackPort.open((local+"/feedback:o").c_str());
    rpcPort.open((local+"/rpc").c_str());
    statePort.setClient(this);

    bool ok=true;
    ok&=Network::connect((remote+"/state:o").c_str(),statePort.getName().c_str(),"udp");
    ok&=Network::connect(feedbackPort.getName().c_str(),(remote+"/feedback:i").c_str(),"tcp");
    ok&=Network::connect(rpcPort.getName().c_str(),(remote+"/rpc").c_str(),"tcp");

    if (!ok)
    {
        statePort.close();
        feedbackPort.close();
        rpcPort.close();

        yError("*** Haptic Device Client: unable to connect to Haptic Device Wrapper, failed to open!");
        return false;
    }

    if (verbosity>0)
        yInfo("*** Haptic Device Client: opened");

    return true;
}

static bool checkForCarrier(const Bytes *header, Searchable& group) {
    Bottle code = group.findGroup("code").tail();
    if (code.size()==0) return false;
    if (matchCarrier(header,code)) {
        ConstString name = group.find("name").asString();
        if (NetworkBase::registerCarrier(name.c_str(),NULL)) {
            return true;
        }
    }
    return false;
}

void configure_search(RosTypeSearch& env, Searchable& p) {
    if (p.check("out")) {
        env.setTargetDirectory(p.find("out").toString().c_str());
    }
    if (p.check("web",Value(0)).asInt()!=0 || p.findGroup("web").size()==1) {
        env.allowWeb();
    }
    if (p.check("soft",Value(0)).asInt()!=0 || p.findGroup("soft").size()==1) {
        env.softFail();
    }
    env.lookForService(p.check("service"));
}

bool RGBDSensorWrapper::openAndAttachSubDevice(Searchable& prop)
{
    Property p;
    subDeviceOwned = new PolyDriver;
    p.fromString(prop.toString().c_str());

    p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
    p.unput("device");
    p.put("device",prop.find("subdevice").asString());  // subdevice was already checked before

    // if error occour during open, quit here.
    yDebug("opening IRGBDSensor subdevice\n");
    subDeviceOwned->open(p);

    if (!subDeviceOwned->isValid())
    {
        yError("opening controlBoardWrapper2 subdevice... FAILED\n");
        return false;
    }
    isSubdeviceOwned = true;
    if(!attach(subDeviceOwned))
        return false;

    // Configuring parsers
    IRgbVisualParams * rgbVis_p;
    IDepthVisualParams * depthVis_p;

    subDeviceOwned->view(rgbVis_p);
    subDeviceOwned->view(depthVis_p);

    if(!parser.configure(sensor_p) )
    {
        yError() << "RGBD wrapper: error configuring interfaces for parsers";
        return false;
    }
    /*
    bool conf = rgbParser.configure(rgbVis_p);
    conf &= depthParser.configure(depthVis_p);

    if(!conf)
    {
        yError() << "RGBD wrapper: error configuring interfaces for parsers";
        return false;
    }
    */

    RateThread::setRate(rate);
    RateThread::start();
    return true;
}

bool AnalogWrapper::openAndAttachSubDevice(Searchable &prop)
{
    Property p;
    subDeviceOwned = new PolyDriver;
    p.fromString(prop.toString().c_str());

//     p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
    p.unput("device");
    p.put("device", prop.find("subdevice").asString());  // subdevice was already checked before

    // if error occour during open, quit here.
    yDebug("opening analogServer subdevice...");
    subDeviceOwned->open(p);

    if (!subDeviceOwned->isValid())
    {
        yError("opening analogServer subdevice... FAILED\n");
        return false;
    }

    subDeviceOwned->view(analogSensor_p);

    if (analogSensor_p == 0)
    {
        yError("Opening IAnalogSensor interface of analogServer subdevice... FAILED\n");
        return false;
    }

    int chNum = analogSensor_p->getChannels();

    if (chNum <= 0)
    {
        yError("Calling analog sensor has invalid channels number %d.\n", chNum);
        return false;
    }

    attach(analogSensor_p);
    RateThread::setRate(_rate);
    RateThread::start();
    return true;
}

bool AcousticMap::open(Searchable& config) {

    bool ok = true;

    if(!config.check("name")) {
        std::cout << "AuditoryMap: Error, module base name not found in configuration. Start the module with the --name option.." << std::endl;
        return false;
    }

    // module base name
    std::string strModuleName = std::string(config.find("name").asString().c_str());

    // look for group EGO_SPHERE_ACOUSTIC_MAP
    Bottle botConfigAcoustic(config.toString().c_str());
    botConfigAcoustic.setMonitor(config.getMonitor());
    if (!config.findGroup("EGO_SPHERE_ACOUSTIC_MAP").isNull()) {
        botConfigAcoustic.clear();
        botConfigAcoustic.fromString(config.findGroup("EGO_SPHERE_ACOUSTIC_MAP", "Loading visual map configuration  from group EGO_SPHERE_ACOUSTIC_MAP.").toString());
    }

    _salienceDecayRate = botConfigAcoustic.check("decayAcoustic",
                         Value(0.95),
                         "Decay for the acoustic saliency map (double).").asDouble();
    _resXAcoustic = botConfigAcoustic.check("resXAcoustic",
                                            Value(80),
                                            "Width of internal acoustic map (int)").asInt();
    _resYAcoustic = botConfigAcoustic.check("resYAcoustic",
                                            Value(60),
                                            "Height of internal acoustic map (int)").asInt();
    _imgCart.resize(_resXAcoustic,_resYAcoustic);
    _imgRemapX.resize(_resXAcoustic,_resYAcoustic);
    _imgRemapY.resize(_resXAcoustic,_resYAcoustic);
    _imgSpher.resize(_resXAcoustic,_resYAcoustic);
    _imgMapResA.resize(_resXAcoustic,_resYAcoustic);

    ok = ok && _prtVctSound.open(std::string(strModuleName + std::string("/mapAuditory/vct_in")).c_str());

    return ok;
}

bool VirtualAnalogWrapper::open(Searchable& config)
{
    yDebug() << config.toString().c_str();

    mIsVerbose = (config.check("verbose","if present, give detailed output"));

    if (mIsVerbose) yDebug() << "running with verbose output\n";

    //thus thread period is useful for output port... this input port has callback so maybe can skip it (?)
    //thread_period = prop.check("threadrate", 20, "thread rate in ms. for streaming encoder data").asInt();

    yDebug() << "Using VirtualAnalogServer\n";

    if (!config.check("networks", "list of networks merged by this wrapper"))
    {
        yError() << "VirtualAnalogWrapper: missing networks parameters";
        return false;
    }

    Bottle *networks=config.find("networks").asList();
    mNSubdevs=networks->size();
    mSubdevices.resize(mNSubdevs);

    mChan2Board.resize(MAX_ENTRIES);
    mChan2BAddr.resize(MAX_ENTRIES);
    for (int i=0; i< MAX_ENTRIES; i++)
    {
        mChan2Board[i]=-1;
        mChan2BAddr[i]=-1;
    }

    int totalJ=0;

    for (int k=0; k<networks->size(); ++k)
    {
        Bottle parameters=config.findGroup(networks->get(k).asString().c_str());

        if (parameters.size()!=5)    // mapping joints using the paradigm: part from - to / network from - to
        {
            yError() << "VirtualAnalogWrapper: check network parameters in part description"
                     << " I was expecting " << networks->get(k).asString().c_str() << " followed by four integers";
            return false;
        }

        int map0=parameters.get(1).asInt();
        int map1=parameters.get(2).asInt();
        int map2=parameters.get(3).asInt();
        int map3=parameters.get(4).asInt();
        if (map0 >= MAX_ENTRIES || map1 >= MAX_ENTRIES || map2>= MAX_ENTRIES || map3>= MAX_ENTRIES ||
            map0 <0             || map1 <0             || map2<0             || map3<0)
        {
            yError() << "VirtualAnalogWrapper: invalid map entries in networks section, failed initial check";
            return false;
        }

        for (int j=map0; j<=map1; ++j)
        {
            mChan2Board[j]=k;
            mChan2BAddr[j]=j-map0+map2;
        }

        if (!mSubdevices[k].configure(map2,map3,networks->get(k).asString().c_str()))
        {
            yError() << "VirtualAnalogWrapper: configure of subdevice ret false";
            return false;
        }

        totalJ+=map1-map0+1;
    }

    // Verify minimum set of parameters required
    if(!config.check("robotName") )   // ?? qui dentro, da dove lo pesco ??
    {
        yError() << "VirtualAnalogWrapper:  missing robotName, check your configuration file!";
        return false;
    }

    if (config.check("deviceId"))
    {
        yError() << "VirtualAnalogWrapper: the parameter 'deviceId' has been deprecated, please use parameter 'name' instead. \n"
                 << "e.g. In the VFT wrapper configuration files of your robot, replace '<param name=""deviceId""> left_arm </param>' \n"
                 << "with '/icub/joint_vsens/left_arm:i' ";
        return false;
    }

    std::string port_name = config.check("name",Value("controlboard"),"Virtual analog wrapper port name, e.g. /icub/joint_vsens/left_arm:i").asString().c_str();
    std::string robot_name = config.find("robotName").asString().c_str();

    if (!mPortInputTorques.open(port_name.c_str()))
    {
        yError() << "VirtualAnalogWrapper: can't open port " << port_name.c_str();
        return false;
    }

    return true;
}

SkinMeshThreadPort::SkinMeshThreadPort(Searchable& config,int period) : RateThread(period),mutex(1)
{
    yDebug("SkinMeshThreadPort running at %g ms.",getRate());
    mbSimpleDraw=config.check("light");

    sensorsNum=0;
    for (int t=0; t<MAX_SENSOR_NUM; ++t)
    {
        sensor[t]=NULL;
    }

    std::string part="/skinGui/";
    std::string part_virtual="";
    if (config.check("name"))
    {
        part ="/";
        part+=config.find("name").asString().c_str();
        part+="/";
    }
    part.append(config.find("robotPart").asString());
    part_virtual = part;
    part_virtual.append("_virtual");
    part.append(":i");
    part_virtual.append(":i");

    skin_port.open(part.c_str());

    // Ideally, we would use a --virtual flag. since this would make the skinmanager xml file unflexible,
    // let's keep the code structure without incurring in any problem whatsoever
    // if (config.check("virtual"))
    if (true)
    {
        skin_port_virtual.open(part_virtual.c_str());
    }

    int width =config.find("width" ).asInt();
    int height=config.find("height").asInt();

    bool useCalibration = config.check("useCalibration");
    if (useCalibration==true)   yInfo("Using calibrated skin values (0-255)");
    else                        yDebug("Using raw skin values (255-0)");
    
    Bottle *color = config.find("color").asList();
    unsigned char r=255, g=0, b=0;
    if(color)
    {
        if(color->size()<3 || !color->get(0).isInt() || !color->get(1).isInt() || !color->get(2).isInt())
        {
            yError("Error while reading the parameter color: three integer values should be specified (%s).", color->toString().c_str());
        }
        else
        {
            r = color->get(0).asInt();
            g = color->get(1).asInt();
            b = color->get(2).asInt();
            yInfo("Using specified color: %d %d %d", r, g, b);
        }
    }
    else
    {
        yDebug("Using red as default color.");
    }
    defaultColor.push_back(r);
    defaultColor.push_back(g);
    defaultColor.push_back(b);

    skinThreshold = config.check("skinThreshold")?config.find("skinThreshold").asDouble():SKIN_THRESHOLD;
    yDebug("Skin Threshold set to %g", skinThreshold);

    yarp::os::Bottle sensorSetConfig=config.findGroup("SENSORS").tail();

    for (int t=0; t<sensorSetConfig.size(); ++t)
    {       
        yarp::os::Bottle sensorConfig(sensorSetConfig.get(t).toString());

        std::string type(sensorConfig.get(0).asString());
      
        if (type=="triangle"       || type=="fingertip" || type=="fingertip2L" || type=="fingertip2R" ||
            type=="triangle_10pad" || type=="quad16"    || type=="palmR"       || type=="palmL")
        {
            int    id=sensorConfig.get(1).asInt();
            double xc=sensorConfig.get(2).asDouble();
            double yc=sensorConfig.get(3).asDouble();
            double th=sensorConfig.get(4).asDouble();
            double gain=sensorConfig.get(5).asDouble();
            int    lrMirror=sensorConfig.get(6).asInt();
            int    layoutNum=sensorConfig.get(7).asInt();

            yDebug("%s %d %f",type.c_str(),id,gain);

            if (id>=0 && id<MAX_SENSOR_NUM)
            {
                if (sensor[id])
                {
                    yError("Triangle %d already exists.",id);
                }
                else
                {
                    if (type=="triangle")
                    {
//.........这里部分代码省略.........

bool ClientCartesianController::open(Searchable &config)
{
    ConstString remote, local, carrier;

    if (config.check("remote"))
        remote=config.find("remote").asString();
    else
        return false;

    if (config.check("local"))
        local=config.find("local").asString();
    else
        return false;
    
    carrier=config.check("carrier",Value("udp")).asString();

    if (config.check("timeout"))
        timeout=config.find("timeout").asDouble();

    portCmd.open((local+"/command:o").c_str());
    portState.open((local+"/state:i").c_str());
    portEvents.open((local+"/events:i").c_str());
    portRpc.open((local+"/rpc:o").c_str());

    bool ok=true;
    ok&=Network::connect(portRpc.getName().c_str(),(remote+"/rpc:i").c_str());
    if (ok)
    {
        Bottle info;
        getInfoHelper(info);
        if (info.check("server_version"))
        {
            double server_version=info.find("server_version").asDouble();
            if (server_version!=CARTCTRL_CLIENT_VER)
            {
                yError("version mismatch => server(%g) != client(%g); please update accordingly",
                       server_version,CARTCTRL_CLIENT_VER);
                return false;
            }
        }
        else
            yWarning("unable to retrieve server version; please update the server");
    }
    else
    {
        yError("unable to connect to the server rpc port!");
        return false;
    }

    ok&=Network::connect(portCmd.getName().c_str(),(remote+"/command:i").c_str(),carrier.c_str());
    ok&=Network::connect((remote+"/state:o").c_str(),portState.getName().c_str(),carrier.c_str());
    ok&=Network::connect((remote+"/events:o").c_str(),portEvents.getName().c_str(),carrier.c_str());    

    // check whether the solver is alive and connected
    if (ok)
    {
        Bottle command, reply;
    
        command.addVocab(IKINCARTCTRL_VOCAB_CMD_GET);
        command.addVocab(IKINCARTCTRL_VOCAB_OPT_ISSOLVERON);
    
        if (!portRpc.write(command,reply))
        {
            yError("unable to get reply from server!");
            close();

            return false;
        }

        if (reply.get(0).asVocab()==IKINCARTCTRL_VOCAB_REP_ACK)
            if (reply.size()>1)
                if (reply.get(1).asVocab()==IKINCARTCTRL_VOCAB_VAL_TRUE)
                    return connected=true;

        yError("unable to connect to solver!");
        close();

        return false;
    }
    else
    {
        yError("unable to connect to server!");
        close();

        return false;
    }
}

bool PinholeCalibTool::configure (Searchable &config){

    _calibImgSize.width = config.check("w",
                                      Value(320),
                                      "Image width for which calibration parameters were calculated (int)").asInt();

    _calibImgSize.height = config.check("h",
                                      Value(240),
                                      "Image height for which calibration parameters were calculated (int)").asInt();

    _drawCenterCross = config.check("drawCenterCross",
                                    Value(0),
                                    "Draw a cross at calibration center (int [0|1]).").asInt()!=0;


    CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 0) = (float)config.check("fx",
                                                        Value(320.0),
                                                        "Focal length x (double)").asDouble();

    CV_MAT_ELEM( *_intrinsic_matrix, float, 0, 1) = 0.0f;
    CV_MAT_ELEM( *_intrinsic_matrix, float, 0, 2) = (float)config.check("cx",
                                                        Value(160.0),
                                                        "Principal point x (double)").asDouble();
    CV_MAT_ELEM( *_intrinsic_matrix, float, 1, 0) = 0.0f;
    CV_MAT_ELEM( *_intrinsic_matrix, float, 1, 1) = (float)config.check("fy",
                                                        Value(320.0),
                                                        "Focal length y (double)").asDouble();
    CV_MAT_ELEM( *_intrinsic_matrix, float, 1, 2) = (float)config.check("cy",
                                                        Value(120.0),
                                                        "Principal point y (double)").asDouble();
    CV_MAT_ELEM( *_intrinsic_matrix, float, 2, 0) = 0.0f;
    CV_MAT_ELEM( *_intrinsic_matrix, float, 2, 1) = 0.0f;
    CV_MAT_ELEM( *_intrinsic_matrix, float, 2, 2) = 1.0f;


    //check to see if the value is read correctly without caring about the default values.
    if ( !config.check("drawCenterCross") ) { stopConfig("drawCenterCross"); return false; }
    if ( !config.check("w") ) { stopConfig("w"); return false;}
    if ( !config.check("h") ) { stopConfig("h"); return false;}
    if ( !config.check("fx") ) { stopConfig("fx"); return false;}
    if ( !config.check("fy") ) { stopConfig("fy"); return false;}
    if ( !config.check("cx") ) { stopConfig("cx"); return false;}
    if ( !config.check("cy") ) { stopConfig("cy"); return false;}
    if ( !config.check("k1") ) { stopConfig("k1"); return false;}
    if ( !config.check("k2") ) { stopConfig("k2"); return false;}
    if ( !config.check("p1") ) { stopConfig("p1"); return false;}
    if ( !config.check("p2") ) { stopConfig("p2"); return false;}


    fprintf(stdout,"fx=%g\n",config.find("fx").asDouble());
    fprintf(stdout,"fy=%g\n",config.find("fy").asDouble());
    fprintf(stdout,"cx=%g\n",config.find("cx").asDouble());
    fprintf(stdout,"cy=%g\n",config.find("cy").asDouble());


    // copy to scaled matrix ;)
    CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 0);
    CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 1);
    CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 2);
    CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 0);
    CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 1);
    CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 2);
    CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 0);
    CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 1);
    CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 2);

     /* init the distortion coeffs */
    CV_MAT_ELEM( *_distortion_coeffs, float, 0, 0) = (float)config.check("k1",
                                                        Value(0.0),
                                                        "Radial distortion 1(double)").asDouble();
    CV_MAT_ELEM( *_distortion_coeffs, float, 0, 1) = (float)config.check("k2",
                                                        Value(0.0),
                                                        "Radial distortion 2(double)").asDouble();
    CV_MAT_ELEM( *_distortion_coeffs, float, 0, 2) = (float)config.check("p1",
                                                        Value(0.0),
                                                        "Tangential distortion 1(double)").asDouble();
    CV_MAT_ELEM( *_distortion_coeffs, float, 0, 3) = (float)config.check("p2",
                                                        Value(0.0),
                                                        "Tangential distortion 2(double)").asDouble();
    _needInit = true;

    return true;
}