예제 #1
0
bool CamCalibModule::configure(yarp::os::ResourceFinder &rf)
{

    ConstString str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
    setName(str.c_str()); // modulePortName

    double maxDelay = rf.check("maxDelay", Value(0.010), "Max delay between image and encoders").asDouble();

    // pass configuration over to bottle
    Bottle botConfig(rf.toString().c_str());
    botConfig.setMonitor(rf.getMonitor());
    // Load from configuration group ([<group_name>]), if group option present
    Value *valGroup; // check assigns pointer to reference
    if(botConfig.check("group", valGroup, "Configuration group to load module options from (string).")) {
        strGroup = valGroup->asString().c_str();
        // is group a valid bottle?
        if (botConfig.check(strGroup.c_str())){
            Bottle &group=botConfig.findGroup(strGroup.c_str(),string("Loading configuration from group " + strGroup).c_str());
            botConfig.fromString(group.toString());
        } else {
            yError() << "Group " << strGroup << " not found.";
            return false;
        }
    } else {
        yError ("There seem to be an error loading parameters (group section missing), stopping module");
        return false;
    }

    string calibToolName = botConfig.check("projection",
                                           Value("pinhole"),
                                           "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();

    _calibTool = CalibToolFactories::getPool().get(calibToolName.c_str());
    if (_calibTool!=NULL) {
        bool ok = _calibTool->open(botConfig);
        if (!ok) {
            delete _calibTool;
            _calibTool = NULL;
            return false;
        }
    }

    if (yarp::os::Network::exists(getName("/in"))) {
        yWarning() << "port " << getName("/in") << " already in use";
    }
    if (yarp::os::Network::exists(getName("/out"))) {
        yWarning() << "port " << getName("/out") << " already in use";
    }
    if (yarp::os::Network::exists(getName("/conf"))) {
        yWarning() << "port " << getName("/conf") << " already in use";
    }
    _prtImgIn.setSaturation(rf.check("saturation",Value(1.0)).asDouble());
    _prtImgIn.open(getName("/in"));
    _prtImgIn.setPointers(&_prtImgOut,_calibTool);
    _prtImgIn.setVerbose(rf.check("verbose"));
    _prtImgIn.setLeftEye((strGroup == "CAMERA_CALIBRATION_LEFT") ? true : false);
    _prtImgIn.setMaxDelay(maxDelay);
    _prtImgIn.setUseIMU(rf.check("useIMU"));
    _prtImgIn.setUseIMU(rf.check("useTorso"));
    _prtImgIn.setUseIMU(rf.check("useEyes"));
    _prtImgIn.useCallback();
    _prtImgOut.open(getName("/out"));
    _configPort.open(getName("/conf"));

    _prtTEncsIn.open(getName("/torso_encs/in"));
    _prtTEncsIn._prtImgIn = &_prtImgIn;
//    _prtTEncsIn.setStrict();
    _prtTEncsIn.useCallback();

    _prtHEncsIn.open(getName("/head_encs/in"));
    _prtHEncsIn._prtImgIn = &_prtImgIn;
//    _prtHEncsIn.setStrict();
    _prtHEncsIn.useCallback();

    _prtImuIn.open(getName("/imu/in"));
    _prtImuIn._prtImgIn = &_prtImgIn;
//    _prtImuIn.setStrict();
    _prtImuIn.useCallback();

    attach(_configPort);
    fflush(stdout);

    _prtImgIn.rpyPort.open(getName("/rpy"));

    return true;
}
bool CamCalibModule::configure(yarp::os::ResourceFinder &rf)
{
    ConstString str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
    verboseExecTime = rf.check("verboseExecTime");

    if      (rf.check("w_align")) align=ALIGN_WIDTH;
    else if (rf.check("h_align")) align=ALIGN_HEIGHT;

    if (rf.check("fps"))
    {
        requested_fps=rf.find("fps").asDouble();
        yInfo() << "Module will run at " << requested_fps;
    }
    else
    {
        yInfo() << "Module will run at max fps";
    }

    setName(str.c_str()); // modulePortName

    Bottle botLeftConfig(rf.toString().c_str());
    Bottle botRightConfig(rf.toString().c_str());

    botLeftConfig.setMonitor(rf.getMonitor());
    botRightConfig.setMonitor(rf.getMonitor());

    string strLeftGroup = "CAMERA_CALIBRATION_LEFT";
    if (botLeftConfig.check(strLeftGroup.c_str()))
    {            
        Bottle &group=botLeftConfig.findGroup(strLeftGroup.c_str(),string("Loading configuration from group " + strLeftGroup).c_str());
        botLeftConfig.fromString(group.toString());
    }
    else
    {
        yError() << "Group " << strLeftGroup << " not found.";
        return false;
    }
    
    string strRightGroup = "CAMERA_CALIBRATION_RIGHT";
    if (botRightConfig.check(strRightGroup.c_str()))
    {            
        Bottle &group=botRightConfig.findGroup(strRightGroup.c_str(),string("Loading configuration from group " + strRightGroup).c_str());
        botRightConfig.fromString(group.toString());
    }
    else
    {
        yError() << "Group " << strRightGroup << " not found.";
        return false;
    }

    string calibToolLeftName  = botLeftConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
    string calibToolRightName = botRightConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();

    calibToolLeft = CalibToolFactories::getPool().get(calibToolLeftName.c_str());
    if (calibToolLeft!=NULL)
    {
        bool ok = calibToolLeft->open(botLeftConfig);
        if (!ok)
        {
            delete calibToolLeft;
            calibToolLeft = NULL;
            return false;
        }
    }
    calibToolRight = CalibToolFactories::getPool().get(calibToolRightName.c_str());
    if (calibToolRight!=NULL)
    {
        bool ok = calibToolRight->open(botRightConfig);
        if (!ok)
        {
            delete calibToolRight;
            calibToolRight = NULL;
            return false;
        }
    }

    if(rf.check("dual"))
    {
        dualImage_mode = true;
        yInfo() << "Dual mode activate!!";
    }

    if(dualImage_mode)
    {
        leftImage  = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;
        rightImage = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;

        // open a single port with name /dual:i
        if (yarp::os::Network::exists(getName("/dual:i")))
        {
            yWarning() << "port " << getName("/dual:i") << " already in use";
        }
        if(! imageInLeft.open(getName("/dual:i")) )
            return false;
        imageInLeft.setStrict(false);
    }
    else
    {
        if (yarp::os::Network::exists(getName("/left:i")))
        {
            yWarning() << "port " << getName("/left:i") << " already in use";
        }
        if (yarp::os::Network::exists(getName("/right:i")))
        {
            yWarning() << "port " << getName("/right:i") << " already in use";
        }
        imageInLeft.open(getName("/left:i"));
        imageInRight.open(getName("/right:i"));
        imageInLeft.setStrict(false);
        imageInRight.setStrict(false);
    }

    if (yarp::os::Network::exists(getName("/out")))
    {
        yWarning() << "port " << getName("/out") << " already in use";
    }
    if (yarp::os::Network::exists(getName("/conf")))
    {
        yWarning() << "port " << getName("/conf") << " already in use";
    }
    imageOut.open(getName("/out:o"));
    configPort.open(getName("/conf"));
    attach(configPort);
    return true;
}