C++ os::ConstString类代码示例

int recursiveDiff(yarp::os::ConstString srcDirName, yarp::os::ConstString destDirName, std::ostream &output)
{
    std::set<std::string> srcFileList;
    bool ok = recursiveFileList(srcDirName.c_str(), "", srcFileList);
    std::set<std::string> destFileList;
    ok=ok && recursiveFileList(destDirName.c_str(), "", destFileList);

    if (!ok)
        return -1;
    size_t nModifiedFiles=0;
    for(std::set<std::string>::iterator srcIt=srcFileList.begin(); srcIt !=srcFileList.end(); ++srcIt)
    {
        std::set<std::string>::iterator destPos=destFileList.find(*srcIt);
        if (destPos!=destFileList.end())
        {
            diff_match_patch<std::string> dmp;
            ConstString srcFileName=srcDirName+ PATH_SEPARATOR + (*srcIt);
            if (isHidden(srcFileName))
                continue;

            std::ifstream in(srcFileName.c_str());
            std::string srcStr((std::istreambuf_iterator<char>(in)), std::istreambuf_iterator<char>());
            in.close();
            in.open((destDirName+ PATH_SEPARATOR +(*destPos)).c_str());
            std::string destStr((std::istreambuf_iterator<char>(in)), std::istreambuf_iterator<char>());
            std::string patchString = dmp.patch_toText(dmp.patch_make(srcStr, destStr));
            if (patchString!= "")
            {
                output << "- " << srcDirName + PATH_SEPARATOR + (*srcIt)<<endl;
                output << "+ " << destDirName + PATH_SEPARATOR + (*destPos) <<endl;
                output << dmp.patch_toText(dmp.patch_make(srcStr, destStr))<<std::endl;
                nModifiedFiles++;
            }
            destFileList.erase(destPos);
        }
//         else
//         {
//             output << "Added file  " << srcDirName+PATH_SEPARATOR +(*srcIt) <<endl;
//             nModifiedFiles++;
//         }
    }

//     for(std::set<std::string>::iterator destIt=destFileList.begin(); destIt !=destFileList.end(); ++destIt)
//     {
//         ConstString destFileName=destDirName+ PATH_SEPARATOR + (*destIt);
//         if(isHidden(destFileName))
//             continue;
//
//         output << "Removed file " << destFileName <<endl;
//         nModifiedFiles++;
//
//     }

    return nModifiedFiles;//tbm
}

    void findFileBase(Property& config, const char *name,
                      bool isDir,
                      Bottle& output, bool justTop) {

        ConstString cap =
            config.check("capability_directory",Value("app")).asString();
        Bottle defCaps =
            config.findGroup("default_capability").tail();

        // check current directory
		if (ConstString(name)==""&&isDir) {
            output.addString(getPwd());
            if (justTop) return;
        }
        ConstString str = check(getPwd(),"","",name,isDir);
        if (str!="") {
            output.addString(str);
            if (justTop) return;
        }

        if (configFilePath!="") {
            ConstString str = check(configFilePath.c_str(),"","",name,isDir);
            if (str!="") {
                output.addString(str);
                if (justTop) return;
            }
        }

        // check app dirs
        for (int i=0; i<apps.size(); i++) {
            str = check(root.c_str(),cap,apps.get(i).asString().c_str(),
                        name,isDir);
            if (str!="") {
                output.addString(str);
                if (justTop) return;
            }
        }

        // check ROOT/app/default/
        for (int i=0; i<defCaps.size(); i++) {
            str = check(root.c_str(),cap,defCaps.get(i).asString().c_str(),
                        name,isDir);
            if (str!="") {
                output.addString(str);
                if (justTop) return;
            }
        }

        if (justTop) {
            if (!quiet) {
                fprintf(RTARGET,"||| did not find %s\n", name);
            }
        }
    }

