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

bool embObjMultiEnc::fromConfig(yarp::os::Searchable &_config)
{
    yDebug()<< "configurazione: ";;
    yDebug() << _config.toString();

    Bottle general = _config.findGroup("JOINTS");
    if(general.isNull())
    {
        yError() << "embObjMultiEnc cannot find general group";
        return false;
    }
    
    Bottle jointsbottle = general.findGroup("listofjoints");
    if (jointsbottle.isNull())
    {
        yError() << "embObjMultiEnc cannot find listofjoints param";
        return false;
    }
          
    Bottle encsbottle = general.findGroup("encoderConversionFactor");
    if (encsbottle.isNull())
    {
        yError() << "embObjMultiEnc cannot find encoderConversionFactor param";
        return false;
    }
     
 
    //jointsbottle contains: "listofjoints 0 1 2 3. So the num of joints is jointsbottle->size() -1 " 
    numofjoints = jointsbottle.size() -1;  
    
    listofjoints.clear();
    for (int i = 1; i < jointsbottle.size(); i++)  listofjoints.push_back(jointsbottle.get(i).asInt());

    yDebug()<< " embObjMultiEnc List of joints: " << numofjoints;
    for(int i=0; i<numofjoints; i++) yDebug() << "pos="<< i << "val="<<  listofjoints[i];
   
    analogdata.resize(numofencperjoint*numofjoints, 0.0);
    encoderConversionFactor.resize(numofencperjoint*numofjoints, 1.0);

    if (numofencperjoint*numofjoints!=encsbottle.size()-1)
    {
        yError() << "embObjMultiEnc invalid size of encoderConversionFactor param";
        return false;
	}
	for (int i=0; i<encsbottle.size()-1; i++)
	{
		encoderConversionFactor[i]=encsbottle.get(i+1).asDouble();
	}
         
    return true;
}

void cartControlReachAvoidThread::getArmHomeOptions(Bottle &b, Vector &poss, Vector &vels)
{
        if (b.check("poss","Getting arm home poss"))
        {
            Bottle &grp=b.findGroup("poss");
            int sz=grp.size()-1;
            int len=sz>7?7:sz;

            for (int i=0; i<len; i++){
                poss[i]=grp.get(1+i).asDouble();
            }
        }

        if (b.check("vels","Getting arm home vels"))
        {
            Bottle &grp=b.findGroup("vels");
            int sz=grp.size()-1;
            int len=sz>7?7:sz;

            for (int i=0; i<len; i++)
                vels[i]=grp.get(1+i).asDouble();
        }
}

    void look()
    {
        Bottle cmd;
        Bottle response;
        cmd.addString("look");
        port.write(cmd,response);

        // pick out map part
        world= response.findGroup("look").findGroup("map");
          
        Bottle &users = response.findGroup("look").findGroup("players");
        
        Bottle *player = users.get(1).asList();
        if (player!=0)
            {
                Bottle &location = player->findGroup("location");
                Value &life = player->find("life");
                std::string playerName = player->get(0).asString();
       
                myX=location.get(1).asInt32(),
                    myY=location.get(2).asInt32(),
                    myLife=life.asInt32();
            }
    }

//---------------------------------------------------------
void readVector(Bottle &rf, string name, Vector &v, int len)
{
    v.resize(len,0.0);
    if(rf.check(name.c_str()))
    {
        Bottle &grp = rf.findGroup(name.c_str());
        for (int i=0; i<len; i++)
            v[i]=grp.get(1+i).asDouble();
    }
    else
    {
        cout<<"Could not find parameters for "<<name<<". "
            <<"Setting everything to zero by default"<<endl;
    }
    displayNameVector(name,v);
}

void readBridgeHeaderVector(Bottle &rf, string name, field &groups, int size)
{
    groups.clear();
    if ( rf.check( name.c_str() ) )
    {
        Bottle &grp = rf.findGroup(name.c_str());
        for ( int i = 0; i < size; i++)
        {
            groups.push_back({grp.get(1+i).asString().c_str(),""});
        }
    }
    else
    {
        cout << "Could not find parameters for " << name << ". "
            << "Setting everything to null by default" << endl;
    }
}

