C++ Bottle::size方法代码示例

void partMover::home_click(GtkButton *button, gtkClassData* currentClassData)
{
  partMover *currentPart = currentClassData->partPointer;
  int * joint = currentClassData->indexPointer;
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  IControlCalibration2 *ical = currentPart->cal;
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  //fprintf(stderr, "Retrieving finder \n");
  ResourceFinder *fnd = currentPart->finder;
  //fprintf(stderr, "Retrieved finder: %p \n", fnd);
  char buffer1[800];
  char buffer2[800];

  strcpy(buffer1, currentPart->partLabel);
  strcpy(buffer2, strcat(buffer1, "_zero"));
  //fprintf(stderr, "Finder retrieved %s\n", buffer2);

  if (!fnd->findGroup(buffer2).isNull() && !fnd->isNull())
    {
      //fprintf(stderr, "Home group was not empty \n");
      bool ok = true;
      Bottle xtmp;
      xtmp = fnd->findGroup(buffer2).findGroup("PositionZero");
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      double positionZero = xtmp.get(*joint+1).asDouble();
      //fprintf(stderr, "%f\n", positionZero);

      xtmp = fnd->findGroup(buffer2).findGroup("VelocityZero");
      //fprintf(stderr, "VALUE VEL is %d \n", fnd->findGroup(buffer2).find("VelocityZero").toString().c_str());
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      double velocityZero = xtmp.get(*joint+1).asDouble();
      //fprintf(stderr, "%f\n", velocityZero);

      if(!ok)
	dialog_message(GTK_MESSAGE_ERROR,(char *) "Check the number of entries in the group",  buffer2, true);
      else
	{
	  ipos->setRefSpeed(*joint, velocityZero);
	  ipos->positionMove(*joint, positionZero);
	}
    }
  else
    {
      //		currentPart->dialog_message(GTK_MESSAGE_ERROR,"No calib file found", strcat("Define a suitable ", strcat(currentPart->partLabel, "Calib")), true);        
      dialog_message(GTK_MESSAGE_ERROR,(char *) "No zero group found in the supplied file. Define a suitable",  buffer2, true);   
    }
  return;
}

void fingerDetector::setModel(Bottle q_0, Bottle q_1, double m, double M, double t, double T)
{
    q0.resize(q_0.size());
    q1.resize(q_1.size());
    for(int i =0; i < q_0.size(); i++)
        q0(i) = q_0.get(i).asDouble();
    for(int i =0; i < q_1.size(); i++)
        q1(i) = q_1.get(i).asDouble();
    min = m;
    max = M;
    minT = t;
    maxT = T;
}

bool embObjMultiEnc::fromConfig(yarp::os::Searchable &_config)
{
    yDebug()<< "configurazione: ";;
    yDebug() << _config.toString();

    Bottle general = _config.findGroup("JOINTS");
    if(general.isNull())
    {
        yError() << "embObjMultiEnc cannot find general group";
        return false;
    }
    
    Bottle jointsbottle = general.findGroup("listofjoints");
    if (jointsbottle.isNull())
    {
        yError() << "embObjMultiEnc cannot find listofjoints param";
        return false;
    }
          
    Bottle encsbottle = general.findGroup("encoderConversionFactor");
    if (encsbottle.isNull())
    {
        yError() << "embObjMultiEnc cannot find encoderConversionFactor param";
        return false;
    }
     
 
    //jointsbottle contains: "listofjoints 0 1 2 3. So the num of joints is jointsbottle->size() -1 " 
    numofjoints = jointsbottle.size() -1;  
    
    listofjoints.clear();
    for (int i = 1; i < jointsbottle.size(); i++)  listofjoints.push_back(jointsbottle.get(i).asInt());

    yDebug()<< " embObjMultiEnc List of joints: " << numofjoints;
    for(int i=0; i<numofjoints; i++) yDebug() << "pos="<< i << "val="<<  listofjoints[i];
   
    analogdata.resize(numofencperjoint*numofjoints, 0.0);
    encoderConversionFactor.resize(numofencperjoint*numofjoints, 1.0);

    if (numofencperjoint*numofjoints!=encsbottle.size()-1)
    {
        yError() << "embObjMultiEnc invalid size of encoderConversionFactor param";
        return false;
	}
	for (int i=0; i<encsbottle.size()-1; i++)
	{
		encoderConversionFactor[i]=encsbottle.get(i+1).asDouble();
	}
         
    return true;
}

	virtual ArActionDesired *fire (ArActionDesired currentDesired)
	{
	Bottle SoundBottle;
	if(Mycopyofmodule->GetBottleData("SOUNDin",&SoundBottle,SamgarModule::NoStep)==true)
		{
			char tune[40];
			for(int uu = 0;uu<SoundBottle.size();uu++)
			{
				tune[uu]=SoundBottle.get(uu).asInt();
			}
			robot.comStrN(ArCommands::SAY,tune,SoundBottle.size());
		}
	return &myDesired;
	}