int recursiveMerge(yarp::os::ConstString srcDirName, yarp::os::ConstString destDirName, yarp::os::ConstString commonParentName, std::ostream &output)
{
    std::set<std::string> srcFileList;
    bool ok = recursiveFileList(srcDirName.c_str(), "", srcFileList);
    std::set<std::string> destFileList;
    ok=ok && recursiveFileList(destDirName.c_str(), "", destFileList);
    std::set<std::string> hiddenFilesList;
    ok=ok && recursiveFileList(commonParentName.c_str(), "", hiddenFilesList);

    if (!ok)
        return -1;

    for(std::set<std::string>::iterator srcIt=srcFileList.begin(); srcIt !=srcFileList.end(); ++srcIt)
    {
            ConstString srcFileName=srcDirName+ PATH_SEPARATOR + (*srcIt);
            if (isHidden(srcFileName))
                continue;

        std::set<std::string>::iterator destPos=destFileList.find(*srcIt);
        if (destPos!=destFileList.end())
        {
            ConstString destFileName=destDirName+ PATH_SEPARATOR + (*destPos);
            std::set<std::string>::iterator hiddenDestPos=hiddenFilesList.find(*srcIt);
            if (hiddenDestPos!=hiddenFilesList.end())
            {
                ConstString hiddenFileName=commonParentName+ PATH_SEPARATOR + (*hiddenDestPos);
                fileMerge(srcFileName, destFileName, hiddenFileName);
            }
            else
            {
                printf("Could not merge automatically, use mergetool\n");
            }
            destFileList.erase(destPos);
        }
//         else
//         {
//             std::set<std::string>::iterator hiddenDestPos=hiddenFilesList.find(*srcIt);
//             if (hiddenDestPos==hiddenFilesList.end())
//             {
//                 output << "File  " << srcDirName+PATH_SEPARATOR +(*srcIt) << " has been added to the original context" << endl;
//             }
//         }
    }

//     for(std::set<std::string>::iterator destIt=destFileList.begin(); destIt !=destFileList.end(); ++destIt)
//     {
//         std::set<std::string>::iterator hiddenDestPos=hiddenFilesList.find(*destIt);
//         if (hiddenDestPos==hiddenFilesList.end())
//             output << "File " << destDirName+PATH_SEPARATOR +(*destIt) << " does not belong to the original context" << endl;
//     }

    return (ok? 0: 1);//tbm
}

bool yarp::dev::LocationsServer::save_locations(yarp::os::ConstString locations_file)
{
    std::ofstream file;
    file.open (locations_file.c_str());

    if(!file.is_open())
    {
        yError() << "sorry unable to open" << locations_file << "locations file";
        return false;
    }

    std::string     s;
    Map2DLocation   l;
    s = " ";

    std::map<std::string, Map2DLocation>::iterator it;
    for (it = m_locations.begin(); it != m_locations.end(); it++)
    {
        l = it->second;
        file << it->first + s + l.map_id + s << l.x << s << l.y << s << l.theta << "\n";
    }

    file.close();
    return true;
}

bool yarp::dev::LocationsServer::load_locations(yarp::os::ConstString locations_file)
{
    std::ifstream file;
    file.open (locations_file.c_str());

    if(!file.is_open())
    {
        yError() << "sorry unable to open" << locations_file << "locations file";
        return false;
    }

    std::string     buffer, name, mapId, xpos, ypos, theta;
    Map2DLocation   location;

    while(!file.eof())
    {
        std::getline(file, buffer);
        std::istringstream iss(buffer);

        iss >> name >> mapId >> xpos >> ypos >> theta;

        location.map_id  = mapId;
        location.x       = std::atof(xpos.c_str());
        location.y       = std::atof(ypos.c_str());
        location.theta   = std::atof(theta.c_str());
        m_locations[name] = location;
    }

    yDebug() << "there are now" << m_locations.size() << "locations";
    file.close();
    return true;
}

