Пример #1
0
/*-------------------------------------------------------------
						loadExclusionAreas
-------------------------------------------------------------*/
void C2DRangeFinderAbstract::loadCommonParams(
	const mrpt::utils::CConfigFileBase &configSource,
	const std::string	  &iniSection )
{
	// Params:
	m_showPreview = configSource.read_bool(iniSection, "preview", false );

	// Load exclusion areas:
	m_lstExclusionPolys.clear();
	m_lstExclusionAngles.clear();

	unsigned int N = 1;

	for(;;)
	{
		vector<double> x,y, z_range;
		configSource.read_vector( iniSection, format("exclusionZone%u_x",N), vector<double>(0), x);
		configSource.read_vector( iniSection, format("exclusionZone%u_y",N), vector<double>(0), y);
		configSource.read_vector( iniSection, format("exclusionZone%u_z",N++), vector<double>(0), z_range);

		if (!x.empty() && !y.empty())
		{
			ASSERT_(x.size()==y.size())

			CObservation2DRangeScan::TListExclusionAreasWithRanges::value_type dat;

			dat.first.setAllVertices(x,y);
			if (z_range.empty())
			{
				dat.second.first  = -std::numeric_limits<double>::max();
				dat.second.second =  std::numeric_limits<double>::max();
			}
			else
			{
				ASSERTMSG_(z_range.size()==2,"exclusionZone%u_z must be a range [z_min z_max]");
				ASSERT_(z_range[0]<=z_range[1]);

				dat.second.first  = z_range[0];
				dat.second.second = z_range[1];
			}

			m_lstExclusionPolys.push_back(dat);
		}
		else break;
	}


	// Load forbiden angles;
	N = 1;

	for(;;)
	{
		const double ini = DEG2RAD( configSource.read_double( iniSection, format("exclusionAngles%u_ini",N), -1000 ) );
		const double end = DEG2RAD( configSource.read_double( iniSection, format("exclusionAngles%u_end",N++), -1000 ) );

		if (ini>-M_PI && end>-M_PI)
		     m_lstExclusionAngles.push_back(make_pair(ini,end));
		else break;
	}
}
Пример #2
0
void CReactiveNavigationSystem::loadConfigFile(
	const mrpt::utils::CConfigFileBase& c)
{
	MRPT_START

	// 1st: load my own params; at the end, call parent's overriden method:
	const std::string sectCfg = "CReactiveNavigationSystem";
	this->params_reactive_nav.loadFromConfigFile(c, sectCfg);

	unsigned int PTG_COUNT = c.read_int(sectCfg, "PTG_COUNT", 0, true);

	// Load robot shape: 1/2 polygon
	// ---------------------------------------------
	vector<float> xs, ys;
	c.read_vector(
		sectCfg, "RobotModel_shape2D_xs", vector<float>(0), xs, false);
	c.read_vector(
		sectCfg, "RobotModel_shape2D_ys", vector<float>(0), ys, false);
	ASSERTMSG_(
		xs.size() == ys.size(),
		"Config parameters `RobotModel_shape2D_xs` and `RobotModel_shape2D_ys` "
		"must have the same length!");
	if (!xs.empty())
	{
		math::CPolygon shape;
		for (size_t i = 0; i < xs.size(); i++) shape.AddVertex(xs[i], ys[i]);
		changeRobotShape(shape);
	}

	// Load robot shape: 2/2 circle
	// ---------------------------------------------
	const double robot_shape_radius =
		c.read_double(sectCfg, "RobotModel_circular_shape_radius", .0, false);
	ASSERT_(robot_shape_radius >= .0);
	if (robot_shape_radius != .0)
	{
		changeRobotCircularShapeRadius(robot_shape_radius);
	}

	// Load PTGs from file:
	// ---------------------------------------------
	// Free previous PTGs:
	for (size_t i = 0; i < PTGs.size(); i++) delete PTGs[i];
	PTGs.assign(PTG_COUNT, nullptr);

	for (unsigned int n = 0; n < PTG_COUNT; n++)
	{
		// Factory:
		const std::string sPTGName =
			c.read_string(sectCfg, format("PTG%u_Type", n), "", true);
		PTGs[n] = CParameterizedTrajectoryGenerator::CreatePTG(
			sPTGName, c, sectCfg, format("PTG%u_", n));
	}

	CAbstractPTGBasedReactive::loadConfigFile(
		c);  // call parent's overriden method:

	MRPT_END
}
/*---------------------------------------------------------------
					loadFromConfigFile
  ---------------------------------------------------------------*/
