C++ Searchable::findGroup方法代码示例

bool BmsBattery::open(yarp::os::Searchable& config)
{
    bool correct=true;

    //debug
    yDebug("%s\n", config.toString().c_str());

    Bottle& group_general = config.findGroup("GENERAL");
    Bottle& group_serial  = config.findGroup("SERIAL_PORT");

    if (group_general.isNull())
    {
        yError() << "Insufficient parameters to BmsBattery, section GENERAL missing";
        return false;
    }

    if (group_serial.isNull())
    {
        yError() << "Insufficient parameters to BmsBattery, section SERIAL_PORT missing";
        return false;
    }

    int period=config.find("thread_period").asInt();
    setRate(period);

    Property prop;
    std::string ps = group_serial.toString();
    prop.fromString(ps);
    prop.put("device", "serialport");

    //open the driver
    driver.open(prop);
    if (!driver.isValid())
    {
        yError() << "Error opening PolyDriver check parameters";
#ifndef DEBUG_TEST 
        return false;
#endif
    }

    //open the serial interface
    driver.view(pSerial);
    if (!pSerial)
    {
        yError("Error opening serial driver. Device not available");
#ifndef DEBUG_TEST 
        return false;
#endif
    }

    // Other options
    this->logEnable = group_general.check("logToFile", Value(0), "enable / disable the log to file").asBool();
    this->verboseEnable = group_general.check("verbose", Value(0), "enable/disable the verbose mode").asBool();
    this->screenEnable = group_general.check("screen", Value(0), "enable/disable the screen output").asBool();
    this->debugEnable = group_general.check("debug", Value(0), "enable/disable the debug mode").asBool();
    this->shutdownEnable = group_general.check("shutdown", Value(0), "enable/disable the automatic shutdown").asBool();

    RateThread::start();
    return true;
}

bool DeviceGroup::open(yarp::os::Searchable& config) {
    if (implementation==NULL) {
        implementation = new DeviceGroupHelper;
    }
    if (implementation==NULL) {
        printf("Out of memory\n");
        return false;
    }

    if (config.check("part","a list of section names, with each section containing a device")) {
        Bottle bot = config.findGroup("part").tail();
        printf("Assembly of: %s\n", bot.toString().c_str());
        for (int i=0; i<bot.size(); i++) {
            ConstString name = bot.get(i).asString();
            printf("  %s -> %s\n", name.c_str(),
                   config.findGroup(name).toString().c_str());
            bool result = HELPER(implementation).add(name,
                          config.findGroup(name));
            if (!result) {
                HELPER(implementation).close();
                return false;
            }
        }
        return true;
    }
    return false;
}

/*Device Driver*/
bool FakeCan::open(yarp::os::Searchable &par)
{
    cerr<<"Opening FakeCan network" << endl;

    //fprintf(stderr, "%s", par.toString().c_str());

    int njoints=par.findGroup("GENERAL").find("Joints").asInt();
    Bottle &can = par.findGroup("CAN");
    Bottle ids=can.findGroup("CanAddresses");

    if (ids.size()<njoints/2)
    {
        fprintf(stderr, "Check ini file, wrong number of board ids or joints\n");
        return false;
    }
    
    for(int i=1;i<=njoints/2;i++)
    {
        FakeBoard *tmp=new FakeBoard;
        int id=ids.get(i).asInt();
        tmp->setId(id);   //just as a test
        tmp->setReplyFifo(&replies);
        tmp->start();
        boardList.push_back(tmp);
    }
        
    return true;
}

// \todo TODO bug ? 
bool checkRequiredParamIsVectorOfString(yarp::os::Searchable& config,
                                     const std::string& paramName,
                                     std::vector<std::string> & output_vector)
{
    bool correct = !(config.findGroup(paramName).isNull());
    if( correct )
    correct = true;
    {
        Bottle ids = config.findGroup(paramName).tail();
        std::cout << "ids : " << ids.toString() << std::endl;
        std::cout << "ids : " << config.find(paramName).toString() << std::endl;
        output_vector.resize(ids.size());
        for(int i = 0; i < ids.size(); i++ )
        {
            output_vector[i] = ids.get(i).asString().c_str();
        }
    }

    if( !correct )
    {
        yError("CanBusInertialMTB: problem loading parameter %s as vector of string",paramName.c_str());
    }

    return correct;
}

