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;
}
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;
}
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++)
{
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;
}
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;
}
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;
}