void  COccupancyGridMap2D::TLikelihoodOptions::loadFromConfigFile(
	const mrpt::utils::CConfigFileBase  &iniFile,
	const std::string &section)
{
	MRPT_LOAD_CONFIG_VAR_CAST(likelihoodMethod, int, TLikelihoodMethod, iniFile, section);

    enableLikelihoodCache               = iniFile.read_bool(section,"enableLikelihoodCache",enableLikelihoodCache);

	LF_stdHit							= iniFile.read_float(section,"LF_stdHit",LF_stdHit);
	LF_zHit								= iniFile.read_float(section,"LF_zHit",LF_zHit);
	LF_zRandom							= iniFile.read_float(section,"LF_zRandom",LF_zRandom);
	LF_maxRange							= iniFile.read_float(section,"LF_maxRange",LF_maxRange);
	LF_decimation						= iniFile.read_int(section,"LF_decimation",LF_decimation);
	LF_maxCorrsDistance					= iniFile.read_float(section,"LF_maxCorrsDistance",LF_maxCorrsDistance);
	LF_useSquareDist					= iniFile.read_bool(section,"LF_useSquareDist",LF_useSquareDist);
	LF_alternateAverageMethod			= iniFile.read_bool(section,"LF_alternateAverageMethod",LF_alternateAverageMethod);

	MI_exponent							= iniFile.read_float(section,"MI_exponent",MI_exponent);
	MI_skip_rays						= iniFile.read_int(section,"MI_skip_rays",MI_skip_rays);
	MI_ratio_max_distance				= iniFile.read_float(section,"MI_ratio_max_distance",MI_ratio_max_distance);

	rayTracing_useDistanceFilter		= iniFile.read_bool(section,"rayTracing_useDistanceFilter",rayTracing_useDistanceFilter);
	rayTracing_stdHit					= iniFile.read_float(section,"rayTracing_stdHit",rayTracing_stdHit);

	consensus_takeEachRange				= iniFile.read_int(section,"consensus_takeEachRange",consensus_takeEachRange);
	consensus_pow						= iniFile.read_float(section,"consensus_pow",consensus_pow);

	iniFile.read_vector(section,"OWA_weights",OWA_weights,OWA_weights);
}
Пример #4
0
/**  Load all the params from a config source, in the format described in saveToConfigFile()
  */
void TMultiResDescOptions::loadFromConfigFile( const mrpt::utils::CConfigFileBase &cfg, const std::string &section )
{
    basePSize = cfg.read_double(section,"basePSize", 23, false );
    comLScl = cfg.read_int(section,"comLScl", 0, false );
    comHScl = cfg.read_int(section,"comHScl", 6, false );
    sg1 = cfg.read_double(section,"sg1", 0.5, false );
    sg2 = cfg.read_double(section,"sg2", 7.5, false );
    sg3 = cfg.read_double(section,"sg3", 8.0, false );
    computeDepth = cfg.read_bool(section,"computeDepth", true, false );
    blurImage = cfg.read_bool(section,"blurImage", true, false );
    fx = cfg.read_double(section,"fx",0.0, false);
    cx = cfg.read_double(section,"cx",0.0, false);
    cy = cfg.read_double(section,"cy",0.0, false);
    baseline = cfg.read_double(section,"baseline",0.0, false);
    computeHashCoeffs = cfg.read_bool(section,"computeHashCoeffs", false, false );


    cfg.read_vector(section,"scales",vector<double>(),scales,false);
    if(scales.size() < 1)
    {
        scales.resize(7);
        scales[0] = 0.5;
        scales[1] = 0.8;
        scales[2] = 1.0;
        scales[3] = 1.2;
        scales[4] = 1.5;
        scales[5] = 1.8;
        scales[6] = 2.0;
    } // end-if
}
Пример #5
0
/*---------------------------------------------------------------
						loadFromConfigFile
  ---------------------------------------------------------------*/