bool ethResources::open(yarp::os::Searchable &config, FEAT_ID request)
{
   // ACE_TCHAR remoteIp_string[64], localIp_string[64];

    //uint16_t loc_port, rem_port;
//    uint8_t   loc_ip1,loc_ip2,loc_ip3,loc_ip4;
//    uint8_t   rem_ip1,rem_ip2,rem_ip3,rem_ip4;

    // Get the pointer to the actual Singleton ethManager
    ethManager = TheEthManager::instance();

    transMutex.wait();

    // Fill 'info' field with human friendly string
    sprintf(info, "ethResources - referred to EMS: %s:%d",request.EMSipAddr.string, request.EMSipAddr.port);
    yTrace() << "Ems ip address " << info;
    if(config.findGroup("GENERAL").find("verbose").asBool())
    {
        infoPkts->_verbose = true;
    }
    else
    {
        infoPkts->_verbose = false;
    }
    //
    //  EMBOBJ INIT
    //
    bool ret;
    eOipv4addr_t eo_locIp = eo_common_ipv4addr(request.PC104ipAddr.ip1, request.PC104ipAddr.ip2, request.PC104ipAddr.ip3, request.PC104ipAddr.ip4);
    eOipv4addr_t eo_remIp = eo_common_ipv4addr(request.EMSipAddr.ip1, request.EMSipAddr.ip2, request.EMSipAddr.ip3, request.EMSipAddr.ip4);
    Bottle groupProtocol = Bottle(config.findGroup("PROTOCOL"));
    if(!init(groupProtocol, eo_locIp, eo_remIp, request.EMSipAddr.port, rxBUFFERsize, request.boardNum))
    {
        ret = false;
        yError() << "cannot init transceiver... maybe wrong board number... check log and config file.";
    }
    else
    {
        ret = true;
//        yDebug() << "Transceiver succesfully initted.";
    }

    boardNum = request.boardNum;
    ACE_UINT32 hostip = (request.EMSipAddr.ip1 << 24) | (request.EMSipAddr.ip2 << 16) | (request.EMSipAddr.ip3 << 8) | (request.EMSipAddr.ip4);
    ACE_INET_Addr myIP((u_short)request.EMSipAddr.port, hostip);
    remote_dev = myIP;
    transMutex.post();

    infoPkts->setBoardNum(boardNum);

    return ret;
}

bool FakeLaser::open(yarp::os::Searchable& config)
{
    bool correct=true;
    info = "Fake Laser device for test/debugging";
    device_status = DEVICE_OK_STANBY;

#if LASER_DEBUG
    yDebug("%s\n", config.toString().c_str());
#endif

    bool br = config.check("GENERAL");
    if (br != false)
    {
        yarp::os::Searchable& general_config = config.findGroup("GENERAL");
        period = general_config.check("Period", Value(50), "Period of the sampling thread").asInt();
    }

    min_distance = 0.1; //m
    max_distance = 2.5;  //m
    min_angle = 0;      //degrees
    max_angle = 359;    //degrees
    resolution = 1.0;   //degrees

    sensorsNum = (int)((max_angle-min_angle)/resolution);
    laser_data.resize(sensorsNum);
    
    yInfo("Starting debug mode");
    yInfo("max_dist %f, min_dist %f", max_distance, min_distance);
    yInfo("max_angle %f, min_angle %f", max_angle, min_angle);
    yInfo("resolution %f", resolution);
    yInfo("sensors %d", sensorsNum);
    Time::turboBoost();
    RateThread::start();
    return true;
}