void OpenLoopConsistency::verifyMode(int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode, yarp::os::ConstString title)
{
    int cmode;
    yarp::dev::InteractionModeEnum imode; 
    int timeout = 0;

    while (1)
    {
        int ok=0;
        for (int i=0; i<n_cmd_joints; i++)
        {
            icmd->getControlMode (jointsList[i],&cmode);
            iimd->getInteractionMode(jointsList[i],&imode);
            if (cmode==desired_control_mode && imode==desired_interaction_mode) ok++;
        }
        if (ok==n_cmd_joints) break;
        if (timeout>100)
        {
            char sbuf[500];
            sprintf(sbuf,"Test (%s) failed: current mode is (%d,%d), it should be (%d,%d)",title.c_str(), desired_control_mode,desired_interaction_mode,cmode,imode);
            RTF_ASSERT_ERROR(sbuf);
        }
        yarp::os::Time::delay(0.2);
        timeout++;
    }
    char sbuf[500];
    sprintf(sbuf,"Test (%s) passed: current mode is (%d,%d)",title.c_str(), desired_control_mode,desired_interaction_mode);
    RTF_TEST_REPORT(sbuf);
}

// \todo TODO bug ? 
bool checkRequiredParamIsVectorOfString(yarp::os::Searchable& config,
                                     const yarp::os::ConstString& paramName,
                                     std::vector<std::string> & output_vector)
{
    bool correct = !(config.findGroup(paramName).isNull());
    if( correct )
    correct = true;
    {
        Bottle ids = config.findGroup(paramName).tail();
        std::cout << "ids : " << ids.toString() << std::endl;
        std::cout << "ids : " << config.find(paramName).toString() << std::endl;
        output_vector.resize(ids.size());
        for(int i = 0; i < ids.size(); i++ )
        {
            output_vector[i] = ids.get(i).asString().c_str();
        }
    }

    if( !correct )
    {
        yError("CanBusInertialMTB: problem loading parameter %s as vector of string",paramName.c_str());
    }

    return correct;
}

static yarp::os::ConstString quoteFree(const yarp::os::ConstString &src) {
    yarp::os::ConstString result = "";
    for (unsigned int i=0; i<src.length(); i++) {
        char ch = src[i];
        if (ch=='"') {
            result += "&quot;";
        } else {
            result += ch;
        }
    }
    return result;
}

int fileMerge(yarp::os::ConstString srcFileName, yarp::os::ConstString destFileName, yarp::os::ConstString commonParentName)
{
    diff_match_patch<std::string> dmp;
    std::ifstream in(srcFileName.c_str());
    std::string srcStr((std::istreambuf_iterator<char>(in)), std::istreambuf_iterator<char>());
    in.close();
    in.open(destFileName.c_str());
    std::string destStr((std::istreambuf_iterator<char>(in)), std::istreambuf_iterator<char>());
    in.close();
    in.open(commonParentName.c_str());
    std::string hiddenDestStr((std::istreambuf_iterator<char>(in)), std::istreambuf_iterator<char>());
    in.close();
    diff_match_patch<string>::Patches patches = dmp.patch_make(hiddenDestStr, destStr);
    std::pair<std::string, std::vector<bool> > mergeResults = dmp.patch_apply(patches, srcStr);
    std::ofstream out(destFileName.c_str());
    out << mergeResults.first;
    out.close();
    //update hidden file from installed one
    out.open(commonParentName.c_str());
    out << srcStr;
    out.close();
    return 0;
}

bool checkRequiredParamIsInt(yarp::os::Searchable& config,
                             const yarp::os::ConstString& paramName)
{
    bool correct = config.check(paramName);
    if( correct )
    {
        correct = config.find(paramName).isInt();
    }

    if( !correct )
    {
        yError("CanBusInertialMTB: problem loading parameter %s as int",paramName.c_str());
    }

    return correct;
}