void  CHMTSLAM::TOptions::loadFromConfigFile(
	const mrpt::utils::CConfigFileBase	&source,
	const std::string		&section)
{
	MRPT_LOAD_CONFIG_VAR( LOG_OUTPUT_DIR, string,    source, section);
	MRPT_LOAD_CONFIG_VAR( LOG_FREQUENCY,  int,     	source, section);

	MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT(SLAM_METHOD, int,  TLSlamMethod,  source, section);

	MRPT_LOAD_CONFIG_VAR( SLAM_MIN_DIST_BETWEEN_OBS, float, 		source, section);
	MRPT_LOAD_CONFIG_VAR_DEGREES( SLAM_MIN_HEADING_BETWEEN_OBS, 	source, section);

	MRPT_LOAD_CONFIG_VAR(MIN_ODOMETRY_STD_XY,float, source,section);
	MRPT_LOAD_CONFIG_VAR_DEGREES( MIN_ODOMETRY_STD_PHI, source,section);

	MRPT_LOAD_CONFIG_VAR( VIEW3D_AREA_SPHERES_HEIGHT, float,  	source, section);
	MRPT_LOAD_CONFIG_VAR( VIEW3D_AREA_SPHERES_RADIUS, float,  	source, section);

	MRPT_LOAD_CONFIG_VAR( random_seed, int, source,section);

	stds_Q_no_odo[2] = RAD2DEG(stds_Q_no_odo[2]);
	source.read_vector(section,"stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo );
	ASSERT_(stds_Q_no_odo.size()==3)

	stds_Q_no_odo[2] = DEG2RAD(stds_Q_no_odo[2]);

	std::string sTLC_detectors = source.read_string(section,"TLC_detectors", "", true );

	mrpt::system::tokenize(sTLC_detectors,", ",TLC_detectors);

	std::cout << "TLC_detectors: " << TLC_detectors.size() << std::endl;

	// load other sub-classes:
	AA_options.loadFromConfigFile(source,section);
}
Пример #6
0
/*---------------------------------------------------------------
						initialize
  ---------------------------------------------------------------*/
void  CHolonomicND::initialize( const mrpt::utils::CConfigFileBase &INI_FILE )
{
    MRPT_START

	const std::string section("ND_CONFIG");

	// Default values:
	WIDE_GAP_SIZE_PERCENT               = 0.50f;
	MAX_SECTOR_DIST_FOR_D2_PERCENT      = 0.25f;
	RISK_EVALUATION_SECTORS_PERCENT     = 0.10f;
	RISK_EVALUATION_DISTANCE            = 0.4f;
	TOO_CLOSE_OBSTACLE                  = 0.15f;
	TARGET_SLOW_APPROACHING_DISTANCE    = 0.20f;

    // Load from config text:
	MRPT_LOAD_CONFIG_VAR(WIDE_GAP_SIZE_PERCENT,double,  INI_FILE,section );
	MRPT_LOAD_CONFIG_VAR(MAX_SECTOR_DIST_FOR_D2_PERCENT,double,  INI_FILE,section );
	MRPT_LOAD_CONFIG_VAR(RISK_EVALUATION_SECTORS_PERCENT,double,  INI_FILE,section );
	MRPT_LOAD_CONFIG_VAR(RISK_EVALUATION_DISTANCE,double,  INI_FILE,section );
	MRPT_LOAD_CONFIG_VAR(TOO_CLOSE_OBSTACLE,double,  INI_FILE,section );
	MRPT_LOAD_CONFIG_VAR(TARGET_SLOW_APPROACHING_DISTANCE,double,  INI_FILE,section );

    INI_FILE.read_vector(section,"factorWeights", vector_double(0), factorWeights, true );
    ASSERT_(factorWeights.size()==4);

    MRPT_END
}
Пример #7
0
/*-------------------------------------------------------------
						loadConfig_sensorSpecific
-------------------------------------------------------------*/
void  CBoardSonars::loadConfig_sensorSpecific(	const mrpt::utils::CConfigFileBase &configSource,
								const std::string	  &iniSection )
{
	MRPT_START

	std::vector<double> aux;											// Auxiliar vector

	// Some parameters ...
	m_usbSerialNumber	= configSource.read_string(iniSection,"USB_serialNumber",m_usbSerialNumber,true);
	m_gain				= configSource.read_int(iniSection,"gain",m_gain,true);
	m_maxRange			= configSource.read_float(iniSection,"maxRange",m_maxRange,true);
	m_minTimeBetweenPings = configSource.read_float(iniSection,"minTimeBetweenPings",m_minTimeBetweenPings,true);
	// ----------------------------------------------------------------------------------------------------------------------
	ASSERT_( m_maxRange>0 && m_maxRange<=11 );
	ASSERT_( m_gain<=16 );


	// Sonar firing order ...
	configSource.read_vector( iniSection, "firingOrder", m_firingOrder, m_firingOrder, true );

	// ----------------------------------------------------------------------------------------------------------------------

	// Individual sonar gains ...
	configSource.read_vector( iniSection, "sonarGains", aux, aux, true );

	std::vector<int32_t>::iterator itSonar;
	std::vector<double>::iterator	itAux;
	for( itSonar = m_firingOrder.begin(), itAux = aux.begin(); itSonar != m_firingOrder.end(); ++itSonar, ++itAux )
		m_sonarGains[ *itSonar ] = *itAux;
	// ----------------------------------------------------------------------------------------------------------------------
	ASSERT_( aux.size() == m_firingOrder.size() );

	// Individual sonar poses
	aux.clear();
	for( itSonar = m_firingOrder.begin(); itSonar != m_firingOrder.end(); ++itSonar )
	{
		configSource.read_vector( iniSection, format("pose%i",*itSonar), aux, aux, true );	// Get the sonar poses
		m_sonarPoses[ *itSonar ] = mrpt::math::TPose3D( aux[0], aux[1], aux[2], DEG2RAD( (float)aux[3]), DEG2RAD( (float)aux[4]), DEG2RAD( (float)aux[5]) );
	}
	// ----------------------------------------------------------------------------------------------------------------------
	ASSERT_( m_sonarGains.size() == m_firingOrder.size() );

	MRPT_END
}
Пример #8
0
/*-------------------------------------------------------------
					loadConfig_sensorSpecific
-------------------------------------------------------------*/
void  CBoardENoses::loadConfig_sensorSpecific(
	const mrpt::utils::CConfigFileBase &configSource,
	const std::string			&iniSection )
{
	MRPT_START

	m_usbSerialNumber = configSource.read_string(iniSection, "USB_serialname","",false);

#ifdef MRPT_OS_WINDOWS
	m_COM_port = configSource.read_string(iniSection, "COM_port_WIN",m_COM_port);
#else
	m_COM_port = configSource.read_string(iniSection, "COM_port_LIN",m_COM_port);
#endif
	m_COM_baud = configSource.read_uint64_t(iniSection, "COM_baudRate",m_COM_baud);

	configSource.read_vector( iniSection, "enose_poses_x", vector<float>(0), enose_poses_x, true);
	configSource.read_vector( iniSection, "enose_poses_y", vector<float>(0), enose_poses_y, true);
	configSource.read_vector( iniSection, "enose_poses_z", vector<float>(0), enose_poses_z, true);

	configSource.read_vector( iniSection, "enose_poses_yaw", vector<float>(0), enose_poses_yaw, true);
	configSource.read_vector( iniSection, "enose_poses_pitch", vector<float>(0), enose_poses_pitch, true);
	configSource.read_vector( iniSection, "enose_poses_roll", vector<float>(0), enose_poses_roll, true);

	ASSERT_( enose_poses_x.size() == enose_poses_y.size() );
	ASSERT_( enose_poses_x.size() == enose_poses_z.size() );
	ASSERT_( enose_poses_x.size() == enose_poses_yaw.size() );
	ASSERT_( enose_poses_x.size() == enose_poses_pitch.size() );
	ASSERT_( enose_poses_x.size() == enose_poses_roll.size() );

	// Pass angles to radians:
	enose_poses_yaw *= M_PIf / 180.0f;
	enose_poses_pitch *= M_PIf / 180.0f;
	enose_poses_roll *= M_PIf / 180.0f;

	MRPT_END

}
Пример #9
0
void CHolonomicND::TOptions::loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section)
{
	MRPT_START

	// Load from config text:
	MRPT_LOAD_CONFIG_VAR(WIDE_GAP_SIZE_PERCENT,double,  source,section );
	MRPT_LOAD_CONFIG_VAR(MAX_SECTOR_DIST_FOR_D2_PERCENT,double,  source,section );
	MRPT_LOAD_CONFIG_VAR(RISK_EVALUATION_SECTORS_PERCENT,double,  source,section );
	MRPT_LOAD_CONFIG_VAR(RISK_EVALUATION_DISTANCE,double,  source,section );
	MRPT_LOAD_CONFIG_VAR(TOO_CLOSE_OBSTACLE,double,  source,section );
	MRPT_LOAD_CONFIG_VAR(TARGET_SLOW_APPROACHING_DISTANCE,double,  source,section );

	source.read_vector(section,"factorWeights", std::vector<double>(), factorWeights, true );
	ASSERT_(factorWeights.size()==4);

	MRPT_END
}
Пример #10
0
/* -----------------------------------------------------
                loadConfig_sensorSpecific
   ----------------------------------------------------- */