bool JointTorqueControl::loadGains(yarp::os::Searchable& config)
{
    if( !config.check("TRQ_PIDS") )
    {
        yError("No TRQ_PIDS group find, initialization failed");
        return false;
    }

    yarp::os::Bottle & bot = config.findGroup("TRQ_PIDS");

    bool gains_ok = true;

    gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"kff",this->axes);
    gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"kp",this->axes);
    gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"ki",this->axes);
    gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"maxPwm",this->axes);
    gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"maxInt",this->axes);
    gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"stictionUp",this->axes);
    gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"stictionDown",this->axes);
    gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"bemf",this->axes);
    gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"coulombVelThr",this->axes);
    gains_ok = gains_ok && checkVectorExistInConfiguration(bot,"frictionCompensation",this->axes);


    if( !gains_ok )
    {
        yError("TRQ_PIDS group is missing some information, initialization failed");
        return false;
    }

    for(int j=0; j < this->axes; j++)
    {
        jointTorqueLoopGains[j].reset();
        motorParameters[j].reset();

        jointTorqueLoopGains[j].kp        = bot.find("kp").asList()->get(j).asDouble();
        jointTorqueLoopGains[j].ki        = bot.find("ki").asList()->get(j).asDouble();
        jointTorqueLoopGains[j].max_pwm   = bot.find("maxPwm").asList()->get(j).asDouble();
        jointTorqueLoopGains[j].max_int   = bot.find("maxInt").asList()->get(j).asDouble();
        motorParameters[j].kff            = bot.find("kff").asList()->get(j).asDouble();
        motorParameters[j].kcp            = bot.find("stictionUp").asList()->get(j).asDouble();
        motorParameters[j].kcn            = bot.find("stictionDown").asList()->get(j).asDouble();
        motorParameters[j].kv             = bot.find("bemf").asList()->get(j).asDouble();
        motorParameters[j].coulombVelThr  = bot.find("coulombVelThr").asList()->get(j).asDouble();
        motorParameters[j].frictionCompensation  = bot.find("frictionCompensation").asList()->get(j).asDouble();
        if (motorParameters[j].frictionCompensation > 1 || motorParameters[j].frictionCompensation < 0) {
            motorParameters[j].frictionCompensation = 0;
            yWarning("[TRQ_PIDS] frictionCompensation parameter is outside the admissible range [0, 1]. FrictionCompensation reset to 0.0");
        }


    }

    return true;

}

bool yarp::dev::IJoypadController::parseActions(const yarp::os::Searchable& cfg, int* count)
{
    int  dummy;
    size_t  i;
    int& actCount = count ? *count : dummy;
    if(!cfg.check(buttActionGroupName))
    {
        myInfo() << "no actions found in the configuration file (no" << buttActionGroupName << "group found)";
        actCount = 0;
        return true;
    }
    Bottle& actionsGroup = cfg.findGroup(buttActionGroupName);

    if(!actionsGroup.size())
    {
        myError() << "no action found under" << buttActionGroupName << "group";
        actCount = 0;
        return false;
    }

    for(i = 1; i < actionsGroup.size(); i++)
    {
        if(!actionsGroup.get(i).isList())
        {
            yDebug() << "error parsing cfg";
            return false;
        }

        Bottle& keyvalue         = *actionsGroup.get(i).asList();
        yDebug() << keyvalue.toString();
        unsigned int buttonCount;
        if(!this->getButtonCount(buttonCount))
        {
            myError() << "unable to get button count while parsing the actions";
            actCount = 0;
            return false;
        }
        if(!keyvalue.get(0).isInt32()                 ||
            keyvalue.get(0).asInt32() < 0             ||
           (unsigned int) keyvalue.get(0).asInt32() > buttonCount-1 ||
           !keyvalue.get(1).isString())
        {
            myError() << "Button's actions parameters must be in the format 'unsigned int string' and the button id must be in range";
            actCount = 0;
            return false;
        }
        myInfo() << "assigning actions" << keyvalue.get(1).asString() << "to button" << keyvalue.get(0).asInt32();
        m_actions[keyvalue.get(0).asInt32()] = keyvalue.get(1).asString();
    }

    actCount = i;
    myInfo() << actCount << "action parsed succesfully";
    return true;
}

bool checkRequiredParamIsVectorOfInt(yarp::os::Searchable& config,
                                     const std::string& paramName,
                                     std::vector<int> & output_vector)
{
    bool correct = !(config.findGroup(paramName).isNull());
    if( correct )
    {
        Bottle ids = config.findGroup(paramName).tail();
        output_vector.resize(ids.size());
        for(int i = 0; i < ids.size(); i++ )
        {
            output_vector[i] = ids.get(i).asInt();
        }
    }

    if( !correct )
    {
        yError("CanBusInertialMTB: problem loading parameter %s as vector of int",paramName.c_str());
    }

    return correct;
}

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

bool embObjMais::fromConfig(yarp::os::Searchable &_config)
{
#if defined(EMBOBJMAIS_USESERVICEPARSER)


    if(false == parser->parseService(_config, serviceConfig))
    {
        return false;
    }

    return true;

#else

    Bottle xtmp;

    int _period = 0;

    // Analog Sensor stuff
    Bottle config = _config.findGroup("GENERAL");
    if (!extractGroup(config, xtmp, "Period","transmitting period of the sensors", 1))
    {
        yError() << "embObjMais Using default value = 0 (disabled)";
        _period = 0;

    }
    else
    {
        _period = xtmp.get(1).asInt();
        yDebug() << "embObjMais::fromConfig() detects embObjMais Using value of" << _period;
    }

    serviceConfig.acquisitionrate = _period;

    return true;
#endif
}

