void
MetricsRequestHandler::fillInDescription(const YarpString & request,
yarp::os::Property & info)
{
ODL_OBJENTER(); //####
ODL_S1s("request = ", request); //####
ODL_P1("info = ", &info); //####
try
{
info.put(MpM_REQREP_DICT_REQUEST_KEY_, request);
info.put(MpM_REQREP_DICT_OUTPUT_KEY_, MpM_REQREP_LIST_START_ MpM_REQREP_DICT_START_
MpM_REQREP_DICT_END_ MpM_REQREP_1_OR_MORE_ MpM_REQREP_LIST_END_);
info.put(MpM_REQREP_DICT_VERSION_KEY_, METRICS_REQUEST_VERSION_NUMBER_);
info.put(MpM_REQREP_DICT_DETAILS_KEY_, T_("Return the measurements for the channels of the "
"service\n"
"Input: nothing\n"
"Output: a list of dictionaries containing "
"measurements for the service channels"));
yarp::os::Value keywords;
yarp::os::Bottle * asList = keywords.asList();
asList->addString(request);
info.put(MpM_REQREP_DICT_KEYWORDS_KEY_, keywords);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT(); //####
} // MetricsRequestHandler::fillInDescription
void Prop::scan_xml(const std::string& txt, yarp::os::Property& data) {
data.clear();
string line;
for (int i=0; i<(int)txt.size(); i++) {
char ch = txt[i];
if (ch=='\"') {
line += " ";
continue;
}
if (ch!='\r'&&ch!='\n') {
line += ch;
continue;
}
Bottle b(line.c_str());
string var = "dud";
for (int j=1; j<b.size(); j++) {
if (b.get(j-1).asString()=="id=") {
var = b.get(j).asString();
continue;
}
if (b.get(j-1).asString()=="import=") {
data.put(var.c_str(),b.get(j));
}
}
}
}
void
WhereRequestHandler::fillInDescription(const YarpString & request,
yarp::os::Property & info)
{
ODL_OBJENTER(); //####
ODL_S1s("request = ", request); //####
ODL_P1("info = ", &info); //####
try
{
info.put(MpM_REQREP_DICT_REQUEST_KEY_, request);
info.put(MpM_REQREP_DICT_VERSION_KEY_, WHERE_REQUEST_VERSION_NUMBER_);
info.put(MpM_REQREP_DICT_DETAILS_KEY_, T_("Return the remembered IP address and port\n"
"Input: nothing\n"
"Output: the remembered IP address and port"));
yarp::os::Value keywords;
yarp::os::Bottle * asList = keywords.asList();
asList->addString(request);
info.put(MpM_REQREP_DICT_KEYWORDS_KEY_, keywords);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT(); //####
} // WhereRequestHandler::fillInDescription
void
StartSumRequestHandler::fillInDescription(const YarpString & request,
yarp::os::Property & info)
{
ODL_OBJENTER(); //####
ODL_S1s("request = ", request); //####
ODL_P1("info = ", &info); //####
try
{
info.put(MpM_REQREP_DICT_REQUEST_KEY_, request);
info.put(MpM_REQREP_DICT_VERSION_KEY_, STARTSUM_REQUEST_VERSION_NUMBER_);
info.put(MpM_REQREP_DICT_DETAILS_KEY_, T_("Start the running sum\n"
"Input: nothing\n"
"Output: nothing"));
yarp::os::Value keywords;
yarp::os::Bottle * asList = keywords.asList();
asList->addString(request);
info.put(MpM_REQREP_DICT_KEYWORDS_KEY_, keywords);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT(); //####
} // StartSumRequestHandler::fillInDescription
bool PortMonitor::configureFromProperty(yarp::os::Property& options) {
if(binder) delete binder;
binder = nullptr;
std::string script = options.check("type", Value("lua")).asString();
std::string filename = options.check("file", Value("modifier")).asString();
std::string constraint = options.check("constraint", Value("")).asString();
// context is used to find the script files
std::string context = options.check("context", Value("")).asString();
// check which monitor should be used
if((binder = MonitorBinding::create(script.c_str())) == nullptr)
{
yError(R"(Currently only 'lua' script and 'dll' object is supported by portmonitor)");
return false;
}
// set the acceptance constraint
binder->setAcceptConstraint(constraint.c_str());
std::string strFile = filename;
if(script != "dll")
{
yarp::os::ResourceFinder rf;
rf.setDefaultContext(context.c_str());
rf.configure(0, nullptr);
strFile = rf.findFile(filename);
if(strFile == "")
strFile = rf.findFile(filename+".lua");
}
bool CameraTest::setup(yarp::os::Property& property) {
if(property.check("name"))
setName(property.find("name").asString());
// updating parameters
RTF_ASSERT_ERROR_IF(property.check("portname"),
"The portname must be given as the test paramter!");
cameraPortName = property.find("portname").asString();
measure_time = property.check("measure_time") ? property.find("measure_time").asInt() : TIMES;
expected_frequency = property.check("expected_frequency") ? property.find("expected_frequency").asInt() : FREQUENCY;
tolerance = property.check("tolerance") ? property.find("tolerance").asInt() : TOLERANCE;
// opening port
RTF_ASSERT_ERROR_IF(port.open("/CameraTest/image:i"),
"opening port, is YARP network available?");
RTF_TEST_REPORT(Asserter::format("Listening to camera for %d seconds",
measure_time));
// connecting
RTF_TEST_REPORT(Asserter::format("connecting from %s to %s",
port.getName().c_str(), cameraPortName.c_str()));
RTF_ASSERT_ERROR_IF(Network::connect(cameraPortName, port.getName()),
"could not connect to remote port, camera unavailable");
return true;
}
bool GazeboYarpJointSensorsDriver::setJointSensorsType(yarp::os::Property & pluginParameters) //WORKS
{
std::cout << ".ini file found, using joint names in ini file" << std::endl;
std::string parameter_name = "gazeboJointSensorsType";
if(!pluginParameters.check(parameter_name.c_str())) {
std::cout << "GazeboYarpJointSensorsDriver::setJointSensorsType() error: cannot find " << parameter_name << " parameter." << std::endl;
return false;
}
std::string sensors_type = pluginParameters.find(parameter_name.c_str()).asString().c_str();
if( sensors_type == "position" ) {
jointsensors_type = Position;
} else if ( sensors_type == "speed" ) {
jointsensors_type = Speed;
} else if ( sensors_type == "torque" ) {
jointsensors_type = Torque;
} else {
std::cerr << "GazeboYarpJointSensorsDriver::setJointSensorsType() error: sensor type " << sensors_type << " not recognized." << std::endl
<< "\t\tThe available types are position, speed and torque." << std::endl;
return false;
}
return true;
}
void addVectorOfStringToProperty(yarp::os::Property& prop, std::string key, std::vector<std::string> & list)
{
prop.addGroup(key);
yarp::os::Bottle & bot = prop.findGroup(key).addList();
for(size_t i=0; i < list.size(); i++)
{
bot.addString(list[i].c_str());
}
return;
}
bool yarpWholeBodyInterface::getYarpWbiProperties(
yarp::os::Property& yarp_wbi_properties)
{
yarp::os::Property buffer;
actuatorInt->getYarpWbiProperties(buffer);
yarp_wbi_properties.fromString(buffer.toString(), false);
stateInt->getYarpWbiProperties(buffer);
yarp_wbi_properties.fromString(buffer.toString(), false);
modelInt->getYarpWbiProperties(buffer);
yarp_wbi_properties.fromString(buffer.toString(), false);
return true;
}
void selectiveAttentionModule::setOptions(yarp::os::Property opt){
//options =opt;
// definition of the name of the module
ConstString name=opt.find("name").asString();
if(name!=""){
printf("||| Module named as :%s \n", name.c_str());
this->setName(name.c_str());
}
ConstString value=opt.find("mode").asString();
if(value!=""){
}
}
bool ExampleTest::setup(yarp::os::Property &property) {
// initialization goes here ...
//updating the test name
if(property.check("name"))
setName(property.find("name").asString());
string example = property.check("example", Value("default value")).asString();
RTF_TEST_REPORT(Asserter::format("Use '%s' for the example param!",
example.c_str()));
return true;
}
bool OpenLoopConsistency::setup(yarp::os::Property& property) {
// updating parameters
RTF_ASSERT_ERROR_IF(property.check("robot"), "The robot name must be given as the test parameter!");
RTF_ASSERT_ERROR_IF(property.check("part"), "The part name must be given as the test parameter!");
RTF_ASSERT_ERROR_IF(property.check("joints"), "The joints list must be given as the test parameter!");
RTF_ASSERT_ERROR_IF(property.check("zero"), "The zero position must be given as the test parameter!");
robotName = property.find("robot").asString();
partName = property.find("part").asString();
zero = property.find("zero").asDouble();
Bottle* jointsBottle = property.find("joints").asList();
RTF_ASSERT_ERROR_IF(jointsBottle!=0,"unable to parse joints parameter");
n_cmd_joints = jointsBottle->size();
RTF_ASSERT_ERROR_IF(n_cmd_joints>0,"invalid number of joints, it must be >0");
Property options;
options.put("device", "remote_controlboard");
options.put("remote", "/"+robotName+"/"+partName);
options.put("local", "/OpenLoopConsistencyTest/"+robotName+"/"+partName);
dd = new PolyDriver(options);
RTF_ASSERT_ERROR_IF(dd->isValid(),"Unable to open device driver");
RTF_ASSERT_ERROR_IF(dd->view(iopl),"Unable to open openloop interface");
RTF_ASSERT_ERROR_IF(dd->view(ienc),"Unable to open encoders interface");
RTF_ASSERT_ERROR_IF(dd->view(iamp),"Unable to open ampliefier interface");
RTF_ASSERT_ERROR_IF(dd->view(ipos),"Unable to open position interface");
RTF_ASSERT_ERROR_IF(dd->view(icmd),"Unable to open control mode interface");
RTF_ASSERT_ERROR_IF(dd->view(iimd),"Unable to open interaction mode interface");
if (!ienc->getAxes(&n_part_joints))
{
RTF_ASSERT_ERROR("unable to get the number of joints of the part");
}
if (n_part_joints<=0)
RTF_ASSERT_ERROR("Error this part has in invalid (<=0) number of jonits");
else if (jointsBottle->size() == 1)
cmd_mode=single_joint;
else if (jointsBottle->size() < n_part_joints)
cmd_mode=some_joints;
else if (jointsBottle->size() == n_part_joints)
cmd_mode=all_joints;
else
RTF_ASSERT_ERROR("invalid joint selection?");
cmd_tot = new double[n_part_joints];
pos_tot=new double[n_part_joints];
jointsList=new int[n_cmd_joints];
cmd_some=new double[n_cmd_joints];
prevcurr_tot=new double[n_part_joints];
prevcurr_some=new double[n_cmd_joints];
for (int i=0; i <n_cmd_joints; i++) jointsList[i]=jointsBottle->get(i).asInt();
return true;
}
bool NetworkProfiler::setPortmonitorParams(std::string portName, yarp::os::Property& param) {
//e.g., set in "/view" (log_raw 1)"
yarp::os::Bottle cmd, reply;
cmd.addString("set");
cmd.addString("in");
cmd.addString(portName.c_str());
Bottle tmp;
tmp.fromString(param.toString());
cmd.add(tmp.get(0));
Contact srcCon = Contact::fromString(portName);
bool ret = yarp::os::NetworkBase::write(srcCon, cmd, reply, true, true, 2.0);
if(!ret) {
yError()<<"Cannot write to"<<portName;
return false;
}
if(reply.size() > 1) {
if(reply.get(0).isString() && reply.get(0).asString() == "fail") {
yError()<<reply.toString();
return false;
}
else if(reply.get(0).isInt() && reply.get(0).asInt() == -1) {
yError()<<reply.toString();
return false;
}
}
return true;
}
void SkinGroup::initOutport(yarp::os::Property &config)
{
std::string outPortName = config.check("out_port", yarp::os::Value("/outport"), "").asString().c_str();
std::cout << "Out Port name ==>" << outPortName << std::endl;
classificationOutport.open(outPortName.c_str());
}
bool SimpleMonitorObject::create(const yarp::os::Property& options)
{
yDebug("created!\n");
yDebug("I am attached to the %s\n",
(options.find("sender_side").asBool()) ? "sender side" : "receiver side");
return true;
}
bool GazeboYarpContactLoadCellArrayDriver::initLinkAssociatedToContactSensor(yarp::os::Property &pluginParameters)
{
m_linkAssociateToSensor = pluginParameters.find("linkName").asString();
const gazebo::physics::Link_V &gazeboModelLinks = m_robot->GetLinks();
std::string linkNameScopedEnding = "::" + m_linkAssociateToSensor;
bool linkFound = false;
for (size_t gazeboLink = 0; gazeboLink < gazeboModelLinks.size(); gazeboLink++)
{
std::string gazeboLinkName = gazeboModelLinks[gazeboLink]->GetScopedName();
if (GazeboYarpPlugins::hasEnding(gazeboLinkName, linkNameScopedEnding))
{
linkFound = true;
m_sensorLink = boost::get_pointer(gazeboModelLinks[gazeboLink]);
break;
}
}
if (!linkFound)
{
yError() << "GazeboYarpContactLoadCellArrayDriver: initContactSensor(): Link associated to sensor not found";
return false;
}
return true;
}
void Prop::scan_hx(const std::string& txt, yarp::os::Property& data) {
data.clear();
string line;
for (int i=0; i<(int)txt.size(); i++) {
char ch = txt[i];
if (ch=='\r'||ch=='\n') {
line = "";
continue;
}
if (ch=='[') {
line += '(';
continue;
}
if (ch==']') {
line += ')';
continue;
}
if (ch==',') {
line += ' ';
continue;
}
if (ch!=';') {
line += ch;
continue;
}
Bottle b(line.c_str());
string var = "dud";
for (int j=1; j<b.size(); j++) {
if (b.get(j-1).asString()=="var") {
var = b.get(j).asString();
continue;
}
if (b.get(j-1).asString()=="=") {
Value v = b.get(j);
ConstString s = v.asString();
if (s=="true") {
data.put(var.c_str(),1);
} else if (s=="false") {
data.put(var.c_str(),0);
} else {
data.put(var.c_str(),v);
}
}
}
}
}
bool PortMonitor::configureFromProperty(yarp::os::Property& options) {
if(binder) delete binder;
binder = NULL;
ConstString script = options.check("type", Value("lua")).asString();
ConstString filename = options.check("file", Value("modifier")).asString();
ConstString constraint = options.check("constraint", Value("")).asString();
// context is used to find the script files
ConstString context = options.check("context", Value("")).asString();
// check which monitor should be used
if((binder = MonitorBinding::create(script.c_str())) == NULL)
{
yError("Currently only \'lua\' script and \'dll\' object is supported by portmonitor");
return false;
}
// set the acceptance constraint
binder->setAcceptConstraint(constraint.c_str());
ConstString strFile = filename;
if(script != "dll")
{
yarp::os::ResourceFinder rf;
rf.setDefaultContext(context.c_str());
rf.configure(0, NULL);
strFile = rf.findFile(filename.c_str());
if(strFile == "")
strFile = rf.findFile(filename+".lua");
}
// provide some useful information for the monitor object
// which can be accessed in the create() callback.
Property info;
info.clear();
info.put("filename", strFile);
info.put("type", script);
info.put("source", options.find("source").asString());
info.put("destination", options.find("destination").asString());
info.put("sender_side", options.find("sender_side").asInt());
info.put("receiver_side",options.find("receiver_side").asInt());
info.put("carrier", options.find("carrier").asString());
PortMonitor::lock();
bReady = binder->load(info);
PortMonitor::unlock();
return bReady;
return false;
}
bool GazeboYarpContactLoadCellArrayDriver::configure(yarp::os::Property& pluginParams)
{
// Get load cell locations with respect to the CG of the link (body1)
yarp::os::Bottle *loadCellNames = pluginParams.find("loadCellNames").asList();
if (loadCellNames->size() == 0)
{
yError() << "GazeboYarpContactLoadCellArrayDriver: Error parsing parameters: \"loadCellNames\" should be followed by list";
return false;
}
yarp::os::Bottle *loadCellX = pluginParams.find("loadCellX").asList();
yarp::os::Bottle *loadCellY = pluginParams.find("loadCellY").asList();
yarp::os::Bottle *loadCellZ = pluginParams.find("loadCellZ").asList();
if (loadCellX->size() == 0 || loadCellY->size() == 0 || loadCellZ->size() == 0)
{
yError() << "GazeboYarpContactLoadCellArrayDriver: Error parsing parameters: \"loadCellX , ..Y, ..Z\" should be followed by list";
return false;
}
if (loadCellX->size() != loadCellNames->size() || loadCellY->size() != loadCellNames->size() || loadCellZ->size() != loadCellNames->size())
{
yError() << "GazeboYarpContactLoadCellArrayDriver: Error parsing parameters: \"loadCellX , ..Y, ..Z\" should be the same size as \"loadCellNames\"";
return false;
}
for (size_t i = 0; i < loadCellNames->size(); i++)
{
yarp::sig::Vector loadCellLoc(3);
loadCellLoc.clear();
loadCellLoc.push_back(loadCellX->get(i).asDouble());
loadCellLoc.push_back(loadCellY->get(i).asDouble());
loadCellLoc.push_back(loadCellZ->get(i).asDouble());
this->m_loadCellLocations.push_back(loadCellLoc);
}
m_contactNormalForces.resize(m_loadCellLocations.size());
if (!this->prepareMappingMatrix())
{
return false;
}
return true;
}
bool ZfpMonitorObject::create(const yarp::os::Property& options)
{
shouldCompress = (options.find("sender_side").asBool());
compressed=NULL;
decompressed=NULL;
buffer=NULL;
sizeToAllocate=0;
sizeToAllocateB=0;
return true;
}
bool SkinGroup::initSensors(yarp::os::Property &config)
{
yarp::os::Bottle skinfiles = config.findGroup("parts").tail();
yarp::os::ConstString skinFilePath = config.check("skinsensorpath", yarp::os::Value("C:/roboskin/software_yarp_related/iCub/app/kaspar/conf"), "specifiy skinsensor config file path").asString();
// yarp::os::ConstString skinFilePath = config.check("skinsensorfilepath", yarp::os::Value("C:/roboskin/software_yarp_related/iCub/app/kaspar/conf"), "specifiy skinsensor config file path").asString();
printf("==== %s %d \n", skinFilePath.c_str(), skinfiles.size());
for(int i = 0; i < skinfiles.size(); i++)
{
yarp::os::Property skinproperty;
yarp::os::Bottle sensorConf(skinfiles.get(i).toString());
if(skinproperty.fromConfigFile((skinFilePath + FILESEPARATOR + sensorConf.get(0).asString())))
{
// put new attribute ifLog to the property before initialising skin sensors
//if(ifLogData)
//{
// skinproperty.put("logdata", ifLogData);
// skinproperty.put("logfilepath", logpath.c_str());
//}
SkinBodyPart *sk = new SkinBodyPart(skinproperty); // assuming property file is correct. No error checking and return. so be very careful
sk->setPortToConnect(sensorConf.get(1).asString());
if(sk->initConfiguration())
{
this->skinParts.push_back(sk);
}
else
{
delete sk;
return false;
}
}
else
{
fprintf(stderr, "Error loading skin sensor config file %s\n", sensorConf.get(0).asString().c_str());
return false;
}
}
return true;
}
void
MatchRequestHandler::fillInDescription(const YarpString & request,
yarp::os::Property & info)
{
ODL_OBJENTER(); //####
ODL_S1s("request = ", request); //####
ODL_P1("info = ", &info); //####
try
{
info.put(MpM_REQREP_DICT_REQUEST_KEY_, request);
info.put(MpM_REQREP_DICT_INPUT_KEY_, MpM_REQREP_INT_ MpM_REQREP_STRING_);
info.put(MpM_REQREP_DICT_OUTPUT_KEY_, MpM_REQREP_LIST_START_ MpM_REQREP_STRING_
MpM_REQREP_0_OR_MORE_ MpM_REQREP_LIST_END_);
info.put(MpM_REQREP_DICT_VERSION_KEY_, MATCH_REQUEST_VERSION_NUMBER_);
info.put(MpM_REQREP_DICT_DETAILS_KEY_, T_("Find a matching service\n"
"Input: an integer (1=return names, 0=return "
"ports) and an expression describing the service "
"to be found\n"
"Output: OK and a list of matching service "
"names/ports or FAILED, with a description of "
"the problem encountered"));
yarp::os::Value keywords;
yarp::os::Bottle * asList = keywords.asList();
asList->addString(request);
asList->addString("find");
info.put(MpM_REQREP_DICT_KEYWORDS_KEY_, keywords);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT(); //####
} // MatchRequestHandler::fillInDescription
void
RGBLEDRequestHandler::fillInDescription(const YarpString & request,
yarp::os::Property & info)
{
ODL_OBJENTER(); //####
ODL_S1s("request = ", request); //####
ODL_P1("info = ", &info); //####
try
{
info.put(MpM_REQREP_DICT_REQUEST_KEY_, request);
info.put(MpM_REQREP_DICT_INPUT_KEY_, MpM_REQREP_ANYTHING_ MpM_REQREP_0_OR_MORE_);
info.put(MpM_REQREP_DICT_OUTPUT_KEY_, MpM_REQREP_ANYTHING_ MpM_REQREP_0_OR_MORE_);
info.put(MpM_REQREP_DICT_VERSION_KEY_, RGBLED_REQUEST_VERSION_NUMBER_);
info.put(MpM_REQREP_DICT_DETAILS_KEY_, T_("Echo back any input\n"
"Input: R G B (floats between 0.0 and 1.0) for "
"colour values"
"Output: 1 if ok"));
yarp::os::Value keywords;
yarp::os::Bottle * asList = keywords.asList();
asList->addString(request);
info.put(MpM_REQREP_DICT_KEYWORDS_KEY_, keywords);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT(); //####
} // RGBLEDRequestHandler::fillInDescription
void
RandomRequestHandler::fillInDescription(const YarpString & request,
yarp::os::Property & info)
{
ODL_OBJENTER(); //####
ODL_S1s("request = ", request); //####
ODL_P1("info = ", &info); //####
try
{
info.put(MpM_REQREP_DICT_REQUEST_KEY_, request);
info.put(MpM_REQREP_DICT_INPUT_KEY_, MpM_REQREP_INT_ MpM_REQREP_0_OR_1_);
info.put(MpM_REQREP_DICT_OUTPUT_KEY_, MpM_REQREP_DOUBLE_ MpM_REQREP_1_OR_MORE_);
info.put(MpM_REQREP_DICT_VERSION_KEY_, RANDOM_REQUEST_VERSION_NUMBER_);
info.put(MpM_REQREP_DICT_DETAILS_KEY_, T_("Generate one or more random numbers\n"
"Input: the number of random values to generate\n"
"Output one or more random numbers per request"));
yarp::os::Value keywords;
yarp::os::Bottle * asList = keywords.asList();
asList->addString(request);
info.put(MpM_REQREP_DICT_KEYWORDS_KEY_, keywords);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT(); //####
} // RandomRequestHandler::fillInDescription
void
PingRequestHandler::fillInDescription(const YarpString & request,
yarp::os::Property & info)
{
ODL_OBJENTER(); //####
ODL_S1s("request = ", request); //####
ODL_P1("info = ", &info); //####
try
{
info.put(MpM_REQREP_DICT_REQUEST_KEY_, request);
info.put(MpM_REQREP_DICT_INPUT_KEY_, MpM_REQREP_STRING_);
info.put(MpM_REQREP_DICT_OUTPUT_KEY_, MpM_REQREP_STRING_);
info.put(MpM_REQREP_DICT_VERSION_KEY_, PING_REQUEST_VERSION_NUMBER_);
info.put(MpM_REQREP_DICT_DETAILS_KEY_, T_("Update the last-pinged time for a service or "
"re-register it\n"
"Input: the channel used by the service\n"
"Output: OK or FAILED, with a description of the "
"problem encountered"));
yarp::os::Value keywords;
yarp::os::Bottle * asList = keywords.asList();
asList->addString(request);
info.put(MpM_REQREP_DICT_KEYWORDS_KEY_, keywords);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT(); //####
} // PingRequestHandler::fillInDescription
void
Test16EchoRequestHandler::fillInDescription(const YarpString & request,
yarp::os::Property & info)
{
ODL_OBJENTER(); //####
ODL_S1s("request = ", request); //####
ODL_P1("info = ", &info); //####
try
{
info.put(MpM_REQREP_DICT_REQUEST_KEY_, request);
info.put(MpM_REQREP_DICT_INPUT_KEY_, MpM_REQREP_ANYTHING_ MpM_REQREP_0_OR_MORE_);
info.put(MpM_REQREP_DICT_OUTPUT_KEY_, MpM_REQREP_ANYTHING_ MpM_REQREP_0_OR_MORE_);
info.put(MpM_REQREP_DICT_VERSION_KEY_, ECHO_REQUEST_VERSION_NUMBER_);
info.put(MpM_REQREP_DICT_DETAILS_KEY_, "Echo back any input");
yarp::os::Value keywords;
yarp::os::Bottle * asList = keywords.asList();
asList->addString(request);
asList->addString("blorg");
asList->addString("blirg");
info.put(MpM_REQREP_DICT_KEYWORDS_KEY_, keywords);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT(); //####
} // Test16EchoRequestHandler::fillInDescription
// *********************************************************************************************************************
// *********************************************************************************************************************
// ICUB WHOLE BODY DYNAMICS ESTIMATOR
// *********************************************************************************************************************
// *********************************************************************************************************************
ExternalWrenchesAndTorquesEstimator::ExternalWrenchesAndTorquesEstimator(int _period,
yarpWholeBodySensors *_sensors,
yarp::os::BufferedPort<iCub::skinDynLib::skinContactList> * _port_skin_contacts,
yarp::os::Property & _wbi_yarp_conf
)
: periodInMilliSeconds(_period),
sensors(_sensors),
port_skin_contacts(_port_skin_contacts),
enable_omega_domega_IMU(false),
min_taxel(0),
wbi_yarp_conf(_wbi_yarp_conf),
assume_fixed_base_from_odometry(false)
{
resizeAll(sensors->getSensorNumber(SENSOR_ENCODER));
resizeFTs(sensors->getSensorNumber(SENSOR_FORCE_TORQUE));
resizeIMUs(sensors->getSensorNumber(SENSOR_IMU));
///< Skin timestamp
last_reading_skin_contact_list_Stamp = -1000.0;
if( _wbi_yarp_conf.check("fixed_base") )
{
assume_fixed_base = true;
fixed_link = _wbi_yarp_conf.find("fixed_base").asString();
}
else
{
assume_fixed_base = false;
}
if( _wbi_yarp_conf.check("assume_fixed_from_odometry") )
{
this->assume_fixed_base_from_odometry = true;
}
}
// Iif a subdevice parameter is given to the wrapper, it will open it as well
// and and attach to it immediatly.
bool yarp::dev::ServerInertial::openAndAttachSubDevice(yarp::os::Property& prop)
{
yarp::os::Value &subdevice = prop.find("subdevice");
IMU_polydriver = new yarp::dev::PolyDriver;
// yDebug("Subdevice %s\n", subdevice.toString().c_str());
if (subdevice.isString())
{
// maybe user isn't doing nested configuration
yarp::os::Property p;
p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
p.fromString(prop.toString());
p.put("device",subdevice.toString());
IMU_polydriver->open(p);
}
else
IMU_polydriver->open(subdevice);
if (!IMU_polydriver->isValid())
{
yError("cannot create device <%s>\n", subdevice.toString().c_str());
return false;
}
// if we are here, poly is valid
IMU_polydriver->view(IMU); // attach to subdevice
if(IMU == NULL)
{
yError("Error, subdevice <%s> has no valid IMU interface\n", subdevice.toString().c_str());
IMU_polydriver->close();
return false;
}
// iTimed interface
IMU_polydriver->view(iTimed); // not mandatory
return true;
}
bool Implement_RgbVisualParams_Sender::getRgbIntrinsicParam(yarp::os::Property &intrinsic)
{
yarp::os::Bottle cmd, response;
cmd.addVocab(VOCAB_RGB_VISUAL_PARAMS);
cmd.addVocab(VOCAB_GET);
cmd.addVocab(VOCAB_INTRINSIC_PARAM);
_port.write(cmd, response);
// Minimal check on response, we suppose the response is always correctly formatted
if((response.get(0).asVocab()) == VOCAB_FAILED)
{
intrinsic.clear();
return false;
}
return Property::copyPortable(response.get(3), intrinsic); // will it really work??
}
bool GazeboYarpJointSensorsDriver::setJointPointers(yarp::os::Property & pluginParameters) //WORKS
{
std::cout << ".ini file found, using joint names in ini file" << std::endl;
yarp::os::Bottle joint_names_bottle = pluginParameters.findGroup("jointNames");
if (joint_names_bottle.isNull()) {
std::cout << "GazeboYarpJointSensorsDriver::setJointPointers() error: cannot find jointNames parameter." << std::endl;
return false;
}
jointsensors_nr_of_channels = joint_names_bottle.size() - 1;
if (jointsensors_nr_of_channels == 0) {
std::cout << "GazeboYarpJointSensorsDriver::setJointPointers() error: no joint selected." << std::endl;
return false;
}
joint_ptrs.resize(jointsensors_nr_of_channels);
const gazebo::physics::Joint_V & gazebo_models_joints = _robot->GetJoints();
for(unsigned int i=0; i < joint_ptrs.size(); i++ ) {
bool joint_found = false;
std::string controlboard_joint_name(joint_names_bottle.get(i+1).asString().c_str());
std::string controlboard_joint_name_scoped_ending = "::" + controlboard_joint_name;
for(unsigned int gazebo_joint = 0; gazebo_joint < gazebo_models_joints.size() && !joint_found; gazebo_joint++ ) {
std::string gazebo_joint_name = gazebo_models_joints[gazebo_joint]->GetScopedName();
if( GazeboYarpPlugins::hasEnding(gazebo_joint_name,controlboard_joint_name_scoped_ending) ) {
joint_found = true;
joint_ptrs[i] = boost::get_pointer(gazebo_models_joints[gazebo_joint]);
}
}
if( !joint_found ) {
std::cout << "GazeboYarpJointSensorsDriver::setJointPointers(): Error, cannot find joint " << controlboard_joint_name << std::endl;
joint_ptrs.resize(0);
jointsensors_nr_of_channels = 0;
return false;
}
}
return true;
}
