/*-------------------------------------------------------------
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;
}
}
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 §ion)
{
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);
}
/** 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 §ion )
{
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
}
/*---------------------------------------------------------------
loadFromConfigFile
---------------------------------------------------------------*/
void CHMTSLAM::TOptions::loadFromConfigFile(
const mrpt::utils::CConfigFileBase &source,
const std::string §ion)
{
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);
}
/*---------------------------------------------------------------
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
}
/*-------------------------------------------------------------
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
}
/*-------------------------------------------------------------
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
}
void CHolonomicND::TOptions::loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string §ion)
{
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
}
/* -----------------------------------------------------
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]) );
}
}
/*---------------------------------------------------------------
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
}