// TODO use it!!
//#warning "Use extractGroup to verify size of parameters matches with number of joints, this will avoid crashes"
static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
{
    size++;  // size includes also the name of the parameter
    Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
    if (tmp.isNull())
    {
        yError () << key1.c_str() << " not found\n";
        return false;
    }

    if(tmp.size()!=size)
    {
        yError () << key1.c_str() << " incorrect number of entries in board.";
        return false;
    }

    out=tmp;
    return true;
}

    PidData getPidData(Bottle &bGroup, const int i)
    {
        PidData pid;

        ostringstream joint;
        joint<<"joint_"<<i;
        Bottle &bJoint=bGroup.findGroup(joint.str().c_str());

        pid.Kp=bJoint.check("Kp",Value(0.0)).asDouble();
        pid.Ki=bJoint.check("Ki",Value(0.0)).asDouble();
        pid.Kd=bJoint.check("Kd",Value(0.0)).asDouble();
        pid.scale=bJoint.check("scale",Value(0.0)).asDouble();
        pid.st_up=bJoint.check("st_up",Value(0.0)).asDouble();
        pid.st_down=bJoint.check("st_down",Value(0.0)).asDouble();
        pid.encs_ratio=bJoint.check("encs_ratio",Value(1.0)).asDouble();
        pid.status=(bJoint.check("status",Value("download")).asString()=="download"?download:upload);
        if (bJoint.check("idling"))
        {
            if (Bottle *bIdlingJoints=bJoint.find("idling").asList())
            {
                pid.idling_joints.clear();
                for (int j=0; j<bIdlingJoints->size(); j++)
                {
                    int k=bIdlingJoints->get(j).asInt();

                    int l;
                    for (l=0; l<rJoints.size(); l++)
                    {
                        if (rJoints.get(l).asInt()==k)
                        {
                            pid.idling_joints.push_back(k);
                            break;
                        }
                    }

                    if (l>=rJoints.size())
                        yError("unrecognized joint %d to put in idle",k);
                }
            }
        }

        return pid;
    }

void cartControlReachAvoidThread::getTorsoOptions(Bottle &b, const char *type, const int i, Vector &sw, Matrix &lim)
{
        if (b.check(type))
        {
            Bottle &grp=b.findGroup(type);
            sw[i]=grp.get(1).asString()=="on"?1.0:0.0;

            if (grp.check("min","Getting minimum value"))
            {
                lim(i,0)=1.0;
                lim(i,1)=grp.find("min").asDouble();
            }

            if (grp.check("max","Getting maximum value"))
            {
                lim(i,2)=1.0;
                lim(i,3)=grp.find("max").asDouble();
            }
        }
}

//generic function that check is key1 is present in input bottle and that the result has size elements
// return true/false
bool RobotranYarpMotionControl::extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
{
    Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
    if (tmp.isNull())
    {
        //yError () << key1.c_str() << " not found\n"; //yError() will be included in the next version of yarp.
        std::cout << key1.c_str() << " not found\n";
        return false;
    }

    if(tmp.size()!=size)
    {
       // yError () << key1.c_str() << " incorrect number of entries";
        std::cout << key1.c_str() << " incorrect number of entries";
        return false;
    }

    out=tmp;
    return true;
}

//generic function that check is key1 is present in input bottle and that the result has size elements
// return true/false
static inline bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
{
    size++;  // size includes also the name of the parameter
    Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
    if (tmp.isNull())
    {
        fprintf(stderr, "%s not found\n", key1.c_str());
        return false;
    }

    if(tmp.size()!=size)
    {
        fprintf(stderr, "%s incorrect number of entries\n", key1.c_str());
        return false;
    }

    out=tmp;

    return true;
}

