bool NetworkClock::read(ConnectionReader& reader) {
Bottle bot;
bool ok = bot.read(reader);
if(closing)
{
_time = -1;
return false;
}
if (!ok && !closing)
{
YARP_ERROR(Logger::get(), "Error reading clock port");
return false;
}
timeMutex.lock();
sec = bot.get(0).asInt32();
nsec = bot.get(1).asInt32();
_time = sec + (nsec*1e-9);
initted = true;
timeMutex.unlock();
listMutex.lock();
Waiters* waiters = static_cast<Waiters*>(pwaiters);
Waiters::iterator waiter_i;
waiter_i = waiters->begin();
while (waiter_i != waiters->end())
{
if (waiter_i->first - _time < 1E-12 )
{
Semaphore *waiterSemaphore = waiter_i->second;
waiter_i = waiters->erase(waiter_i);
if (waiterSemaphore)
waiterSemaphore->post();
}
else
++waiter_i;
}
listMutex.unlock();
return true;
}
void Pointing::run(const Bottle &args) {
yInfo() << "Pointing::run";
Bottle *sensation = sensation_port_in.read();
string obj_name, sentence;
bool no_objects = true;
if (args.size()!=0) {
yDebug()<<args.toString() << args.size();
obj_name = args.get(0).asList()->get(0).asString();
yDebug() << "Object selected: " << obj_name;
sentence = "Okay, this is the ";
no_objects=false;
} else {
if(sensation->size()==0) {
iCub->lookAtPartner();
iCub->say("There are no objects I can point at.");
iCub->home();
return;
}
int id = yarp::os::Random::uniform(0, sensation->size() - 1);
obj_name = sensation->get(id).asList()->get(1).asString();
yDebug() << "Randomly selected: " << id << " " << obj_name;
sentence = "I could point to the ";
}
yDebug() << "[pointing]: opc checkout";
iCub->say(sentence + obj_name, false);
bool succeeded = iCub->point(obj_name);
if (no_objects){
bool look_success = iCub->lookAtPartner();
if (succeeded) {
iCub->say("Do you know that this is a " + obj_name, false);
} else {
iCub->say("I couldn't find the " + obj_name, false);
}
if(look_success) {
yarp::os::Time::delay(1.5);
}
}
iCub->home();
}
void testBlob() {
report(0,"testing blob...");
Bottle bot("{4 42 255} 10 20");
checkEqual(bot.size(),3,"plausible parse");
checkTrue(bot.get(0).isBlob(),"blob present");
checkEqual((int)bot.get(0).asBlobLength(),3,"blob length");
checkEqual(bot.get(0).asBlob()[1],42, "blob match");
report(0,"testing blob with internal null...");
char blob[12]="hello\0world";
const Value v ((void*)blob, sizeof(blob));
checkEqual(12,(int)v.asBlobLength(),"value length");
checkFalse(v.isNull(),"value non-null");
Bottle b;
b.add(v);
checkEqual(b.size(),1,"insertion happened");
checkTrue(b.get(0).isBlob(),"insertion is right type");
checkEqual(12,(int)b.get(0).asBlobLength(),"length within bottle");
}
bool Recognition::respond(const Bottle& command, Bottle& reply) {
if(command.get(0).asString() == "actionStarted"){
actionDone = false;
}
else if (command.get(0).asString() == "actionStoped"){
actionDone = true;
}
if(command.get(0).asString() == "observ"){
lookAtHand = true;
}else if(command.get(0).asString() == "stopObserv"){
lookAtHand = false;
}
reply.clear();
reply.addString("Recongition module over");
return true;
}
bool TRACKERManager::pauseTracker(int id)
{
mutex.wait();
bool reply = true;
Bottle ids = getIDs();
bool gotid = false;
for (int i = 0; i <ids.size(); i++)
{
if (id == ids.get(i).asInt())
{
gotid = true;
yDebug()<< "got the ID " << id <<" == "<< ids.get(i).asInt();
}
}
if (gotid)
{
yDebug() << "[TRACKERManager::pauseTracker] attempting to pause tracker id " << id ;
if (workerThreads[id]->isRunning())
{
workerThreads[id]->suspend();
pausedThreads.push_back(id);
yDebug() << "[TRACKERManager::pauseTracker] done pausing tracker id " << id ;
reply = true;
}
else
{
yError() << "[TRACKERManager::pauseTracker] failed to pause tracker id %d " << id << " - Not running.. ";
reply=false;
}
}
else
{
yError() << "[TRACKERManager::pauseTracker] failed to pause tracker id %d " << id << " - Not running.. ";
reply = false;
}
mutex.post();
return reply;
}
void testSerialCopy() {
report(0,"test serialization by copy");
Value v(3.14);
Bottle b;
b.read(v);
checkEqualish(b.get(0).asDouble(),3.14,"copy to bottle succeeded");
Bottle b2;
b.write(b2);
checkEqualish(b2.get(0).asDouble(),3.14,"copy from bottle succeeded");
}
bool tutorialModule::respond(const Bottle& command, 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;
}
Vector PMPthread::Bottle2Vector(Bottle Bot)
{
Vector v(Bot.size());
for (int i = 0; i < Bot.size(); i++)
{
v(i) = Bot.get(i).asDouble();
}
return v;
}
void PointedLocation::onRead(Bottle &b)
{
fprintf(stdout, "got read from points size %d \n",b.size());
if (b.size()>1)
{
loc.x=(int)b.get(0).asDouble();
loc.y=(int)b.get(1).asDouble();
rxTime=Time::now();
}
}
/**
* Add a new entity to the timeKnowledge
* input format :
* <string name> <string timeArg1> <string timeArg2>
*/
void timeKnowledge::addKnowledge(Bottle bInput)
{
if (bInput.size() != 3)
{
yInfo() << "\t" << "Error in addKnowledge : fromBottle. Wrong number of inputs (need 3 inputs : <string name> <string timeArg1> <string timeArg2>)" ;
return;
}
if (!(bInput.get(0).isString() && bInput.get(1).isString() && bInput.get(2).isString()))
{
yInfo() << "\t" << "Error in addKnowledge : fromBottle. Wrong format of inputs (need 3 inputs : <string name> <string timeArg1> <string timeArg2>)" ;
return;
}
struct tm tmTimeArg1 = abmReasoningFunction::string2Time(bInput.get(1).asString().c_str()),
tmTimeArg2 = abmReasoningFunction::string2Time(bInput.get(2).asString().c_str());
timeArg1.push_back(tmTimeArg1);
timeArg2.push_back(tmTimeArg2);
}
void ParticleFilter::onRead(Bottle &target)
{
//CvPoint trackLoc;
if (target.size()>1)
{
loc.x=(int)target.get(0).asDouble();
loc.y=(int)target.get(1).asDouble();
//fprintf( stdout, "particle something: %s \n\n",target.toString().c_str() );
}
}
//execute action, stop to go out
Bottle learnPrimitive::execute(){
Bottle bOutput ;
Bottle bRecognized, //received FROM speech recog with transfer information (1/0 (bAnswer))
bAnswer, //response from speech recog without transfer information, including raw sentence
bSemantic; // semantic information of the content of the recognition
string sSay = " What do you want me to do?";
yInfo() << sSay;
iCub->say(sSay);
Bottle bCurrentOrder = nodeNameAction("any");
string orderType = bCurrentOrder.get(0).asString();
if(orderType == "stop"){
string sSay = " Allright, thank you for the exercice";
yInfo() << sSay;
iCub->say(sSay);
return bCurrentOrder ;
}
string orderVerb = bCurrentOrder.get(1).asString();
string orderArg = bCurrentOrder.get(2).asString();
sSay = orderVerb + "ing my " + orderArg;
if(orderArg == "one" || orderArg == "two" || orderArg == "three" || orderArg == "four" || orderArg == "five"){
sSay = orderVerb + "ing " + orderArg;
}
sSay = sSay ;
yInfo() << sSay;
iCub->say(sSay);
if(orderType == "proto-action") {
protoCommand(orderVerb, orderArg);
} else if (orderType == "primitive") {
primitiveCommand(orderVerb, orderArg);
} else {
actionCommand(orderVerb, orderArg);
}
yarp::os::Time::delay(6);
return execute();
}
bool activate(bool force = false) {
if (force) {
// wipe if forced
clear();
}
// return if namespaces already present
if (spaces.size()!=0) return true;
// read namespace list from config file
NameConfig conf;
if (!conf.fromFile()) {
double now = Time::now();
static double last_shown = now-10;
if (now-last_shown>3) {
last_shown = now;
fprintf(stderr,"warning: YARP name server(s) not configured, ports will be anonymous\n");
fprintf(stderr,"warning: check your namespace and settings with 'yarp detect'\n");
}
return false;
}
Bottle ns = conf.getNamespaces();
// loop through namespaces
for (int i=0; i<ns.size(); i++) {
ConstString n = ns.get(i).asString();
NameConfig conf2;
// read configuration of individual namespace
if (!conf2.fromFile(n.c_str())) {
fprintf(stderr, "Could not find namespace %s\n",
n.c_str());
continue;
}
String mode = conf2.getMode();
Contact address = conf2.getAddress().addName(n);
if (mode=="yarp"||mode=="//") {
// add a yarp namespace
NameSpace *ns = new YarpNameSpace(address);
spaces.push_back(ns);
} else if (mode=="ros") {
// add a ros namespace
NameSpace *ns = new RosNameSpace(address);
spaces.push_back(ns);
} else if (mode=="local") {
NameSpace *ns = new YarpDummyNameSpace;
spaces.push_back(ns);
} else {
// shrug
YARP_SPRINTF1(Logger::get(),error,
"cannot deal with namespace of type %s",
mode.c_str());
return false;
}
}
// cache flags
scan();
return true;
}
bool autobiographicalMemory::storeImageOIDs(int instance) {
Bottle bRequest;
ostringstream osStoreOIDReq;
osStoreOIDReq << "SELECT \"time\", img_provider_port, relative_path FROM visualdata WHERE img_oid IS NULL";
if (instance >= 0) {
osStoreOIDReq << " AND instance = " << instance;
}
bRequest = requestFromString(osStoreOIDReq.str());
if (bRequest.size() > 0 && bRequest.get(0).toString() != "NULL") {
yInfo() << "[storeImageOIDs] This may take a while!";
}
else {
return true;
}
ostringstream osStoreOID;
for (int i = 0; i < bRequest.size(); i++) {
string imgTime = bRequest.get(i).asList()->get(0).toString().c_str();
string imgProviderPort = bRequest.get(i).asList()->get(1).toString().c_str();
string imgRelativePath = bRequest.get(i).asList()->get(2).toString().c_str();
string fullPath = storingPath + "/" + imgRelativePath;
database_mutex.lock();
unsigned int new_img_oid = ABMDataBase->lo_import(fullPath.c_str());
database_mutex.unlock();
osStoreOID << "UPDATE visualdata SET img_oid=" << new_img_oid;
osStoreOID << " WHERE time='" << imgTime << "' and img_provider_port = '" << imgProviderPort << "';";
if (((i+1) % 100 == 0 && i != 0) || i == bRequest.size() - 1) {
requestFromString(osStoreOID.str());
yInfo() << "[storeImageOIDs] Saved " << i+1 << " images out of " << bRequest.size();
osStoreOID.str("");
}
}
yInfo() << "[storeImageOIDs] All images saved.";
return true;
}
bool respond(const Bottle& command, Bottle& reply)
{
reply.clear();
if (command.get(0).isInt())
{
if (command.get(0).asInt()==0)
{
fprintf(stderr,"Asking recalibration...\n");
if (inv_dyn)
{
inv_dyn->suspend();
inv_dyn->calibrateOffset();
inv_dyn->resume();
}
fprintf(stderr,"Recalibration complete.\n");
reply.addString("Recalibrated");
return true;
}
}
if (command.get(0).isString())
{
if (command.get(0).asString()=="help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands:");
reply.addString("calib all");
reply.addString("calib arms");
reply.addString("calib legs");
reply.addString("calib feet");
return true;
}
else if (command.get(0).asString()=="calib")
{
fprintf(stderr,"Asking recalibration...\n");
if (inv_dyn)
{
calib_enum calib_code=CALIB_ALL;
if (command.get(1).asString()=="all") calib_code=CALIB_ALL;
else if (command.get(1).asString()=="arms") calib_code=CALIB_ARMS;
else if (command.get(1).asString()=="legs") calib_code=CALIB_LEGS;
else if (command.get(1).asString()=="feet") calib_code=CALIB_FEET;
inv_dyn->suspend();
inv_dyn->calibrateOffset(calib_code);
inv_dyn->resume();
}
fprintf(stderr,"Recalibration complete.\n");
reply.addString("Recalibrated");
return true;
}
}
reply.addString("Unknown command");
return true;
}
void CubCartThread::getTarget()
{
Bottle command;
printf("current command: left %f %f %f, right %f %f %f\n", Lxd[0], Lxd[1], Lxd[2], Rxd[0], Rxd[1], Rxd[2]);
if(inputPort.read(command))
{
if(command.get(0).asString()=="left")
{
icartR->stopControl(); //stop the right arm
printf("recieved command: left %f %f %f\n", command.get(1).asDouble(), command.get(2).asDouble(), command.get(3).asDouble());
Lxd[0] = command.get(1).asDouble() - 0.065; //left arm negative in front
Lxd[1] = command.get(2).asDouble() - 0.065; //left arm negative on correct side of body (x)
Lxd[2] = command.get(3).asDouble(); //positive from waist up
// sanity check the position before moving there
if(Lxd[0] < -0.5) Lxd[0] = -0.5;
if(Lxd[0] > -0.1) Lxd[0] = -0.1;
if(Lxd[1] < -0.4) Lxd[1] = -0.4;
if(Lxd[1] > 0.1) Lxd[1] = 0.1;
if(Lxd[2] < -0.2) Lxd[2] = -0.2;
if(Lxd[2] > 0.4) Lxd[2] = 0.4;
icartL->goToPose(Lxd,Lod);
Time::delay(3);
icartL->stopControl(); //stop the left arm
}
else if(command.get(0).asString()=="right")
{
icartL->stopControl(); //stop the left arm
printf("recieved command: right %f %f %f\n", command.get(1).asDouble(), command.get(2).asDouble(), command.get(3).asDouble());
Rxd[0] = command.get(1).asDouble() - 0.04; //right arm negative in front
Rxd[1] = command.get(2).asDouble() + 0.05; //right arm positive on correct side of body
Rxd[2] = command.get(3).asDouble(); //positive from waist up
// sanity check the position before moving there
if(Rxd[0] < -0.5) Rxd[0] = -0.5;
if(Rxd[0] > -0.1) Rxd[0] = -0.1;
if(Rxd[1] > 0.4) Rxd[1] = 0.4;
if(Rxd[1] < 0.0) Rxd[1] = 0.0;
if(Rxd[2] < -0.2) Rxd[2] = -0.2;
if(Rxd[2] > 0.4) Rxd[2] = 0.4;
icartR->goToPose(Rxd,Rod);
Time::delay(3);
icartR->stopControl(); //stop the right arm
}
}
}
Bottle VisuoThread::recogMSR(string &obj_name)
{
Bottle bDetect;
Bottle *bMSR=recMSRPort.read(false);
if(bMSR!=NULL)
{
bDetect=*bMSR;
obj_name=bDetect.get(0).asString().c_str();
}
return bDetect;
}
void SegmentationPoint::segment(Bottle &b)
{
if (getOutputCount()>0)
{
//send 2D x y coordinates to segmentator
Bottle request;
request.addDouble(b.get(0).asDouble());
request.addDouble(b.get(1).asDouble());
write(request);
}
}
//*************************************************************************************************************************
int MotorFrictionIdentificationThread::convertWbiIdToNumericJointId(const Bottle &b)
{
if(b.size()==0 ||
!(b.get(0).isString()) )
{
return -1;
}
string jointName = b.get(0).asString();
int jid;
bool ret = robot->getJointList().idToIndex(jointName,jid);
if(!ret)
{
jid = -1;
}
return jid;
}
bool LSSVMCalibrator::toProperty(Property &info) const
{
Bottle data;
Bottle &values=data.addList();
values.addString(impl->toString().c_str());
info.clear();
info.put("type",type.c_str());
info.put("calibration_data",data.get(0));
return Calibrator::toProperty(info);
}
bool Rectification::respond(const Bottle& command, Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n" +
"(where <n> is an integer number) \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;
}
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);
}
}
}
// reading modified from react-control reactCtrlThread::getCollisionPointsFromPort
// writing adapted from iCub_Sim.cpp OdeSdlSimulation::inspectWholeBodyContactsAndSendTouch()
bool fillSkinContactFromAggregPort(BufferedPort<Bottle> &inPort, const double amplification, skinContactList &sCL)
{
SkinPart sp = SKIN_PART_UNKNOWN;
//all in the link FoR
Vector geocenter(3,0.0); //geocenter from skin / average activation locus from the pps
Vector normal(3,0.0);
Vector force(3,0.0);
Vector moment(3,0.0); //will remain zero
double normalized_activation = 0.0;
std::vector<unsigned int> taxel_list;
taxel_list.clear(); //we will be always passing empty list
Bottle* collPointsMultiBottle = inPort.read(false);
if(collPointsMultiBottle != NULL)
{
//printf("fillSkinContactFromAggregPort(): There were %d bottles on the port.\n",collPointsMultiBottle->size());
for(int i=0; i< collPointsMultiBottle->size();i++)
{
sp = SKIN_PART_UNKNOWN;
geocenter.zero(); normal.zero(); normalized_activation = 0.0;
Bottle* collPointBottle = collPointsMultiBottle->get(i).asList();
//printf("Bottle %d contains %s \n", i,collPointBottle->toString().c_str());
sp = (SkinPart)(collPointBottle->get(0).asInt());
geocenter(0) = collPointBottle->get(1).asDouble();
geocenter(1) = collPointBottle->get(2).asDouble();
geocenter(2) = collPointBottle->get(3).asDouble();
normal(0) = collPointBottle->get(4).asDouble();
normal(1) = collPointBottle->get(5).asDouble();
normal(2) = collPointBottle->get(6).asDouble();
normalized_activation = collPointBottle->get(13).asDouble();
//hack - in current iCubGui version, in bvhnode.h, ForceArrow,
//there is an atan that gives a wrong direction for force vectors of the form (0 0 1)
force(0)=0.000001; force(1)=0.000001;
force(2) = -1.0*amplification*normalized_activation*normal(2);
//see iCubGui/src/objectsthread.h ObjectsManager::manage(iCub::skinDynLib::skinContactList &forces)
//printf("fillDynContactFromAggregPort: setting dynContact: Body part: %s Linknum: %d CoP: %s F: %s M: %s\n",
//BodyPart_s[SkinPart_2_BodyPart[sp].body].c_str(),getLinkNum(sp),geoCenter.toString(3,3).c_str(),
// (-1.0*normal).toString(3,3).c_str(),moment.toString(3,3).c_str());
skinContact sc(SkinPart_2_BodyPart[sp].body,sp,getLinkNum(sp),geocenter,geocenter,taxel_list,
amplification*normalized_activation,normal,force,moment);
// In skinManager/src/compensator.cpp, Compensator::getContacts():
// set an estimate of the force that is with normal direction and intensity equal to the pressure
// d.setForceModule(-0.05*activeTaxels*pressure*normal);
sCL.push_back(sc);
}
return true;
}
else{
//printf("fillDynContactFromAggregPort(): no tactile/pps vectors on the port.\n") ;
return false;
};
}
bool respond(const Bottle &command, Bottle &reply)
{
bool f;
if(command.get(0).asString()=="model")
{
f=loc5->changeModel(command.get(1).asString());
if(f)
{
reply.addString("Ok, model changed!" );
return true;
}
else
{
reply.addString("Something wrong in changing model :(" );
}
}
else if(command.get(0).asString()=="downsampling")
{
f=loc5->changeParameter("down", command.get(1).asInt());
if(f)
{
reply.addString("Ok, down sampling parameter changed!" );
return true;
}
else
{
reply.addString("Something wrong in changing down sampling parameter :(" );
return false;
}
}
else
return RFModule::respond(command,reply);
return true;
}
bool
getCameraLedInfo (const Bottle& b)
{
int channelCount = b.get (0).asInt ();//get number of active leds
vectorList.clear ();
for (int i = 1; i < channelCount * 5; i = i + 5)
{
LEDVector led;
led.channelName = b.get (i).asString ();//channel name
if (led.channelName == "Channel102")
continue;
//now convert led coordinates from KDL frame back to the iCub frame
/*
led.coordinates[0] = -b.get (i + 3).asDouble ();//KDL's "-z" -> icub's "x"
led.coordinates[1] = -b.get (i + 1).asDouble ();//KDL's "-x" -> icub's "y"
led.coordinates[2] = b.get (i + 2).asDouble ();//KDL's "y" -> icub's "z"
*/
//convert vz to icub frame
led.coordinates[0] = -b.get (i + 1).asDouble ();// VZ::-x -> icub::x
led.coordinates[1] = b.get (i + 2).asDouble ();// VZ::y -> icub::y
led.coordinates[2] = -b.get (i + 3).asDouble (); // VZ::-z -> icub::z
// std::cout << led.channelName << led.coordinates[0] << " " << led.coordinates[1] << " " << led.coordinates[2]
// << std::endl;
if (fabs (led.coordinates[0]) < 0.001 && fabs (led.coordinates[1]) < 0.001 && fabs (led.coordinates[2]) < 0.001)
return false;
led.coordinates[0] += 0.38;
led.coordinates[1] -= 0.095;
led.coordinates[2] -= 0.0018;
// cout << led.channelName << " " << led.coordinates[0] << " " << led.coordinates[1] << " " << led.coordinates[2]<< endl;
vectorList.push_back (led);
}
return true;
}
bool visualFilterModule::respond(const Bottle& command, Bottle& reply)
{
string helpMessage = string(getName().c_str()) +
" commands are: \n" +
"help \n" +
"quit \n" +
"set thr <n> ... set the threshold \n" +
"(where <n> is an integer number) \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;
}
bool cartControlReachAvoidThread::getAvoidanceGainFromPort()
{
Bottle* avoidanceGainBottle = inportAvoidanceGain.read(false);
if(avoidanceGainBottle != NULL){
avoidanceGain = avoidanceGainBottle->get(0).asDouble();
yDebug("getAvoidanceGainFromPort(): Reading %f from port and setting to global avoidanceGain.\n",avoidanceGain);
return true;
}
else{
return false;
}
}
void fromBottle(Bottle& bot, bool wipe=true) {
if (wipe) {
clear();
}
for (int i=0; i<bot.size(); i++) {
Value& bb = bot.get(i);
if (bb.isList()) {
Bottle *sub = bb.asList();
putBottle(bb.asList()->get(0).toString().c_str(),*sub);
}
}
}
double* YarpAdapter::readGloveInput() {
Bottle* in = inPort[GLOVE_IN].read(false);
if (in == NULL) {
return oldGloveData;
}
for (int i = 0; i < 5; i++)
oldGloveData[i] = in->get(i).asDouble();
gloveInitialized = true;
return oldGloveData;
}
void PMPthread::Bottle2Matrix(Bottle Bot, Matrix &m)
{
int k=0;
for(int i=0; i<m.rows(); i++)
{
for(int j=0; j<m.cols(); j++)
{
m(i,j) = Bot.get(k).asDouble();
k++;
}
}
}