void  CBoardIR::loadConfig_sensorSpecific(
	const mrpt::utils::CConfigFileBase &configSource,
	const std::string	  &iniSection )
{
#ifdef MRPT_OS_WINDOWS
	m_COMname = configSource.read_string(iniSection, "COM_port_WIN", m_COMname, true );
#else
	m_COMname = configSource.read_string(iniSection, "COM_port_LIN", m_COMname, true );
#endif

	m_COMbauds		= configSource.read_int( iniSection, "baudRate",m_COMbauds, true );

	std::vector<double> aux;				// Auxiliar vector
	for( unsigned int i = 0; i < 6; i++ )
	{
		configSource.read_vector( iniSection, format("pose%d",i), aux, aux, true );	// Get the IR poses
		m_IRPoses[i] = mrpt::math::TPose3D( aux[0], aux[1], aux[2], DEG2RAD( (float)aux[3]), DEG2RAD( (float)aux[4]), DEG2RAD( (float)aux[5]) );
	}
}
Пример #11
0
/*---------------------------------------------------------------
						loadConfigFile
  ---------------------------------------------------------------*/
void CReactiveNavigationSystem::loadConfigFile(const mrpt::utils::CConfigFileBase &ini, const mrpt::utils::CConfigFileBase &robotIni )
{
	MRPT_START

	m_collisionGridsMustBeUpdated = true;

	// Load config from INI file:
	// ------------------------------------------------------------
	robotName = robotIni.read_string("ROBOT_NAME","Name", "ReactiveParams" /* Default section for the rest of params */);

	unsigned int PTG_COUNT = ini.read_int(robotName,"PTG_COUNT",0, true );

	refDistance = ini.read_float(robotName,"MAX_REFERENCE_DISTANCE",5 );
	colGridRes = ini.read_float(robotName,"LUT_CELL_SIZE",0.0f );
	// backwards compt config file:
	if (!colGridRes) colGridRes = ini.read_float(robotName,"RESOLUCION_REJILLA_X",0.02f );

	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(robotMax_V_mps,float,  ini,robotName);
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(robotMax_W_degps,float,  ini,robotName);
	MRPT_LOAD_CONFIG_VAR(SPEEDFILTER_TAU,float,  ini,robotName);

	ini.read_vector( robotName, "weights", vector<float>(0), weights, true );
	ASSERT_(weights.size()==6);

	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(minObstaclesHeight,float,  ini,robotName);
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(maxObstaclesHeight,float,  ini,robotName);

	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(DIST_TO_TARGET_FOR_SENDING_EVENT,float,  ini,robotName);

	badNavAlarm_AlarmTimeout = ini.read_float("GLOBAL_CONFIG","ALARM_SEEMS_NOT_APPROACHING_TARGET_TIMEOUT", badNavAlarm_AlarmTimeout, true);

	// Load robot shape:
	// ---------------------------------------------
	math::CPolygon		shape;
	vector<float>        xs,ys;

	ini.read_vector(robotName,"RobotModel_shape2D_xs",vector<float>(0), xs, true );
	ini.read_vector(robotName,"RobotModel_shape2D_ys",vector<float>(0), ys, true );
	ASSERT_(xs.size()==ys.size());

	// Add to polygon
	for (size_t i=0;i<xs.size();i++)
		shape.AddVertex(xs[i],ys[i]);

	changeRobotShape( shape );

	// Load PTGs from file:
	// ---------------------------------------------
	// Free previous PTGs:
	for (size_t i=0;i<PTGs.size();i++)	delete PTGs[i];
	PTGs.assign(PTG_COUNT,NULL);

	printf_debug("\n");

	MRPT_TODO("Refactor loading params & simulating trajectories?")

	for ( unsigned int n=0;n<PTG_COUNT;n++ )
	{
		// load params of this PTG:

		TParameters<double> params;
		params["ref_distance"] = refDistance;
		params["resolution"]   = colGridRes;

		params["PTG_type"]	= ini.read_int(robotName,format("PTG%u_Type", n ),1, true );
		params["v_max"]		= ini.read_float(robotName,format("PTG%u_v_max_mps", n ), 5, true);
		params["w_max"]		= DEG2RAD(ini.read_float(robotName,format("PTG%u_w_max_gps", n ), 0, true));
		params["K"]			= ini.read_int(robotName,format("PTG%u_K", n ), 1, false);
		params["cte_a0v"]	= DEG2RAD( ini.read_float(robotName,format("PTG%u_cte_a0v_deg", n ), 0, false) );
		params["cte_a0w"]	= DEG2RAD( ini.read_float(robotName,format("PTG%u_cte_a0w_deg", n ), 0, false) );

		const int nAlfas = ini.read_int(robotName,format("PTG%u_nAlfas", n ),100, true );

		// Generate it:
		printf_debug("[loadConfigFile] Generating PTG#%u...",n);

		PTGs[n] = CParameterizedTrajectoryGenerator::CreatePTG(params);

		printf_debug(PTGs[n]->getDescription().c_str());

		const float min_dist = 0.015f;
		m_timelogger.enter("PTG.simulateTrajectories");
		PTGs[n]->simulateTrajectories(
		    nAlfas,					// alphas,
		    75,						// max.tim,
		    refDistance,			// max.dist,
		    10*refDistance/min_dist,	// max.n,
		    0.0005f,				// diferencial_t
		    min_dist					// min_dist
			);
		m_timelogger.leave("PTG.simulateTrajectories");

		// Just for debugging, etc.
		//PTGs[n]->debugDumpInFiles(n);

		printf_debug("...OK!\n");
	}
	printf_debug("\n");

	this->loadHolonomicMethodConfig(ini,"GLOBAL_CONFIG");

	// Mostrar configuracion cargada de fichero:
	// --------------------------------------------------------
	printf_debug("\tLOADED CONFIGURATION:\n");
	printf_debug("-------------------------------------------------------------\n");

	ASSERT_(!m_holonomicMethod.empty())
	printf_debug("  Holonomic method \t\t= %s\n",typeid(m_holonomicMethod[0]).name());
	printf_debug("\n  GPT Count\t\t\t= %u\n", (int)PTG_COUNT );
	printf_debug("  Max. ref. distance\t\t= %f\n", refDistance );
	printf_debug("  Cells resolution \t= %.04f\n", colGridRes );
	printf_debug("  Max. speed (v,w)\t\t= (%.04f m/sec, %.04f deg/sec)\n", robotMax_V_mps, robotMax_W_degps );
	printf_debug("  Robot Shape Points Count \t= %u\n", m_robotShape.verticesCount() );
	printf_debug("  Obstacles 'z' axis range \t= [%.03f,%.03f]\n", minObstaclesHeight, maxObstaclesHeight );
	printf_debug("\n\n");

	m_init_done = true;

	MRPT_END
}