void toXmlRpcValue(Value& vin, XmlRpcValue& vout) {
    if (vin.isInt()) {
        vout = vin.asInt();
    } else if (vin.isDouble()) {
        vout = vin.asDouble();
    } else if (vin.isString()) {
        vout = vin.asString();
    } else if (vin.isVocab()) {
        vout = ConstString("[") + vin.toString() + "]";
    } else if (vin.isList()) {
        Bottle *bot = vin.asList();
        bool struc = true;
        int offset = 0;
        ConstString tag = bot->get(0).asString();
        if (tag=="list") {
            struc = false;
            offset = 1;
        } else if (tag=="dict") {
            struc = true;
            offset = 1;
        } else {
            // auto-detect
            for (int i=0; i<bot->size(); i++) {
                Value& vi = bot->get(i);
                if (!vi.isList()) {
                    struc = false;
                    break;
                }
                if (vi.asList()->size()!=2) {
                    struc = false;
                    break;
                }
            }
        }
        if (struc) {
            vout = XmlRpcValue();
            for (int i=offset; i<bot->size(); i++) {
                Bottle *boti = bot->get(i).asList();
                XmlRpcValue& vouti=vout[boti->get(0).toString()]=XmlRpcValue();
                toXmlRpcValue(boti->get(1),vouti);
            }
        } else {
            vout = XmlRpcValue();
            for (int i=offset; i<bot->size(); i++) {
                XmlRpcValue& vouti = vout[i] = XmlRpcValue();
                toXmlRpcValue(bot->get(i),vouti);
            }
        }
    }
}

    virtual void testAcquire() {
        report(0, "checking acquire/release...");

        BufferedPort<Bottle> in;
        BufferedPort<Bottle> out;
        in.setStrict();
        out.setStrict();
        in.open("/in");
        out.open("/out");
        Network::connect("/out","/in");

        out.prepare().fromString("1");
        out.write(true);
        
        Bottle *bot = in.read();
        checkTrue(bot!=NULL,"Inserted message received");
        if (bot!=NULL) {
            checkEqual(bot->size(),1,"right length");
        }

        out.prepare().fromString("1 2");
        out.write(true);

        void *key = in.acquire();
        Bottle *bot2 = in.read();
        checkTrue(bot2!=NULL,"Inserted message received");
        if (bot2!=NULL) {
            checkEqual(bot2->size(),2,"right length");
        }

        out.prepare().fromString("1 2 3");
        out.write(true);
        
        void *key2 = in.acquire();
        Bottle *bot3 = in.read();
        checkTrue(bot3!=NULL,"Inserted message received");
        if (bot3!=NULL) {
            checkEqual(bot3->size(),3,"right length");
        }
        if (bot2!=NULL) {
            checkEqual(bot2->size(),2,"original (2) still ok");
        }
        if (bot!=NULL) {
            checkEqual(bot->size(),1,"original (1) still ok");
        }

        in.release(key);
        in.release(key2);
    }