bool embObjInertials::fromConfig(yarp::os::Searchable &_config)
{
    Bottle xtmp;

    // Analog Sensor stuff
    Bottle config = _config.findGroup("GENERAL");
    if (!extractGroup(config, xtmp, "Period","transmitting period of the sensors", 1))
    {
        yError() << "embObjInertials Using default value = 0 (disabled)";
        _period = 0;
    }
    else
    {
        _period = xtmp.get(1).asInt();
        yDebug() << "embObjInertials::fromConfig() detects embObjInertials Using value of" << _period;
    }


#if 0
    if (!extractGroup(config, xtmp, "NumberOfSensors","Number of sensors managed", 1))
    {
        yWarning("embObjInertials: Using default value = 1 for _numofsensors\n");
        _numofsensors = 1;
    }
    else
    {
        _numofsensors = xtmp.get(1).asInt();
    }
#endif


    _numofsensors = 1;

    // if we have a AS_Type_INERTIAL_MTB, then we may have more than one. for sure not zero.

    {
        Bottle tmp;
        _numofsensors = 0;

        tmp = config.findGroup("enabledAccelerometers");
        _numofsensors += (tmp.size()-1); // sensors holds strings "enabledAccelerometers" and then all the others, thus i need a -1

        tmp = config.findGroup("enabledGyroscopes");
        _numofsensors += (tmp.size()-1); // sensors holds strings "enabledGyroscopes" and then all the others, thus i need a -1

#if 0
        if (!extractGroup(config, xtmp, "enabledAccelerometers", "Position of managed sensors expressed as strings", 1))
        {
            yWarning("embObjInertials: cannot find enabledAccelerometers\n");
            _numofsensors = 1;
        }
        else
        {
            _numofsensors = xtmp.size();
        }
#endif

    }


    return true;
}

