/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CBoardENoses::loadConfig_sensorSpecific(
const mrpt::config::CConfigFileBase& configSource,
const std::string& iniSection)
{
MRPT_START
m_usbSerialNumber =
configSource.read_string(iniSection, "USB_serialname", "", false);
#ifdef _WIN32
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
}
/*---------------------------------------------------------------
loadFromConfigFile
---------------------------------------------------------------*/
void CHMTSLAM::TOptions::loadFromConfigFile(
const mrpt::config::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);
}
void CReactiveNavigationSystem3D::loadConfigFile(
const mrpt::config::CConfigFileBase& c)
{
MRPT_START
m_PTGsMustBeReInitialized = true;
// 1st: load my own params; at the end, call parent's overriden method:
const std::string s = "CReactiveNavigationSystem3D";
unsigned int num_levels;
vector<float> xaux, yaux;
// Read config params which describe the robot shape
num_levels = c.read_int(s, "HEIGHT_LEVELS", 1, true);
m_robotShape.resize(num_levels);
for (unsigned int i = 1; i <= num_levels; i++)
{
m_robotShape.setHeight(
i - 1, c.read_float(s, format("LEVEL%d_HEIGHT", i), 1.0, true));
m_robotShape.setRadius(
i - 1, c.read_float(s, format("LEVEL%d_RADIUS", i), 0.5, false));
c.read_vector(
s, format("LEVEL%d_VECTORX", i), vector<float>(0), xaux, false);
c.read_vector(
s, format("LEVEL%d_VECTORY", i), vector<float>(0), yaux, false);
ASSERT_(xaux.size() == yaux.size());
for (unsigned int j = 0; j < xaux.size(); j++)
m_robotShape.polygon(i - 1).AddVertex(xaux[j], yaux[j]);
}
// Load PTGs from file:
// ---------------------------------------------
// levels = m_robotShape.heights.size()
unsigned int num_ptgs = c.read_int(s, "PTG_COUNT", 1, true);
m_ptgmultilevel.resize(num_ptgs);
// Read each PTG parameters, and generate K x N collisiongrids
// K - Number of PTGs
// N - Number of height sections
for (unsigned int j = 1; j <= num_ptgs; j++)
{
for (unsigned int i = 1; i <= m_robotShape.size(); i++)
{
MRPT_LOG_INFO_FMT(
"[loadConfigFile] Generating PTG#%u at level %u...", j, i);
const std::string sPTGName =
c.read_string(s, format("PTG%d_TYPE", j), "", true);
CParameterizedTrajectoryGenerator* ptgaux =
CParameterizedTrajectoryGenerator::CreatePTG(
sPTGName, c, s, format("PTG%d_", j));
m_ptgmultilevel[j - 1].PTGs.push_back(ptgaux);
}
}
MRPT_LOG_DEBUG_FMT(
" Robot height sections = %u\n",
static_cast<unsigned int>(m_robotShape.size()));
CAbstractPTGBasedReactive::loadConfigFile(
c); // call parent's overriden method:
MRPT_END
}