static bool checkParam(const Bottle& input, RGBDSensorParamParser::RGBDParam& param, bool& found)
{
    bool ret = false;
    Bottle bt=input.findGroup(param.name).tail(); // the first element is the name of the parameter

    if (!bt.isNull())
    {
        Bottle* b;
        if (param.size>1 && bt.size()==1)
        {
            b = bt.get(0).asList();
        }
        else
            b = &bt;
        if (b->isNull())
        {
            yError()<<"RGBDSensorParamParser: check"<<param.name<<"in config file";
            return false;
        }
        if (b->size() != param.size)
        {
            yError() << "RGBDSensorParamParser: parameter" << param.name << "size should be" << param.size;
            return false;
        }
        param.val.resize(param.size);
        for (size_t i=0;i<b->size();i++)
        {
            ret = true;
            param.val[i] = b->get(i);
            found = true;
        }
    }
    else
    {
        ret   = true;
        found = false;
    }
    return ret;
}

 void fromCommand(int argc, char *argv[],bool wipe=true) {
     String tag = "";
     Bottle accum;
     Bottle total;
     bool qualified = false;
     for (int i=0; i<argc; i++) {
         String work = argv[i];
         bool isTag = false;
         if (work.length()>=2) {
             if (work[0]=='-'&&work[1]=='-') {
                 work = work.substr(2,work.length()-2);
                 isTag = true;
                 if (work.find("::")!=String::npos) {
                     qualified = true;
                 }
             }
         }
         if (isTag) {
             if (tag!="") {
                 total.addList().copy(accum);
             }
             tag = work;
             accum.clear();
         } else {
             if (work.find("\\")!=String::npos) {
                 // Specifically when reading from the command
                 // line, we will allow windows-style paths.
                 // Hence we have to break the "\" character
                 String buf = "";
                 for (unsigned int i=0; i<work.length(); i++) {
                     buf += work[i];
                     if (work[i]=='\\') {
                         buf += work[i];
                     }
                 }
                 work = buf;
             }
         }
         accum.add(Value::makeValue(work.c_str()));
     }
     if (tag!="") {
         total.addList().copy(accum);
     }
     if (!qualified) {
         fromBottle(total,wipe);
         return;
     }
     if (wipe) {
         clear();
     }
     Bottle *cursor = NULL;
     for (int i=0; i<total.size(); i++) {
         cursor = NULL;
         Bottle *term = total.get(i).asList();
         if (!term) continue;
         ConstString key = term->get(0).asString();
         ConstString base = key;
         while (key.length()>0) {
             int at = key.find("::");
             base = key;
             if (at>=0) {
                 base = key.substr(0,at);
                 key = key.substr(at+2);
             } else {
                 key = "";
             }
             Bottle& result = (cursor!=NULL)? (cursor->findGroup(base.c_str())) : owner.findGroup(base.c_str());
             if (result.isNull()) {
                 if (!cursor) {
                     cursor = &putBottle((base).c_str());
                 } else {
                     cursor = &cursor->addList();
                 }
                 cursor->addString(base);
             } else {
                 cursor = &result;
             }
         }
         if (cursor) {
             cursor->copy(*term);
             cursor->get(0) = Value(base);
         }
     }
 }

