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

void proactiveTagging::configureOPC(yarp::os::ResourceFinder &rf)
{
    //Populate the OPC if required
    yDebug() << "Populating OPC...";

    //1. Populate AddOrRetrieve part
    Bottle grpOPC_AOR = rf.findGroup("OPC_AddOrRetrieve");
    bool shouldPopulate_AOR = grpOPC_AOR.find("populateOPC").asInt() == 1;
    if (shouldPopulate_AOR)
    {
        Bottle *objectList = grpOPC_AOR.find("objectName").asList();
        subPopulateObjects(objectList, true);

        Bottle *bodyPartList = grpOPC_AOR.find("bodypartName").asList();
        Bottle *bodyPartJointList = grpOPC_AOR.find("bodypartJoint").asList();
        subPopulateBodyparts(bodyPartList, bodyPartJointList, true);
    }

    //2. Populate Add part (allows several object with same base name, e.g. object, object_1, object_2, ..., object_n)
    Bottle grpOPC_Add = rf.findGroup("OPC_Add");
    bool shouldPopulate_Add = grpOPC_Add.find("populateOPC").asInt() == 1;
    if (shouldPopulate_Add)
    {
        Bottle *objectList = grpOPC_Add.find("objectName").asList();
        subPopulateObjects(objectList, false);

        Bottle *bodyPartList = grpOPC_Add.find("bodypartName").asList();
        Bottle *bodyPartJointList = grpOPC_Add.find("bodypartJoint").asList();
        subPopulateBodyparts(bodyPartList, bodyPartJointList, false);
    }

    yDebug() << "configureOPC done";
}

bool ModelOTL::configure(yarp::os::ResourceFinder &rf) {
    bool bEveryThingisGood = true;

    moduleName = rf.check("name",Value("modelOTL"),"module name (string)").asString();

    part = rf.check("part",Value("left_arm")).asString();

    Bottle &babbl_par = rf.findGroup("babbling_param");
    train_duration = babbl_par.check("train_duration", Value(20.0)).asDouble();
    test_duration = babbl_par.check("test_duration", Value(10.0)).asDouble();

    Bottle &oesgp_par = rf.findGroup("oesgp_learner");
    p_input_dim = oesgp_par.check("input_dim", Value(2)).asInt();
    p_output_dim = oesgp_par.check("output_dim", Value(1)).asInt();
    p_reservoir_size = oesgp_par.check("reservoir_size", Value(100)).asDouble();
    p_input_weight = oesgp_par.check("input_weight", Value(1.0)).asDouble();
    p_output_feedback_weight = oesgp_par.check("output_feedback_weight", Value(0.0)).asDouble();
    p_leak_rate = oesgp_par.check("leak_rate", Value(0.9)).asDouble();
    p_connectivity = oesgp_par.check("connectivity", Value(0.1)).asDouble();
    p_spectral_radius = oesgp_par.check("spectral_radius", Value(0.90)).asDouble();
    p_use_inputs_in_state = oesgp_par.check("use_inputs_in_state", Value(1)).asInt();
    p_noise = oesgp_par.check("noise", Value(0.000001)).asDouble();
    p_epsilon = oesgp_par.check("epsilon", Value(1e-3)).asDouble();
    p_capacity = oesgp_par.check("capacity", Value(10)).asInt();
    p_random_seed = oesgp_par.check("random_seed", Value(0)).asInt();

    setName(moduleName.c_str());

    // Open handler port
    if (!handlerPort.open("/" + getName() + "/rpc")) {
        cout << getName() << ": Unable to open port " << "/" << getName() << "/rpc" << endl;
        bEveryThingisGood = false;
    }

    if (!portVelocityOut.open("/" + getName() + "/" + part + "/velocity:o")) {
        cout << getName() << ": Unable to open port " << "/" << getName() << "/" << part << "/velocity:o" << endl;
        bEveryThingisGood = false;
    }

    if (!portPredictionErrors.open("/" + getName() + "/portPredictionErrors")) {
        cout << getName() << ": Unable to open port " << "/" + getName() + "/portPredictionErrors" << endl;
        bEveryThingisGood = false;
    }

    if (!portInputsfromBabbling.open("/" + getName() + "/portInputs")) {
        cout << getName() << ": Unable to open port " << "/" + getName() + "/portInputs" << endl;
        bEveryThingisGood = false;
    }

    if (!portOutputsfromSensoryProc.open("/" + getName() + "/portOutputsfromSensoryProc")) {
        cout << getName() << ": Unable to open port " << "/" + getName() + "/portOutputsfromSensoryProc" << endl;
        bEveryThingisGood = false;
    }


    attach(handlerPort);

    return bEveryThingisGood;
}

