C++ ResourceFinder::toString方法代码示例

bool crawlGeneratorModule::configure(yarp::os::ResourceFinder &rf)
{
	Property options(rf.toString());
	int period = 25; // in ms

	if(options.check("period"))
	{
		period = options.find("period").asInt();
	}

	if(options.check("part"))
	{
		partName=options.find("part").asString().c_str();
		printf("module taking care of part %s\n",partName.c_str());
	}


	theThread = new GeneratorThread(period);
	if(!theThread->init(rf))
	{
		printf("Failed to initialize the thread\n");
		fflush(stdout);
		return false;
	}

	theThread->start();

	return true;

}

    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        Property options;
        options.fromString(rf.toString());
        char robotName[255];
        Bottle *jointsList=0;
        std::string moduleName = "directPositionControl";
        Time::turboBoost();

        options.put("device", "remote_controlboard");
        if(options.check("robot"))
            strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
        else
            strncpy(robotName, "icub", sizeof(robotName));

        if (options.check("name"))
        {
            moduleName = options.find("name").asString();
        }

        if(options.check("part"))
        {
            sprintf(partName, "%s", options.find("part").asString().c_str());

            char tmp[255];
            sprintf(tmp, "/%s/%s/%s/client", moduleName.c_str(), robotName, partName);
            options.put("local",tmp);
        
            sprintf(tmp, "/%s/%s", robotName, partName);
            options.put("remote", tmp);
        
            sprintf(tmp, "/%s/%s/rpc", moduleName.c_str(), partName);
            rpc_port.open(tmp);
            
            options.put("carrier", "tcp");
            
            attach(rpc_port);
        }
        else
        {
            yError("Please specify part (e.g. --part head)\n");
            return false;
        }

        if(options.check("joints"))
        {
            jointsList = options.find("joints").asList();
            if (jointsList==0) yError("Unable to parts 'joints' parameter\n");
        }
        else
        {
            yError("Please specify the joints to control (e.g. --joints ""(0 1 2)"" ");
            return false;
        }

        //opening the device driver
        if (!driver.open(options))
        {
            yError("Error opening device, check parameters\n");
            return false;
        }

        ///starting the thread
        int period = CONTROL_PERIOD;
        if(options.check("period"))
            period = options.find("period").asInt();
        
        yInfo("control rate is %d ms",period);

        pThread=new positionDirectControlThread(period);
        pThread->init(&driver, moduleName, partName, robotName, jointsList);
        pThread->start();

        return true;
    }

