int merge(yarp::os::Bottle& mergeArg, folderType fType, bool verbose)
{
ConstString contextName;
if (mergeArg.size() >1 )
contextName=mergeArg.get(1).asString().c_str();
if (contextName=="")
{
printf("No %s name provided\n", fType==CONTEXTS ? "context" : "robot");
return 0;
}
yarp::os::ResourceFinder rf;
rf.setVerbose(verbose);
if (mergeArg.size() >2 )
{
for (int i=2; i<mergeArg.size(); ++i)
{
ConstString fileName=mergeArg.get(i).asString();
if(fileName != "")
{
ResourceFinderOptions opts;
opts.searchLocations=ResourceFinderOptions::User;
ConstString userFileName=rf.findPath((getFolderStringName(fType) + PATH_SEPARATOR +contextName + PATH_SEPARATOR + fileName).c_str(), opts);
ConstString hiddenFileName=rf.findPath((getFolderStringNameHidden(fType) + PATH_SEPARATOR +contextName+ PATH_SEPARATOR + fileName).c_str(), opts);
opts.searchLocations=ResourceFinderOptions::Installed;
ConstString installedFileName=rf.findPath((getFolderStringName(fType) + PATH_SEPARATOR +contextName+ PATH_SEPARATOR + fileName).c_str(), opts);
if (userFileName!="" && hiddenFileName != "" && installedFileName !="")
fileMerge(installedFileName, userFileName, hiddenFileName);
else if (userFileName!="" && installedFileName !="")
printf("Need to use mergetool\n");
else
printf("Could not merge file %s\n", fileName.c_str());
}
}
}
else
{
ResourceFinderOptions opts;
opts.searchLocations=ResourceFinderOptions::User;
ConstString userPath=rf.findPath((getFolderStringName(fType) + PATH_SEPARATOR +contextName).c_str(), opts);
ConstString hiddenUserPath=rf.findPath((getFolderStringNameHidden(fType) + PATH_SEPARATOR +contextName).c_str(), opts);
opts.searchLocations=ResourceFinderOptions::Installed;
ConstString installedPath=rf.findPath((getFolderStringName(fType) + PATH_SEPARATOR +contextName).c_str(), opts);
recursiveMerge(installedPath, userPath, hiddenUserPath);
}
return 0;
}
/*! @brief Convert a YARP list into a %JavaScript object.
@param[in] jct The %JavaScript engine context.
@param[in,out] theData The output object.
@param[in] inputValue The value to be processed. */
static void
convertList(JSContext * jct,
JS::MutableHandleValue theData,
const yarp::os::Bottle & inputValue)
{
ODL_ENTER(); //####
ODL_P2("jct = ", jct, "inputValue = ", &inputValue); //####
JSObject * valueArray = JS_NewArrayObject(jct, 0);
if (valueArray)
{
JS::RootedObject arrayRooted(jct);
JS::RootedValue anElement(jct);
JS::RootedId aRootedId(jct);
arrayRooted = valueArray;
for (int ii = 0, mm = inputValue.size(); mm > ii; ++ii)
{
yarp::os::Value aValue(inputValue.get(ii));
convertValue(jct, &anElement, aValue);
if (JS_IndexToId(jct, ii, &aRootedId))
{
JS_SetPropertyById(jct, arrayRooted, aRootedId, anElement);
}
}
theData.setObject(*valueArray);
}
ODL_EXIT(); //####
} // convertList
bool
RandomRequestHandler::processRequest(const YarpString & request,
const yarp::os::Bottle & restOfInput,
const YarpString & senderChannel,
yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
# pragma unused(request,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
ODL_OBJENTER(); //####
ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
"senderChannel = ", senderChannel); //####
ODL_P1("replyMechanism = ", replyMechanism); //####
bool result = true;
try
{
int count;
_response.clear();
if (0 < restOfInput.size())
{
yarp::os::Value number(restOfInput.get(0));
if (number.isInt())
{
count = number.asInt();
}
else
{
count = -1;
}
}
else
{
count = 1;
}
if (count > 0)
{
for (int ii = 0; ii < count; ++ii)
{
_response.addDouble(yarp::os::Random::uniform());
}
}
else
{
ODL_LOG("! (count > 0)"); //####
}
sendResponse(replyMechanism);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT_B(result); //####
return result;
} // RandomRequestHandler::processRequest
void WorldRpcInterface::setRot( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
if ((command.size() - n) == 3) {
double x = command.get(n).asDouble()*M_PI/180.; n++; //std::cout << x << std::endl; // x position
double y = command.get(n).asDouble()*M_PI/180.; n++; //std::cout << y << std::endl; // y position
double z = command.get(n).asDouble()*M_PI/180.; n++; //std::cout << z << std::endl; // z position
object->setCartesianOrientation( QVector3D(x,y,z) );
reply.addString("Set rotation (about x,y,z in degrees).");
} else {
// replace rotation part of object's T matrix
QMatrix4x4 rt = object->getT();
for (int i = 0; i<3; i++) {
for (int j = 0; j<3; j++) {
rt(i, j) = command.get(n).asDouble(); n++;
}
}
object->setT(rt);
reply.addString("Set full rotation matrix.");
}
}
}
bool skinManager::bottleToVector(const yarp::os::Bottle& b, yarp::sig::Vector& v){
for(int i=0; i<b.size(); i++)
if(b.get(i).isDouble() || b.get(i).isInt())
v.push_back(b.get(i).asDouble());
else
return false;
return true;
}
void wysiwyd::wrdac::SubSystem_Reactable::SendOSC(yarp::os::Bottle &oscMsg)
{
yarp::os::Bottle cmd;
cmd.addString("osc");
for(int i=0; i<oscMsg.size(); i++)
cmd.add(oscMsg.get(i));
std::cout<<"OSC>>"<<cmd.toString().c_str()<<std::endl;
portRTrpc.write(cmd);
}
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);
}
}
}
bool
TruncateFloatFilterInputHandler::handleInput(const yarp::os::Bottle & input,
const YarpString & senderChannel,
yarp::os::ConnectionWriter * replyMechanism,
const size_t numBytes)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
# pragma unused(senderChannel,replyMechanism,numBytes)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
ODL_OBJENTER(); //####
ODL_S2s("senderChannel = ", senderChannel, "got ", input.toString()); //####
ODL_P1("replyMechanism = ", replyMechanism); //####
ODL_I1("numBytes = ", numBytes); //####
bool result = true;
try
{
yarp::os::Bottle outBottle;
for (int ii = 0, mm = input.size(); mm > ii; ++ii)
{
yarp::os::Value aValue(input.get(ii));
if (aValue.isInt())
{
outBottle.addInt(aValue.asInt());
}
else if (aValue.isDouble())
{
outBottle.addInt(static_cast<int>(aValue.asDouble()));
}
}
if ((0 < outBottle.size()) && _outChannel)
{
if (! _outChannel->write(outBottle))
{
ODL_LOG("(! _outChannel->write(message))"); //####
#if defined(MpM_StallOnSendProblem)
Stall();
#endif // defined(MpM_StallOnSendProblem)
}
}
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT_B(result); //####
return result;
} // TruncateFloatFilterInputHandler::handleInput
/* Respond function */
bool opcManager::respond(const yarp::os::Bottle& bCommand, yarp::os::Bottle& bReply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"connect + name\n" +
"quit \n";
bReply.clear();
string keyWord = bCommand.get(0).asString().c_str();
if (keyWord == "quit") {
bReply.addString("quitting");
return false;
}
else if (keyWord == "help") {
cout << helpMessage;
bReply.addString("ok");
}
else if (keyWord == "connect") {
bReply = connect(bCommand);
}
else if (keyWord == "updateBeliefs") {
bReply.addString("nack");
if (bCommand.size() == 2)
{
if (bCommand.get(1).isString())
{
bReply.addList() = updateBelief(bCommand.get(1).toString().c_str());
}
}
}
else if (keyWord == "synchronise")
{
bReply.addString("ack");
bReply.addList() = synchoniseOPCs();
}
else if (keyWord == "executeActivity")
{
bReply.addString("ack");
bReply.addList() = simulateActivity(bCommand);
}
else if (keyWord == "diffOPC")
{
bReply.addString("ack");
bReply.addList() = diffOPC();
}
return true;
}
bool GaussianAE::decode(const yarp::os::Bottle &packet, size_t &pos)
{
if (LabelledAE::decode(packet, pos) && pos + 3 <= packet.size())
{
_gaei[0] = packet.get(pos++).asInt();
_gaei[1] = packet.get(pos++).asInt();
_gaei[2] = packet.get(pos++).asInt();
return true;
}
return false;
}
void WorldRpcInterface::startSimSyncer(const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n) {
if ((command.size() - n) != 1) {
reply.addString("Please provide the refresh period for the synchronization thread in seconds");
return;
}
double period = command.get(n).asDouble(); n++;
if (model->getSimSyncer().isRunning()) {
model->getSimSyncer().stop();
}
model->getSimSyncer().setRefreshPeriod(period);
model->getSimSyncer().start();
reply.addString("ok");
}
bool FTCalibrationDataset::fromBottle(const yarp::os::Bottle &bot)
{
if( bot.size() != 2
|| !(bot.get(0).isString())
|| !(bot.get(1).isList())
|| !(bot.get(1).asList()->size() == 2)
|| !(bot.get(1).asList()->get(0).isString())
|| !(bot.get(1).asList()->get(0).asString() == "mass")
|| !(bot.get(1).asList()->get(1).isDouble())
)
{
return false;
}
dataset_name = bot.get(0).asString();
added_mass = bot.get(1).asList()->get(1).asDouble();
return true;
}
bool WorldRpcInterface::parseSimRTBottle(const std::string name, const yarp::os::Bottle& command, int& n, QMatrix4x4 &rt) {
if ( (command.size()-n) != 6) {
return false;
}
//position
double px = command.get(n).asDouble(); n++;
double py = command.get(n).asDouble(); n++;
double pz = command.get(n).asDouble(); n++;
// rotation
double rx = command.get(n).asDouble(); n++;
double ry = command.get(n).asDouble(); n++;
double rz = command.get(n).asDouble(); n++;
// special simulator rotation:
QQuaternion qrx = QQuaternion::fromAxisAndAngle( QVector3D(1, 0, 0), -rz);
QQuaternion qry = QQuaternion::fromAxisAndAngle( QVector3D(0, 1, 0), -rx);
QQuaternion qrz = QQuaternion::fromAxisAndAngle( QVector3D(0, 0, 1), ry);
rt.setToIdentity();
rt.rotate(qry * (qrz*qrx));
// cylinders are rotated 90 degrees on the z-axis with respect to iCubSIM:
QRegExp rxtype("([^\\d]+)(?:\\s*\\d+)"); // check for: {[one or more non-numeric characters]: return as part 1} {[zero or more whitespace characters followed by one or more numeric characters]: do not return}
int pos = rxtype.indexIn(QString(name.c_str()));
if (pos > -1) {
yarp::os::Value type(rxtype.cap(1).toStdString().c_str());
int vtype = type.asVocab();
if (vtype == VOCAB_CYL || vtype == VOCAB_SCYL) {
rt.rotate( QQuaternion::fromAxisAndAngle( QVector3D(0, 0, 1), 90));
}
}
// position translation
rt(0, 3) = -(pz + 0.026);
rt(1, 3) = -px;
rt(2, 3) = py-0.5976;
return true;
}
bool AnalogServerHandler::_handleIAnalog(yarp::os::Bottle &cmd, yarp::os::Bottle &reply)
{
yTrace();
if (is==0)
return false;
int msgsize=cmd.size();
int code=cmd.get(1).asVocab();
switch (code)
{
case VOCAB_CALIBRATE:
if (msgsize==2)
is->calibrateSensor();
else
{
//read Vector of values and pass to is->calibrate();
}
return true;
break;
case VOCAB_CALIBRATE_CHANNEL:
if (msgsize==3)
{
int ch=cmd.get(2).asInt();
is->calibrateChannel(ch);
}
if (msgsize==4)
{
int ch=cmd.get(2).asInt();
double v=cmd.get(3).asDouble();
is->calibrateChannel(ch, v);
}
return true;
break;
default:
return false;
}
}
std::string WireTwiddler::fromTemplate(const yarp::os::Bottle& msg) {
string result = "";
// assume we want to remove any meta-information
int len = msg.size();
int code = -1;
for (int i=0; i<len; i++) {
Value&v = msg.get(i);
int icode = v.getCode();
if (i==0) code = icode;
if (icode!=code) code = -1;
}
string codeName = nameThatCode(code);
if (code == -1) {
result += "list ";
} else {
result += "vector ";
result += codeName;
result += " ";
}
result += NetType::toString(len).c_str();
result += " ";
for (int i=0; i<len; i++) {
Value&v = msg.get(i);
if (!v.isList()) {
if (code == -1) {
result += nameThatCode(v.getCode());
result += " ";
}
result += "* ";
} else {
result += fromTemplate(*v.asList());
}
}
return result;
}
/*! @brief Convert a YARP dictionary into a %JavaScript object.
@param[in] jct The %JavaScript engine context.
@param[in,out] theData The output object.
@param[in] inputAsList The input dictionary as a list. */
static void
convertDictionary(JSContext * jct,
JS::MutableHandleValue theData,
const yarp::os::Bottle & inputAsList)
{
ODL_ENTER(); //####
ODL_P3("jct = ", jct, "theData = ", &theData, "inputAsList = ", &inputAsList); //####
JS::RootedObject empty(jct);
JSObject * valueObject = JS_NewObject(jct, NULL);
if (valueObject)
{
JS::RootedObject objectRooted(jct);
JS::RootedValue anElement(jct);
objectRooted = valueObject;
for (int ii = 0, mm = inputAsList.size(); mm > ii; ++ii)
{
yarp::os::Value anEntry(inputAsList.get(ii));
if (anEntry.isList())
{
yarp::os::Bottle * entryAsList = anEntry.asList();
if (entryAsList && (2 == entryAsList->size()))
{
yarp::os::Value aValue(entryAsList->get(1));
convertValue(jct, &anElement, aValue);
JS_SetProperty(jct, objectRooted, entryAsList->get(0).toString().c_str(),
anElement);
}
}
}
theData.setObject(*valueObject);
}
ODL_EXIT(); //####
} // convertDictionary
/* Respond function */
bool babbler::respond(const yarp::os::Bottle& bCommand, yarp::os::Bottle& bReply)
{
std::string helpMessage = std::string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"F + (int)Frequency\n" +
"RF \n" +
"quit \n";
bReply.clear();
std::string keyWord = bCommand.get(0).asString().c_str();
if (keyWord == "quit") {
bReply.addString("quitting");
return false;
}
else if (keyWord == "help") {
cout << helpMessage;
bReply.addString("ok");
}
else if (keyWord == "F") {
if (bCommand.size() == 2)
{
if (bCommand.get(1).isInt())
{
setNewFrequency(bCommand.get(1).asInt());
bReply.addInt(f);
}
}
}
else if (keyWord == "RF") {
bReply.addInt(newRandomFrequency());
}
return true;
}
bool NetworkProfiler::getPortmonitorParams(std::string portName, yarp::os::Bottle& param) {
//e.g., get in /portname"
yarp::os::Bottle cmd;
cmd.addString("get");
cmd.addString("in");
cmd.addString(portName.c_str());
Contact srcCon = Contact::fromString(portName);
bool ret = yarp::os::NetworkBase::write(srcCon, cmd, param, true, true, 2.0);
if(!ret) {
yError()<<"Cannot write to"<<portName;
return false;
}
if(param.size() > 1) {
if(param.get(0).isString() && param.get(0).asString() == "fail") {
yError()<<param.toString();
return false;
}
else if(param.get(0).isInt() && param.get(0).asInt() == -1) {
yError()<<param.toString();
return false;
}
}
return true;
}
void showBottle(yarp::os::Bottle& anUnknownBottle, int indentation) {
for (int i = 0; i < anUnknownBottle.size(); i++) {
for (int j = 0; j < indentation; j++) {
printf(" ");
}
printf("[%d]: ", i);
yarp::os::Value& element = anUnknownBottle.get(i);
switch (element.getCode()) {
case BOTTLE_TAG_INT:
printf("int %d\n", element.asInt());
break;
case BOTTLE_TAG_DOUBLE:
printf("float %g\n", element.asDouble());
break;
case BOTTLE_TAG_STRING:
printf("string \"%s\"\n", element.asString().c_str());
break;
case BOTTLE_TAG_BLOB:
printf("binary blob of length %d\n", element.asBlobLength());
break;
case BOTTLE_TAG_VOCAB:
printf("vocab [%s]\n",
yarp::os::Vocab::decode(element.asVocab()).c_str());
break;
default:
if (element.isList()) {
yarp::os::Bottle *lst = element.asList();
printf("list of %d elements\n", lst->size());
showBottle(*lst, indentation + 2);
} else {
printf("unrecognized type\n");
}
break;
}
}
}
bool
MatchRequestHandler::processRequest(const YarpString & request,
const yarp::os::Bottle & restOfInput,
const YarpString & senderChannel,
yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
# pragma unused(request,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
ODL_OBJENTER(); //####
ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
"senderChannel = ", senderChannel); //####
ODL_P1("replyMechanism = ", replyMechanism); //####
bool result = true;
try
{
// We are expecting an integer and a string as the parameter
_response.clear();
if (2 == restOfInput.size())
{
yarp::os::Value condition(restOfInput.get(0));
yarp::os::Value argument(restOfInput.get(1));
if (condition.isInt() && argument.isString())
{
int conditionAsInt = condition.asInt();
YarpString argAsString(argument.toString());
ODL_S1s("argAsString <- ", argAsString); //####
size_t endPos;
Parser::MatchExpression * matcher =
Parser::MatchExpression::CreateMatcher(argAsString,
argAsString.length(),
0, endPos,
_validator);
if (matcher)
{
ODL_LOG("(matcher)"); //####
// Hand off the processing to the Registry Service. First, put the 'OK' response
// in the output buffer, as we have successfully parsed the request.
_response.addString(MpM_OK_RESPONSE_);
if (! static_cast<RegistryService &>(_service).processMatchRequest(matcher,
0 != conditionAsInt,
_response))
{
ODL_LOG("(! static_cast<RegistryService &>(_service)." //####
"processMatchRequest(matcher, 0 != conditionAsInt, " //####
"_response))"); //####
_response.clear();
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Invalid criteria");
}
delete matcher;
}
else
{
ODL_LOG("! (matcher)"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Invalid criteria");
}
}
else
{
ODL_LOG("! (argument.isString())"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Invalid criteria");
}
}
else
{
ODL_LOG("! (1 == restOfInput.size())"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Missing criteria or extra arguments to request");
}
sendResponse(replyMechanism);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT_B(result); //####
return result;
} // MatchRequestHandler::processRequest
bool
UnrealOutputViconInputHandler::handleInput(const yarp::os::Bottle & input,
const YarpString & senderChannel,
yarp::os::ConnectionWriter * replyMechanism,
const size_t numBytes)
{
#if (! defined(OD_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
# pragma unused(senderChannel,replyMechanism)
# endif // MAC_OR_LINUX_
#endif // ! defined(OD_ENABLE_LOGGING_)
ODL_OBJENTER(); //####
ODL_S2s("senderChannel = ", senderChannel, "got ", input.toString()); //####
ODL_P1("replyMechanism = ", replyMechanism); //####
ODL_I1("numBytes = ", numBytes); //####
bool result = true;
try
{
if (_owner.isActive())
{
if (INVALID_SOCKET == _outSocket)
{
cerr << "invalid socket" << endl; //!!!!
}
else
{
int numSubjects = input.size();
if (0 < numSubjects)
{
bool okSoFar = true;
#if (! defined(MpM_UseCustomStringBuffer))
std::stringstream outBuffer;
#endif // ! defined(MpM_UseCustomStringBuffer)
// cerr << "# subjects = " << numSubjects << endl; //!!!!
#if defined(MpM_UseCustomStringBuffer)
_outBuffer.reset().addLong(numSubjects).addString(LINE_END_);
#else // ! defined(MpM_UseCustomStringBuffer)
outBuffer << numSubjects << LINE_END_;
#endif // ! defined(MpM_UseCustomStringBuffer)
for (int ii = 0; okSoFar && (numSubjects > ii); ++ii)
{
yarp::os::Value & aValue = input.get(ii);
if (aValue.isList())
{
yarp::os::Bottle * asBottle = aValue.asList();
if (asBottle)
{
if (2 == asBottle->size())
{
yarp::os::Value & firstValue = asBottle->get(0);
if (firstValue.isString())
{
YarpString subjName = firstValue.asString();
yarp::os::Value & secondValue = asBottle->get(1);
if (secondValue.isDict())
{
yarp::os::Property * segments = secondValue.asDict();
// cerr << subjName.c_str() << endl; //!!!!
if (segments)
{
YarpString segmentsAsString =
segments->toString();
// cerr << ":" << segments->toString() << //!!!!
// ":" << endl; //!!!!
yarp::os::Bottle segmentsAsBottle =
segmentsAsString;
cerr << ":" << segmentsAsBottle.size() << ":" <<
segmentsAsBottle.toString() << ":" <<
endl; //!!!!
#if defined(MpM_UseCustomStringBuffer)
_outBuffer.addString(subjName).addTab();
_outBuffer.addLong(segmentsAsBottle.size()).
addString("\t0" LINE_END_);
okSoFar = dumpSegments(_outBuffer, segmentsAsBottle,
_scale);
#else // ! defined(MpM_UseCustomStringBuffer)
outBuffer << subjName.c_str() << "\t" <<
segmentsAsBottle.size() <<
"\t0" LINE_END_;
okSoFar = dumpSegments(outBuffer, segmentsAsBottle,
_scale);
#endif // ! defined(MpM_UseCustomStringBuffer)
}
else
{
cerr << "bad segments pointer" << endl; //!!!!
okSoFar = false;
}
}
else if (secondValue.isList())
{
yarp::os::Bottle * asList = secondValue.asList();
if (asList)
{
yarp::os::Property segments;
if (ListIsReallyDictionary(*asList, segments))
{
YarpString segmentsAsString =
segments.toString();
// cerr << ":" << segments->toString() << ":" <<
// endl; //!!!!
yarp::os::Bottle segmentsAsBottle =
segmentsAsString;
cerr << ":" << segmentsAsBottle.size() << ":" <<
segmentsAsBottle.toString() << ":" <<
endl; //!!!!
#if defined(MpM_UseCustomStringBuffer)
_outBuffer.addString(subjName).addTab();
_outBuffer.addLong(segmentsAsBottle.size()).
addString("\t0" LINE_END_);
okSoFar = dumpSegments(_outBuffer,
segmentsAsBottle,
_scale);
#else // ! defined(MpM_UseCustomStringBuffer)
outBuffer << subjName.c_str() << "\t" <<
segmentsAsBottle.size() <<
"\t0" LINE_END_;
okSoFar = dumpSegments(outBuffer,
segmentsAsBottle,
_scale);
#endif // ! defined(MpM_UseCustomStringBuffer)
}
else
{
cerr << "not a dictionary" << endl; //!!!!
okSoFar = false;
}
}
else
{
cerr << "bad segments pointer" << endl; //!!!!
okSoFar = false;
}
}
else
{
cerr << "not a dictionary" << endl; //!!!!
okSoFar = false;
}
}
else
{
cerr << "not a string" << endl; //!!!!
okSoFar = false;
}
}
else
{
cerr << "not 2 pieces in list" << endl; //!!!!
okSoFar = false;
}
}
else
{
cerr << "bad subject pointer" << endl; //!!!!
okSoFar = false;
}
}
else
{
cerr << "subject is not a list" << endl; //!!!!
okSoFar = false;
}
}
if (okSoFar)
{
#if defined(MpM_UseCustomStringBuffer)
_outBuffer.addString("END" LINE_END_);
#else // ! defined(MpM_UseCustomStringBuffer)
outBuffer << "END" LINE_END_;
#endif // ! defined(MpM_UseCustomStringBuffer)
const char * outString;
size_t outLength;
#if (! defined(MpM_UseCustomStringBuffer))
std::string buffAsString(outBuffer.str());
#endif // ! defined(MpM_UseCustomStringBuffer)
#if defined(MpM_UseCustomStringBuffer)
outString = _outBuffer.getString(outLength);
#else // ! defined(MpM_UseCustomStringBuffer)
outString = buffAsString.c_str();
outLength = buffAsString.length();
#endif // ! defined(MpM_UseCustomStringBuffer)
if (outString && outLength)
{
int retVal = send(_outSocket, outString,
static_cast<int>(outLength), 0);
cerr << "send--> " << retVal << endl; //!!!!
if (0 > retVal)
{
_owner.deactivateConnection();
}
else
{
SendReceiveCounters toBeAdded(0, 0, retVal, 1);
_owner.incrementAuxiliaryCounters(toBeAdded);
}
}
}
}
else
{
cerr << "no subjects" << endl; //!!!!
}
}
}
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT_B(result); //####
return result;
} // UnrealOutputViconInputHandler::handleInput
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
{
reply.clear();
yDebug("receiving command from port\n");
int index = 0;
int cmdSize = command.size();
while(cmdSize>0)
{
switch(command.get(index).asVocab())
{
case VOCAB4('s','u','s','p'):
reply.addVocab(Vocab::encode("ack"));
vc->halt();
cmdSize--;
index++;
break;
case VOCAB3('r','u','n'):
reply.addVocab(Vocab::encode("ack"));
vc->go();
cmdSize--;
index++;
break;
//this set current position reference
case VOCAB3('s','e','t'):
if (command.size()>=3)
{
int i=command.get(index+1).asInt();
double pos=command.get(index+2).asDouble();
vc->setRef(i, pos);
index +=3;
cmdSize-=3;
}
else
{
cmdSize--;
index++;
yError("Invalid set message, ignoring\n");
}
reply.addVocab(Vocab::encode("ack"));
break;
//this set maximum velocity (limiter)
case VOCAB4('s','v','e','l'):
if(command.size()>=3)
{
int i=command.get(index+1).asInt();
double vel = command.get(index+2).asDouble();
vc->setVel(i,vel);
index += 3;
cmdSize-=3;;
reply.addVocab(Vocab::encode("ack"));
}
else
{
cmdSize--;
index++;
yError("Invalid set vel message, ignoring\n");
reply.addVocab(Vocab::encode("fail"));
}
break;
case VOCAB4('g','a','i','n'):
if(command.size()>=3)
{
int i=command.get(index+1).asInt();
double gain = command.get(index+2).asDouble();
vc->setGain(i,gain);
index+=3;
cmdSize-=3;
reply.addVocab(Vocab::encode("ack"));
}
else
{
cmdSize--;
index++;
yError("Invalid set gain message, ignoring\n");
reply.addVocab(Vocab::encode("fail"));
}
break;
case VOCAB4('h','e','l','p'):
fprintf(stdout,"VelocityControl module, valid commands are:\n");
fprintf(stdout,"- [susp] suspend the controller (command zero velocity)\n");
fprintf(stdout,"- [run] start (and resume after being suspended) the controller\n");
fprintf(stdout,"- [quit] quit the module (exit)\n");
fprintf(stdout,"- [set] <j> <p> move joint j to p (degrees)\n");
fprintf(stdout,"- [svel] <j> <v> set maximum speed for joint j to v (deg/sec)\n");
fprintf(stdout,"- [gain] <j> <k> set P gain for joint j to k\n");
fprintf(stdout,"- [help] to get this help\n");
fprintf(stdout,"\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
reply.addVocab(Vocab::encode("many"));
reply.addVocab(Vocab::encode("ack"));
reply.addString("VelocityControl module, valid commands are:");
reply.addString("- [susp] suspend the controller (command zero velocity)");
reply.addString("- [run] start (and resume after being suspended) the controller");
reply.addString("- [quit] quit the module (exit)");
reply.addString("- [set] <j> <p> move joint j to p (degrees)");
reply.addString("- [svel] <j> <v> set maximum speed for joint j to v (deg/sec)");
reply.addString("- [gain] <j> <k> set P gain for joint j to k");
reply.addString("- [help] to get this help");
reply.addString("\n typical commands:\n gain 0 10\n svel 0 10\n run\n set 0 x\n\n");
break;
default:
yError("Invalid command, ignoring\n");
reply.addVocab(Vocab::encode("fail"));
//cmdSize--;
//index++;
//return respond(command, reply); // call default
break;
}
return true;
}
return false;
}
static bool
dumpSegments(std::stringstream & outBuffer,
yarp::os::Bottle & segmentsAsBottle,
const double scale)
#endif // ! defined(MpM_UseCustomStringBuffer)
{
ODL_ENTER(); //####
ODL_P2("outBuffer = ", &outBuffer, "segmentsAsBottle = ", &segmentsAsBottle); //####
ODL_D1("scale = ", scale); //####
bool okSoFar = true;
int numSegments = segmentsAsBottle.size();
if (0 < numSegments)
{
for (int ii = 0; okSoFar && (numSegments > ii); ++ii)
{
yarp::os::Value & aValue = segmentsAsBottle.get(ii);
if (aValue.isList())
{
yarp::os::Bottle * asList = aValue.asList();
if (asList && (2 == asList->size()))
{
yarp::os::Value & keyValue = asList->get(0);
yarp::os::Value & valueValue = asList->get(1);
if (keyValue.isString() && valueValue.isList())
{
YarpString keyString = keyValue.asString();
yarp::os::Bottle * valueList = valueValue.asList();
#if defined(MpM_UseCustomStringBuffer)
outBuffer.addString(keyString);
#else // ! defined(MpM_UseCustomStringBuffer)
outBuffer << keyString.c_str();
#endif // ! defined(MpM_UseCustomStringBuffer)
if (valueList && (7 == valueList->size()))
{
int numValues = valueList->size();
for (int jj = 0; okSoFar && (numValues > jj); ++jj)
{
double elementAsDouble;
yarp::os::Value & valueElement = valueList->get(jj);
if (valueElement.isDouble())
{
elementAsDouble = valueElement.asDouble();
}
else if (valueElement.isInt())
{
elementAsDouble = valueElement.asInt();
}
else
{
cerr << "value not an integer or a float" << endl; //!!!!
okSoFar = false;
}
if (okSoFar)
{
if (3 > jj)
{
elementAsDouble *= scale;
}
#if defined(MpM_UseCustomStringBuffer)
outBuffer.addTab().addDouble(elementAsDouble);
#else // ! defined(MpM_UseCustomStringBuffer)
outBuffer << "\t" << elementAsDouble;
#endif // ! defined(MpM_UseCustomStringBuffer)
}
}
#if defined(MpM_UseCustomStringBuffer)
outBuffer.addString(LINE_END_);
#else // ! defined(MpM_UseCustomStringBuffer)
outBuffer << LINE_END_;
#endif // ! defined(MpM_UseCustomStringBuffer)
}
else
{
cerr << "bad list pointer or incorrect list size" << endl; //!!!!
okSoFar = false;
}
}
else
{
cerr << "segment not a string and a list" << endl; //!!!!
okSoFar = false;
}
}
else
{
cerr << "segment not a 2-element list" << endl; //!!!!
okSoFar = false;
}
}
else
{
cerr << "segment not a list" << endl; //!!!!
okSoFar = false;
}
}
}
else
{
cerr << "no segments" << endl; //!!!!
okSoFar = false;
}
ODL_EXIT_B(okSoFar); //####
return okSoFar;
} // dumpSegments
void TaskYarpInterface::parseIncomingMessage(yarp::os::Bottle& input, yarp::os::Bottle& reply)
{
int btlSize = input.size();
for (auto i=0; i<btlSize; ++i) {
int msg;
int testInt = input.get(0).asInt();
std::string testStr = input.get(0).asString();
if ((testStr=="") && (testInt!=0)) {
msg = TASK_MESSAGE(testInt);
} else {
msg = TaskMessageHandler::stringToTaskManagerMessageTag(testStr);
}
switch (msg) {
case GET_TASK_STATE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TASK_STATE";
}
TaskState state = this->task->getTaskState();
state.putIntoBottle(reply);
}break;
case GET_STIFFNESS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_STIFFNESS";
}
util::pourEigenMatrixIntoBottle(this->getStiffnessMatrix(), reply);
}break;
case GET_DAMPING:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_DAMPING";
}
util::pourEigenMatrixIntoBottle(this->getDampingMatrix(), reply);
}break;
case GET_WEIGHTS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_WEIGHTS";
}
util::pourEigenVectorIntoBottle(this->getWeight(), reply);
}break;
case GET_DESIRED_TASK_STATE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_DESIRED_TASK_STATE";
}
TaskState state = this->task->getDesiredTaskState();
state.putIntoBottle(reply);
}break;
case GET_TASK_POSITION:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TASK_POSITION";
}
if ((this->task->getMetaTaskType()==Task::META_TASK_TYPE::POSITION) || (this->task->getMetaTaskType()==Task::META_TASK_TYPE::COM)) {
Eigen::Vector3d pos = this->task->getTaskState().getPosition().getTranslation();
util::pourEigenVectorIntoBottle(pos, reply);
}
}break;
case GET_DESIRED_TASK_POSITION:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_DESIRED_TASK_POSITION";
}
if ((this->task->getMetaTaskType()==Task::META_TASK_TYPE::POSITION) || (this->task->getMetaTaskType()==Task::META_TASK_TYPE::COM)) {
Eigen::Vector3d pos = this->task->getDesiredTaskState().getPosition().getTranslation();
util::pourEigenVectorIntoBottle(pos, reply);
}
}break;
case GET_ACTIVITY_STATUS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_ACTIVITY_STATUS";
}
if (this->isActivated()) {
reply.addInt(TASK_IS_ACTIVATED);
} else {
reply.addInt(TASK_IS_DEACTIVATED);
}
}break;
case GET_DIMENSION:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_DIMENSION";
}
OCRA_WARNING("this->task->getWeight() " << this->task->getWeight().size())
reply.addInt(this->task->getWeight().size());
}break;
case GET_TYPE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TYPE";
}
reply.addInt(this->task->getMetaTaskType());
}break;
case GET_TYPE_AS_STRING:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TYPE";
}
reply.addString(this->task->getMetaTaskTypeAsString());
}break;
case GET_NAME:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_NAME";
}
reply.addString(this->task->getName());
}break;
case GET_CONTROL_PORT_NAMES:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_CONTROL_PORT_NAMES";
}
reply.addString(this->inputControlPortName);
reply.addString(this->outputControlPortName);
reply.addString(this->desiredStateOutputPortName);
}break;
case GET_TASK_PORT_NAME:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TASK_PORT_NAME";
}
reply.addString(this->getPortName());
}break;
case GET_TASK_ERROR:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: GET_TASK_ERROR";
}
util::pourEigenVectorIntoBottle(this->getTaskError(), reply);
}break;
case SET_STIFFNESS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_STIFFNESS";
}
++i;
setStiffness(input.get(i).asDouble());
reply.addInt(OCRA_SUCCESS);
}break;
case SET_STIFFNESS_VECTOR:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_STIFFNESS_VECTOR";
}
int indexesToSkip;
this->setStiffness(util::pourBottleIntoEigenVector(util::trimBottle(input, i+1), indexesToSkip) );
i += indexesToSkip;
reply.addInt(OCRA_SUCCESS);
}break;
case SET_STIFFNESS_MATRIX:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_STIFFNESS_MATRIX";
}
int indexesToSkip;
this->setStiffness(util::pourBottleIntoEigenMatrix(util::trimBottle(input, i+1), indexesToSkip) );
i += indexesToSkip;
reply.addInt(OCRA_SUCCESS);
}break;
case SET_DAMPING:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_DAMPING";
}
++i;
this->setDamping(input.get(i).asDouble());
reply.addInt(OCRA_SUCCESS);
}break;
case SET_DAMPING_VECTOR:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_DAMPING_VECTOR";
}
int indexesToSkip;
this->setDamping(util::pourBottleIntoEigenVector(util::trimBottle(input, i+1), indexesToSkip) );
i += indexesToSkip;
reply.addInt(OCRA_SUCCESS);
}break;
case SET_DAMPING_MATRIX:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_DAMPING_MATRIX";
}
int indexesToSkip;
this->setDamping(util::pourBottleIntoEigenMatrix(util::trimBottle(input, i+1), indexesToSkip) );
i += indexesToSkip;
reply.addInt(OCRA_SUCCESS);
}break;
case SET_WEIGHT:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_WEIGHT";
}
++i;
this->setWeight(input.get(i).asDouble());
reply.addInt(OCRA_SUCCESS);
}break;
case SET_WEIGHT_VECTOR:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_WEIGHT_VECTOR";
}
int indexesToSkip;
this->setWeight(util::pourBottleIntoEigenVector(util::trimBottle(input, i+1), indexesToSkip) );
i += indexesToSkip;
reply.addInt(OCRA_SUCCESS);
}break;
case SET_DESIRED_TASK_STATE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_DESIRED_TASK_STATE";
}
int indexesToSkip;
TaskState state;
bool extractSuccess = state.extractFromBottle(util::trimBottle(input, i+1), indexesToSkip);
i += indexesToSkip;
if (extractSuccess) {
// TODO: Set to setDesiredTaskState (needs internal traj gen.) See Task::setDesiredTaskState()
this->setDesiredTaskStateDirect(state);
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case SET_DESIRED_TASK_POSITION:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: SET_DESIRED_TASK_POSITION";
}
if (((this->task->getMetaTaskType()==Task::META_TASK_TYPE::POSITION) || (this->task->getMetaTaskType()==Task::META_TASK_TYPE::COM)) && (input.size()>=4)){
Eigen::Vector3d pos(input.get(i+1).asDouble(), input.get(i+2).asDouble(), input.get(i+3).asDouble());
TaskState state;
state.setPosition(Eigen::Displacementd(pos, Eigen::Rotation3d::Identity()));
// TODO: Set to setDesiredTaskState (needs internal traj gen.) See Task::setDesiredTaskState()
this->setDesiredTaskStateDirect(state);
i += 3;
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case ACTIVATE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: ACTIVATE";
}
if(this->activate()) {
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case DEACTIVATE:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: DEACTIVATE";
}
if(this->deactivate()) {
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case OPEN_CONTROL_PORTS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: OPEN_CONTROL_PORTS";
}
if(openControlPorts()) {
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case CLOSE_CONTROL_PORTS:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: CLOSE_CONTROL_PORTS";
}
if(closeControlPorts()) {
reply.addInt(OCRA_SUCCESS);
} else {
reply.addInt(OCRA_FAILURE);
}
}break;
case HELP:
{
if (logMessages) {
yLog.info() << " ["<< this->task->getName() <<"]: " << "Processing request: HELP";
}
}break;
default:
{
yLog.warning() << "Unrecognized request tag.";
}break;
}
}
}
bool OpcInspector::respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
string key1 = command.get(0).asString().c_str();
if( key1 == "relation")
{
string key2 = command.get(1).asString().c_str();
if( key2 == "add")
{
string subject = command.get(2).asString().c_str();
string verb = command.get(3).asString().c_str();
string object = command.get(4).asString().c_str();
string ccplace = "none";
string cctime = "none";
string ccmanner = "none";
if (command.size()>5)
ccplace = command.get(5).asString().c_str();
if (command.size()>6)
cctime = command.get(6).asString().c_str();
if (command.size()>7)
ccmanner = command.get(7).asString().c_str();
if(opc->getEntity(subject) == NULL)
{
reply.addString("Warning: " + subject + "not present in OPC. Created as an adjective");
opc->addAdjective(subject);
}
if(opc->getEntity(verb) == NULL)
{
reply.addString("Warning: " + verb + "not present in OPC. Created as an action");
opc->addAction(verb);
}
if(opc->getEntity(object) == NULL)
{
reply.addString("Warning: " + object + "not present in OPC. Created as an adjective");
opc->addAdjective(object);
}
if(ccplace!= "none" && opc->getEntity(ccplace) == NULL)
{
reply.addString("Warning: " + ccplace + "not present in OPC. Created as an adjective");
opc->addAdjective(ccplace);
}
if(cctime!= "none" && opc->getEntity(cctime) == NULL)
{
reply.addString("Warning: " + cctime + "not present in OPC. Created as an adjective");
opc->addAdjective(cctime);
}
if(ccmanner!= "none" && opc->getEntity(ccmanner) == NULL)
{
reply.addString("Warning: " + ccmanner + "not present in OPC. Created as an adjective");
opc->addAdjective(ccmanner);
}
Relation r(subject,verb,object,ccplace,cctime,ccmanner);
opc->addRelation(r);
reply.addString("Relation addded");
}
if( key2 == "rem")
{
string subject = command.get(2).asString().c_str();
string verb = command.get(3).asString().c_str();
string object = command.get(4).asString().c_str();
string ccplace = "none";
string cctime = "none";
string ccmanner = "none";
if (command.size()>5)
ccplace = command.get(5).asString().c_str();
if (command.size()>6)
cctime = command.get(6).asString().c_str();
if (command.size()>7)
ccmanner = command.get(7).asString().c_str();
if(opc->getEntity(subject) == NULL)
{
reply.addString("Warning: " + subject + "not present in OPC. Created as an adjective");
opc->addAdjective(subject);
}
if(opc->getEntity(verb) == NULL)
{
reply.addString("Warning: " + verb + "not present in OPC. Created as an action");
opc->addAction(verb);
}
if(opc->getEntity(object) == NULL)
{
reply.addString("Warning: " + object + "not present in OPC. Created as an adjective");
opc->addAdjective(object);
}
if(ccplace!= "none" && opc->getEntity(ccplace) == NULL)
{
reply.addString("Warning: " + ccplace + "not present in OPC. Created as an adjective");
opc->addAdjective(ccplace);
}
if(cctime!= "none" && opc->getEntity(cctime) == NULL)
{
reply.addString("Warning: " + cctime + "not present in OPC. Created as an adjective");
opc->addAdjective(cctime);
}
if(ccmanner!= "none" && opc->getEntity(ccmanner) == NULL)
{
reply.addString("Warning: " + ccmanner + "not present in OPC. Created as an adjective");
opc->addAdjective(ccmanner);
}
Relation r(subject,verb,object,ccplace,cctime,ccmanner);
opc->removeRelation(r);
reply.addString("Relation removed");
}
if( key2 == "check")
{
string subject = command.get(2).asString().c_str();
string verb = command.get(3).asString().c_str();
string object = command.get(4).asString().c_str();
string ccplace = "any";
string cctime = "any";
string ccmanner = "any";
if (command.size()>5)
ccplace = command.get(5).asString().c_str();
if (command.size()>6)
cctime = command.get(6).asString().c_str();
if (command.size()>7)
ccmanner = command.get(7).asString().c_str();
if(opc->getEntity(subject) == NULL)
{
reply.addString("Warning: " + subject + "not present in OPC. Created as an adjective");
opc->addAdjective(subject);
}
if(opc->getEntity(verb) == NULL)
{
reply.addString("Warning: " + verb + "not present in OPC. Created as an action");
opc->addAction(verb);
}
if(opc->getEntity(object) == NULL)
{
reply.addString("Warning: " + object + "not present in OPC. Created as an adjective");
opc->addAdjective(object);
}
if(ccplace!= "any" && opc->getEntity(ccplace) == NULL)
{
reply.addString("Warning: " + ccplace + "not present in OPC. Created as an adjective");
opc->addAdjective(ccplace);
}
if(cctime!= "any" && opc->getEntity(cctime) == NULL)
{
reply.addString("Warning: " + cctime + "not present in OPC. Created as an adjective");
opc->addAdjective(cctime);
}
if(ccmanner!= "any" && opc->getEntity(ccmanner) == NULL)
{
reply.addString("Warning: " + ccmanner + "not present in OPC. Created as an adjective");
opc->addAdjective(ccmanner);
}
Relation r(subject,verb,object,ccplace,cctime,ccmanner);
bool result = opc->containsRelation(r);
if (result)
reply.addString("OPC contains " + r.toString());
else
reply.addString("OPC does not contain " + r.toString());
}
}
if( key1 == "object")
{
string key2 = command.get(1).asString().c_str();
string objectName = command.get(2).asString().c_str();
Object* objPtr = (Object* )opc->getEntity(objectName);
if( objPtr == NULL)
{
reply.addString("Warning: " + objectName + "not present in OPC. Created as a generic object");
objPtr = opc->addObject(objectName);
}
if( key2 == "display")
{
reply.addString(objPtr->toString());
}
if( key2 == "show")
{
objPtr->m_present = true;
opc->commit(objPtr);
}
if( key2 == "hide")
{
objPtr->m_present = false;
opc->commit(objPtr);
}
}
return true;
}
int remove(yarp::os::Bottle& removeArg, folderType fType, bool verbose)
{
ConstString contextName;
if (removeArg.size() >1 )
contextName=removeArg.get(1).asString().c_str();
if (contextName=="")
{
printf("No %s name provided\n", fType==CONTEXTS ? "context" : "robot" );
return 0;
}
yarp::os::ResourceFinder rf;
rf.setVerbose(verbose);
ResourceFinderOptions opts;
opts.searchLocations=ResourceFinderOptions::User;
ConstString targetPath=rf.findPath((getFolderStringName(fType) + PATH_SEPARATOR +contextName).c_str(), opts);
if (targetPath=="")
{
printf("Could not find %s %s !\n",fType==CONTEXTS ? "context" : "robot", contextName.c_str());
return 0;
}
else
{
if (removeArg.size() <3 )
{
char choice='n';
printf("Are you sure you want to remove this folder: %s ? (y/n): ", targetPath.c_str());
int got = scanf("%c",&choice);
if (choice=='y')
{
int result= recursiveRemove(targetPath.c_str());
if (result < 0)
printf("ERRORS OCCURRED WHILE REMOVING %s\n", targetPath.c_str());
else
printf("Removed folder %s\n", targetPath.c_str());
//remove hidden folder:
ConstString hiddenPath= rf.findPath((getFolderStringNameHidden(fType) + PATH_SEPARATOR +contextName).c_str(), opts);
if (hiddenPath != "")
recursiveRemove(hiddenPath.c_str(), false);
return result;
}
else
{
printf("Skipped\n");
return 0;
}
}
else
{
char choice='n';
printf("Are you sure you want to remove files from this folder: %s ? (y/n): ", targetPath.c_str());
int got = scanf("%c",&choice);
if (choice=='y')
{
bool ok=true;
bool removeHidden=true;
ConstString hiddenPath= rf.findPath((getFolderStringNameHidden(fType) + PATH_SEPARATOR +contextName).c_str(), opts);
if (hiddenPath != "")
removeHidden=false;
for (int i=2; i<removeArg.size(); ++i)
{
ConstString fileName=removeArg.get(i).asString();
if(fileName != "")
{
ok = (recursiveRemove(targetPath+ PATH_SEPARATOR + fileName) >=0 ) && ok;
recursiveRemove(hiddenPath + PATH_SEPARATOR + fileName, false);
}
}
if (ok)
{
printf("Removed selected files from %s\n", targetPath.c_str());
return 0;
}
else
{
printf("ERRORS OCCURRED WHILE IMPORTING FILES FOR %s %s\n", fType==CONTEXTS ? "CONTEXT" : "ROBOT" , contextName.c_str());
return 1;
}
}
else
{
printf("Skipped\n");
return 0;
}
}
}
}
int import(yarp::os::Bottle& importArg, folderType fType, bool verbose)
{
ConstString contextName;
if (importArg.size() >1 )
contextName=importArg.get(1).asString().c_str();
if (contextName=="")
{
printf("No %s name provided\n", fType==CONTEXTS ? "context" : "robot" );
return 0;
}
yarp::os::ResourceFinder rf;
rf.setVerbose(verbose);
ResourceFinderOptions opts;
opts.searchLocations=ResourceFinderOptions::Installed;
ConstString originalpath=rf.findPath((getFolderStringName(fType) + PATH_SEPARATOR +contextName).c_str(), opts);
ConstString destDirname=rf.getDataHome() + PATH_SEPARATOR + getFolderStringName(fType) + PATH_SEPARATOR + contextName;
//tmp:
ConstString hiddenDirname=rf.getDataHome() + PATH_SEPARATOR + getFolderStringNameHidden(fType) + PATH_SEPARATOR + contextName;
prepareHomeFolder(rf, fType);
if (importArg.size() >2 )
{
yarp::os::mkdir((destDirname).c_str());
yarp::os::mkdir((hiddenDirname).c_str());
bool ok=true;
for (int i=2; i<importArg.size(); ++i)
{
ConstString fileName=importArg.get(i).asString();
if(fileName != "")
{
ok = (recursiveCopy(originalpath+ PATH_SEPARATOR + fileName, destDirname + PATH_SEPARATOR + fileName) >=0 ) && ok;
recursiveCopy(originalpath+ PATH_SEPARATOR + fileName, hiddenDirname + PATH_SEPARATOR + fileName, true, false);
}
}
if (ok)
{
printf("Copied selected files to %s\n", destDirname.c_str());
return 0;
}
else
{
printf("ERRORS OCCURRED WHILE IMPORTING FILES FOR %s %s\n", fType==CONTEXTS ? "CONTEXT" : "ROBOT" , contextName.c_str());
return 1;
}
}
else
{
int result= recursiveCopy(originalpath, destDirname);
recursiveCopy(originalpath, hiddenDirname, true, false);
if (result < 0)
printf("ERRORS OCCURRED WHILE IMPORTING %s %s\n", fType==CONTEXTS ? "CONTEXT" : "ROBOT", contextName.c_str());
else
{
printf("Copied %s %s from %s to %s .\n", fType==CONTEXTS ? "context" : "robot", contextName.c_str(), originalpath.c_str(), destDirname.c_str());
printf("Current locations for this %s:\n", fType==CONTEXTS ? "context" : "robot");
yarp::os::Bottle paths=rf.findPaths((getFolderStringName(fType) + PATH_SEPARATOR +contextName).c_str());
for (int curCont=0; curCont<paths.size(); ++curCont)
printf("%s\n", paths.get(curCont).asString().c_str());
}
return result;
}
}
bool
PingRequestHandler::processRequest(const YarpString & request,
const yarp::os::Bottle & restOfInput,
const YarpString & senderChannel,
yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
# pragma unused(request,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
ODL_OBJENTER(); //####
ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
"senderChannel = ", senderChannel); //####
ODL_P1("replyMechanism = ", replyMechanism); //####
bool result = true;
try
{
// Validate the name as a channel name
_response.clear();
if (1 == restOfInput.size())
{
yarp::os::Value argument(restOfInput.get(0));
if (argument.isString())
{
YarpString argAsString(argument.toString());
if (Endpoint::CheckEndpointName(argAsString))
{
RegistryService & theService = static_cast<RegistryService &>(_service);
theService.reportStatusChange(argAsString,
RegistryService::kRegistryPingFromService);
if (theService.checkForExistingService(argAsString))
{
// This service is already known, so just update the last-checked time.
theService.updateCheckedTimeForChannel(argAsString);
}
else if (theService.checkForExistingService(argAsString))
{
// Second try - something happened with the first call.
// This service is already known, so just update the last-checked time.
theService.updateCheckedTimeForChannel(argAsString);
}
else
{
// Send a 'name' request to the channel
YarpString aName = GetRandomChannelName(HIDDEN_CHANNEL_PREFIX_
BUILD_NAME_("ping_",
DEFAULT_CHANNEL_ROOT_));
ClientChannel * outChannel = new ClientChannel;
if (outChannel)
{
if (outChannel->openWithRetries(aName, STANDARD_WAIT_TIME_))
{
if (outChannel->addOutputWithRetries(argAsString,
STANDARD_WAIT_TIME_))
{
yarp::os::Bottle message1(MpM_NAME_REQUEST_);
yarp::os::Bottle reply;
if (outChannel->writeBottle(message1, reply))
{
if (theService.processNameResponse(argAsString,
ServiceResponse(reply)))
{
yarp::os::Bottle message2(MpM_LIST_REQUEST_);
if (outChannel->writeBottle(message2, reply))
{
if (theService.processListResponse(argAsString,
ServiceResponse(reply)))
{
// Remember the response
_response.addString(MpM_OK_RESPONSE_);
theService.updateCheckedTimeForChannel(argAsString);
}
else
{
ODL_LOG("! (theService.processList" //####
"Response(argAsString, reply))"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Invalid response to "
"'list' request");
}
}
else
{
ODL_LOG("! (outChannel->" //####
"writeBottle(message2, reply))"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Could not write to channel");
#if defined(MpM_StallOnSendProblem)
Stall();
#endif // defined(MpM_StallOnSendProblem)
}
}
else
{
ODL_LOG("! (theService.processNameResponse(" //####
"argAsString, reply))"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Invalid response to 'name' "
"request");
}
}
else
{
ODL_LOG("! (outChannel->writeBottle(message1, " //####
"reply))"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Could not write to channel");
#if defined(MpM_StallOnSendProblem)
Stall();
#endif // defined(MpM_StallOnSendProblem)
}
#if defined(MpM_DoExplicitDisconnect)
if (! Utilities::NetworkDisconnectWithRetries(outChannel->name(),
argAsString,
STANDARD_WAIT_TIME_))
{
ODL_LOG("(! Utilities::NetworkDisconnectWith" //####
"Retries(outChannel->name(), " //####
"argAsString, STANDARD_WAIT_TIME_))"); //####
}
#endif // defined(MpM_DoExplicitDisconnect)
}
else
{
ODL_LOG("! (outChannel->addOutputWithRetries(" //####
"argAsString, STANDARD_WAIT_TIME_))"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Could not connect to channel");
_response.addString(argAsString);
}
#if defined(MpM_DoExplicitClose)
outChannel->close();
#endif // defined(MpM_DoExplicitClose)
}
else
{
ODL_LOG("! (outChannel->openWithRetries(aName, " //####
"STANDARD_WAIT_TIME_))"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Channel could not be opened");
}
BaseChannel::RelinquishChannel(outChannel);
}
else
{
ODL_LOG("! (outChannel)");
}
}
}
else
{
ODL_LOG("! (Endpoint::CheckEndpointName(argAsString))"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Invalid channel name");
}
}
else
{
ODL_LOG("! (argument.isString())"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Invalid channel name");
}
}
else
{
ODL_LOG("! (1 == restOfInput.size())"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Missing channel name or extra arguments to request");
}
sendResponse(replyMechanism);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT_B(result); //####
return result;
} // PingRequestHandler::processRequest
void Transforms_client_storage::onRead(yarp::os::Bottle &b)
{
m_now = Time::now();
RecursiveLockGuard guard(m_mutex);
if (m_count>0)
{
double tmpDT = m_now - m_prev;
m_deltaT += tmpDT;
if (tmpDT>m_deltaTMax)
m_deltaTMax = tmpDT;
if (tmpDT<m_deltaTMin)
m_deltaTMin = tmpDT;
//compare network time
/*if (tmpDT*1000<TRANSFORM_TIMEOUT)
{
state = b.get(5).asInt();
}
else
{
state = TRANSFORM_TIMEOUT;
}*/
}
m_prev = m_now;
m_count++;
m_lastBottle = b;
Stamp newStamp;
getEnvelope(newStamp);
//initialization (first received data)
if (m_lastStamp.isValid() == false)
{
m_lastStamp = newStamp;
}
//now compare timestamps
// if ((1000 * (newStamp.getTime() - m_lastStamp.getTime()))<TRANSFORM_TIMEOUT)
if (1)
{
m_state = IFrameTransform::TRANSFORM_OK;
m_transforms.clear();
int bsize= b.size();
for (int i = 0; i < bsize; i++)
{
//this includes: timed yarp transforms, static yarp transforms, ros transforms
Bottle* bt = b.get(i).asList();
if (bt != nullptr)
{
FrameTransform t;
t.src_frame_id = bt->get(0).asString();
t.dst_frame_id = bt->get(1).asString();
t.timestamp = bt->get(2).asDouble();
t.translation.tX = bt->get(3).asDouble();
t.translation.tY = bt->get(4).asDouble();
t.translation.tZ = bt->get(5).asDouble();
t.rotation.w() = bt->get(6).asDouble();
t.rotation.x() = bt->get(7).asDouble();
t.rotation.y() = bt->get(8).asDouble();
t.rotation.z() = bt->get(9).asDouble();
m_transforms.push_back(t);
}
}
}
else
{
m_state = IFrameTransform::TRANSFORM_TIMEOUT;
}
m_lastStamp = newStamp;
}
bool
UnregisterRequestHandler::processRequest(const YarpString & request,
const yarp::os::Bottle & restOfInput,
const YarpString & senderChannel,
yarp::os::ConnectionWriter * replyMechanism)
{
#if (! defined(ODL_ENABLE_LOGGING_))
# if MAC_OR_LINUX_
# pragma unused(request,senderChannel)
# endif // MAC_OR_LINUX_
#endif // ! defined(ODL_ENABLE_LOGGING_)
ODL_OBJENTER(); //####
ODL_S3s("request = ", request, "restOfInput = ", restOfInput.toString(), //####
"senderChannel = ", senderChannel); //####
ODL_P1("replyMechanism = ", replyMechanism); //####
bool result = true;
try
{
// Validate the name as a channel name
_response.clear();
if (1 == restOfInput.size())
{
yarp::os::Value argument(restOfInput.get(0));
if (argument.isString())
{
YarpString argAsString(argument.toString());
if (Endpoint::CheckEndpointName(argAsString))
{
RegistryService & theService = static_cast<RegistryService &>(_service);
theService.reportStatusChange(argAsString,
RegistryService::kRegistryUnregisterService);
// Forget the information associated with the channel name
if (theService.removeServiceRecord(argAsString))
{
_response.addString(MpM_OK_RESPONSE_);
if (argAsString != MpM_REGISTRY_ENDPOINT_NAME_)
{
theService.removeCheckedTimeForChannel(argAsString);
}
}
else
{
ODL_LOG("! (theService.removeServiceRecord(argAsString))"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Could not remove service");
}
}
else
{
ODL_LOG("! (Endpoint::CheckEndpointName(argAsString))"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Invalid channel name");
}
}
else
{
ODL_LOG("! (argument.isString())"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Invalid channel name");
}
}
else
{
ODL_LOG("! (1 == restOfInput.size())"); //####
_response.addString(MpM_FAILED_RESPONSE_);
_response.addString("Missing channel name or extra arguments to request");
}
sendResponse(replyMechanism);
}
catch (...)
{
ODL_LOG("Exception caught"); //####
throw;
}
ODL_OBJEXIT_B(result); //####
return result;
} // UnregisterRequestHandler::processRequest