bool embObjInertials::sendConfig2MTBboards(Searchable& globalConfig)
{
    eOprotID32_t id32 = eo_prot_ID32dummy;

    // configuration specific for skin-inertial device

    Bottle config = globalConfig.findGroup("GENERAL");


    // prepare the config of the inertial sensors: datarate and the mask of the enabled ones.

    eOas_inertial_sensorsconfig_t inertialSensorsConfig = {0};

    inertialSensorsConfig.accelerometers = 0;
    inertialSensorsConfig.gyroscopes = 0;
    inertialSensorsConfig.datarate = _period;


    // meglio sarebbe mettere un controllo sul fatto che ho trovato enabledAccelerometers e che ha size coerente

    Bottle sensors;

    sensors = config.findGroup("enabledAccelerometers");

    int numofaccelerometers = sensors.size()-1;    // sensors holds strings "enabledAccelerometers" and then all the others, thus i need a -1

    for(int i=0; i<numofaccelerometers; i++)
    {
        eOas_inertial_position_t pos = eoas_inertial_pos_none;

        yarp::os::ConstString strpos = sensors.get(i+1).asString();

        pos = getLocationOfInertialSensor(strpos);  // prendi la posizione dalla stringa strpos: fai una funzione apposita

        if(eoas_inertial_pos_none != pos)
        {
            _fromInertialPos2DataIndexAccelerometers[pos] = i;
            inertialSensorsConfig.accelerometers   |= EOAS_ENABLEPOS(pos);
        }
    }

    sensors = config.findGroup("enabledGyroscopes");

    int numofgyroscopess = sensors.size()-1;    // sensors holds strings "enabledGyroscopes" and then all the others, thus i need a -1

    for(int i=0; i<numofgyroscopess; i++)
    {
        eOas_inertial_position_t pos = eoas_inertial_pos_none;

        yarp::os::ConstString strpos = sensors.get(i+1).asString();

        pos = getLocationOfInertialSensor(strpos);  // prendi la posizione dalla stringa strpos: fai una funzione apposita

        if(eoas_inertial_pos_none != pos)
        {
            _fromInertialPos2DataIndexGyroscopes[pos] = numofaccelerometers+i;
            inertialSensorsConfig.gyroscopes   |= EOAS_ENABLEPOS(pos);
        }
    }

    // configure the sensors (datarate and position)

    id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_config_sensors);
    if(false == res->setRemoteValueUntilVerified(id32, &inertialSensorsConfig, sizeof(inertialSensorsConfig), 10, 0.010, 0.050, 2))
    {
        yError() << "FATAL: embObjInertials::sendConfig2MTBboards() had an error while calling setRemoteValueUntilVerified() for config in BOARD" << res->getName() << "with IP" << res->getIPv4string();
        return false;
    }
    else
    {
        if(verbosewhenok)
        {
            yDebug() << "embObjInertials::sendConfig2MTBboards() correctly configured enabled sensors with period" << _period << "in BOARD" << res->getName() << "with IP" << res->getIPv4string();
        }
    }


//    // start the configured sensors

//    eOmc_inertial_commands_t startCommand = {0};
//    startCommand.enable = 1;

//    id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_inertial, 0, eoprot_tag_as_inertial_cmmnds_enable);
//    if(!res->addSetMessage(id32, (uint8_t*) &startCommand))
//    {
//        yError() << "ethResources::sendConfig2MTBboards() fails to command the start transmission of the inertials";
//        return false;
//    }

    return true;
}