void Manager::processMotionPoints(Bottle &b)
{
   // fprintf(stdout,"create mat\n");

    //create MAT image
    cv::Mat imgMat(Size(320,240),CV_8UC3);
    cv::Mat imgClean(Size(320,240),CV_8UC3);
    //vector<Point> points;
    imgMat = Scalar::all(0);
    imgClean = Scalar::all(255);
   // fprintf(stdout,"filling up the data\n");
    for (int x=1; x<b.size(); x++)
    {
        Point pt;
        pt.x = b.get(x).asList()->get(0).asInt();
        pt.y = b.get(x).asList()->get(1).asInt();
        imgMat.at<cv::Vec3b>(pt.y,pt.x)[0] = 255 ;
        imgMat.at<cv::Vec3b>(pt.y,pt.x)[1] = 0 ;
        imgMat.at<cv::Vec3b>(pt.y,pt.x)[2] = 0 ;
        imgClean.at<cv::Vec3b>(pt.y,pt.x)[0] = 255 ;
        imgClean.at<cv::Vec3b>(pt.y,pt.x)[1] = 0 ;
        imgClean.at<cv::Vec3b>(pt.y,pt.x)[2] = 0 ;
    }
    //imgClean = imgMat;
    int n = 10;
    int an = n > 0 ? n : -n;
    int element_shape = MORPH_RECT;
    Mat element = getStructuringElement(element_shape, Size(an*2+1, an*2+1), Point(an, an) );
    morphologyEx(imgMat, imgMat, CV_MOP_CLOSE, element);

    Bottle data;
    //fprintf(stdout,"before process image\n");
    data = processImage(b, imgMat, imgClean, lineDetails); //image analisis and bottle cleaning
    //fprintf(stdout,"before process blobs\n");
    
    if (data.size() > 0)
        processBlobs(data, imgClean, lineDetails); // kmeans

    ImageOf<PixelRgb> outImg;// = new ImageOf<PixelRgb>;
    //fprintf(stdout,"done1 with data %d %d\n", imgClean.cols, imgClean.rows);
    outImg.resize( imgClean.cols, imgClean.rows );
    IplImage ipl_img = imgClean;
    cvCopyImage(&ipl_img, (IplImage*)outImg.getIplImage());
    imgOutPort.prepare() = outImg;
    imgOutPort.write();
    //delete[] outImg;
    //fprintf(stdout,"ok\n");
    }

/* Connect the opc client to an OPC */
Bottle opcManager::connect(Bottle bInput)
{
    Bottle bOutput;

    if (bInput.size() != 2) {
        bOutput.addString("Error in connect, wrong number of input");
    }

    if (!bInput.get(1).isString()){
        bOutput.addString("Error in connect, wrong format of input");
    }

    realOPC = new OPCClient(moduleName.c_str());
    int iTry = 0;
    while (!realOPC->isConnected())
    {
        cout << "Connecting to realOPC..." << realOPC->connect("OPC") << endl;
        Time::delay(0.5);
        iTry++;
        if (iTry > 20)
        {
            bOutput.addString("Connection failed, please check your port");
            return bOutput;
        }
    }
    realOPC->update();
    bOutput.addString("Connection done");
    return bOutput;
}

	bool respond(const Bottle& command, Bottle& reply) {

		//handle information requests
		string msg(command.get(0).asString().c_str());
		if (msg == "rate") {
			if (command.size() < 2) {
				reply.add(-1);
			}
			else {
				int newrate = command.get(1).asInt();
				bool rssuc;
				if (newrate > 0) {
					rssuc = thr->setRate(newrate);
					if (rssuc)
						reply.add(1);
					else
						reply.add(-1);
				}
			}
		}
		else {
			reply.add(-1);
		}


		return true;

	}

void OpenNI2SkeletonData::storeData(Bottle& b){
    Bottle *list;
    int userID;
    UserSkeleton *tuserSkeleton;
    if(b.get(0).isString()){
        userID = b.get(1).asInt();
        string vocab = b.get(0).asString().c_str();
        if(vocab.compare("CALIBRATING FOR USER") == 0){
            userSkeleton[userID-1].skeletonState = nite::SKELETON_CALIBRATING;
        }
    }
    
    else if(b.get(0).isList()){
        list = b.get(0).asList();
        userID = list->get(1).asInt();
        userSkeleton[userID-1].skeletonState = nite::SKELETON_TRACKED;//USER STATUS
        tuserSkeleton = &(userSkeleton[userID-1]);
        int jointIndex = 0;
        for(int i = 1; i < b.size(); i+=6){
            list = b.get(i+1).asList();// position elements
            tuserSkeleton->skeletonPointsPos[jointIndex][0] = list->get(0).asDouble();
            tuserSkeleton->skeletonPointsPos[jointIndex][1] = list->get(1).asDouble();
            tuserSkeleton->skeletonPointsPos[jointIndex][2] = list->get(2).asDouble();
            tuserSkeleton->skeletonPosConf[jointIndex] = b.get(i+2).asDouble();
            list = b.get(i+4).asList();// orientation elements
            tuserSkeleton->skeletonPointsOri[jointIndex][0] = list->get(0).asDouble();
            tuserSkeleton->skeletonPointsOri[jointIndex][1] = list->get(1).asDouble();
            tuserSkeleton->skeletonPointsOri[jointIndex][2] = list->get(2).asDouble();
            tuserSkeleton->skeletonPointsOri[jointIndex][3] = list->get(3).asDouble();
            tuserSkeleton->skeletonOriConf[jointIndex] = b.get(i+5).asDouble();
            jointIndex++;
        }
    }
}

