///Local Component parameters read at start
///Reading parameters from config file or passed in command line, with Ice machinery
///We need to supply a list of accepted values to each call
void SpecificMonitor::readConfig(RoboCompCommonBehavior::ParameterList ¶ms )
{
//Read params from config file
//Example
RoboCompCommonBehavior::Parameter aux;
aux.editable = false;
configGetString( "joystickUniversal.Device", aux.value,"/dev/input/js0");
params["joystickUniversal.Device"] = aux;
aux.editable = false;
configGetString( "joystickUniversal.NumAxes", aux.value,"2");
params["joystickUniversal.NumAxes"] = aux;
aux.editable = false;
configGetString( "joystickUniversal.NumButtons", aux.value,"1");
params["joystickUniversal.NumButtons"] = aux;
aux.editable = false;
configGetString( "joystickUniversal.BasicPeriod", aux.value,"100");
params["joystickUniversal.BasicPeriod"] = aux;
aux.editable = false;
configGetString( "joystickUniversal.NormalizationValue", aux.value,"10");
params["joystickUniversal.NormalizationValue"] = aux;
aux.editable = false;
configGetString( "joystickUniversal.VelocityAxis", aux.value,"vel");
params["joystickUniversal.VelocityAxis"] = aux;
aux.editable = false;
configGetString( "joystickUniversal.DirectionAxis", aux.value,"dir");
params["joystickUniversal.DirectionAxis"] = aux;
for (int i=0; i < atoi(params.at("joystickUniversal.NumAxes").value.c_str()); i++)
{
aux.editable = false;
std::string s = QString::number(i).toStdString();
configGetString( "joystickUniversal.Axis_" + s, aux.value , "4");
params["joystickUniversal.Axis_" + s] = aux;
rDebug("joystickUniversal.Axis_"+QString::fromStdString(s)+" = " + QString::fromStdString(params.at("joystickUniversal.Axis_" + s).value));
QStringList list = QString::fromStdString(aux.value).split(",");
if (list.size() != 4)
qFatal("joystickUniversalComp::Monitor::readConfig(): ERROR reading axis. Only %d parameters for motor %d.", list.size(), i);
aux.value=list[0].toStdString();
params["joystickUniversal.Axis_" + s +".Name"] = aux;
rDebug("joystickUniversal.Axis_" + s + ".Name = " + params.at("joystickUniversal.Axis_" + s +".Name").value);
aux.value=list[1].toStdString();
params["joystickUniversal.Axis_" + s +".MinRange"]= aux;
rDebug("joystickUniversal.Axis_"+s+".MinRange = "+ params["joystickUniversal.Axis_" + s +".MinRange"].value);
aux.value=list[2].toStdString();
params["joystickUniversal.Axis_" + s +".MaxRange"]= aux;
rDebug("joystickUniversal.Axis_"+s+".MaxRange = "+ params["joystickUniversal.Axis_" + s +".MaxRange"].value);
aux.value=list[3].toStdString();
params["joystickUniversal.Axis_" + s +".Inverted"]= aux;
rDebug("joystickUniversal.Axis_"+s+".Inverted = "+ params["joystickUniversal.Axis_" + s +".Inverted"].value);
}
}
bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
RoboCompRGBDBus::CameraParams camParams;
try
{
RoboCompCommonBehavior::Parameter par = params.at("CameraV4L.Device0.Name");
camParams.name = par.value;
par = params.at("CameraV4L.Device0.FPS");
camParams.colorFPS = atoi(par.value.c_str());
par = params.at("CameraV4L.Device0.Width");
camParams.colorWidth = atoi(par.value.c_str());
par = params.at("CameraV4L.Device0.Height");
camParams.colorHeight = atoi(par.value.c_str());
}
catch(std::exception e)
{ qFatal("\nAborting. Error reading config params"); }
std::array<string, 6> list = { "0", "1", "2", "3", "4", "5" };
qDebug() << __FUNCTION__ << "Opening device:" << camParams.name.c_str();
if( camParams.name == "default")
grabber.open(0);
else if ( find( begin(list), end(list), camParams.name) != end(list))
grabber.open(atoi(camParams.name.c_str()));
else
grabber.open(camParams.name);
if(grabber.isOpened() == false) // check if we succeeded
qFatal("Aborting. Could not open default camera %s", camParams.name.c_str());
else
qDebug() << __FUNCTION__ << "Camera " << QString::fromStdString(camParams.name) << " opened!";
//Setting grabber
camParams.colorFPS = 20;
grabber.set(CV_CAP_PROP_FPS, camParams.colorFPS);
camParams.colorFocal = 400;
grabber.set(CV_CAP_PROP_FRAME_HEIGHT, 480); //Get from PARAMS
grabber.set(CV_CAP_PROP_FRAME_WIDTH, 640);
//One frame to get real sizes
Mat frame;
grabber >> frame; // get a new frame from camera
Size s = frame.size();
double rate = grabber.get(CV_CAP_PROP_FPS);
qDebug() << __FUNCTION__ << "Current frame size:" << s.width << "x" << s.height << ". RGB 8 bits format at" << rate << "fps";
camParams.colorWidth = s.width;
camParams.colorHeight = s.height;
writeBuffer.resize( camParams.colorWidth * camParams.colorHeight * 3);
readBuffer.resize( camParams.colorWidth * camParams.colorHeight * 3);
cameraParamsMap[camParams.name] = camParams;
timer.start(30);
return true;
}
void
RoboCompCommonBehavior::__writeParameterList(::IceInternal::BasicStream* __os, const ::RoboCompCommonBehavior::ParameterList& v)
{
__os->writeSize(::Ice::Int(v.size()));
::RoboCompCommonBehavior::ParameterList::const_iterator p;
for(p = v.begin(); p != v.end(); ++p)
{
__os->write(p->first);
p->second.__write(__os);
}
}
void
RoboCompCommonBehavior::__readParameterList(::IceInternal::BasicStream* __is, ::RoboCompCommonBehavior::ParameterList& v)
{
::Ice::Int sz;
__is->readSize(sz);
while(sz--)
{
::std::pair<const ::std::string, ::RoboCompCommonBehavior::Parameter> pair;
__is->read(const_cast< ::std::string&>(pair.first));
::RoboCompCommonBehavior::ParameterList::iterator __i = v.insert(v.end(), pair);
__i->second.__read(__is);
}
}
bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
try
{
RoboCompCommonBehavior::Parameter par = params.at("HRIAgent.InnerModel") ;
if( QFile(QString::fromStdString(par.value)).exists() == true)
{
qDebug() << __FILE__ << __FUNCTION__ << __LINE__ << "Reading Innermodel file " << QString::fromStdString(par.value);
innerModel = new InnerModel(par.value);
qDebug() << __FILE__ << __FUNCTION__ << __LINE__ << "Innermodel file read OK!" ;
}
else
{
qDebug() << __FILE__ << __FUNCTION__ << __LINE__ << "Innermodel file " << QString::fromStdString(par.value) << " does not exists";
qFatal("Exiting now.");
}
}
catch(std::exception e)
{
qFatal("Error reading config params");
}
timer.start(Period);
return true;
}
bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
this->params = params;
try
{
innerModel = new InnerModel(params.at("InnerModel").value);
#ifdef USE_QTGUI
viewer = new InnerViewer(innerModel); //makes a copy of innermodel for internal use
#endif
}
catch (std::exception e)
{
qFatal("Aborting. Error reading config params"); //WE COULD THROW HERE AND HAVA A NICE EXIT FROM MAIN
}
//////////////////////////////
//Initial update InnerModel from robot
//////////////////////////////
innerModel->newTransform("virtualRobot", "static", innerModel->getNode("robot"));
updateInnerModel(innerModel, tState);
//////////////////////////////////////
/// Initialize sampler of free space
/////////////////////////////////////
sampler.initialize(innerModel, params);
///////////////////
//Planner
///////////////////
plannerPRM.initialize(&sampler, params);
///////////////////
//Initializes the elastic band structure "road"
///////////////////
road.initialize(innerModel, params);
///////////////////
//This object creates and maintains the road (elastic band) adapting it to the real world using a laser device
///////////////////
elasticband.initialize( params);
//////////////////////////////////////////////////////////////////////////
//Low level controller that drives the robot on the road
//by computing VAdv and VRot from the relative position wrt to the local road
/////////////////////////////////////////////////////////////////////////////////
controller = new Controller(innerModel, laserData, params, 2 );
qDebug() << __FUNCTION__ << "All objects initialized";
#ifdef USE_QTGUI
graphdraw.draw(plannerPRM, viewer);
viewer->start();
#endif
Period = 100;
timer.start(Period);
return true;
};
bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
//Inintializing parameters for CGR
LoadParameters();
//Inintializing InnerModel with ursus.xml
try
{
RoboCompCommonBehavior::Parameter par = params.at("InnerModelPath");
qDebug() << QString::fromStdString(par.value);
if( QFile::exists(QString::fromStdString(par.value)) )
{
innerModel = new InnerModel(par.value);
innerModelViewer = new InnerModelViewer(innerModel, "root", osgView->getRootGroup(), true);
}
else
{
std::cout << "Innermodel path " << par.value << " not found. "; qFatal("Abort");
}
}
catch(std::exception e)
{
qFatal("Error reading config params");
}
printf("NumParticles : %d\n",numParticles);
printf("Alpha1 : %f\n",motionParams.Alpha1);
printf("Alpha2 : %f\n",motionParams.Alpha2);
printf("Alpha3 : %f\n",motionParams.Alpha3);
printf("UsePointCloud : %d\n",usePointCloud?1:0);
printf("UseLIDAR : %d\n",noLidar?0:1);
printf("Visualizations : %d\n",debugLevel>=0?1:0);
printf("\n");
//double seed = floor(fmod(GetTimeSec()*1000000.0,1000000.0));
//if(debugLevel>-1) printf("Seeding with %d\n",(unsigned int)seed);
//srand(seed);
InnerModelDraw::addTransform(innerModelViewer,"poseRob1","floor");
InnerModelDraw::addTransform(innerModelViewer,"poseRob2","floor");
//Una vez cargado el innermodel y los parametros, cargamos los mapas con sus lineas y las pintamos.
string mapsFolder("etc/maps");
localization = new VectorLocalization2D(mapsFolder.c_str());
localization->initialize(numParticles,
curMapName.c_str(),initialLoc,initialAngle,locUncertainty,angleUncertainty);
qDebug()<<"<<<<<<<<<<<<<<< setparams1 >>>>>>>>>>>";
drawLines();
qDebug()<<"<<<<<<<<<<<<<<< setparams2 >>>>>>>>>>>";
drawParticles();
qDebug()<<"<<<<<<<<<<<<<<< setparams3 >>>>>>>>>>>";
timer.start(Period);
qDebug()<<"<<<<<<<<<<<<<<< setparamsF >>>>>>>>>>>";
return true;
}
void ElasticBand::initialize( const RoboCompCommonBehavior::ParameterList& params )
{
ROBOT_WIDTH = std::stof(params.at("RobotXWidth").value);
ROBOT_LENGTH = std::stoi(params.at("RobotZLong").value);
qDebug() << __FUNCTION__ << "Robot dimensions in ElasticBand: " << ROBOT_WIDTH << ROBOT_LENGTH;
////////////////////////////
/// points if the frontal edge of the robot used to test robot's hypothetical positions in the laser field
////////////////////////////
pointsMat = QMat::zeros(3,4);
//front edge
pointsMat(0,0) = -ROBOT_WIDTH/2;
pointsMat(0,2) = ROBOT_LENGTH/2;
pointsMat(0,3) = 1.f;
pointsMat(1,0) = ROBOT_WIDTH/2;
pointsMat(1,2) = ROBOT_LENGTH/2;
pointsMat(1,3) = 1.f;
pointsMat(2,0) = 0;
pointsMat(2,2) = ROBOT_LENGTH/2;
pointsMat(2,3) = 1.f;
//lower
// pointsMat(3,0) = -xs/2;
// pointsMat(3,2) = -zs/2;
// pointsMat(3,3) = 1.f;
// pointsMat(4,0) = xs/2;
// pointsMat(4,2) = -zs/2;
// pointsMat(4,3) = 1.f;
// pointsMat(5,0) = 0;
// pointsMat(5,2) = -zs/2;
// pointsMat(5,3) = 1.f;
//middle
// pointsMat(6,0) = -xs/2;
// pointsMat(6,2) = 0;
// pointsMat(6,3) = 1.f;
// pointsMat(7,0) = xs/2;
// pointsMat(7,2) = 0;
// pointsMat(7,3) = 1.f;
pointsMat = pointsMat.transpose();
}
void PlannerPRM::initialize(Sampler* sampler_, const RoboCompCommonBehavior::ParameterList ¶ms)
{
sampler = sampler_;
////////////////////////
/// Initialize RRTplaner
////////////////////////
plannerRRT.initialize(sampler);
////////////////////////
/// Check if graph already exists
////////////////////////
if( QFile(graphFileName).exists())
{
qDebug() << __FUNCTION__ << "Graph file exits. Loading";
readGraphFromFile(graphFileName);
}
else
{
try
{
nPointsInGraph = std::stoi(params.at("PlannerGraphPoints").value);
nNeighboursInGraph = std::stoi(params.at("PlannerGraphNeighbours").value);
maxDistToSearchmm = std::stof(params.at("PlannerGraphMaxDistanceToSearch").value);
robotRadiusmm = std::stof(params.at("RobotRadius").value);
}
catch(...)
{ qFatal("Planner-Initialize. Aborting. Some Planner graph parameters not found in config file"); }
qDebug() << __FUNCTION__ << "No graph file found. Creating with " << nPointsInGraph << "nodes and " << nNeighboursInGraph << "neighboors";
QList<QVec> pointList = sampler->sampleFreeSpaceR2(nPointsInGraph);
if( pointList.size() < nNeighboursInGraph )
qFatal("Planner-Initialize. Aborting. Could not find enough free points to build de graph");
qDebug() << __FUNCTION__ << "Creating with " << nPointsInGraph << "nodes and " << nNeighboursInGraph << "neighboors";
constructGraph(pointList, nNeighboursInGraph, maxDistToSearchmm, robotRadiusmm); ///GET From IM ----------------------------------
qDebug() << __FUNCTION__ << "Graph constructed with " << pointList.size() << "points";
writeGraphToFile(graphFileName);
}
graphDirtyBit = true;
}
bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
///INNERMODEL
try
{
RoboCompCommonBehavior::Parameter par = params.at("InnerModelPath");
innerModel = new InnerModel(par.value);
}
catch(std::exception e) { qFatal("Error reading config params"); }
timer.start(Period);
return true;
}
/**
* @brief Initializes the Sampler with the limits of robot's workspace and a point to innermodel.
* The method uses that pointer to create a copy of innermodel, so the Sampler can use to test valid
* robot configurations without interfering with the original one
*
* @param inner pointer to innerModel object
* @param outerRegion_ QRectF delimiting the robot's workspace
* @param innerRegions_ List of QRectF polygons delimiting forbidden regions inside robot's workspace
* @return void
*/
void Sampler::initialize(InnerModel *inner, const RoboCompCommonBehavior::ParameterList ¶ms)
{
qDebug() << __FUNCTION__ << "Sampler: Copying InnerModel...";
innerModelSampler = inner->copy();
try
{
outerRegion.setLeft(std::stof(params.at("OuterRegionLeft").value));
outerRegion.setRight(std::stof(params.at("OuterRegionRight").value));
outerRegion.setBottom(std::stof(params.at("OuterRegionBottom").value));
outerRegion.setTop(std::stof(params.at("OuterRegionTop").value));
qDebug() << __FUNCTION__ << "OuterRegion from config: " << outerRegion;
}
catch(...)
{ qFatal("Sampler-Initialize. Aborting. OuterRegion parameters not found in config file");} //CHANGE TO THROW
//innerRegions = innerRegions_;
// foreach(QRectF ir, innerRegions_)
// if( ir.isNull() == false)
// qFatal("Sampler-Initialize. Aborting. An InnerRegion is not a valid rectangle");
//
if(outerRegion.isNull())
qFatal("Sampler-Initialize. Aborting. OuterRegion is not properly initialized"); //CHANGE TO THROW
robotNodes.clear(); restNodes.clear();
QStringList ls = QString::fromStdString(params.at("ExcludedObjectsInCollisionCheck").value).replace(" ", "" ).split(',');
qDebug() << __FUNCTION__ << ls.size() << "objects read for exclusion list";
foreach( QString s, ls)
excludedNodes.insert(s);
// Compute the list of meshes that correspond to robot, world and possibly some additionally excluded ones
recursiveIncludeMeshes(innerModelSampler->getRoot(), "robot", false, robotNodes, restNodes, excludedNodes);
//Init random sequence generator
qsrand( QTime::currentTime().msec() );
}
bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
try
{
RoboCompCommonBehavior::Parameter par = params.at("InnerModel") ;
if( QFile(QString::fromStdString(par.value)).exists() == true)
{
qDebug() << __FILE__ << __FUNCTION__ << __LINE__ << "Reading Innermodel file " << QString::fromStdString(par.value);
innerModel = new InnerModel(par.value);
}
else
qFatal("Exiting now.");
}
catch(std::exception e) { qFatal("Error reading Innermodel param");}
InnerModelNode *nodeParent = innerModel->getNode("root");
if( innerModel->getNode("target") == NULL)
{
InnerModelTransform *node = innerModel->newTransform("target", "static", nodeParent, 0, 0, 0, 0, 0., 0, 0.);
nodeParent->addChild(node);
}
if (innerViewer)
{
osgView->getRootGroup()->removeChild(innerViewer);
delete innerViewer;
}
innerViewer = new InnerModelViewer(innerModel, "root", osgView->getRootGroup(), false);
QMutexLocker ml(mutex);
timer.start(Period);
IMVCamera cam = innerViewer->cameras["rgbd"];
const int width = cam.RGBDNode->width;
const int height = cam.RGBDNode->height;
if (color.size() != (uint)width*height)
{
color.resize ( width*height );
depth.resize ( width*height );
points.resize ( width*height );
}
return true;
}
bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
try
{
RoboCompCommonBehavior::Parameter par = params.at("InnerModelPath");
innerModel = std::make_shared<InnerModel>(par.value);
}
catch(std::exception e) { qFatal("Error reading config params"); }
qDebug() << __FILE__ ;
// Scene
scene.setSceneRect(-12000, -6000, 38000, 16000);
view.setScene(&scene);
view.scale(1, -1);
view.setParent(scrollArea);
//view.setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers)));
view.fitInView(scene.sceneRect(), Qt::KeepAspectRatio );
grid.initialize( TDim{ tilesize, -12000, 25000, -6000, 10000}, TCell{0, true, false, nullptr, 0.} );
for(auto &[key, value] : grid)
{
auto tile = scene.addRect(-tilesize/2,-tilesize/2, 100,100, QPen(Qt::NoPen));
tile->setPos(key.x,key.z);
value.rect = tile;
}
robot = scene.addRect(QRectF(-200, -200, 400, 400), QPen(), QBrush(Qt::blue));
noserobot = new QGraphicsEllipseItem(-50,100, 100,100, robot);
noserobot->setBrush(Qt::magenta);
target = QVec::vec3(0,0,0);
//qDebug() << __FILE__ << __FUNCTION__ << "CPP " << __cplusplus;
connect(buttonSave, SIGNAL(clicked()), this, SLOT(saveToFile()));
connect(buttonRead, SIGNAL(clicked()), this, SLOT(readFromFile()));
view.show();
timer.start();
return true;
}
/**
* \brief setParams method. It stores the innermodel file
*/
bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
try
{
RoboCompCommonBehavior::Parameter par = params.at("InnerModel") ;
if( QFile(QString::fromStdString(par.value)).exists() == true)
{
qDebug() << __FILE__ << __FUNCTION__ << __LINE__ << "Reading Innermodel file " << QString::fromStdString(par.value);
innerModel = new InnerModel(par.value);
}
else qFatal("Exiting now.");
}catch(std::exception e) { qFatal("Error reading Innermodel param");}
// QMutexLocker ml(mutex);
// INITIALIZED = true;
qDebug()<<"INITIALIZED: "<<INITIALIZED;
timer.start(Period);
return true;
}
bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
//
// Read InnerModel insertions
//
printf("read innermodel insertions\n");
RoboCompCommonBehavior::Parameter par;
try
{
par = params.at("AGMInner.InnerModels");
}
catch(std::exception e)
{
qFatal("Error reading config params: %s\n", e.what());
}
for (auto s : QString::fromStdString(par.value).split(";"))
{
auto v = s.split(",");
if( QFile(v[0]).exists() == true)
{
std::string sstr = v[0].toStdString();
printf("reading innermodel file %s\n", sstr.c_str());
InnerModel *innerModel = new InnerModel(sstr);
printf("%s ---> %s\n", v[0].toStdString().c_str(), v[1].toStdString().c_str());
innerModelInfoVector.push_back(std::pair<InnerModel *, QString>(innerModel, v[1]));
}
else
{
qFatal("File %s specifed in config file not found: Exiting now.", v[0].toStdString().c_str());
}
}
//
// Read initial AGM model
printf("read initial model\n");
worldModel = AGMModel::SPtr(new AGMModel(params.at("AGMInner.InitialModel").value));
qDebug()<<"initial model read with " << worldModel->numberOfSymbols() << " symbols\n";
if (worldModel->numberOfSymbols() > 0)
{
AGMModel::SPtr newModel(new AGMModel(worldModel));
for (auto p : innerModelInfoVector)
{
printf("Include in %d\n", p.second.toInt());
AGMInner::includeInnerModel(newModel, p.second.toInt(), p.first);
}
// printf("send\n");
// sendModificationProposal(worldModel, newModel);
printf("save agm model\n");
newModel->save(params.at("AGMInner.OutputFile").value);
printf("save extracted innermodel\n");
AGMInner::extractInnerModel(newModel, "world")->save("extractInnerModel.xml");
printf("The job was done. Exiting...\n");
exit(0);
}
return true;
}
void WayPoints::initialize(InnerModel* inner, const RoboCompCommonBehavior::ParameterList& params)
{
innerModel = inner;
threshold = std::stof(params.at("ArrivalTolerance").value);
}
bool SpecificWorker::setParams(RoboCompCommonBehavior::ParameterList params)
{
//Default value
m_width = 640;
m_height = 480;
try
{
INPUTIFACE = Camera;
RoboCompCommonBehavior::Parameter par = params.at("InputInterface");
if (par.value == "RGBD")
{
INPUTIFACE = RGBD;
}
else if ( par.value == "RGBDBus")
{
INPUTIFACE = RGBDBus;
}
else if ( par.value == "Camera")
{
INPUTIFACE = Camera;
try
{
RoboCompCommonBehavior::Parameter par = params.at("CameraResolution");
if ( par.value == "320x240" )
{
m_width = 320;
m_height = 240;
}
if ( par.value == "640x480" )
{
m_width = 640;
m_height = 480;
}
}
catch(std::exception e)
{}
}
else
qFatal("InputInterface");
}
catch(std::exception e)
{
qFatal("Error reading config params");
}
try
{
RoboCompCommonBehavior::Parameter par = params.at("AprilTagsFamily") ;
if (par.value == "tagCodes16h5")
m_tagDetector = new ::AprilTags::TagDetector(::AprilTags::tagCodes16h5);
else if ( par.value == "tagCodes25h7")
m_tagDetector = new ::AprilTags::TagDetector(::AprilTags::tagCodes25h7);
else if ( par.value == "tagCodes25h9")
m_tagDetector = new ::AprilTags::TagDetector(::AprilTags::tagCodes25h9);
else if ( par.value == "tagCodes36h11")
m_tagDetector = new ::AprilTags::TagDetector(::AprilTags::tagCodes36h11);
else if ( par.value == "tagCodes36h9")
m_tagDetector = new ::AprilTags::TagDetector(::AprilTags::tagCodes36h9);
else
m_tagDetector = new ::AprilTags::TagDetector(::AprilTags::tagCodes16h5);
}
catch(std::exception e) { qFatal("Error reading config params"); }
try
{
RoboCompCommonBehavior::Parameter par = params.at("CameraName");
camera_name=par.value;
}
catch(std::exception e) { qFatal("Error reading config params"); }
try
{
RoboCompCommonBehavior::Parameter par = params.at("InnerModelPath");
innermodel_path=par.value;
}
catch(std::exception e) { qFatal("Error reading config params"); }
m_px = m_width/2;
m_py = m_height/2;
image_gray.create(m_height,m_width,CV_8UC1);
image_color.create(m_height,m_width,CV_8UC3);
innermodel = new InnerModel(innermodel_path);
m_fx = innermodel->getCameraFocal(camera_name.c_str());
m_fy = innermodel->getCameraFocal(camera_name.c_str());
qDebug() << QString::fromStdString(innermodel_path) << " " << QString::fromStdString(camera_name);
qDebug() << "FOCAL LENGHT:" << innermodel->getCameraFocal(camera_name.c_str());
//Reading id sets size to create a map
try
{
RoboCompCommonBehavior::Parameter par = params.at("ID:0-10");
Q_ASSERT(par.value > 0 and par.value < 500);
for(int i=0;i<=10; i++)
tagsSizeMap.insert( i, QString::fromStdString(par.value).toFloat());
}
catch(std::exception e) { qFatal("Error reading config params");}
try
{
RoboCompCommonBehavior::Parameter par = params.at("ID:11-20");
Q_ASSERT(par.value > 0 and par.value < 500);
for(int i=11;i<=20; i++)
tagsSizeMap.insert( i, QString::fromStdString(par.value).toFloat());
}
catch(std::exception e) { std::cout << e.what() << std::endl;}
try
{
RoboCompCommonBehavior::Parameter par = params.at("ID:21-30");
Q_ASSERT(par.value > 0 and par.value < 500);
for(int i=21;i<=30; i++)
tagsSizeMap.insert( i, QString::fromStdString(par.value).toFloat());
}
catch(std::exception e) { std::cout << e.what() << std::endl;}
//DONE
//Default value for IDs not defined before
timer.start(Period);
return true;
};