ethResources* TheEthManager::getResource(yarp::os::Searchable &config)
{
    std::string   str;
    Bottle        xtmp2;
    ACE_TCHAR     remote_address[64];
    ACE_TCHAR     address_tmp_string[64];
    ACE_UINT16    rem_port;
    ACE_UINT32    rem_ip1,rem_ip2,rem_ip3,rem_ip4;

    if(config.findGroup("GENERAL").find("Verbose").asInt())
        {str=config.toString().c_str();}
    else
        {str=" ";}
    yTrace() << str;


    /* Get a Socket for the PC104 This needs a reading from config file, at least for the port to use...
    * If the socket is already initialized the createSocket function does nothing, it is also thread safe  */
    ACE_UINT32 loc_ip1,loc_ip2,loc_ip3,loc_ip4;
    xtmp2 = config.findGroup("PC104IpAddress");
    strcpy(address_tmp_string, xtmp2.get(1).asString().c_str());

    // ACE format
    sscanf(address_tmp_string,"%d.%d.%d.%d",&loc_ip1, &loc_ip2, &loc_ip3, &loc_ip4);
    ACE_INET_Addr loc_dev(rem_port, (loc_ip1<<24)|(loc_ip2<<16)|(loc_ip3<<8)|loc_ip4);

    if(!createSocket(loc_dev))
        {return false;}


    //
    // Get EMS ip addresses from config file, to see if we need to instantiate a new Resources or simply return
    // a pointer to an already existing object
    //

    Bottle xtmp = Bottle(config.findGroup("ETH"));
    xtmp2 = xtmp.findGroup("IpAddress");
    strcpy(remote_address, xtmp2.get(1).asString().c_str());
    sscanf(remote_address,"%d.%d.%d.%d",&rem_ip1, &rem_ip2, &rem_ip3, &rem_ip4);

    // Get EMS CmdPort from config file
    xtmp2 = xtmp.findGroup("CmdPort");
    rem_port = xtmp2.get(1).asInt();

    // ACE format
    ACE_INET_Addr remote_addr_tmp;
    remote_addr_tmp.set(rem_port, (rem_ip1<<24)|(rem_ip2<<16)|(rem_ip3<<8)|rem_ip4);

    ethResources *newRes = NULL;

    managerMutex.wait();
    ethResIt iterator = EMS_list.begin();

    while(iterator != EMS_list.end())
    {
        if((*iterator)->getRemoteAddress() == remote_addr_tmp)
        {
            // device already exist.
            newRes = (*iterator);
            break;
        }
        iterator++;
    }

    if(NULL == newRes)
    {
        // device doesn't exist yet, create it
        yTrace() << "Creating EMS device with IP " << remote_address;
        newRes = new ethResources;
        if(!newRes->open(config))
        {
            printf("Error creating new EMS!!");
            if(NULL != newRes)
                delete newRes;

            newRes = NULL;
        }
        else
        {
            nBoards++;
            EMS_list.push_back(newRes);
        }
    }
    managerMutex.post();
    return newRes;
}

void YarpPluginSelector::scan() {
    config.clear();
    plugins.clear();
    search_path.clear();
    NetworkBase::lock();
    Property& state = NameClient::getNameClient().getPluginState();
    config = state;
    NetworkBase::unlock();
    bool need_scan = true;
    if (config.check("last_update_time")) {
        if (Time::now()-config.find("last_update_time").asDouble()<5) {
            need_scan = false;
        }
    }
    if (need_scan) {
        YARP_SPRINTF0(Logger::get(),
                      debug,
                      "Scanning. I'm scanning. I hope you like scanning too.");
        NetworkBase::lock();
        ResourceFinder& rf = ResourceFinder::getResourceFinderSingleton();
        if (!rf.isConfigured()) {
            rf.configure(0,YARP_NULLPTR);
        }
        rf.setQuiet(true);
        Bottle plugins = rf.findPaths("plugins");
        if (plugins.size()==0) {
            plugins = rf.findPaths("share/yarp/plugins");
        }
        if (plugins.size()>0) {
            for (int i=0; i<plugins.size(); i++) {
                ConstString target = plugins.get(i).asString();
                YARP_SPRINTF1(Logger::get(),
                              debug,
                              "Loading configuration files related to plugins from %s.", target.c_str());
                config.fromConfigDir(target, "inifile", false);
            }
        } else {
            YARP_SPRINTF0(Logger::get(),
                          debug,
                          "Plugin directory not found");
        }
        config.put("last_update_time",Time::now());
        state = config;
        NetworkBase::unlock();
    }

    Bottle inilst = config.findGroup("inifile").tail();
    for (int i=0; i<inilst.size(); i++) {
        ConstString inifile = inilst.get(i).asString();
        Bottle inigroup = config.findGroup(inifile);
        Bottle lst = inigroup.findGroup("plugin").tail();
        for (int i=0; i<lst.size(); i++) {
            ConstString plugin_name = lst.get(i).asString();
            Bottle group = inigroup.findGroup(plugin_name);
            group.add(Value::makeValue(ConstString("(inifile \"") + inifile + "\")"));
            if (select(group)) {
                plugins.addList() = group;
            }
        }
        lst = inigroup.findGroup("search").tail();
        for (int i=0; i<lst.size(); i++) {
            ConstString search_name = lst.get(i).asString();
            Bottle group = inigroup.findGroup(search_name);
            search_path.addList() = group;
        }
    }
}