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

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;
}

void Bottle::copy(const Bottle& alt, int first, int len)
{
    implementation->edit();
    if (alt.isNull()) {
        clear();
        implementation->invalid = true;
        return;
    }
    implementation->copyRange(alt.implementation, first, len);
}

static bool checkParam(const Bottle& input, RGBDSensorParamParser::RGBDParam& param, bool& found)
{
    bool ret = false;
    Bottle bt=input.findGroup(param.name).tail(); // the first element is the name of the parameter

    if (!bt.isNull())
    {
        Bottle* b;
        if (param.size>1 && bt.size()==1)
        {
            b = bt.get(0).asList();
        }
        else
            b = &bt;
        if (b->isNull())
        {
            yError()<<"RGBDSensorParamParser: check"<<param.name<<"in config file";
            return false;
        }
        if (b->size() != param.size)
        {
            yError() << "RGBDSensorParamParser: parameter" << param.name << "size should be" << param.size;
            return false;
        }
        param.val.resize(param.size);
        for (size_t i=0;i<b->size();i++)
        {
            ret = true;
            param.val[i] = b->get(i);
            found = true;
        }
    }
    else
    {
        ret   = true;
        found = false;
    }
    return ret;
}

Vector ReachManager::Solve(const Vector &xd, string partName, string &resultPart)
{
    double minNorm2;
    Bottle *resultQBottle;
    Vector resultQ(7);


    if(partName == "both")
    {
        //ask to resolve for some xyz position
        Bottle cmd, replyLeft, replyRight;
        cmd.clear();

        cout << "SOLVING WITH RIGHT ARM" << endl;

        iCubArmCartesianSolver::addTargetOption(cmd,xd);

        iKinPorts["right"]->out.write(cmd);
        iKinPorts["left"]->out.write(cmd);
        iKinPorts["right"]->in.wait(replyRight);
        iKinPorts["left"]->in.wait(replyLeft);

        if(replyRight.isNull() || replyLeft.isNull())
        {
            cout << "\n\nERROR ! \n\n" << endl;
            resultPart = "none";
            return resultQ;
        }

        Bottle *xdBottleRight = CartesianSolver::getTargetOption(replyRight);
        Bottle *xBottleRight = CartesianSolver::getEndEffectorPoseOption(replyRight);
        Bottle *qBottleRight = CartesianSolver::getJointsOption(replyRight);


        if (!xdBottleRight || !xBottleRight || !qBottleRight)
        {
            cout << "\n\nERROR ! \n\n" << endl;
            resultPart = "none";
            return resultQ;
        }
        if (xdBottleRight->isNull() || xBottleRight->isNull() || qBottleRight->isNull())
        {
            cout << "\n\nERROR ! \n\n" << endl;
            resultPart = "none";
            return resultQ;
        }

        Vector deltaRight(3);
        deltaRight[0] = xBottleRight->get(0).asDouble() - xdBottleRight->get(0).asDouble();
        deltaRight[1] = xBottleRight->get(1).asDouble() - xdBottleRight->get(1).asDouble();
        deltaRight[2] = xBottleRight->get(2).asDouble() - xdBottleRight->get(2).asDouble();

        double deltaRightNorm2 = deltaRight[0]*deltaRight[0] + deltaRight[1]*deltaRight[1] + deltaRight[2]*deltaRight[2];

        cout << "right error : " << sqrt(deltaRightNorm2) << endl;
        cout<<"xd      ="<<xdBottleRight->toString()<<endl;
        cout<<"x       ="<<xBottleRight->toString()<<endl;
        cout<<"q [rad] ="<<qBottleRight->toString()<<endl;
        cout<<endl;


        cout << "SOLVING WITH LEFT ARM" << endl;

        Bottle *xdBottleLeft = CartesianSolver::getTargetOption(replyLeft);
        Bottle *xBottleLeft = CartesianSolver::getEndEffectorPoseOption(replyLeft);
        Bottle *qBottleLeft = CartesianSolver::getJointsOption(replyLeft);

        if (!xdBottleLeft || !xBottleLeft || !qBottleLeft)
        {
            cout << "\n\nERROR ! \n\n" << endl;
            resultPart = "none";
            return resultQ;
        }
        if (xdBottleLeft->isNull() || xBottleLeft->isNull() || qBottleLeft->isNull())
        {
            cout << "\n\nERROR ! \n\n" << endl;
            resultPart = "none";
            return resultQ;
        }

        Vector deltaLeft(3);
        deltaLeft[0] = xBottleLeft->get(0).asDouble() - xdBottleLeft->get(0).asDouble();
        deltaLeft[1] = xBottleLeft->get(1).asDouble() - xdBottleLeft->get(1).asDouble();
        deltaLeft[2] = xBottleLeft->get(2).asDouble() - xdBottleLeft->get(2).asDouble();

        double deltaLeftNorm2 = deltaLeft[0]*deltaLeft[0] + deltaLeft[1]*deltaLeft[1] + deltaLeft[2]*deltaLeft[2];

        cout << "left error : " << sqrt(deltaLeftNorm2) << endl;
        cout<<"xd      ="<<xdBottleLeft->toString()<<endl;
        cout<<"x       ="<<xBottleLeft->toString()<<endl;
        cout<<"q [rad] ="<<qBottleLeft->toString()<<endl;
        cout<<endl;

        if(deltaLeftNorm2<deltaRightNorm2)
        {
            resultPart = "left";
            resultQBottle = qBottleLeft;
        }
        else
        {
//.........这里部分代码省略.........

bool   SimToolLoaderModule::configure(yarp::os::ResourceFinder &rf) {

    Bottle table;
    Bottle temp;
    string objectName = "obj";

    /* module name */
    moduleName = rf.check("name", Value("simtoolloader"),
                          "Module name (string)").asString();

    setName(moduleName.c_str());

    /* Hand used */
    hand=rf.find("hand").asString().c_str();
    if ((hand!="left") && (hand!="right"))
        hand="right";

    /* port names */
    simToolLoaderSimOutputPortName  = "/";
    simToolLoaderSimOutputPortName += getName( rf.check("simToolLoaderSimOutputPort",
                                     Value("/sim:rpc"),
                                     "Loader output port(string)")
                                     .asString() );

    simToolLoaderCmdInputPortName  = "/";
    simToolLoaderCmdInputPortName += getName( rf.check("simToolLoaderCmdInputPort",
                                     Value("/cmd:i"),
                                     "Loader input port(string)")
                                     .asString() );

    /* open ports */
    if (!simToolLoaderSimOutputPort.open(
            simToolLoaderSimOutputPortName.c_str())) {

        cout << getName() << ": unable to open port"
        << simToolLoaderSimOutputPortName << endl;
        return false;
    }

    if (!simToolLoaderCmdInputPort.open(
            simToolLoaderCmdInputPortName.c_str())) {

        cout << getName() << ": unable to open port"
        << simToolLoaderCmdInputPortName << endl;
        return false;
    }

    /* Rate thread period */
    threadPeriod = rf.check("threadPeriod", Value(0.5),
        "Control rate thread period key value(double) in seconds ").asDouble();

    /* Read Table Configuration */
    table = rf.findGroup("table");

    /* Read the Objects configurations */
    vector<Bottle> object;

    cout << "Loading objects to buffer" << endl;
    bool noMoreModels = false;
    int n =0;
    while (!noMoreModels){      // read until there are no more objects.
        stringstream s;
        s.str("");
        s << objectName << n;
        string objNameNum = s.str();
        temp = rf.findGroup("objects").findGroup(objNameNum);

        if(!temp.isNull()) {
            cout << "object" << n << ", id: " << objNameNum;
            cout << ", model: " << temp.get(2).asString() << endl;
            object.push_back(temp);
            temp.clear();
            n++;
        }else {
            noMoreModels = true;
        }
    }
    numberObjs = n;

    cout << "Loaded " << object.size() << " objects " << endl;

    /* Create the control rate thread */
    ctrlThread = new CtrlThread(&simToolLoaderSimOutputPort,
                                &simToolLoaderCmdInputPort,
                                threadPeriod,
                                table, object,hand);
    /* Starts the thread */
    if (!ctrlThread->start()) {
        delete ctrlThread;
        return false;
    }

    return true;
}

bool embObjAnalogSensor::open(yarp::os::Searchable &config)
{
    std::string str;
    if(config.findGroup("GENERAL").find("verbose").asBool())
        str=config.toString().c_str();
    else
        str="\n";
    yTrace() << str;

    // Read stuff from config file
    if(!fromConfig(config))
    {
        yError() << "embObjAnalogSensor missing some configuration parameter. Check logs and your config file.";
        return false;
    }

    // Tmp variables
    Bottle          groupEth;
    ACE_TCHAR       address[64];
    ACE_UINT16      port;
    bool            ret;

    Bottle groupTransceiver = Bottle(config.findGroup("TRANSCEIVER"));
    if(groupTransceiver.isNull())
    {
        yError() << "embObjAnalogSensor::open(): Can't find TRANSCEIVER group in xml config files";
        return false;
    }

    Bottle groupProtocol = Bottle(config.findGroup("PROTOCOL"));
    if(groupProtocol.isNull())
    {
        yError() << "embObjAnalogSensor::open(): Can't find PROTOCOL group in xml config files";
        return false;
    }

    // Get both PC104 and EMS ip addresses and port from config file
    groupEth  = Bottle(config.findGroup("ETH"));
    Bottle parameter1( groupEth.find("PC104IpAddress").asString() );    // .findGroup("IpAddress");
    port      = groupEth.find("CmdPort").asInt();              // .get(1).asInt();
    snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%s", parameter1.toString().c_str());
    _fId.pc104IPaddr.port = port;

    Bottle parameter2( groupEth.find("IpAddress").asString() );    // .findGroup("IpAddress");
    snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%s", parameter2.toString().c_str());
    _fId.boardIPaddr.port = port;

    sscanf(_fId.boardIPaddr.string,"\"%d.%d.%d.%d", &_fId.boardIPaddr.ip1, &_fId.boardIPaddr.ip2, &_fId.boardIPaddr.ip3, &_fId.boardIPaddr.ip4);
    sscanf(_fId.pc104IPaddr.string,"\"%d.%d.%d.%d", &_fId.pc104IPaddr.ip1, &_fId.pc104IPaddr.ip2, &_fId.pc104IPaddr.ip3, &_fId.pc104IPaddr.ip4);

    snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%u.%u.%u.%u:%u", _fId.boardIPaddr.ip1, _fId.boardIPaddr.ip2, _fId.boardIPaddr.ip3, _fId.boardIPaddr.ip4, _fId.boardIPaddr.port);
    snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%u.%u.%u.%u:%u", _fId.pc104IPaddr.ip1, _fId.pc104IPaddr.ip2, _fId.pc104IPaddr.ip3, _fId.pc104IPaddr.ip4, _fId.pc104IPaddr.port);

    // debug info
    memset(info, 0x00, sizeof(info));
    snprintf(info, sizeof(info), "embObjAnalogSensor: referred to EMS: %d at address %s\n", _fId.boardNumber, address);
    snprintf(_fId.name, sizeof(_fId.name), "%s", info);       // Saving User Friendly Id

    // Set dummy values
    _fId.boardNumber  = FEAT_boardnumber_dummy;

    Value val = config.findGroup("ETH").check("Ems", Value(1), "Board number");
    if(val.isInt())
        _fId.boardNumber = val.asInt();
    else
    {
        yError () << "embObjAnalogSensor: EMS Board number identifier not found for IP" << _fId.pc104IPaddr.string;
        return false;
    }


    ethManager = TheEthManager::instance();
    if(NULL == ethManager)
    {
        yFatal() << "embObjAnalogSensor::open() cannot instantiate ethManager";
        return false;
    }

    // N.B.: use a dynamic_cast<> to extract correct interface when using this pointer
    _fId.interface = this;

    _fId.endpoint = eoprot_endpoint_analogsensors;
    if(AS_STRAIN == _as_type)
        _fId.type = ethFeatType_AnalogStrain;
    else if(AS_MAIS == _as_type)
        _fId.type = ethFeatType_AnalogMais;

    /* Once I'm ok, ask for resources, through the _fId struct I'll give the ip addr, port and
    *  and boardNum to the ethManager in order to create the ethResource requested.
    */
    res = ethManager->requestResource(config, groupTransceiver, groupProtocol, _fId);
    if(NULL == res)
    {
        yError() << "embObjAnalogSensor::open() fails because could not instantiate the ethResource board" << _fId.boardNumber << " ... unable to continue";
        return false;
    }

    if(false == res->isEPmanaged(eoprot_endpoint_analogsensors))
    {
        yError() << "embObjAnalogSensor::open() detected that EMS "<< _fId.boardNumber << " does not support analog sensors";
//.........这里部分代码省略.........

bool MainWindow::initGuiStatus(){
    Bottle reply = portThread.sendRpcCommand(true, get_binarization);
    if(string(reply.toString().c_str()).compare("on") == 0){
        ui->btnBinarization->setEnabled(true);
        ui->btnBinarization->setText("ON");
        ui->btnBinarization->setChecked(true);
    }else{
        ui->btnBinarization->setEnabled(true);
        ui->btnBinarization->setText("OFF");
        ui->btnBinarization->setChecked(false);
    }

    reply = portThread.sendRpcCommand(true, get_smooth_filter);
    if(string(reply.toString().c_str()).compare("on") == 0){
        ui->btnSmooth->setText("ON");
        ui->sliderContainer->setEnabled(true);
        ui->btnSmooth->setChecked(true);
    }else{
        ui->btnSmooth->setText("OFF");
        ui->sliderContainer->setEnabled(false);
        ui->btnSmooth->setChecked(false);
    }

    reply = portThread.sendRpcCommand(true, get_smooth_factor);
    currentSmoothFactor = reply.get(0).asDouble();
    ui->sliderScaleSmooth->setValue(currentSmoothFactor * 10);

    onSmoothValueChanged(currentSmoothFactor * 10);
    //gtk_adjustment_set_value(scaleSmooth->range.adjustment, currentSmoothFactor);

    reply = portThread.sendRpcCommand(true, get_threshold);
    if(reply.isNull() || reply.size()==0 || !reply.get(0).isInt()){
        printLog("Error while getting the safety threshold");
        return false;
    }else{
        currentThreshold = reply.get(0).asInt();
        ui->spinThreashold->setValue(currentThreshold);
    }

    reply = portThread.sendRpcCommand(true, get_gain);
    if(reply.isNull() || reply.size()==0 || (!reply.get(0).isDouble() && !reply.get(0).isInt())){
        printLog("Error while getting the compensation gain");
        return false;
    }else{
        currentCompGain = reply.get(0).asDouble();
        ui->spinCompGain->setValue(currentCompGain);
    }

    reply = portThread.sendRpcCommand(true,get_cont_gain);
    if(reply.isNull() || reply.size()==0 || (!reply.get(0).isDouble() && !reply.get(0).isInt())){
        printLog("Error while getting the contact compensation gain");
        return false;
    }else{
        currentContCompGain = reply.get(0).asDouble();
        ui->spinCompContactGain->setValue(currentContCompGain);
    }

    reply = portThread.sendRpcCommand(true,get_max_neigh_dist);
    if(reply.isNull() || reply.size()==0 || (!reply.get(0).isDouble() && !reply.get(0).isInt())){
        printLog("Error while getting the max neighbor distance");
        return false;
    }else{
        currentMaxNeighDist = reply.get(0).asDouble();
        ui->spinNeighbor->setValue(currentMaxNeighDist * 100);
        //gtk_adjustment_set_value(spinMaxNeighDist->adjustment, 1e2*currentMaxNeighDist);
    }

    // get module information
    reply = portThread.sendRpcCommand(true, get_info);
    if(reply.isNull() || reply.size()!=3){
        printLog("Error while reading the module information");
        //gtk_label_set_text(lblInfo, reply.toString().c_str());
        return false;
    }
    stringstream ss;
    ss<< reply.get(0).toString().c_str()<< endl;
    ss<< reply.get(1).toString().c_str()<< "\nInput ports:";
    Bottle* portList = reply.get(2).asList();
    portNames.resize(portList->size()/2);
    portDim.resize(portList->size()/2);
    //int numTaxels = 0;
    for(unsigned int i=0;i<portDim.size();i++){
        portNames[i] = portList->get(i*2).toString().c_str();
        portDim[i] = portList->get(i*2+1).asInt();
        //numTaxels += portDim[i];
        ss<< "\n - "<< portNames[i]<< " ("<< portDim[i]<< " taxels)";
    }
    ui->infoPanel->setPlainText(QString("%1").arg(ss.str().c_str()));
    //gtk_label_set_text(lblInfo, ss.str().c_str());


    // check whether the skin calibration is in process
    reply = portThread.sendRpcCommand(true, is_calibrating);
    if(string(reply.toString().c_str()).compare("yes")==0){
        loadingWidget.start();
        calibratingTimer.start();
        //gtk_widget_show(GTK_WIDGET(progBarCalib));
        //g_timeout_add(100, progressbar_calibration, NULL);
        //gtk_widget_set_sensitive(GTK_WIDGET(btnCalibration), false);
    }
//.........这里部分代码省略.........

bool learnPrimitive::updateAction(ResourceFinder &rf){
    Bottle bOutput;

    //1. Primitive
    Bottle bAction = rf.findGroup("Action");

    if (!bAction.isNull())
    {
        Bottle * bActionName = bAction.find("ActionName").asList();
        Bottle * bActionArg  = bAction.find("ActionArg").asList();

       if(bActionName->isNull() || bActionArg->isNull()){
           yError() << " [updateAction] : one of the primitiveAction conf is null : ActionName, ActionArg" ;
            return false ;
        }

        int actionSize = -1 ;
        if(bActionName->size() == bActionArg->size()){
            actionSize =  bActionName->size() ;
        } else {
            yError() << " [updateAction] : one of the Action conf has different size!" ;
            return false ;
        }

        if(actionSize == 0) {
            yWarning() << " [updateAction] : there is no Action defined at startup!" ;
        } else {

            for(int i = 0; i < actionSize ; i++) {
                //insert protoaction even if already there
                Bottle bSubAction;
                string currentActionName = bActionName->get(i).asString();
                string currentActionArg  = bActionArg->get(i).asString();

                bSubAction.addString(currentActionName);
                bSubAction.addString(currentActionArg);

                string concat = currentActionName + "_" + currentActionArg;

               // yInfo() << " Looking for " << concat ;

                Bottle bCurrent = rf.findGroup(concat);
                if(bCurrent.isNull()){
                    yError() << " [updateAction] : " << concat << "is NOT defined" ;
                    return false ;
                }

                Bottle * bListSubAction = bCurrent.find("actionList").asList();

                //yInfo() << "Subaction found : " << bListSubAction->toString() ;
                if(bListSubAction->isNull()){
                    yError() << " [updateAction] : " << concat << "is there but is not defined (actionList)" ;
                    return false ;
                }

                bSubAction.addList() = *bListSubAction ;
                yInfo() << "Complex Action added : (" <<bSubAction.get(0).asString() << ", " << bSubAction.get(1).asString() << ", ( " << bSubAction.get(2).asList()->toString()<< "))" ;
                vActionBottle.push_back(bSubAction);
            }
        }
    } else {
        yError() << " error in learnPrimitive::updatePrimitive | Primitive_Action is NOT defined in the learnPrimitive.ini";
        return false;
    }


    return true;
}

bool learnPrimitive::updatePrimitive(ResourceFinder &rf){
    Bottle bOutput;

    //1. Primitive
    Bottle bPrimitiveAction = rf.findGroup("Primitive_Action");

    if (!bPrimitiveAction.isNull())
    {
        Bottle * bPrimitiveActionName = bPrimitiveAction.find("primitiveActionName").asList();
        Bottle * bPrimitiveActionArg  = bPrimitiveAction.find("primitiveActionArg").asList();

       if(bPrimitiveActionName->isNull() || bPrimitiveActionArg->isNull()){
           yError() << " [updatePrimitiveAction] : one of the primitiveAction conf is null : primitiveActionName, primitiveActionArg" ;
            return false ;
        }

        int primitiveActionSize = -1 ;
        if(bPrimitiveActionName->size() == bPrimitiveActionArg->size()){
            primitiveActionSize =  bPrimitiveActionName->size() ;
        } else {
            yError() << " [updatePrimitiveAction] : one of the primitiveAction conf has different size!" ;
            return false ;
        }

        if(primitiveActionSize == 0) {
            yWarning() << " [updatePrimitiveAction] : there is no primitiveAction defined at startup!" ;
        } else {

            for(int i = 0; i < primitiveActionSize ; i++) {
                //insert protoaction even if already there
                Bottle bPrimitive;
                string currentPrimName = bPrimitiveActionName->get(i).asString();
                string currentPrimArg  = bPrimitiveActionArg->get(i).asString();

                bPrimitive.addString(currentPrimName);
                bPrimitive.addString(currentPrimArg);

                string concat = currentPrimName + "_" + currentPrimArg;
                Bottle bCurrentPrim = rf.findGroup(concat);
                if(bCurrentPrim.isNull()){
                    yError() << " [updatePrimitiveAction] : " << concat << "is NOT defined" ;
                    return false ;
                }
                Bottle * bListProtoAction = bCurrentPrim.find("actionList").asList();
                if(bListProtoAction->isNull()){
                    yError() << " [updatePrimitiveAction] : " << concat << "is there but is not defined (actionList)" ;
                    return false ;
                }
                bPrimitive.addList() = *bListProtoAction ;
                yInfo() << "Primitive Action added : (" <<bPrimitive.get(0).asString() << ", " << bPrimitive.get(1).asString() << ", ( " << bPrimitive.get(2).asList()->toString()<< "))" ;
                vPrimitiveActionBottle.push_back(bPrimitive);
            }
        }
    } else {
        yError() << " error in learnPrimitive::updatePrimitive | Primitive_Action is NOT defined in the learnPrimitive.ini";
        return false;
    }


    return true;
}

bool learnPrimitive::updateProtoAction(ResourceFinder &rf){
    Bottle bOutput;

    //1. Protoaction
    Bottle bProtoAction = rf.findGroup("Proto_Action");

    if (!bProtoAction.isNull())
    {
        Bottle * bProtoActionName = bProtoAction.find("protoActionName").asList();
        Bottle * bProtoActionEnd = bProtoAction.find("protoActionEnd").asList();
        Bottle * bProtoActionSpeed = bProtoAction.find("protoActionSpeed").asList();

       if(bProtoActionName->isNull() || bProtoActionEnd->isNull() || bProtoActionSpeed->isNull()){
           yError() << " [updateProtoAction] : one of the protoAction conf is null : protoActionName, protoActionEnd, protoActionSpeed" ;
            return false ;
        }


        int protoActionSize = -1 ;
        if(bProtoActionName->size() == bProtoActionEnd->size() && bProtoActionEnd->size() == bProtoActionSpeed->size()){
            protoActionSize =  bProtoActionName->size() ;
        } else {
            yError() << " [updateProtoAction] : one of the protoAction conf has different size!" ;
            return false ;
        }

        if(protoActionSize == 0) {
            yWarning() << " [updateProtoAction] : there is no protoaction defined at startup!" ;
        } else {

            for(int i = 0; i < protoActionSize ; i++) {

                //insert protoaction even if already there
                yInfo() << "ProtoAction added : (" << bProtoActionName->get(i).asString() << ", " << bProtoActionEnd->get(i).asInt() << ", " << bProtoActionSpeed->get(i).asDouble() << ")" ;
                mProtoActionEnd[bProtoActionName->get(i).asString()] = bProtoActionEnd->get(i).asInt();
                mProtoActionSpeed[bProtoActionName->get(i).asString()] = bProtoActionSpeed->get(i).asDouble();
            }
        }
    } else {
        yError() << " error in learnPrimitive::updateProtoAction | Proto_Action is NOT defined in the learnPrimitive.ini";
        return false;
    }

    //2. Effect of bodypart
    Bottle bBodyPart = rf.findGroup("BodyPart");

    if (!bBodyPart.isNull())
    {
        Bottle * bBodyPartName = bBodyPart.find("bodyPartName").asList();
        Bottle * bBodyPartEnd = bBodyPart.find("bodyPartProtoEnd").asList();
        Bottle * bBodyPartSpeed = bBodyPart.find("bodyPartProtoSpeed").asList();
        
        //Crash if no match : isnull is not good for that

        if(bBodyPartName->isNull() || bBodyPartEnd->isNull() || bBodyPartSpeed->isNull()){
            yError() << "[updateProtoAction] : one of the bodyPartProto conf is null : bodyPartName, bodyPartProtoEnd, bodyPartProtoSpeed" ;
            return false ;
        }


        int bodyPartSize = -1 ;
        if(bBodyPartName->size() == bBodyPartEnd->size() && bBodyPartEnd->size() == bBodyPartSpeed->size()){
            bodyPartSize =  bBodyPartName->size() ;
        } else {
            yError() << "[updateProtoAction] : one of the bodyPartProto conf has different size!" ;
            return false ;
        }

        if(bodyPartSize == 0) {
            yWarning() << "[updateProtoAction] : there is no bodyPartProto defined at startup!" ;
        } else {
            for(int i = 0; i < bodyPartSize ; i++) {

                //insert protoaction even if already there
                yInfo() << "bodyPartProto added : (" << bBodyPartName->get(i).asString() << ", " << bBodyPartEnd->get(i).asInt() << ", " << bBodyPartSpeed->get(i).asDouble() << ")" ;
                mBodyPartEnd[bBodyPartName->get(i).asString()] = bBodyPartEnd->get(i).asInt();
                mBodyPartSpeed[bBodyPartName->get(i).asString()] = bBodyPartSpeed->get(i).asDouble();
            }
        }
    } else {
        yError() << " error in learnPrimitive::updateProtoAction | BodyPart is NOT defined in the learnPrimitive.ini";
        return false;
    }

    return true;
}

/* ******* Configure module                                                 ********************************************** */   
bool RPCControllerModule::configure(ResourceFinder &rf){
    using std::string;
    using yarp::os::Network;

    cout << dbgTag << "Starting. \n";

    /* ****** Configure the Module                            ****** */
    // Get resource finder and extract properties
    moduleName = rf.check("name", Value("RPCController"), "The module name.").asString().c_str();
    period = rf.check("period", 1.0).asDouble();
    robotName = rf.check("robot", Value("icub"), "The robot name.").asString().c_str();
    string portNameRoot = "/" + moduleName + "/";


    /* ******* Ports.                                       ******* */
    rpcModule.open((portNameRoot + "cmd:io").c_str());
    portExperimentStepsOut.open((portNameRoot + "experiment/status:o").c_str());
    
    
    /* ******* Read RPC port parameters.                    ******* */
    Bottle parGroup = rf.findGroup("rpc");
    if (!parGroup.isNull()) {
        if (parGroup.check("rpcServer")) {
            string rpcServer = parGroup.find("rpcServer").asString().c_str();
            // Connect to RPC server
            if (!Network::connect(rpcModule.getName(), rpcServer)) {
                cerr << dbgTag << "Could not connect to the specified RPC server: " << rpcServer << ". \n";
                return false;
            }
        } else {
            cerr << dbgTag << "Could not find the rpcServer name in the specified configuration file. \n";
            return false;
        }
    } else {
        cerr << dbgTag << "Could not find the [rpc] parameter group in the specified configuration file. \n";
        return false;
    }

    /* ******* Read experiment parameters.                  ******* */
    parGroup = rf.findGroup("experiment");
    if (!parGroup.isNull()) {
        // Number of columns in array
        if (parGroup.check("nCols")) {
            int nCols = parGroup.find("nCols").asInt();

            // Experiment parameters
            Bottle *confExpParams = parGroup.find("cmdT").asList();
            if (confExpParams != NULL) {
                for (int i = 0; i < confExpParams->size(); ++i) {
                    ExperimentParams tmpPar;
                    tmpPar.cmd = confExpParams->get(i).asString().c_str();
                    tmpPar.time = confExpParams->get(i + 1).asDouble();
                    expParams.push_back(tmpPar);

                    i++;
                }
            } else {
                cerr << dbgTag << "Could not find the experiment parameter list in the supplied configuration file. \n";
                return false;
            }
        } else {
            cerr << dbgTag << "Could not find nCols parameter in supplied configuration file. \n";
            return false;
        }
    } else {
        cerr << dbgTag << "Could not find the experiment parameter group [experiment] in the supplied configuration file. \n";
        return false;
    }

#ifndef NODEBUG
    cout << "DEBUG: " << dbgTag << "Experiment parameters are: \n";
    for (size_t i = 0; i < expParams.size(); ++i) {
        cout << "DEBUG " << dbgTag << expParams[i].cmd << " " << expParams[i].time << "\n";
        i++;
    }
#endif


    
    cout << dbgTag << "Started correctly. \n";

    return true;
}

bool embObjVirtualAnalogSensor::open(yarp::os::Searchable &config)
{
    std::string str;
    if(config.findGroup("GENERAL").find("verbose").asBool())
    {
        str=config.toString().c_str();
        _verbose = true;
    }
    else
        str=" ";

    yTrace() << str;

    // Read stuff from config file
    if(!fromConfig(config))
    {
        yError() << "embObjAnalogSensor missing some configuration parameter. Check logs and your config file.";
        return false;
    }

    // Tmp variables
    Bottle          groupEth;
    ACE_TCHAR       address[64];
    ACE_UINT16      port;

    Bottle groupTransceiver = Bottle(config.findGroup("TRANSCEIVER"));
    if(groupTransceiver.isNull())
    {
        yError() << "embObjVirtualAnalogSensor: Can't find TRANSCEIVER group in config files";
        return false;
    }

    Bottle groupProtocol = Bottle(config.findGroup("PROTOCOL"));
    if(groupProtocol.isNull())
    {
        yError() << "embObjVirtualAnalogSensor: Can't find PROTOCOL group in config files";
        return false;
    }


    // Get both PC104 and EMS ip addresses and port from config file
    groupEth  = Bottle(config.findGroup("ETH"));
    Bottle parameter1( groupEth.find("PC104IpAddress").asString() );    // .findGroup("IpAddress");
    port      = groupEth.find("CmdPort").asInt();              // .get(1).asInt();
    snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%s", parameter1.toString().c_str());
    _fId.pc104IPaddr.port = port;

    Bottle parameter2( groupEth.find("IpAddress").asString() );    // .findGroup("IpAddress");
    snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%s", parameter2.toString().c_str());
    _fId.boardIPaddr.port = port;

    sscanf(_fId.boardIPaddr.string,"\"%d.%d.%d.%d", &_fId.boardIPaddr.ip1, &_fId.boardIPaddr.ip2, &_fId.boardIPaddr.ip3, &_fId.boardIPaddr.ip4);
    sscanf(_fId.pc104IPaddr.string,"\"%d.%d.%d.%d", &_fId.pc104IPaddr.ip1, &_fId.pc104IPaddr.ip2, &_fId.pc104IPaddr.ip3, &_fId.pc104IPaddr.ip4);

    snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%u.%u.%u.%u:%u", _fId.boardIPaddr.ip1, _fId.boardIPaddr.ip2, _fId.boardIPaddr.ip3, _fId.boardIPaddr.ip4, _fId.boardIPaddr.port);
    snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%u.%u.%u.%u:%u", _fId.pc104IPaddr.ip1, _fId.pc104IPaddr.ip2, _fId.pc104IPaddr.ip3, _fId.pc104IPaddr.ip4, _fId.pc104IPaddr.port);

    //   Debug info
    snprintf(_fId.name, sizeof(_fId.name), "embObjAnalogSensor: referred to EMS: %d at address %s\n", _fId.boardNumber, address);       // Saving User Friendly Id


    Value val =config.findGroup("ETH").check("Ems",Value(1), "Board number");
    if(val.isInt())
        _fId.boardNumber =val.asInt();
    else
    {
        yError () << "embObjAnalogSensor: EMS Board number identifier not found for IP" << _fId.pc104IPaddr.string;
        return false;
    }


    ethManager = TheEthManager::instance();
    if(NULL == ethManager)
    {
        yError() << "Unable to instantiate ethManager";
        return false;
    }

    // N.B.: use a dynamic_cast to extract correct interface when using this pointer
    _fId.interface = this;
    _fId.type = ethFeatType_AnalogVirtual;

    /* Once I'm ok, ask for resources, through the _fId struct I'll give the ip addr, port and
    *  and boradNum to the ethManagerin order to create the ethResource requested.
    * I'll Get back the very same sturct filled with other data useful for future handling
    * like the EPvector and EPhash_function */
    res = ethManager->requestResource(config, groupTransceiver, groupProtocol, _fId);
    if(NULL == res)
    {
        yError() << "EMS device not instantiated... unable to continue";
        cleanup();
        return false;
    }

    /*IMPORTANT: implement isEpManagedByBoard like every embObj obj when virtaulAnalogSensor will be exist in eo proto!!!!*/
//    if(!isEpManagedByBoard())
//    {
//        yError() << "EMS "<< _fId.boardNumber << "is not connected to virtual analog sensor";
//        return false;
//    }
//.........这里部分代码省略.........

bool icubFinger::calibrateIndexMiddle(){

    // Open the finger
    Finger::open();

    // Set the proximal to 0 and distal to 180
    setAngle(_proximalJointIndex, 0, 30);
    setAngle(_distalJointIndex, 180, 30);
    while(!checkMotionDone()){
        ;
    }



    // TODO: remove this for the simulation. There is not such data
    // available during simulation
    Bottle *fingerEnc;
    for(int i = 0; i <= _fingerEncoders->getPendingReads(); i++)
        fingerEnc = _fingerEncoders->read();

    if(!fingerEnc->isNull())
    {
        _maxProximal = fingerEnc->get(_proximalEncoderIndex).asDouble();
        _minMiddle = fingerEnc->get(_middleEncoderIndex).asDouble();
        _minDistal = fingerEnc->get(_distalEncoderIndex).asDouble();
    }



    setAngle(_distalJointIndex, 0, 30);
    while(!checkMotionDone()){
        ;
    }

    setAngle(_proximalJointIndex, 90, 30);
    while(!checkMotionDone()){
        ;
    }



    // TODO: remove this for the simulation. There is not such data
    // available during simulation
    for(int i = 0; i < _fingerEncoders->getPendingReads(); i++)
        fingerEnc = _fingerEncoders->read();
    if(!fingerEnc->isNull())
    {
        _minProximal = fingerEnc->get(_proximalEncoderIndex).asDouble();
        _maxMiddle = fingerEnc->get(_middleEncoderIndex).asDouble();
        _maxDistal = fingerEnc->get(_distalEncoderIndex).asDouble();
    }

    setAngle(_proximalJointIndex, 0);
    while(!checkMotionDone()){
        ;
    }

    checkMinMax(_minDistal, _maxDistal);
    checkMinMax(_minMiddle, _minMiddle);
    checkMinMax(_minProximal, _minProximal);

    cout << "Calibration mins:" <<
            _minProximal << "\t" <<
            _minMiddle << "\t" <<
            _minDistal << "\t" << endl;

    cout << "Calibration maxes:" <<
            _maxProximal << "\t" <<
            _maxMiddle << "\t" <<
            _maxDistal << "\t" << endl;
}

/*
* Send a rpc command to ABM to obtain the kinematicStructure of part of the body (using the joint info)
* Send a rpc command to kinematicStructure if no kinematicStructure were provided
* input: joint of the bodypart to be moved (e.g. m_joint_number) + body part type (e.g. left_arm, right_arm, ...)
*/
Bottle proactiveTagging::assignKinematicStructureByJoint(int BPjoint, std::string sBodyPartType, bool forcingKS) {
    //1. extract instance from singleJointAction for joint BPjoint, from ABM
    Bottle bResult, bOutput;
    ostringstream osRequest;
    osRequest << "SELECT max(main.instance) FROM main, contentarg WHERE main.instance = contentarg.instance AND activityname = 'singleJointBabbling' AND main.begin = TRUE AND contentarg.role = 'limb' AND contentarg.argument = '" << BPjoint << "' ;";
    yInfo() << "assignKinematicStructureByJoint request: " << osRequest.str();
    bResult = iCub->getABMClient()->requestFromString(osRequest.str().c_str());

    if (bResult.toString() == "NULL") {
        yError() << " error in proactiveTagging::assignKinematicStructureByJoint | for joint " << BPjoint << " | No instance corresponding to singleJointBabbling for this part" ;
        bOutput.addString("error");
        bOutput.addString("No instance corresponding to singleJointBabbling for this part");
        return bOutput;
    }
    int ksInstance = atoi(bResult.get(0).asList()->get(0).asString().c_str());

    Bottle bResultCheckKS = checkForKinematicStructure(ksInstance, forcingKS);
    if(bResultCheckKS.get(0).asString() == "error") {
        return bResultCheckKS;
    }
    yInfo() << " [assignKinematicStructureByJoint] | for joint " << BPjoint << " | instance found : " << ksInstance;

    //WRITE IN OPC
    iCub->opc->checkout();
    list<Entity*> lEntities = iCub->opc->EntitiesCache();
    Bottle bListEntChanged;
    for (auto& entity: lEntities) //go through all entity
    {
        yInfo() << "Checking if entity " << entity->name() << " has entitytype = bodypart : ----> " << entity->entity_type() ;
        if (entity->entity_type() == "bodypart")                                             //check bodypart entity
        {
            //pb with the casting: BPtemp is empty
            Bodypart* BPtemp = dynamic_cast<Bodypart*>(entity);
            if(!BPtemp) {
                yDebug() << "Could not cast " << entity->name() << " to Bodypart";
                continue;
            }
            if(BPtemp->m_joint_number == BPjoint) {                                             //if corresponding joint : change it
                //BPtemp->m_kinStruct_instance = ksInstance;
                bListEntChanged.addString(BPtemp->name());
                yInfo() << "Change" << BPtemp->name() << "to kinematic instance" << ksInstance;
                iCub->opc->commit(BPtemp);

                break;
            }
        }
    }

    yInfo() << "Out of the loop for checking entity in OPC, number of entityChanged : " << bListEntChanged.size() ;

    if(bListEntChanged.isNull()){
        yWarning() << "assignKinematicStructureByJoint | for joint " << BPjoint << " | no bodypart has been found with this joint!";
        bOutput.addString("warning");
        bOutput.addString("no bodypart has been found with this joint!");
        return bOutput;
    }

    bOutput.addString("ack");
    bOutput.addInt(ksInstance);
    bOutput.addList() = bListEntChanged;

    return bOutput;
}