Пример #1
0
    virtual bool configure(ResourceFinder &rf)
    {
        Time::turboBoost();

        //check if the yarp networ is running
        if (yarp.checkNetwork()==false)
        {
            return false;
        }

        moduleName = rf.check("name", Value(1), "module name (string)").asString();
        setName(moduleName.c_str());
        
        period = rf.check("period", Value(5000), "update period (int)").asInt();

        string pname  = "/" + moduleName + ":o";
        monitorOutput.open(pname.c_str());
         
        picBlocks = rf.findFile(rf.check("pic_blocks", Value(1), "module name (string)").asString());
        picBackground = rf.findFile(rf.check("pic_background", Value(1), "module name (string)").asString());
        picNumbers = rf.findFile(rf.check("pic_numbers", Value(1), "module name (string)").asString());

        graphics = new GraphicsManager(picBackground.c_str(),picBlocks.c_str(),picNumbers.c_str());
        m_timer = Glib::signal_timeout().connect(sigc::mem_fun(*this, &CtrlModule::on_timeout), period);
        on_timeout();

        //start GTK loop
        gtk_main->run(*graphics);

        return true;
    }
Пример #2
0
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
}
Пример #3
0
    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);
            }
        }
    }
Пример #4
0
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
}
Пример #5
0
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;
}
Пример #6
0
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;
}
Пример #9
0
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;
}
Пример #10
0
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;
}
Пример #13
0
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;
}
Пример #14
0
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++)
        {
            double value = bval->get(i).asDouble();
            m_kPWM[i]=value;
        }
        return true;
    }
    if (key == "SHORTCUT_all_pos_kp")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            yarp::dev::Pid tmp_pid;
            getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);
            tmp_pid.kp = bval->get(i).asDouble();
            setPid(VOCAB_PIDTYPE_POSITION, i,tmp_pid);
        }
        return true;
    }
    if (key == "SHORTCUT_all_pos_kd")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            yarp::dev::Pid tmp_pid;
            getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);
            tmp_pid.kd = bval->get(i).asDouble();
            setPid(VOCAB_PIDTYPE_POSITION, i,tmp_pid);
        }
        return true;
    }
    if (key == "SHORTCUT_all_pos_ki")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            yarp::dev::Pid tmp_pid;
            getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);
            tmp_pid.ki = bval->get(i).asDouble();
            setPid(VOCAB_PIDTYPE_POSITION, i,tmp_pid);
        }
        return true;
    }
    if (key == "VelocityTimeout")
    {
        for (size_t i = 0; i < m_numberOfJoints; i++)
        {
            double value = bval->get(i).asDouble();
            m_velocity_watchdog[i]->modifyDuration(value);
        }
        return true;
    }

    yWarning("setRemoteVariable(): Unknown variable %s", key.c_str());
    return false;
}
Пример #17
0
bool WireWriter::writeBinary(const yarp::os::ConstString& tag) {
    writer.appendInt(BOTTLE_TAG_BLOB);
    writer.appendInt((int)tag.length());
    writer.appendBlock(tag.c_str(),tag.length());
    return !writer.isError();
}
bool GazeboYarpControlBoardDriver::getRemoteVariable(yarp::os::ConstString key, yarp::os::Bottle& val)
{
    val.clear();
    if (key == "hardwareDamping")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetDamping(0);  r.addDouble(tmp); }
        return true;
    }
    if (key == "hardwareFriction")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetParam(std::string("friction"),0);  r.addDouble(tmp); }
        return true;
    }
    if (key == "hardwareHiStop")
    {
        yarp::os::Bottle& r = val.addList();
        for (size_t i = 0; i< m_numberOfJoints; i++)
        {
#if GAZEBO_MAJOR_VERSION >= 8
            double upperLimit = m_jointPointers[i]->UpperLimit(0);
#else
            double upperLimit = m_jointPointers[i]->GetUpperLimit(0).Radian();
#endif
            double tmp = convertGazeboToUser(i, upperLimit);
            r.addDouble(tmp);
        }
        return true;
    }
    if (key == "hardwareLowStop")
    {
        yarp::os::Bottle& r = val.addList();
        for (size_t i = 0; i< m_numberOfJoints; i++) {
#if GAZEBO_MAJOR_VERSION >= 8
            double lowerLimit = m_jointPointers[i]->LowerLimit(0);
#else
            double lowerLimit = m_jointPointers[i]->GetLowerLimit(0).Radian();
#endif
            double tmp = convertGazeboToUser(i, lowerLimit);
            r.addDouble(tmp);
        }
        return true;
    }
    if (key == "hardwareEffortLimit")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetEffortLimit(0);  r.addDouble(tmp); }
        return true;
    }
    if (key == "hardwareVelocityLimit")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp = m_jointPointers[i]->GetVelocityLimit(0);  r.addDouble(tmp); }
        return true;
    }
    if (key == "yarp_jntMaxVel")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp_min,tmp_max; getVelLimits(i,&tmp_min,&tmp_max);  r.addDouble(tmp_max); }
        return true;
    }
    if (key == "yarp_jntMaxPos")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp_min,tmp_max; getLimits(i,&tmp_min,&tmp_max);  r.addDouble(tmp_max); }
        return true;
    }
    if (key == "yarp_kPWM")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { r.addDouble(m_kPWM[i]); }
        return true;
    }
    if (key == "yarp_jntMinPos")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { double tmp_min,tmp_max; getLimits(i,&tmp_min,&tmp_max);  r.addDouble(tmp_min); }
        return true;
    }
    if (key == "SHORTCUT_all_pos_kp")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { yarp::dev::Pid tmp_pid; getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);  r.addDouble(tmp_pid.kp); }
        return true;
    }
    if (key == "SHORTCUT_all_pos_kd")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { yarp::dev::Pid tmp_pid; getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);  r.addDouble(tmp_pid.kd); }
        return true;
    }
    if (key == "SHORTCUT_all_pos_ki")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { yarp::dev::Pid tmp_pid; getPid(VOCAB_PIDTYPE_POSITION, i,&tmp_pid);  r.addDouble(tmp_pid.ki); }
        return true;
    }
    if (key == "VelocityTimeout")
    {
        yarp::os::Bottle& r = val.addList(); for (size_t i = 0; i< m_numberOfJoints; i++) { r.addDouble(m_velocity_watchdog[i]->getDuration()); }
        return true;
    }
    yWarning("getRemoteVariable(): Unknown variable %s", key.c_str());
    return false;
}
Пример #19
0
    bool configureFromPolicy(Property& config, const char *policyName) {
        this->policyName = policyName;
        if (verbose) {
            fprintf(RTARGET,"||| policy set to %s\n", policyName);
        }
        String rootVar = policyName;
        const char *result =
            yarp::os::getenv(rootVar.c_str());
        bool needEnv = false;
#ifdef YARP2_WINDOWS
        needEnv = true;
#endif
        if (result==NULL) {
            root = "";
        } else {
            root = result;
        }
        root = config.check(policyName,Value(root)).asString().c_str();
        if (root == "") {
            if (verbose||needEnv) {
                fprintf(RTARGET,"||| environment variable %s not set\n",
                        rootVar.c_str());
            }
            if (needEnv) {
                return false;
            }
        } else {
            if (verbose) {
                fprintf(RTARGET,"||| %s: %s\n",
                        rootVar.c_str(),root.c_str());
            }
        }
        String checked = "";
        String userConfig = expandUserFileName(ConstString(policyName) + ".ini").c_str();
        String rootConfig = String(root.c_str()) + "/" + policyName + ".ini";
        String altConfig = String("/etc/yarp/policies/") + policyName + ".ini";
#ifndef YARP_NO_DEPRECATED
        String deprecatedConfig = String("/etc/") + policyName + ".ini"; // FIXME Deprecated
#endif // YARP_NO_DEPRECATED
        bool ok = false;
        if (!ok) {
            if (root!="") {
                if (verbose) {
                    fprintf(RTARGET,"||| loading policy from %s\n",
                            rootConfig.c_str());
                }
                checked += " " + rootConfig;
                ok = config.fromConfigFile(rootConfig.c_str(),false);
            }
        }
        if (!needEnv) {
            if (!ok) {
                if (verbose) {
                    fprintf(RTARGET,"||| loading policy from %s\n",
                            userConfig.c_str());
                }
                checked += " " + userConfig;
                ok = config.fromConfigFile(userConfig.c_str(),false);
            }
            if (!ok) {
                if (verbose) {
                    fprintf(RTARGET,"||| loading policy from %s\n",
                            altConfig.c_str());
                }
                checked += " " + altConfig;
                ok = config.fromConfigFile(altConfig.c_str(),false);
            }
#ifndef YARP_NO_DEPRECATED
            if (!ok) {
                if (verbose) {
                    fprintf(RTARGET,"||| loading policy from %s\n",
                            deprecatedConfig.c_str());
                }
                checked += " " + deprecatedConfig;
                ok = config.fromConfigFile(deprecatedConfig.c_str(),false);
                if (ok) {
                    fprintf(RTARGET, "||| WARNING: Loading policies from /etc/ is deprecated,\n"
                                     "|||          you should move them in /etc/yarp/policies/ .\n");
                }
            }
#endif // YARP_NO_DEPRECATED
            if (!ok) {
                altConfig = String("/usr/local/etc/") + policyName + ".ini";
                if (verbose) {
                    fprintf(RTARGET,"||| loading policy from %s\n",
                            altConfig.c_str());
                }
                checked += " " + altConfig;
                ok = config.fromConfigFile(altConfig.c_str(),false);
            }
        }
        /*
          // this would violate the spec
        if (!ok) {
            if (verbose) {
                fprintf(RTARGET,"||| in desperation, loading policy from %s\n",
                        policyName);
            }
            checked += " ";
            checked += policyName;
            ok = config.fromConfigFile(policyName);
        }
        */
        if (!ok) {
            if (!quiet) {
                fprintf(RTARGET,"||| failed to load policy from%s\n",
                        checked.c_str());
            }
            return false;
        }

        // currently only support "capability" style configuration
        if (config.check("style",Value("")).asString()!="capability") {
            if (!quiet) {
                fprintf(RTARGET,"||| policy \"style\" can currently only be \"capability\"\n");
            }
            return false;
        }

        return true;
    }