bool utManagerThread::getPointFromStereo()
{
    Bottle cmdSFM;
    Bottle respSFM;
    cmdSFM.clear();
    respSFM.clear();
    cmdSFM.addString("Root");
    cmdSFM.addInt(int(templatePFTrackerPos(0)));
    cmdSFM.addInt(int(templatePFTrackerPos(1)));
    SFMrpcPort.write(cmdSFM, respSFM);

    // Read the 3D coords and compute the distance to the set reference frame origin
    if (respSFM.size() == 3)
    {
        Vector SFMtmp(3,0.0);
        SFMtmp(0) = respSFM.get(0).asDouble(); // Get the X coordinate
        SFMtmp(1) = respSFM.get(1).asDouble(); // Get the Y coordinate
        SFMtmp(2) = respSFM.get(2).asDouble(); // Get the Z coordinate

        if (SFMtmp(0) == 0.0 && SFMtmp(1) == 0.0 && SFMtmp(2) == 0.0)
        {
            return false;
        }

        SFMPos = SFMtmp;
        return true;
    } 

    return false;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
bool iCub::skinForceControl::identifyCommand(const Bottle &commandBot, const string *cmdList, unsigned int cmdListSize, unsigned int &cmdId, Bottle &params)
{
    for(unsigned int i=0; i<cmdListSize; i++){
		stringstream stream(cmdList[i]);
		string word;
		int wordCounter=0;
		bool found = true;

		while(stream>>word){
			if (commandBot.get(wordCounter).asString() != word.c_str()){
				found=false;
				break;
			}
            //printf("%s = %s\n", commandBot.get(wordCounter).asString().c_str(), word.c_str());
			wordCounter++;
		}
		if(found){
			cmdId = i;
            for(int k=wordCounter; k<commandBot.size(); k++)
                params.add(commandBot.get(k));
			return true;
		}
	}

	return false;
}

bool SerialDeviceDriver::send(const Bottle& msg)
{
    if (msg.size() > 0) {
        int message_size = msg.get(0).asString().length();

        if (message_size > 0) {
            if (verbose)
            {
                yDebug("Sending string: %s", msg.get(0).asString().c_str());
            }
    
            // Write message to the serial device
	    
            ssize_t bytes_written = _serial_dev.send_n((void *) msg.get(0).asString().c_str(), message_size);

            if (bytes_written == -1) {
                ACE_ERROR((LM_ERROR, ACE_TEXT ("%p\n"), ACE_TEXT ("send")));
                return false;
            }
        } else {
           if (verbose) yDebug("The input command bottle contains an empty string. \n");
           return false;
        }
    } else {
        if (verbose) yDebug("The input command bottle is empty. \n");
        return false;
    }

    return true;
} 

    string toString()
    {
        ostringstream stream;
        stream<<"["<<part<<"]"<<endl;
        stream<<"device   "<<device<<endl;

        for (int i=0; i<rJoints.size(); i++)
        {
            int j=rJoints.get(i).asInt();
            PidData &pid=pids[j];

            Property prop;
            prop.put("Kp",pid.Kp);
            prop.put("Ki",pid.Ki);
            prop.put("Kd",pid.Kd);
            prop.put("scale",pid.scale);
            prop.put("st_up",pid.st_up);
            prop.put("st_down",pid.st_down);
            prop.put("encs_ratio",pid.encs_ratio);
            prop.put("status",pid.status==download?"download":"upload");

            stream<<"joint_"<<j<<"   ";
            stream<<prop.toString().c_str()<<endl;
        }

        return stream.str();
    }

 void checkCallback() {
     report(0, "checking callback...");
     BufferedPort<Bottle> out;
     PortReaderBufferTestHelper in;
     out.open("/out");
     in.open("/in");
     in.useCallback();
     Network::connect("/out","/in");
     Network::sync("/out");
     Network::sync("/in");
     out.prepare().fromString("1 2 3");
     out.write();
     int rep = 0;
     while (in.count==0 && rep<50) {
         Time::delay(0.1);
         rep++;
     }
     checkEqual(in.count,3,"got message #1");
     in.disableCallback();
     out.prepare().fromString("1 2 3 4");
     out.write(true);
     Bottle *datum = in.read();
     checkTrue(datum!=NULL, "got message #2");
     checkEqual(datum->size(),4,"message is ok");
     in.useCallback();
     in.count = 0;
     out.prepare().fromString("1 2 3 4 5");
     out.write(true);
     rep = 0;
     while (in.count==0 && rep<50) {
         Time::delay(0.1);
         rep++;
     }
     checkEqual(in.count,5,"got message #3");
 }