bool checkRequiredParamIsVectorOfInt(yarp::os::Searchable& config,
                                     const yarp::os::ConstString& paramName,
                                     std::vector<int> & output_vector)
{
    bool correct = !(config.findGroup(paramName).isNull());
    if( correct )
    {
        Bottle ids = config.findGroup(paramName).tail();
        output_vector.resize(ids.size());
        for(int i = 0; i < ids.size(); i++ )
        {
            output_vector[i] = ids.get(i).asInt();
        }
    }

    if( !correct )
    {
        yError("CanBusInertialMTB: problem loading parameter %s as vector of int",paramName.c_str());
    }

    return correct;
}

bool fileCopy(yarp::os::ConstString srcFileName, yarp::os::ConstString destFileName, bool force, bool verbose)
{
    char buf[BUFSIZ];
    size_t size;
    FILE* source = fopen(srcFileName.c_str(), "rb");
    if (source == NULL)
    {
        if (verbose)
            printf("Could not open source file %s\n", srcFileName.c_str());
        return false;
    }
    if (!yarp::os::stat(destFileName.c_str()))
    {
        if (force)
        {
            fileRemove(destFileName);
        }
        else
        {
            if (verbose)
                printf("File already exists : %s\n",destFileName.c_str());
            fclose(source);
            return false;
        }
    }
    FILE* dest = fopen(destFileName.c_str(), "wb");
    if(dest ==NULL)
    {
        if (verbose)
            printf("Could not open target file %s\n",destFileName.c_str());
        fclose(source);
        return false;
    }
    // clean and more secure
    // feof(FILE* stream) returns non-zero if the end of file indicator for stream is set

    while ((size = fread(buf, 1, BUFSIZ, source)))
    {
        fwrite(buf, 1, size, dest);
    }

    fclose(source);
    fclose(dest);
    return true;
}

Contact NameServiceOnTriples::query(const yarp::os::ConstString& portName,
                                    NameTripleState& act,
                                    const yarp::os::ConstString& prefix,
                                    bool nested) {
    if (!nested) lock();
    Triple t;
    t.setNameValue("port",portName.c_str());
    int result = act.mem.find(t, YARP_NULLPTR);
    TripleContext context;
    context.setRid(result);
    if (result!=-1) {
        ConstString host = "";
        if (ConstString(prefix)!="") {
            printf("LOOKING AT IPS FOR %s\n", prefix.c_str());
            t.setNameValue("ips","*");
            list<Triple> lst = act.mem.query(t,&context);
            for (list<Triple>::iterator it=lst.begin();it!=lst.end();it++) {
                printf("LOOKING AT IPS %s\n", it->value.c_str());
                if (it->value.find(prefix)==0) {
                    host = it->value;
                    break;
                }
            }
        }
        if (host=="") {
            t.setNameValue("host","*");
            list<Triple> lst = act.mem.query(t,&context);
            if (lst.size()>0) {
                host = lst.begin()->value.c_str();
            }
        }
        if (host=="") {
            host = "localhost";
        }
        t.setNameValue("socket","*");
        list<Triple> lst = act.mem.query(t,&context);
        int sock = 10000;
        if (lst.size()>0) {
            sock = atoi(lst.begin()->value.c_str());
        }
        t.setNameValue("carrier","*");
        ConstString carrier = "tcp";
        lst = act.mem.query(t,&context);
        if (lst.size()>0) {
            carrier = lst.begin()->value.c_str();
        }
        t.setNameValue("type","*");
        ConstString typ = "*";
        lst = act.mem.query(t,&context);
        if (lst.size()>0) {
            typ = lst.begin()->value.c_str();
        }
        if (!nested) unlock();
        Contact result = Contact(portName, carrier, host, sock);
        if (typ!="" && typ!="*") {
            NestedContact nc;
            nc.fromString(result.getName());
            nc.setTypeName(typ);
            result.setNestedContact(nc);
        }
        return result;
    }
    if (!nested) unlock();
    if (delegate && !nested) {
        return delegate->queryName(portName);
    }
    return Contact();
}