bool homeostaticModule::configure(yarp::os::ResourceFinder &rf)
{
    manager = homeostasisManager();
    
    moduleName = rf.check("name",Value("homeostasis")).asString().c_str();
    setName(moduleName.c_str());

    cout<<moduleName<<": finding configuration files..."<<endl;
    period = rf.check("period",Value(0.1)).asDouble();

    cout << "Initializing drives";
    Bottle grpHomeostatic = rf.findGroup("HOMEOSTATIC");
    Bottle *drivesList = grpHomeostatic.find("drives").asList();
    cout << "Initializing Drives... " << endl;
    if (drivesList)
    {
        cout << "Configuration: Found " << drivesList->size() << " drives. " << endl;
        for (int d = 0; d<drivesList->size(); d++)
        {
            cout << d << endl;
            //Read Drive Configuration
            string driveName = drivesList->get(d).asString().c_str();
            addNewDrive(driveName, grpHomeostatic);
            
        }
    }
    cout << "Opening RPC..."<< endl;
     
    rpc.open ( ("/"+moduleName+"/rpc").c_str());
    attach(rpc);

	
	cout<<"Configuration done."<<endl;
    return true;
}

void ReactiveLayer::configureTactile(yarp::os::ResourceFinder &rf)
{
    //Initialise the tactile response
    Bottle grpTactile = rf.findGroup("TACTILE");
    Bottle *tactileStimulus = grpTactile.find("stimuli").asList();

    if (tactileStimulus)
    {
        for (int d = 0; d<tactileStimulus->size(); d++)
        {
            string tactileStimulusName = tactileStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpTactile.find((tactileStimulusName + "-sentence").c_str()).asList();
            for (int s = 0; s<bSentences->size(); s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            //choregraphies
            Bottle *bChore = grpTactile.find((tactileStimulusName + "-chore").c_str()).asList();
            for (int sC = 0; bChore && sC<bChore->size(); sC++)
            {
                response.m_choregraphies.push_back(bChore->get(sC).asString().c_str());
            }
            std::string sGroupTemp = tactileStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpTactile.find(sGroupTemp.c_str()).asList();
            for (int i = 0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble();
            }
            tactileEffects[tactileStimulusName] = response;
        }
    }
}

void ICubClient::LoadPostures(yarp::os::ResourceFinder &rf)
{
    posturesKnown.clear();

    int posCount = rf.check("posturesCount", yarp::os::Value(0)).asInt();
    //cout<<"Loading posture: "<<endl;
    for (int i = 0; i < posCount; i++)
    {
        std::stringstream ss;
        ss<<"posture_" << i;
        Bottle postureGroup = rf.findGroup(ss.str().c_str());
        BodyPosture p;
        std::string name = postureGroup.find("name").asString().c_str();
        //std::cout<<"\t"<<name<<std::endl;
        Bottle* bHead = postureGroup.find("head").asList();
        Bottle* bLArm = postureGroup.find("left_arm").asList();
        Bottle* bRArm = postureGroup.find("right_arm").asList();
        Bottle* bTorso = postureGroup.find("torso").asList();

        p.head.resize(6);
        for(int i=0;i<6;i++)
            p.head[i] = bHead->get(i).asDouble();
        p.left_arm.resize(16);
        for(int i=0;i<16;i++)
            p.left_arm[i] = bLArm->get(i).asDouble();        
        p.right_arm.resize(16);
        for(int i=0;i<16;i++)
            p.right_arm[i] = bRArm->get(i).asDouble();
        p.torso.resize(3);
        for(int i=0;i<3;i++)
            p.torso[i] = bTorso->get(i).asDouble();

        posturesKnown[name] = p;
    }
}

void ICubClient::LoadChoregraphies(yarp::os::ResourceFinder &rf)
{
    choregraphiesKnown.clear();

    int posCount = rf.check("choregraphiesCount", yarp::os::Value(0)).asInt();
    cout<<"Loading Choregraphies: "<<endl;
    for (int i = 0; i < posCount; i++)
    {
        std::stringstream ss;
        ss<<"chore_" << i;
        Bottle postureGroup = rf.findGroup(ss.str().c_str());

        std::string name = postureGroup.find("name").asString().c_str();
        std::cout<<"\t"<<name<<std::endl;
        Bottle* sequence = postureGroup.find("sequence").asList();

        std::list< std::pair<std::string, double> > seq;
        for(int s=0; s<sequence->size(); s++)
        {
            Bottle* element = sequence->get(s).asList();
            std::string elementName = element->get(0).asString().c_str();
            double elementTime = element->get(1).asDouble();
            seq.push_back(std::pair<std::string,double>(elementName,elementTime));
            //std::cout<<"\t \t"<<elementName<< "\t" << elementTime << std::endl;
        }
        choregraphiesKnown[name] = seq;
    }
}

void ReactiveLayer::configureSalutation(yarp::os::ResourceFinder &rf)
{
    ;
    //Initialise the gestures response
    Bottle grpSocial = rf.findGroup("SOCIAL");
    salutationLifetime = grpSocial.check("salutationLifetime", Value(15.0)).asDouble();
    Bottle *socialStimulus = grpSocial.find("stimuli").asList();

    if (socialStimulus)
    {
        for (int d = 0; d<socialStimulus->size(); d++)
        {
            string socialStimulusName = socialStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpSocial.find((socialStimulusName + "-sentence").c_str()).asList();
            for (int s = 0; s<bSentences->size(); s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            std::string sGroupTemp = socialStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpSocial.find(sGroupTemp.c_str()).asList();
            for (int i = 0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble();
            }
            salutationEffects[socialStimulusName] = response;
        }
    }

    //Add the relevant Entities for handling salutation
    iCub->opc->addOrRetrieveAction("is");
    iCub->opc->addOrRetrieveAdjective("saluted");
}

void AdaptiveLayer::configureGestures(yarp::os::ResourceFinder &rf)
{
    //Initialise the gestures response
    Bottle grpGesture = rf.findGroup("GESTURES");
    Bottle *gestureStimulus = grpGesture.find("stimuli").asList();

    if (gestureStimulus)
    {
        for(int d=0; d<gestureStimulus->size(); d++)
        {
            string gestureStimulusName = gestureStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpGesture.find((gestureStimulusName + "-sentence").c_str()).asList();
            for(int s=0;s<bSentences->size();s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            std::string sGroupTemp = gestureStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpGesture.find( sGroupTemp.c_str()).asList();
            for(int i=0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i+1).asDouble();
            }
            gestureEffects[gestureStimulusName] = response;
        }
    }
}

bool QuaternionEKF::readEstimatorParams(yarp::os::ResourceFinder &rf, quaternionEKFParams &estimatorParams)
{
    yarp::os::Bottle botParams;
    botParams = rf.findGroup("QuaternionEKF");
    yInfo("QuaternionEKF params are: %s ", botParams.toString().c_str());
    if ( botParams.isNull() )
    {
        yError("[QuaternionEKF::readEstimatorParams] No parameters were read from QuaternionEKF group");
        return false;
    } else {
        estimatorParams.stateSize = botParams.find("state_size").asInt();
        estimatorParams.inputSize = botParams.find("input_size").asInt();
        estimatorParams.measurementSize = botParams.find("measurement_size").asInt();
        estimatorParams.muSystemNoise = botParams.find("mu_system_noise").asDouble();
        estimatorParams.sigmaSystemNoise = botParams.find("sigma_system_noise").asDouble();
        estimatorParams.sigmaMeasurementNoise = botParams.find("sigma_measurement_noise").asDouble();
        estimatorParams.sigmaGyro = botParams.find("sigma_gyro_noise").asDouble();
        estimatorParams.piorMu = botParams.find("prior_mu_state").asDouble();
        estimatorParams.priorCovariance = botParams.find("prior_cov_state").asDouble();
        estimatorParams.muGyroNoise = botParams.find("mu_gyro_noise").asDouble();
        estimatorParams.floatingBaseAttitude = botParams.find("floating_base_attitude").asBool();
        estimatorParams.rot_from_ft_to_acc_bottle = new yarp::os::Bottle(*botParams.find("rot_from_ft_to_acc").asList());
    }

    botParams.clear();
    botParams = rf.findGroup("module_parameters");
    if ( botParams.isNull() )
    {
        yError("[QuaternionEKF::readEstimatorParams] No parameters were read from 'module_params' group. period is needed!");
        return false;
    } else {
        estimatorParams.period = botParams.find("period").asInt();
        estimatorParams.robotPrefix = botParams.find("robot").asString();
        estimatorParams.streamMeasurements = botParams.find("stream_measurements").asBool();
    }
    return true;
}

    /**
    * Performs module configuration, parsing user options stored in the resource finder.
    * Available options are described in main module documentation.
    * @return true if the module was successfully configured and opened, false otherwise.
    */
    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        yarp::os::Time::turboBoost();

        m_rpcPort.open("/"+m_module_name+"/rpc");
        attach(m_rpcPort);
        //attachTerminal();
        m_rf = rf;
        
        //configuration file checking
        Bottle general_group = m_rf.findGroup("GENERAL");
        if (general_group.isNull())
        {
            yError() << "Missing GENERAL group!";
            return false;
        }

        Bottle initial_group = m_rf.findGroup("INITIAL_POS");
        if (initial_group.isNull())
        {
            yError() << "Missing INITIAL_POS group!";
            return false;
        }

        Bottle localization_group = m_rf.findGroup("LOCALIZATION");
        if (localization_group.isNull())
        {
            yError() << "Missing LOCALIZATION group!";
            return false;
        }

        Bottle ros_group = m_rf.findGroup("ROS");

        Bottle tf_group = m_rf.findGroup("TF");
        if (tf_group.isNull())
        {
            yError() << "Missing TF group!";
            return false;
        }

        Bottle odometry_group = m_rf.findGroup("ODOMETRY");
        if (odometry_group.isNull())
        {
            yError() << "Missing ODOMETRY group!";
            return false;
        }

        Bottle map_group = m_rf.findGroup("MAP");
        if (map_group.isNull())
        {
            yError() << "Missing MAP group!";
            return false;
        }
        yDebug() << map_group.toString();

        //general group
        if (general_group.check("module_name") == false)
        {
            yError() << "Missing `module_name` in [GENERAL] group";
            return false;
        }
        m_module_name = general_group.find("module_name").asString();

        if (general_group.check("enable_ros") == false)
        {
            yError() << "Missing `ros_enable` in [GENERAL] group";
            return false;
        }
        m_ros_enabled = (general_group.find("enable_ros").asInt()==1);

        //ROS group
        if (m_ros_enabled)
        {
            m_rosNode = new yarp::os::Node("/"+m_module_name);
            
            //initialize an occupancy grid publisher (every time the localization is re-initialized, the map is published too)
            if (ros_group.check ("occupancygrid_topic"))
            {
                m_topic_occupancyGrid = ros_group.find ("occupancygrid_topic").asString();
                if (!m_rosPublisher_occupancyGrid.topic(m_topic_occupancyGrid))
                {
                     if (m_rosNode)
                     {
                         delete m_rosNode;
                         m_rosNode=0;
                     }
                     yError() << "localizationModule: unable to publish data on " << m_topic_occupancyGrid << " topic, check your yarp-ROS network configuration";
                     return false;
                }
            }

             //initialize an initial pose publisher
            if (ros_group.check ("initialpose_topic")) 
            {
                m_topic_initial_pose = ros_group.find ("initialpose_topic").asString();
//.........这里部分代码省略.........

bool BehaviorManager::configure(yarp::os::ResourceFinder &rf)
{
    moduleName = rf.check("name",Value("BehaviorManager")).asString();
    setName(moduleName.c_str());
    yInfo()<<moduleName<<": finding configuration files...";//<<endl;
    period = rf.check("period",Value(1.0)).asDouble();

    Bottle grp = rf.findGroup("BEHAVIORS");
    Bottle behaviorList = *grp.find("behaviors").asList();

    rpc_in_port.open("/" + moduleName + "/trigger:i");
    yInfo() << "RPC_IN : " << rpc_in_port.getName();

    for (int i = 0; i<behaviorList.size(); i++)
    {
        behavior_name = behaviorList.get(i).asString();
        if (behavior_name == "tagging") {
            behaviors.push_back(new Tagging(&mut, rf, "tagging"));
        } else if (behavior_name == "pointing") {
            behaviors.push_back(new Pointing(&mut, rf, "pointing"));
        } else if (behavior_name == "dummy") {
            behaviors.push_back(new Dummy(&mut, rf, "dummy"));
        } else if (behavior_name == "dummy2") {
            behaviors.push_back(new Dummy(&mut, rf, "dummy2"));
        }  else if (behavior_name == "reactions") {
            behaviors.push_back(new Reactions(&mut, rf, "reactions"));
        }  else if (behavior_name == "followingOrder") {
            behaviors.push_back(new FollowingOrder(&mut, rf, "followingOrder"));
        }  else if (behavior_name == "narrate") {
            behaviors.push_back(new Narrate(&mut, rf, "narrate"));
        }  else if (behavior_name == "recognitionOrder") {
            behaviors.push_back(new RecognitionOrder(&mut, rf, "recognitionOrder"));
        }  else if (behavior_name == "greeting") {
            behaviors.push_back(new Greeting(&mut, rf, "greeting"));
        }  else if (behavior_name == "ask") {
            behaviors.push_back(new Ask(&mut, rf, "ask"));
        }  else if (behavior_name == "speech") {
            behaviors.push_back(new Speech(&mut, rf, "speech"));
        } else if (behavior_name == "moveObject") {
            behaviors.push_back(new MoveObject(&mut, rf, "moveObject"));
            // other behaviors here
        }  else {
            yDebug() << "Behavior " + behavior_name + " not implemented";
            return false;
        }
    }

    //Create an iCub Client and check that all dependencies are here before starting
    bool isRFVerbose = false;
    iCub = new wysiwyd::wrdac::ICubClient(moduleName, "behaviorManager","client.ini",isRFVerbose);

    if (!iCub->connect())
    {
        yInfo() << "iCubClient : Some dependencies are not running...";
        Time::delay(1.0);
    }
    if (rf.check("use_ears",Value("false")).asBool())
    {
        yDebug()<<"using ears";
        while (!Network::connect("/ears/behavior:o", rpc_in_port.getName())) {
            yWarning() << "Ears is not reachable";
            yarp::os::Time::delay(0.5);
        }
    }else{
        yDebug()<<"not using ears";
    }

    // id = 0;
    for(auto& beh : behaviors) {
        beh->configure();
        beh->openPorts(moduleName);
        beh->iCub = iCub;

        if (beh->from_sensation_port_name != "None") {
            while (!Network::connect(beh->from_sensation_port_name, beh->sensation_port_in.getName())) {
                yInfo()<<"Connecting "<< beh->from_sensation_port_name << " to " <<  beh->sensation_port_in.getName();// <<endl;
                yarp::os::Time::delay(0.5);
            }
        }
        if (beh->external_port_name != "None") {
            while (!Network::connect(beh->rpc_out_port.getName(), beh->external_port_name)) {
                yInfo()<<"Connecting "<< beh->rpc_out_port.getName() << " to " <<  beh->external_port_name;// <<endl;
                yarp::os::Time::delay(0.5);
            }   
        }

    }

    attach(rpc_in_port);
    yInfo("Init done");

    return true;
}

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

    Bottle table;
    Bottle temp;
    string objectName = "obj";

    /* module name */
    moduleName = rf.check("name", Value("simtoolloader"),
                          "Module name (string)").asString();

    setName(moduleName.c_str());

    /* Hand used */
    hand=rf.find("hand").asString().c_str();
    if ((hand!="left") && (hand!="right"))
        hand="right";

    /* port names */
    simToolLoaderSimOutputPortName  = "/";
    simToolLoaderSimOutputPortName += getName( rf.check("simToolLoaderSimOutputPort",
                                     Value("/sim:rpc"),
                                     "Loader output port(string)")
                                     .asString() );

    simToolLoaderCmdInputPortName  = "/";
    simToolLoaderCmdInputPortName += getName( rf.check("simToolLoaderCmdInputPort",
                                     Value("/cmd:i"),
                                     "Loader input port(string)")
                                     .asString() );

    /* open ports */
    if (!simToolLoaderSimOutputPort.open(
            simToolLoaderSimOutputPortName.c_str())) {

        cout << getName() << ": unable to open port"
        << simToolLoaderSimOutputPortName << endl;
        return false;
    }

    if (!simToolLoaderCmdInputPort.open(
            simToolLoaderCmdInputPortName.c_str())) {

        cout << getName() << ": unable to open port"
        << simToolLoaderCmdInputPortName << endl;
        return false;
    }

    /* Rate thread period */
    threadPeriod = rf.check("threadPeriod", Value(0.5),
        "Control rate thread period key value(double) in seconds ").asDouble();

    /* Read Table Configuration */
    table = rf.findGroup("table");

    /* Read the Objects configurations */
    vector<Bottle> object;

    cout << "Loading objects to buffer" << endl;
    bool noMoreModels = false;
    int n =0;
    while (!noMoreModels){      // read until there are no more objects.
        stringstream s;
        s.str("");
        s << objectName << n;
        string objNameNum = s.str();
        temp = rf.findGroup("objects").findGroup(objNameNum);

        if(!temp.isNull()) {
            cout << "object" << n << ", id: " << objNameNum;
            cout << ", model: " << temp.get(2).asString() << endl;
            object.push_back(temp);
            temp.clear();
            n++;
        }else {
            noMoreModels = true;
        }
    }
    numberObjs = n;

    cout << "Loaded " << object.size() << " objects " << endl;

    /* Create the control rate thread */
    ctrlThread = new CtrlThread(&simToolLoaderSimOutputPort,
                                &simToolLoaderCmdInputPort,
                                threadPeriod,
                                table, object,hand);
    /* Starts the thread */
    if (!ctrlThread->start()) {
        delete ctrlThread;
        return false;
    }

    return true;
}

void AllostaticController::configureAllostatic(yarp::os::ResourceFinder &rf)
{
    //The homeostatic module should be running in parallel, independent from this, so the objective of
    //this config would be to have a proper list  and connect to each port

    homeo_name = "homeostasis";
    string homeo_rpc_name = "/" + homeo_name + "/rpc";
    string to_h_rpc_name="/"+moduleName+"/toHomeoRPC:o";
    to_homeo_rpc.open(to_h_rpc_name);

    while(!Network::connect(to_h_rpc_name,homeo_rpc_name))
    {
        yDebug()<<"Trying to connect to homeostasis...";
        yDebug() << "from " << to_h_rpc_name << " to " << homeo_rpc_name;
        yarp::os::Time::delay(0.2);
    }

    yInfo() << "Initializing drives...";//<<endl;
    Bottle grpAllostatic = rf.findGroup("ALLOSTATIC");
    drivesList = *grpAllostatic.find("drives").asList();
    
    Bottle cmd;

    priority_sum = 0.;
    double priority;
    for (int d = 0; d<drivesList.size(); d++)
    {
        cmd.clear();
        string driveName = drivesList.get(d).asString();
        yInfo() << ("Initializing drive " + driveName);

        cmd.addString("add");
        cmd.addString("conf");
        Bottle drv;
        drv.clear();
        Bottle aux;
        aux.clear();
        aux.addString("name");
        aux.addString(driveName);
        drv.addList()=aux;
        aux.clear();
        drv.addList()=grpAllostatic;
        cmd.addList()=drv;
        Bottle rply;
        rply.clear();
        rply.get(0).asString();
        to_homeo_rpc.write(cmd,rply);

        AllostaticDrive alloDrive;
        alloDrive.name = driveName;

        Value cmds = grpAllostatic.check((driveName + "-sensation-on"), Value("None"));
        alloDrive.sensationOnCmd = *cmds.asList();
        cmds = grpAllostatic.check((driveName + "-sensation-off"), Value("None"));
        alloDrive.sensationOffCmd = *cmds.asList();

        cmds = grpAllostatic.check((driveName + "-before-trigger"), Value("None"));
        if (!(cmds.isString() && cmds.asString() == "None")) {
            alloDrive.beforeTriggerCmd = *cmds.asList();
        }

        cmds = grpAllostatic.check((driveName + "-after-trigger"), Value("None"));
        if (!(cmds.isString() && cmds.asString() == "None")) {
            alloDrive.afterTriggerCmd = *cmds.asList();
        }        

        alloDrive.homeoPort = &to_homeo_rpc;

        alloDrive.inputSensationPort = new BufferedPort<Bottle>;
        string portName = "/" + moduleName + "/" + driveName + "/sensation:i";
        alloDrive.inputSensationPort->open(portName);

        openPorts(driveName);

        string sensationPort = grpAllostatic.check((driveName + "-sensation-port"), Value("None")).asString();
        string pn = "/" + moduleName + "/" + driveName + "/sensation:i";
        while(!Network::connect(sensationPort, pn)) {
            yDebug()<<"Connecting " << sensationPort << " to " << pn;
            yarp::os::Time::delay(0.5);
        }


        // set drive priorities. Default to 1.
        priority = grpAllostatic.check((driveName + "-priority"), Value(1.)).asDouble();
        priority_sum += priority;
        drivePriorities.push_back(priority);

        //Under effects
        string under_port_name = grpAllostatic.check((driveName + "-under-behavior-port"), Value("None")).asString();
        string under_cmd_name = grpAllostatic.check((driveName + "-under-behavior"), Value("None")).asString();

        bool active = false;
        
        if (under_port_name != "None" && under_cmd_name != "None")
        {
            active = true;
            string out_port_name = "/" + moduleName + "/" + driveName + "/under_action:o";
            alloDrive.behaviorUnderPort = new Port();
            alloDrive.behaviorUnderPort->open(out_port_name);
            yDebug() << "trying to connect to" << under_port_name;
//.........这里部分代码省略.........

    bool stereoCalibModule::configure(yarp::os::ResourceFinder &rf)
    {
        Bottle ports_root = rf.findGroup("ports_root");
        moduleName = ports_root.find("name").asString();
        setName(moduleName.c_str());

        //Port names
        Bottle input_port_names = rf.findGroup("input_port_names");
        leftImagePortName = "/" + moduleName + (String) input_port_names.find("left_image_port").asString();
        rightImagePortName = "/" + moduleName + (String) input_port_names.find("right_image_port").asString();
        encodersPortName = "/" + moduleName + (String) input_port_names.find("encoders_port").asString();
        rpcPointRequestPortName = "/" + moduleName + (String) input_port_names.find("rpc_point_request_port").asString();

        Bottle output_port_names = rf.findGroup("output_port_names");
        leftImageOutPortName = "/" + moduleName + (String) output_port_names.find("left_image_out_port").asString();
        rightImageOutPortName = "/" + moduleName + (String) output_port_names.find("right_image_out_port").asString();
        disparityImageOutPortName = "/" + moduleName + (String) output_port_names.find("disparity_image_out_port").asString();

        // open ports
        if (!leftImagePort.open(
                leftImagePortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << leftImagePortName << endl;
            return false;
        }
        _stereoCalibThread_data.leftImagePort = &leftImagePort;


        if (!rightImagePort.open(
                rightImagePortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << rightImagePortName << endl;
            return false;
        }
        _stereoCalibThread_data.rightImagePort= &rightImagePort;


        if (!encodersPort.open(
                encodersPortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << encodersPortName << endl;
            return false;
        }
        _stereoCalibThread_data.encodersPort = &encodersPort;

        if (!rpcPointRequestPort.open(
                rpcPointRequestPortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << rpcPointRequestPortName << endl;
            return false;
        }
        _worldPointThread_data.rpcPointRequestPort = &rpcPointRequestPort;

        if (!leftImageOutPort.open(
                leftImageOutPortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << leftImageOutPortName << endl;
            return false;
        }
        _disparityThread_data.leftImageOutPort = &leftImageOutPort;

        if (!rightImageOutPort.open(
                rightImageOutPortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << rightImageOutPortName << endl;
            return false;
        }
        _disparityThread_data.rightImageOutPort = &rightImageOutPort;//*/

        if (!disparityImageOutPort.open(
                disparityImageOutPortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << disparityImageOutPortName << endl;
            return false;
        }
        _disparityThread_data.disparityImageOutPort = &disparityImageOutPort;//*/

        // Rate threads period
        _stereoCalibThread_data.threadPeriod = rf.check("threadPeriod", Value(0.001),
            "Control rate thread period key value(double) in seconds ").asDouble();
        _disparityThread_data.threadPeriod = _stereoCalibThread_data.threadPeriod;
        _worldPointThread_data.threadPeriod = _stereoCalibThread_data.threadPeriod;

        //Calibration parameters from left camera
        Bottle left_camera_intrinsics = rf.findGroup("left_camera_intrinsics");
        _stereoCalibThread_data._imagesBase_initial_parameters.left_resx = left_camera_intrinsics.find("resx").asInt();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_resy = left_camera_intrinsics.find("resy").asInt();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_cx = left_camera_intrinsics.find("cx").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_cy = left_camera_intrinsics.find("cy").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_fx = left_camera_intrinsics.find("fx").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_fy = left_camera_intrinsics.find("fy").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_k1 = left_camera_intrinsics.find("k1").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_k2 = left_camera_intrinsics.find("k2").asDouble();
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for debugAcc was found. ");
        return false;
    }
    
    if (rf.check("using2acc")) {
        using2acc = rf.find("using2acc").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for using2acc was found.");
        return false;
    }
    
    if ( rf.check("calib") ) {
        calib = rf.find("calib").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for calib was found.");
        return false;
    }
    
    // ------------ IMU PORT ---------------------------------------
    /*TODO This should be configurable! The number of input ports
     depending on the amount of sensor readings.*/
    std::string tmpOffline = "offline";
    std::string tmpOnline  = "online";
    //  If the estimate is done online
    if (!calib) {
        if (!tmpOnline.compare(mode)) {
            yInfo(" [quaternionEKFModule::configure] Online estimation will be performed");
            std::string gyroMeasPortName = "/";
            gyroMeasPortName += local;
            gyroMeasPortName += "/imu:i";
            if (!gyroMeasPort.open(gyroMeasPortName.c_str())) {
                yError("[quaternionEKFModule::configure] Could not open gyroMeasPort");
                return false;
            }
            // If using two accelerometers, open another port for the second reading
            if (using2acc) {
                std::string gyroMeasPortName2 = "/"; gyroMeasPortName2 += local; gyroMeasPortName2 += "/imu2:i";
                if (!gyroMeasPort2.open(gyroMeasPortName2.c_str())) {
                    yError("[quaternionEKFModule::configure] Coult not open gyroMeasPort2");
                    return false;
                }
            }

            // Obtaining filter parameters from configuration file
            yarp::os::Property filterParams;

            // If using Direct method with atan2
            if(!usingEKF)
            {
                if( !rf.check(DIRECT_GROUP_PARAMS_NAME) )  {
                    yError("[quaternionEKFModule::configure] Could not load DIRECT-FILTER-PARAMS group from config file");
                    return false;
                } else   {
                    filterParams.fromString(rf.findGroup(DIRECT_GROUP_PARAMS_NAME).tail().toString());
                    yInfo(" [quaternionEKFModule::configure] Filter parameters are: %s ", filterParams.toString().c_str());
                }
            }
            else
            {
                if( !rf.check(FILTER_GROUP_PARAMS_NAME) )  {
                    yError("[quaternionEKFModule::configure] Could not load EKF-PARAMS group from config file");
                    return false;
                } else   {
                    filterParams.fromString(rf.findGroup(FILTER_GROUP_PARAMS_NAME).tail().toString());
                    yInfo(" [quaternionEKFModule::configure] Filter parameters are: %s ", filterParams.toString().c_str());
                }
            }
            // ----------- THREAD INSTANTIATION AND CALLING -----------------
            quatEKFThread = new quaternionEKFThread(period, local, robotName, autoconnect, usingxsens, usingEKF, inWorldRefFrame, gravityVec, usingSkin, sensorPortName, debugGyro, debugAcc, verbose, filterParams, &gyroMeasPort, &gyroMeasPort2);
            if (!quatEKFThread->start()) {
                yError("Error starting quaternionEKFThread!");
                return false;
            }
            
            yInfo(" [quaternionEKFModule::configure] quaternionEKFThread started");
            
        } else {
            // If the estimate is done offline, read from file with a datadumper format and don't create the thread.
            if(!tmpOffline.compare(mode)) {
                yInfo(" [quaternionEKFModule::configure] Offline batch estimation will be performed");
                
                // **** Initialization
                // Create dataDumper parser
                m_parser = new dataDumperParser(DATAFILE);
                m_parser->parseFile();
                m_parser->countLines();
                
                // Change period of the module thread
                
            } else {
                yError("[quaternionEKFModule::configure] An invalid option was passed to 'mode'. Available options are 'offline' or 'online'.");
                return false;
            }
        }
    } else { 
        std::cout << "Calibrating only... Done bby updateModule()" << std::endl;
    }
  return true;
}