///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 &params )
{
	//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);

	}
}
Exemplo n.º 2
0
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();
}
Exemplo n.º 9
0
void PlannerPRM::initialize(Sampler* sampler_, const RoboCompCommonBehavior::ParameterList &params)
{	
	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;
}
Exemplo n.º 11
0
/**
 * @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 &params)
{
	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() );
}
Exemplo n.º 12
0
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;
}
Exemplo n.º 13
0
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;
}
Exemplo n.º 15
0
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;
}
Exemplo n.º 16
0
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;
};