示例#1
0
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;
}
示例#2
0
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();
}
示例#3
0
    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;
}
示例#5
0
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;
}
示例#6
0
 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");
 }
示例#7
0
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;
}
示例#8
0
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;
}
示例#9
0
文件: utils.cpp 项目: atabakd/karma
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();
    }
}
示例#10
0
/**
* 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);
}
示例#11
0
文件: utils.cpp 项目: atabakd/karma
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() );
    }
}
示例#12
0
//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();
}
示例#13
0
 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;
 }
示例#14
0
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;
}
示例#15
0
    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;
    }
示例#16
0
    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
            }
        }
    }
示例#17
0
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;
}
示例#18
0
文件: utils.cpp 项目: atabakd/karma
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;
}
示例#20
0
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);
}
示例#21
0
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;
}
示例#22
0
    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;
        };   
    }
示例#24
0
    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;

    }
示例#25
0
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;
}
示例#26
0
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;
    }
}
示例#28
0
 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;
}
示例#30
0
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++;
		}
	}
}