bool TorqueBalancingReferencesGenerator::configure (yarp::os::ResourceFinder &rf)
{
    using namespace yarp::os;
    using namespace yarp::sig;
    using namespace Eigen;

    
    if (!rf.check("wbi_config_file", "Checking wbi configuration file")) {
        std::cout << "No WBI configuration file found.\n";
        return false;
    }
    
    Property wbiProperties;
    if (!wbiProperties.fromConfigFile(rf.findFile("wbi_config_file"))) {
        std::cout << "Not possible to load WBI properties from file.\n";
        return false;
    }
    wbiProperties.fromString(rf.toString(), false);

    //retrieve the joint list
    std::string wbiList = rf.find("wbi_joint_list").asString();

    wbi::IDList iCubMainJoints;
    if (!yarpWbi::loadIdListFromConfig(wbiList, wbiProperties, iCubMainJoints)) {
        std::cout << "Cannot find joint list\n";
        return false;
    }

    size_t actuatedDOFs = iCubMainJoints.size();

    //create an instance of wbi
    m_robot = new yarpWbi::yarpWholeBodyInterface("torqueBalancingReferencesGenerator", wbiProperties);
    if (!m_robot) {
        std::cout << "Could not create wbi object.\n";
        return false;
    }

    m_robot->addJoints(iCubMainJoints);
    if (!m_robot->init()) {
        std::cout << "Could not initialize wbi object.\n";
        return false;
    }
    
    robotName = rf.check("robot", Value("icubSim"), "Looking for robot name").asString();
//     numberOfPostures = rf.check("numberOfPostures", Value(0), "Looking for numberOfPostures").asInt();

    directionOfOscillation.resize(3,0.0);
    frequencyOfOscillation = 0;
    amplitudeOfOscillation = 0;

    if (!loadReferences(rf,postures,comTimeAndSetPoints,actuatedDOFs,changePostural, changeComWithSetPoints,amplitudeOfOscillation,frequencyOfOscillation, directionOfOscillation  ))
    {
        return false;
    }
    loadReferences(rf,postures,comTimeAndSetPoints,actuatedDOFs,changePostural, changeComWithSetPoints,amplitudeOfOscillation,frequencyOfOscillation, directionOfOscillation);
    for(int i=0; i < postures.size(); i++ )
    {
        std::cerr << "[INFO] time_" << i << " = " << postures[i].time << std::endl;
        std::cerr << "[INFO] posture_" << i << " = " << postures[i].qDes.toString()<< std::endl;
    }
    for(int i=0; i < comTimeAndSetPoints.size(); i++ )
    {
        std::cerr << "[INFO] time_" << i << " = " << comTimeAndSetPoints[i].time << std::endl;
        std::cerr << "[INFO] com_" << i << " = " <<  comTimeAndSetPoints[i].comDes.toString()<< std::endl;
    }
//     
    std::cout << "[INFO] Number of DOFs: " << actuatedDOFs << std::endl;
    
    std::cerr << "[INFO] changePostural: " << changePostural << std::endl;
    
    std::cerr << "[INFO] changeComWithSetPoints: " << changeComWithSetPoints << std::endl;
    
    std::cerr << "[INFO] amplitudeOfOscillation: " << amplitudeOfOscillation << std::endl;

    std::cout << "[INFO] frequencyOfOscillation " << frequencyOfOscillation << std::endl;
    
    std::cerr << "[INFO] directionOfOscillation: " << directionOfOscillation.toString() << std::endl;

    q0.resize(actuatedDOFs, 0.0);
    com0.resize(3, 0.0);
    comDes.resize(3, 0.0);
    DcomDes.resize(3, 0.0);
    DDcomDes.resize(3, 0.0);
    m_robot->getEstimates(wbi::ESTIMATE_JOINT_POS, q0.data());
    
    double world2BaseFrameSerialization[16];
    double rotoTranslationVector[7];
    wbi::Frame world2BaseFrame;
    m_robot->getEstimates(wbi::ESTIMATE_BASE_POS, world2BaseFrameSerialization);
    wbi::frameFromSerialization(world2BaseFrameSerialization, world2BaseFrame);
    m_robot->forwardKinematics(q0.data(), world2BaseFrame, wbi::wholeBodyInterface::COM_LINK_ID, rotoTranslationVector);
    com0[0] = rotoTranslationVector[0];    
    com0[1] = rotoTranslationVector[1];
    com0[2] = rotoTranslationVector[2];
    
    timeoutBeforeStreamingRefs = rf.check("timeoutBeforeStreamingRefs", Value(20), "Looking for robot name").asDouble();

    period = rf.check("period", Value(0.01), "Looking for module period").asDouble();
    
    std::cout << "timeoutBeforeStreamingRefs: " << timeoutBeforeStreamingRefs << "\n";
//.........这里部分代码省略.........

bool CamCalibModule::configure(yarp::os::ResourceFinder &rf)
{

    ConstString str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
    setName(str.c_str()); // modulePortName

    double maxDelay = rf.check("maxDelay", Value(0.010), "Max delay between image and encoders").asDouble();

    // pass configuration over to bottle
    Bottle botConfig(rf.toString().c_str());
    botConfig.setMonitor(rf.getMonitor());
    // Load from configuration group ([<group_name>]), if group option present
    Value *valGroup; // check assigns pointer to reference
    if(botConfig.check("group", valGroup, "Configuration group to load module options from (string).")) {
        strGroup = valGroup->asString().c_str();
        // is group a valid bottle?
        if (botConfig.check(strGroup.c_str())){
            Bottle &group=botConfig.findGroup(strGroup.c_str(),string("Loading configuration from group " + strGroup).c_str());
            botConfig.fromString(group.toString());
        } else {
            yError() << "Group " << strGroup << " not found.";
            return false;
        }
    } else {
        yError ("There seem to be an error loading parameters (group section missing), stopping module");
        return false;
    }

    string calibToolName = botConfig.check("projection",
                                           Value("pinhole"),
                                           "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();

    _calibTool = CalibToolFactories::getPool().get(calibToolName.c_str());
    if (_calibTool!=NULL) {
        bool ok = _calibTool->open(botConfig);
        if (!ok) {
            delete _calibTool;
            _calibTool = NULL;
            return false;
        }
    }

    if (yarp::os::Network::exists(getName("/in"))) {
        yWarning() << "port " << getName("/in") << " already in use";
    }
    if (yarp::os::Network::exists(getName("/out"))) {
        yWarning() << "port " << getName("/out") << " already in use";
    }
    if (yarp::os::Network::exists(getName("/conf"))) {
        yWarning() << "port " << getName("/conf") << " already in use";
    }
    _prtImgIn.setSaturation(rf.check("saturation",Value(1.0)).asDouble());
    _prtImgIn.open(getName("/in"));
    _prtImgIn.setPointers(&_prtImgOut,_calibTool);
    _prtImgIn.setVerbose(rf.check("verbose"));
    _prtImgIn.setLeftEye((strGroup == "CAMERA_CALIBRATION_LEFT") ? true : false);
    _prtImgIn.setMaxDelay(maxDelay);
    _prtImgIn.setUseIMU(rf.check("useIMU"));
    _prtImgIn.setUseIMU(rf.check("useTorso"));
    _prtImgIn.setUseIMU(rf.check("useEyes"));
    _prtImgIn.useCallback();
    _prtImgOut.open(getName("/out"));
    _configPort.open(getName("/conf"));

    _prtTEncsIn.open(getName("/torso_encs/in"));
    _prtTEncsIn._prtImgIn = &_prtImgIn;
//    _prtTEncsIn.setStrict();
    _prtTEncsIn.useCallback();

    _prtHEncsIn.open(getName("/head_encs/in"));
    _prtHEncsIn._prtImgIn = &_prtImgIn;
//    _prtHEncsIn.setStrict();
    _prtHEncsIn.useCallback();

    _prtImuIn.open(getName("/imu/in"));
    _prtImuIn._prtImgIn = &_prtImgIn;
//    _prtImuIn.setStrict();
    _prtImuIn.useCallback();

    attach(_configPort);
    fflush(stdout);

    _prtImgIn.rpyPort.open(getName("/rpy"));

    return true;
}

bool CamCalibModule::configure(yarp::os::ResourceFinder &rf)
{
    ConstString str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
    verboseExecTime = rf.check("verboseExecTime");

    if      (rf.check("w_align")) align=ALIGN_WIDTH;
    else if (rf.check("h_align")) align=ALIGN_HEIGHT;

    if (rf.check("fps"))
    {
        requested_fps=rf.find("fps").asDouble();
        yInfo() << "Module will run at " << requested_fps;
    }
    else
    {
        yInfo() << "Module will run at max fps";
    }

    setName(str.c_str()); // modulePortName

    Bottle botLeftConfig(rf.toString().c_str());
    Bottle botRightConfig(rf.toString().c_str());

    botLeftConfig.setMonitor(rf.getMonitor());
    botRightConfig.setMonitor(rf.getMonitor());

    string strLeftGroup = "CAMERA_CALIBRATION_LEFT";
    if (botLeftConfig.check(strLeftGroup.c_str()))
    {            
        Bottle &group=botLeftConfig.findGroup(strLeftGroup.c_str(),string("Loading configuration from group " + strLeftGroup).c_str());
        botLeftConfig.fromString(group.toString());
    }
    else
    {
        yError() << "Group " << strLeftGroup << " not found.";
        return false;
    }
    
    string strRightGroup = "CAMERA_CALIBRATION_RIGHT";
    if (botRightConfig.check(strRightGroup.c_str()))
    {            
        Bottle &group=botRightConfig.findGroup(strRightGroup.c_str(),string("Loading configuration from group " + strRightGroup).c_str());
        botRightConfig.fromString(group.toString());
    }
    else
    {
        yError() << "Group " << strRightGroup << " not found.";
        return false;
    }

    string calibToolLeftName  = botLeftConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
    string calibToolRightName = botRightConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();

    calibToolLeft = CalibToolFactories::getPool().get(calibToolLeftName.c_str());
    if (calibToolLeft!=NULL)
    {
        bool ok = calibToolLeft->open(botLeftConfig);
        if (!ok)
        {
            delete calibToolLeft;
            calibToolLeft = NULL;
            return false;
        }
    }
    calibToolRight = CalibToolFactories::getPool().get(calibToolRightName.c_str());
    if (calibToolRight!=NULL)
    {
        bool ok = calibToolRight->open(botRightConfig);
        if (!ok)
        {
            delete calibToolRight;
            calibToolRight = NULL;
            return false;
        }
    }

    if(rf.check("dual"))
    {
        dualImage_mode = true;
        yInfo() << "Dual mode activate!!";
    }

    if(dualImage_mode)
    {
        leftImage  = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;
        rightImage = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;

        // open a single port with name /dual:i
        if (yarp::os::Network::exists(getName("/dual:i")))
        {
            yWarning() << "port " << getName("/dual:i") << " already in use";
        }
        if(! imageInLeft.open(getName("/dual:i")) )
            return false;
        imageInLeft.setStrict(false);
    }
    else
    {
        if (yarp::os::Network::exists(getName("/left:i")))
        {
//.........这里部分代码省略.........

    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        Property options;
        options.fromString(rf.toString());
        char robotName[255];
        Bottle *jointsList=0;
        std::string moduleName = "torqueObserver";
        Time::turboBoost();

        reset_offset = true;
        for (int i=0; i<6; i++) initial_offset[i]=0.0;

       /* options.put("device", "remote_controlboard");
        if(options.check("robot"))
            strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
        else
            strncpy(robotName, "icub", sizeof(robotName));

        if (options.check("name"))
        {
            moduleName = options.find("name").asString();
        }

        if(options.check("part"))
        {
            sprintf(partName, "%s", options.find("part").asString().c_str());

            sprintf(tmp, "/%s/%s/%s/client", moduleName.c_str(), robotName, partName);
            options.put("local",tmp);
        
            sprintf(tmp, "/%s/%s", robotName, partName);
            options.put("remote", tmp);
        
            sprintf(tmp, "/%s/%s/rpc", moduleName.c_str(), partName);
            rpc_port.open(tmp);
            
            options.put("carrier", "tcp");
            
            attach(rpc_port);
        }
        else
        {
            yError("Please specify part (e.g. --part head)\n");
            return false;
        }

        //opening the device driver
        if (!driver.open(options))
        {
            yError("Error opening device, check parameters\n");
            return false;
        }

        bool ret = true;
        idir = 0;
        icmd = 0;
        ret &= driver.view(idir);
        ret &= driver.view(icmd);
      */
        char pin[255];
        sprintf(pin, "/%s/torque:i", moduleName.c_str());
        inport.open(pin);
        char pout[255];
        sprintf(pout, "/%s/torque:o", moduleName.c_str());
        outport.open(pout);

        bool b1 = yarp::os::Network::connect("/icub/left_leg/analog:o",pin);
        bool b2 = yarp::os::Network::connect(pout,"/icub/left_leg/analog:o");
        if (!b1)
        {
            yWarning("failed input connection");
        }
        if (!b2)
        {
            yWarning("failed ouput connection");
        }
        return true;
    }

    //CTmodified: virtual bool open(yarp::os::Searchable &s)
	virtual bool configure(yarp::os::ResourceFinder &rf) 
    {
        //CTmodified: Property options(s.toString());
		Property options(rf.toString());
        Property stiffnessOptions;

        char robotName[255];
        Time::turboBoost();    
        options.put("device", "remote_controlboard");
        if(options.check("robot"))
            strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
        else
            strncpy(robotName, "icub", sizeof(robotName));
	
        if(options.check("part"))
            {
                char tmp[255];
                sprintf(tmp, "/%s/vc/%s/client",
                        robotName,
                        options.find("part").asString().c_str());
                options.put("local",tmp);
	    
                sprintf(tmp, "/%s/%s", 
                        robotName,
                        options.find("part").asString().c_str());
                options.put("remote", tmp);
	    
                sprintf(tmp,"/%s/vc/%s/input",
                        robotName,
                        options.find("part").asString().c_str());
                input_port.open(tmp);
            
                options.put("carrier", "mcast");
            
                //CTmodified: attach(input_port,true); Eliminated: Bufferedport is attaching in the constructor??				
            }
        else
            {
                fprintf(stderr, "Please specify part (e.g. --part head)\n");
                return false;
            }
            
            
        ////end of the modif////////////
    
        if (!driver.open(options))
            {
                fprintf(stderr, "Error opening device, check parameters\n");
                return false;
            }

        ///we start the thread
        int period = CONTROL_RATE;
        if(options.check("period"))
            period = options.find("period").asInt();
        
        printf("control rate is %d ms",period);

        if (!options.check("part"))
            return false;
        
        sprintf(partName, "%s", options.find("part").asString().c_str());

        vc=new velImpControlThread(period);
        
		if(options.check("file"))
		{
			stiffnessOptions.fromConfigFile(options.find("file").asString().c_str());
			
		}
		else
		{
			const char *cubPath;
			cubPath = yarp::os::getenv("ICUB_ROOT"); //previously set to ICUB_DIR
			if(cubPath == NULL) 
			{
				printf("velImpControl::init>> ERROR getting the environment variable ICUB_DIR, exiting\n");
				return false;
			}
			string cubPathStr(cubPath);
			//stiffnessOptions.fromConfigFile((cubPathStr + "/app/crawlingApplication/conf/" + partName + "StiffnessConfig.ini").c_str());
			stiffnessOptions.fromConfigFile((cubPathStr + "/contrib/src/crawlingTest/app/conf/" + partName + "StiffnessConfig.ini").c_str());
			
		}

		if(stiffnessOptions.check("njoints"))
		{
			vc->njoints = stiffnessOptions.find("njoints").asInt();
			printf("\nControlling %d dofs\n", vc->njoints);
		}
		else
		{
			printf("Please specify the number of joints in the config file");
			return false;
		}
		
		vc->impContr.resize(vc->njoints);
		vc->swingStiff.resize(vc->njoints);
		vc->stanceStiff.resize(vc->njoints);
//.........这里部分代码省略.........