void OpenLoopConsistency::verifyOutputDiff(double verify_val, yarp::os::ConstString title)
{
    double value;
    char sbuf[500];
    if (cmd_mode==single_joint)
    {
        for (int i=0; i<n_cmd_joints; i++)
        {
            iopl->getOutput(jointsList[i],&value);
            if (value!=verify_val)
            {
                sprintf(sbuf,"Test (%s) passed j%d, current output is (%f!=%f)",title.c_str(),i,value,verify_val);
                RTF_TEST_REPORT(sbuf);
            }
            else
            {
                sprintf(sbuf,"Test (%s) failed: current output is (%f), it should be (%f)",title.c_str(), value, verify_val);
                RTF_ASSERT_ERROR(sbuf);
            }
        }
    }
    else if (cmd_mode==some_joints)
    {
        //same of single_joint, since multiple joint is not currently supported
        for (int i=0; i<n_cmd_joints; i++)
        {
            iopl->getOutput(jointsList[i],&value);
            if (value!=verify_val)
            {
                sprintf(sbuf,"Test (%s) passed j%d current output is (%f!=%f)",title.c_str(), i,value,verify_val);
                RTF_TEST_REPORT(sbuf);
            }
            else
            {
                sprintf(sbuf,"Test (%s) failed: current output is (%f), it should be (%f)",title.c_str(), value, verify_val);
                RTF_ASSERT_ERROR(sbuf);
            }
        }
    }
    else if (cmd_mode==all_joints)
    {
        int ok=0;
        iopl->getOutputs(cmd_tot);
        for (int i=0; i<n_part_joints; i++)
        {
            if (verify_val!=cmd_tot[i]) ok++;
        }
        if (ok==n_part_joints)
        {
            sprintf(sbuf,"Test (%s) passed current output is (%f!=%f)",title.c_str(),value,verify_val);
            RTF_TEST_REPORT(sbuf);
        }
        else
        {
            sprintf(sbuf,"Test (%s) failed: only %d joints (of %d) are ok",title.c_str(),ok,n_part_joints);
            RTF_ASSERT_ERROR(sbuf);
        }
    }
    else
    {
        RTF_ASSERT_ERROR("Invalid cmd_mode");
    }
    yarp::os::Time::delay(0.010);
}

bool GazeboYarpControlBoardDriver::setRemoteVariable(yarp::os::ConstString key, const yarp::os::Bottle& val)
{
    std::string s1 = val.toString();
    yarp::os::Bottle* bval = val.get(0).asList();
    if (bval == 0)
    {
        yWarning("setRemoteVariable(): Protocol error %s", s1.c_str());
        return false;
    }

    std::string s2 = bval->toString();

    if (key == "hardwareDamping")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetDamping(0,value);
        }
        return true;
    }
    if (key == "hardwareFriction")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetParam("friction",0,value);
        }
        return true;
    }
    if (key == "hardwareEffortLimit")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetEffortLimit(0,value);
        }
        return true;
    }
    if (key == "hardwareVelocityLimit")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetVelocityLimit(0,value);
        }
        return true;
    }
    if (key == "hardwareHiStop")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetUpperLimit(0,convertUserToGazebo(i,value));
        }
        return true;
    }
    if (key == "hardwareLowStop")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_jointPointers[i]->SetLowerLimit(0,convertUserToGazebo(i,value));
        }
        return true;
    }
    if (key == "yarp_jntMaxVel")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            setVelLimits(i,0,value);
        }
        return true;
    }
    if (key == "yarp_jntMaxPos")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            double t_max=0, t_min=0;
            getLimits(i,&t_min,&t_max);
            setLimits(i,t_min,value);
        }
        return true;
    }
    if (key == "yarp_jntMinPos")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            double t_max=0, t_min=0;
            getLimits(i,&t_min,&t_max);
            setLimits(i,value,t_max);
        }
        return true;
    }
    if (key == "yarp_kPWM")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
//.........这里部分代码省略.........