bool parametricCalibrator::open(yarp::os::Searchable& config)
{
    yTrace();
    Property p;
    p.fromString(config.toString());

    if (p.check("GENERAL") == false)
    {
        yError() << "Parametric calibrator: missing [GENERAL] section";
        return false;
    }

    if (p.findGroup("GENERAL").check("deviceName"))
    {
        deviceName = p.findGroup("GENERAL").find("deviceName").asString();
    }
    else
    {
        yError() << "Parametric calibrator: missing deviceName parameter";
        return false;
    }

    std::string str;
    if (config.findGroup("GENERAL").find("verbose").asInt())
    {
        str = config.toString().c_str();
        yTrace() << deviceName.c_str() << str;
    }

    // Check clearHwFaultBeforeCalibration
    Value val_clearHwFault = config.findGroup("GENERAL").find("clearHwFaultBeforeCalibration");
    if (val_clearHwFault.isNull())
    {
        clearHwFault = false;
    }
    else
    {
        if (!val_clearHwFault.isBool())
        {
            yError() << deviceName.c_str() << ": clearHwFaultBeforeCalibration bool param is different from accepted values (true / false). Assuming false";
            clearHwFault = false;
        }
        else
        {
            clearHwFault = val_clearHwFault.asBool();
            if (clearHwFault)
                yInfo() << deviceName.c_str() << ":  clearHwFaultBeforeCalibration option enabled\n";
        }
    }

    // Check Vanilla = do not use calibration!
    skipCalibration = config.findGroup("GENERAL").find("skipCalibration").asBool();// .check("Vanilla",Value(1), "Vanilla config");
    skipCalibration = !!skipCalibration;
    if (skipCalibration)
    {
        yWarning() << deviceName << ": skipping calibration!! This option was set in general.xml file.";
        yWarning() << deviceName << ": BE CAREFUL USING THE ROBOT IN THIS CONFIGURATION! See 'skipCalibration' param in config file";
    }

    int nj = 0;
    if (p.findGroup("GENERAL").check("joints"))
    {
        nj = p.findGroup("GENERAL").find("joints").asInt();
    }
    else if (p.findGroup("GENERAL").check("Joints"))
    {
        // This is needed to be backward compatibile with old iCubInterface
        nj = p.findGroup("GENERAL").find("Joints").asInt();
    }
    else
    {
        yError() << deviceName.c_str() << ": missing joints parameter";
        return false;
    }

    type = new unsigned char[nj];
    param1 = new double[nj];
    param2 = new double[nj];
    param3 = new double[nj];
    maxPWM = new int[nj];

    zeroPos = new double[nj];
    zeroVel = new double[nj];
    currPos = new double[nj];
    currVel = new double[nj];
    homePos = new double[nj];
    homeVel = new double[nj];
    zeroPosThreshold = new double[nj];

    int i = 0;

    Bottle& xtmp = p.findGroup("CALIBRATION").findGroup("calibration1");
    if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of Calibration1 params " << xtmp.size() << " " << nj; return false; }
    for (i = 1; i < xtmp.size(); i++) param1[i - 1] = xtmp.get(i).asDouble();

    xtmp = p.findGroup("CALIBRATION").findGroup("calibration2");
    if (xtmp.size() - 1 != nj) { yError() << deviceName << ": invalid number of Calibration2 params"; return false; }
    for (i = 1; i < xtmp.size(); i++) param2[i - 1] = xtmp.get(i).asDouble();

    xtmp = p.findGroup("CALIBRATION").findGroup("calibration3");
//.........这里部分代码省略.........

bool laserHokuyo::open(yarp::os::Searchable& config)
{
    bool correct=true;
    internal_status = HOKUYO_STATUS_NOT_READY;
    info = "Hokuyo Laser";
    device_status = DEVICE_OK_STANBY;

#if LASER_DEBUG
    yDebug("%s\n", config.toString().c_str());
#endif

    bool br = config.check("GENERAL");
    if (br == false)
    {
        yError("cannot read 'GENERAL' section");
        return false;
    }
    yarp::os::Searchable& general_config = config.findGroup("GENERAL");

    //list of mandatory options
    //TODO change comments
    period = general_config.check("Period", Value(50), "Period of the sampling thread").asInt();
    start_position = general_config.check("Start_Position", Value(0), "Start position").asInt();
    end_position = general_config.check("End_Position", Value(1080), "End Position").asInt();
    error_codes = general_config.check("Convert_Error_Codes", Value(0), "Substitute error codes with legal measurments").asInt();
    yarp::os::ConstString s = general_config.check("Laser_Mode", Value("GD"), "Laser Mode (GD/MD").asString();

    if (general_config.check("Measurement_Units"))
    {
        yError() << "Deprecated parameter 'Measurement_Units'. Please Remove it from the configuration file.";
    }

    if (error_codes==1)
    {
        yInfo("'error_codes' option enabled. Invalid samples will be substituded with out-of-range measurements.");
    }
    if (s=="GD")
    {
        laser_mode = GD_MODE;
        yInfo("Using GD mode (single acquisition)");
    }
    else if (s=="MD")
    {
        laser_mode = MD_MODE;
        yInfo("Using MD mode (continuous acquisition)");
    }
    else
    {
        laser_mode = GD_MODE;
        yError("Laser_mode not found. Using GD mode (single acquisition)");
    }

    bool ok = general_config.check("Serial_Configuration");
    if (!ok)
    {
        yError("Cannot find configuration file for serial port communication!");
        return false;
    }
    yarp::os::ConstString serial_filename = general_config.find("Serial_Configuration").asString();

    //string st = config.toString();
    setRate(period);

    Property prop;

    prop.put("device", "serialport");
    ok = prop.fromConfigFile(serial_filename.c_str(),config,false);
    if (!ok)
    {
        yError("Unable to read from serial port configuration file");
        return false;
    }

    pSerial=0;

    driver.open(prop);
    if (!driver.isValid())
    {
        yError("Error opening PolyDriver check parameters");
        return false;
    }

    driver.view(pSerial);
    
    if (!pSerial)
    {
        yError("Error opening serial driver. Device not available");
        return false;
    }

    Bottle b;
    Bottle b_ans;
    string ans;

    // *** Check if the URG device is present ***
    b.addString("SCIP2.0\n");
    pSerial->send(b);
    yarp::os::Time::delay(0.040);
    pSerial->receive(b_ans);
    if (b_ans.size()>0)
//.........这里部分代码省略.........

bool parametricCalibrator::open(yarp::os::Searchable& config)
{
    yTrace();
    Property p;
    p.fromString(config.toString());

    if (p.check("GENERAL")==false)
    {
      yError() << "missing [GENERAL] section"; 
      return false;
    } 

    if(p.findGroup("GENERAL").check("deviceName"))
    {
      deviceName = p.findGroup("GENERAL").find("deviceName").asString();
    } 
    else
    {
      yError() << "missing deviceName parameter"; 
      return false;
    } 

    std::string str;
    if(config.findGroup("GENERAL").find("verbose").asInt())
    {
        str=config.toString().c_str();
        yTrace() << deviceName.c_str() << str;
    }  

    // Check Vanilla = do not use calibration!
    skipCalibration =config.findGroup("GENERAL").find("skipCalibration").asBool() ;// .check("Vanilla",Value(1), "Vanilla config");
    skipCalibration = !!skipCalibration;
    if(skipCalibration )
        yWarning() << "parametric calibrator: skipping calibration!! This option was set in general.xml file.";

    int nj = 0;
    if(p.findGroup("GENERAL").check("joints"))
    {
        nj = p.findGroup("GENERAL").find("joints").asInt();
    }
    else
    {
        yError() << deviceName.c_str() <<  ": missing joints parameter" ;
        return false;
    }

    type = new unsigned char[nj];
    param1 = new double[nj];
    param2 = new double[nj];
    param3 = new double[nj];
    maxPWM = new int[nj];

    zeroPos = new double[nj];
    zeroVel = new double[nj];
    currPos = new double[nj];
    currVel = new double[nj];
    homePos = new double[nj];
    homeVel = new double[nj];
    zeroPosThreshold = new double[nj];

    int i=0;

    Bottle& xtmp = p.findGroup("CALIBRATION").findGroup("calibration1");
    if (xtmp.size()-1!=nj) {yError() << deviceName << ": invalid number of Calibration1 params " << xtmp.size()<< " " << nj; return false;}
    for (i = 1; i < xtmp.size(); i++) param1[i-1] = xtmp.get(i).asDouble();

    xtmp = p.findGroup("CALIBRATION").findGroup("calibration2");
    if (xtmp.size()-1!=nj) {yError() << "invalid number of Calibration2 params"; return false;}
    for (i = 1; i < xtmp.size(); i++) param2[i-1] = xtmp.get(i).asDouble();

    xtmp = p.findGroup("CALIBRATION").findGroup("calibration3");
    if (xtmp.size()-1!=nj) {yError() << "invalid number of Calibration3 params"; return false;}
    for (i = 1; i < xtmp.size(); i++) param3[i-1] = xtmp.get(i).asDouble();

    xtmp = p.findGroup("CALIBRATION").findGroup("calibrationType");
    if (xtmp.size()-1!=nj) {yError() << "invalid number of Calibration3 params"; return false;}
    for (i = 1; i < xtmp.size(); i++) type[i-1] = (unsigned char) xtmp.get(i).asDouble();

    xtmp = p.findGroup("CALIBRATION").findGroup("positionZero");
    if (xtmp.size()-1!=nj) {yError() << "invalid number of PositionZero params"; return false;}
    for (i = 1; i < xtmp.size(); i++) zeroPos[i-1] = xtmp.get(i).asDouble();

    xtmp = p.findGroup("CALIBRATION").findGroup("velocityZero");
    if (xtmp.size()-1!=nj) {yError() << "invalid number of VelocityZero params"; return false;}
    for (i = 1; i < xtmp.size(); i++) zeroVel[i-1] = xtmp.get(i).asDouble();

    xtmp = p.findGroup("HOME").findGroup("positionHome");
    if (xtmp.size()-1!=nj) {yError() << "invalid number of PositionHome params"; return false;}
    for (i = 1; i < xtmp.size(); i++) homePos[i-1] = xtmp.get(i).asDouble();

    xtmp = p.findGroup("HOME").findGroup("velocityHome");
    if (xtmp.size()-1!=nj) {yError() << "invalid number of VelocityHome params"; return false;}
    for (i = 1; i < xtmp.size(); i++) homeVel[i-1] = xtmp.get(i).asDouble();

    xtmp = p.findGroup("CALIBRATION").findGroup("maxPwm");
    if (xtmp.size()-1!=nj) {yError() << "invalid number of MaxPwm params"; return false;}
    for (i = 1; i < xtmp.size(); i++) maxPWM[i-1] =  xtmp.get(i).asInt();

    xtmp = p.findGroup("CALIBRATION").findGroup("posZeroThreshold");
    if (xtmp.size()-1!=nj) {yError() << "invalid number of PosZeroThreshold params"; return false;}
//.........这里部分代码省略.........