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