bool WorldRpcInterface::handler( const yarp::os::Bottle& command, yarp::os::Bottle& reply )
{
int n = 0; // identifier of the current bottle element
int cmd; // the command (see command vocabs in header)
yarp::os::ConstString prefix = command.get(n).asString();
if ( prefix=="help" )
{
reply.addVocab(yarp::os::Vocab::encode("many"));
reply.addString("\nMoBeE world interface, arguments within brackets: \n");
reply.addString( "ls: list objects");
reply.addString( "mk sph [radius] [xpos] [ypos] [zpos]: create sphere");
reply.addString( "mk cyl [radius] [height] [xpos] [ypos] [zpos]: create sphere");
reply.addString( "mk box [xsize] [ysize] [zsize] [xpos] [ypos] [zpos]: create sphere");
reply.addString( "set [objectname] [xpos] [ypos] [zpos]: set object location (m)");
reply.addString( "def [objectname] [targ/obs]: set object class to target or obstacle");
reply.addString( "get [objectname]: return object state");
reply.addString( "rot [objectname] [xrot] [yrot] [zrot]: set object rotation (degrees)");
reply.addString( "rot [objectname] [1*9 rotation matrix]: set object rotation cosine matrix");
reply.addString( "rm [objectname]: remove object (persistent objects cannot be removed)");
reply.addString( "clr: remove all but persistent objects from the world, and reset object counters");
reply.addString( "grab [objectname] [robotname] [markername]: attach object to robot marker");
reply.addString( "grab [objectname] [robotname]: detach object from robot");
reply.addString("\niCub simulator synchronization commands:");
reply.addString( "sim [objectname] [xpos] [ypos] [zpos] [xrot] [yrot] [zrot]: set rototranslation as returned in iCubSim coordinates");
reply.addString( "srun [period]: run iCubSim synchronization thread");
reply.addString( "sstp: stop iCubSim synchronization thread");
reply.addString( "sync: do one iCubSim synchronization step");
return true;
}
else if ( prefix == "ls, mk (sph, cyl, box), set, def (obs/tgt), get, rot, rm, clr, grab, sim, srun, sstp, sync" ) { n++; }
cmd = command.get(n).asVocab(); n++;
switch (cmd)
{
case VOCAB_LS: getList(reply); break;
case VOCAB_MK: make(command,reply,n); break;
case VOCAB_SET: set(command,reply,n); break;
case VOCAB_DEF: respClass(command,reply,n); break;
case VOCAB_GET: getState(command,reply,n); break;
case VOCAB_ROT: setRot(command,reply,n); break;
case VOCAB_REM: removeObject(command,reply,n); break;
case VOCAB_GRAB: grabObject(command,reply,n); break;
case VOCAB_SIM: setRTfromSim(command, reply, n); break;
case VOCAB_SIMSYNC_RUN: startSimSyncer(command, reply, n); break;
case VOCAB_SIMSYNC_STOP: model->getSimSyncer().stop(); reply.addString("ok"); break;
case VOCAB_SIMSYNC_NOW: model->getSimSyncer().step(); reply.addString("ok"); break;
case VOCAB_CLEAR:
printf("CLEARING THE WORLD\n");
model->clearTheWorld();
s = 0; c = 0; b = 0; ss = 0; sc = 0; sb = 0; // set counters to 0, for icub simulator compatibility
reply.addString("Removed all world objects");
printf("FINISHED CLEARING THE WORLD\n");
break;
default: reply.addString("Unknown RPC command"); return false;
}
return true;
}
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.");
}
}
}
void WorldRpcInterface::grabObject( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject(command, reply, n);
if (!object)
return;
QString robotName = command.get(n).asString().c_str(); n++;
KinematicModel::Robot *robot = model->getRobot(robotName);
if (!robot) {
reply.addString("Robot not found.");
return;
}
reply.addString("Robot found.");
QString markerName = command.get(n).asString().c_str(); n++;
int markerIndex = -1;
//KinematicModel::RobotObservation robotObs = robot->observe();
//for (int i = 0; i<robotObs.getNumMarkers(); i++) {
// if (robotObs.markerName(i).compare(markerName) == 0)
// markerIndex = i;
//}
if (markerIndex < 0) {
reply.addString("Marker not found, releasing object.");
// return;
} else
reply.addString("Marker found, grabbing object.");
model->grabObject(object, robot, markerIndex);
//printf("Grabbing object \"%s\" with marker \"%s\" on robot \"%s\"\n", object->getName().toStdString().c_str(), markerName.toStdString().c_str(), robotName.toStdString().c_str());
}
bool GuiUpdaterModule::respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"reset \n" +
"help \n" +
"quit \n" ;
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
else if (command.get(0).asString()=="reset") {
cout << "Reset"<<endl;
resetGUI();
reply.addString("ok");
}
return true;
}
/**
* Parser for VOCAB commands. It is called by virtual bool respond().
* @param command the bottle containing the user command
* @param reply the bottle which will be returned to the RPC client
* @return true if the command was successfully parsed
*/
bool parse_respond_vocab(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
int request = command.get(1).asVocab();
if (request == VOCAB_NAV_GET_CURRENT_POS)
{
//plannerThread->setNewAbsTarget(loc);
reply.addVocab(VOCAB_OK);
reply.addString(m_localization_data.map_id);
reply.addDouble(m_localization_data.x);
reply.addDouble(m_localization_data.y);
reply.addDouble(m_localization_data.theta);
}
else if (request == VOCAB_NAV_SET_INITIAL_POS)
{
yarp::dev::Map2DLocation loc;
loc.map_id = command.get(2).asString();
loc.x = command.get(3).asDouble();
loc.y = command.get(4).asDouble();
loc.theta = command.get(5).asDouble();
initializeLocalization(loc);
reply.addVocab(VOCAB_OK);
}
else
{
reply.addVocab(VOCAB_ERR);
}
return true;
}
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 YarpManager::onRead(yarp::os::Bottle& b) {
std::string name = b.get(0).asString().c_str();
int value = b.get(1).asInt();
if(name == "acuity") setAcuity(value);
else if(name == "fov") setFov(value);
else if(name == "brightness") setBrightness(value);
else if(name == "threshold") setTreshold(value);
else std::cout<<"Bottle message incorrect"<<std::endl;
}
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;
}
/* 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;
}
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;
}
void WorldRpcInterface::set( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
double x = command.get(n).asDouble(); n++; //std::cout << x << std::endl; // x position
double y = command.get(n).asDouble(); n++; //std::cout << y << std::endl; // y position
double z = command.get(n).asDouble(); n++; //std::cout << z << std::endl; // z position
object->setPosition( QVector3D(x,y,z) );
reply.addString("Set Cartesian position of object.");
}
}
void Rangefinder2DInputPortProcessor::onRead(yarp::os::Bottle &b)
{
now=SystemClock::nowSystem();
mutex.wait();
if (count>0)
{
double tmpDT=now-prev;
deltaT+=tmpDT;
if (tmpDT>deltaTMax)
deltaTMax=tmpDT;
if (tmpDT<deltaTMin)
deltaTMin=tmpDT;
//compare network time
if (tmpDT*1000<LASER_TIMEOUT)
{
state = b.get(1).asInt();
}
else
{
state = IRangefinder2D::DEVICE_TIMEOUT;
}
}
prev=now;
count++;
lastBottle=b;
Stamp newStamp;
getEnvelope(newStamp);
//initialialization (first received data)
if (lastStamp.isValid()==false)
{
lastStamp = newStamp;
}
//now compare timestamps
if ((1000*(newStamp.getTime()-lastStamp.getTime()))<LASER_TIMEOUT)
{
state = b.get(1).asInt();
}
else
{
state = IRangefinder2D::DEVICE_TIMEOUT;
}
lastStamp = newStamp;
mutex.post();
}
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
/*! @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
/**
* Parser for string commands. It is called by virtual bool respond().
* @param command the bottle containing the user command
* @param reply the bottle which will be returned to the RPC client
* @return true if the command was successfully parsed
*/
bool parse_respond_string(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
if (command.get(0).isString() && command.get(0).asString() == "getLoc")
{
std::string s = std::string("Current Location is: ") + m_localization_data.toString();
reply.addString(s);
}
else if (command.get(0).isString() && command.get(0).asString() == "initLoc")
{
yarp::dev::Map2DLocation loc;
loc.map_id = command.get(1).asString();
loc.x = command.get(2).asDouble();
loc.y = command.get(3).asDouble();
loc.theta = command.get(4).asDouble();
initializeLocalization(loc);
std::string s = std::string("Localization initialized to: ") + loc.toString();
reply.addString(s);
}
else
{
reply.addString("Unknown command.");
}
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);
}
virtual bool cmdRegister(yarp::os::Bottle& cmd,
yarp::os::Bottle& reply,
yarp::os::Contact& remote) {
ConstString name = cmd.get(1).asString();
ConstString carrier = cmd.get(2).asString();
ConstString machine = cmd.get(3).asString();
int number = cmd.get(4).asInt();
if (name=="...") {
name = "/tmp/";
for (int i=0; i<20; i++) {
double x = Rand::scalar('a','z'+1);
name = name + (char)x;
}
}
if (carrier==""||carrier=="...") {
carrier = "tcp";
}
if (machine==""||machine=="...") {
machine = remote.getHost();
}
if (number==0) {
number = allocatePortNumber();
if (number==0) {
printf("Out of port numbers!\n");
} else {
lastNumber = number;
}
}
if (numbers.find(number)==numbers.end()) {
numbers[number] = 1;
} else {
numbers[number]++;
}
Entry entry;
entry.name = name.c_str();
entry.carrier = carrier.c_str();
entry.machine = machine.c_str();
entry.portNumber = number;
names[name.c_str()] = entry;
Bottle cmd2;
cmd2.add("query");
cmd2.add(name.c_str());
return cmdQuery(cmd2,reply,remote);
}
void WorldRpcInterface::respClass( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
//model->clearWorldObject(object);
model->removeWorldObject(object);
int type = command.get(n).asVocab();
QColor collidingColor,freeColor;
switch (type)
{
case VOCAB_OBSTACLE:
freeColor = Qt::blue;
freeColor = freeColor.lighter();
collidingColor = freeColor;
freeColor.setAlphaF(0.5);
collidingColor.setAlphaF(0.5);
object->setResponseClass(model->OBSTACLE());
object->setFreeColor( freeColor );
object->setCollidingColor( collidingColor );
reply.addString("Changed object type to 'obstacle'.");
break;
case VOCAB_TARGET:
freeColor = Qt::green;
freeColor = freeColor.lighter();
collidingColor = freeColor;
freeColor.setAlphaF(1.0);
collidingColor.setAlphaF(0.5);
object->setResponseClass(model->TARGET());
object->setFreeColor( freeColor );
object->setCollidingColor( collidingColor );
reply.addString("Changed object type to 'target'.");
break;
default:
reply.addString("Unknown definition, use 'obs' or 'tgt'.");
}
model->appendObject( object );
}
}
void objectGeneratorSim::createObject(std::string ob, yarp::os::Bottle size, yarp::os::Bottle pos, yarp::os::Bottle colour, bool target)
{
yarp::os::Bottle pair;
pair.clear();
cmd.clear();
cmd.addString("world");
cmd.addString("mk");
cmd.addString(ob.c_str());
pair.addString(ob);
if(ob=="box"){
cmd.addDouble(size.get(0).asDouble());
cmd.addDouble(size.get(1).asDouble());
cmd.addDouble(size.get(2).asDouble());
listSize[0] +=1;
pair.addInt(listSize[0]);
}else if(ob == "sph"){
cmd.addDouble(size.get(0).asDouble());
listSize[1] +=1;
pair.addInt(listSize[1]);
}else if (ob == "cyl"){
cmd.addDouble(size.get(0).asDouble());
listSize[2] +=1;
pair.addInt(listSize[2]);
}else{
cout<<"nothing created!!!!"<<endl;
}
cmd.addDouble(pos.get(0).asDouble());
cmd.addDouble(pos.get(1).asDouble());
cmd.addDouble(pos.get(2).asDouble());
cmd.addDouble(colour.get(0).asDouble());
cmd.addDouble(colour.get(1).asDouble());
cmd.addDouble(colour.get(2).asDouble());
portSim.write(cmd);
//increase ob list and add ob to the list
//add radius
pair.addDouble(size.get(0).asDouble());
if (target==true){
targets.append(pair);
}else{
objects.append(pair);
}
}
/**
* Handles command line commands to interact with the motors and run procedures
* @return true unless quit is called
*/
bool SarsaLearner::respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n" ;
reply.clear();
if (command.get(0).asString()=="quit") {
reply.addString("quitting");
return false;
}
else if (command.get(0).asString()=="help") {
cout << helpMessage;
reply.addString("ok");
}
return true;
}
virtual bool cmdQuery(yarp::os::Bottle& cmd,
yarp::os::Bottle& reply,
yarp::os::Contact& remote) {
reply.addString("old");
ConstString name = cmd.get(1).asString();
Contact c = Network::queryName(name);
if (c.isValid()) {
appendEntry(reply,c);
}
return true;
}
yarp::sig::Vector iCub::skinDynLib::vectorFromBottle(const yarp::os::Bottle b, int in, const int size)
{
yarp::sig::Vector v(size,0.0);
for (int i = 0; i < size; i++)
{
v[i] = b.get(in).asDouble();
in++;
}
return v;
}
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
{
reply.clear();
if (command.get(0).isString())
{
if (command.get(0).asString()=="help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands:");
reply.addString("currently nothing");
return true;
}
else if (command.get(0).asString()=="***")
{
return true;
}
}
reply.addString("Unknown command");
return true;
}
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
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;
}
virtual bool cmdQuery(yarp::os::Bottle& cmd,
yarp::os::Bottle& reply,
yarp::os::Contact& remote) {
reply.addString("old");
string name = cmd.get(1).asString().c_str();
map<string,Entry>::iterator it = names.find(name);
if (it==names.end()) {
return true;
}
Entry& e = it->second;
appendEntry(reply,e);
return true;
}
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 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;
}
