void CRandomFieldGridMap3D::TInsertionOptions::loadFromConfigFile(
const mrpt::utils::CConfigFileBase &iniFile,
const std::string §ion)
{
GMRF_lambdaPrior = iniFile.read_double(section.c_str(), "GMRF_lambdaPrior", GMRF_lambdaPrior);
GMRF_skip_variance = iniFile.read_bool(section.c_str(),"GMRF_skip_variance", GMRF_skip_variance);
}
void CImpinjRFID::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
MRPT_START
// TEMPORARILY DISABLED
/* pose_x_1 = configSource.read_float(iniSection,"pose_x_1",0,true);
pose_y_1 = configSource.read_float(iniSection,"pose_y_1",0,true);
pose_z_1 = configSource.read_float(iniSection,"pose_z_1",0,true);
pose_roll_1 = configSource.read_float(iniSection,"pose_roll_1",0,true);
pose_pitch_1 = configSource.read_float(iniSection,"pose_pitch_1",0,true);
pose_yaw_1 = configSource.read_float(iniSection,"pose_yaw_1",0,true);
pose_x_2 = configSource.read_float(iniSection,"pose_x_2",0,true);
pose_y_2 = configSource.read_float(iniSection,"pose_y_2",0,true);
pose_z_2 = configSource.read_float(iniSection,"pose_z_2",0,true);
pose_roll_2 = configSource.read_float(iniSection,"pose_roll_2",0,true);
pose_pitch_2 = configSource.read_float(iniSection,"pose_pitch_2",0,true);
pose_yaw_2 = configSource.read_float(iniSection,"pose_yaw_2",0,true);
*/
IPm = configSource.read_string(iniSection,"local_IP","127.0.0.1",false);
reader_name = configSource.read_string(iniSection,"reader_name","", true);
port = configSource.read_int(iniSection,"listen_port",0,true);
driver_path = configSource.read_string(iniSection,"driver_path","",true);
MRPT_END
}
/*---------------------------------------------------------------
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);
}
/* -----------------------------------------------------
loadConfig_sensorSpecific
----------------------------------------------------- */
void CNTRIPEmitter::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase& c, const std::string& s)
{
#ifdef MRPT_OS_WINDOWS
m_com_port = c.read_string(s, "COM_port_WIN", "");
#else
m_com_port = c.read_string(s, "COM_port_LIN", "");
#endif
m_raw_output_file_prefix = c.read_string(s, "raw_output_file_prefix", "");
ASSERTMSG_(
!m_raw_output_file_prefix.empty() || !m_com_port.empty(),
"At least one of either raw file output or serial COM file must be "
"specified in configuration file!");
if (!m_com_port.empty())
{
m_com_bauds = c.read_int(s, "baudRate", m_com_bauds, true);
}
m_transmit_to_server =
c.read_bool(s, "transmit_to_server", m_transmit_to_server);
m_ntrip_args.mountpoint =
mrpt::system::trim(c.read_string(s, "mountpoint", "", true));
m_ntrip_args.server =
mrpt::system::trim(c.read_string(s, "server", "", true));
m_ntrip_args.port = c.read_int(s, "port", 2101, true);
m_ntrip_args.user = mrpt::system::trim(c.read_string(s, "user", ""));
m_ntrip_args.password =
mrpt::system::trim(c.read_string(s, "password", ""));
}
/*-------------------------------------------------------------
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
}
void CHolonomicVFF::TOptions::saveToConfigFile(mrpt::utils::CConfigFileBase &cfg , const std::string §ion) const
{
MRPT_START
const int WN = 40, WV = 20;
cfg.write(section,"TARGET_SLOW_APPROACHING_DISTANCE",TARGET_SLOW_APPROACHING_DISTANCE, WN,WV, "For stopping gradually");
cfg.write(section,"TARGET_ATTRACTIVE_FORCE",TARGET_ATTRACTIVE_FORCE, WN,WV, "Dimension-less (may have to be tuned depending on the density of obstacle sampling)");
MRPT_END
}
/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CIMUIntersense::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase& configSource,
const std::string& iniSection)
{
m_sensorPose.setFromValues(
configSource.read_float(iniSection, "pose_x", 0, false),
configSource.read_float(iniSection, "pose_y", 0, false),
configSource.read_float(iniSection, "pose_z", 0, false),
DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
m_sensitivity =
configSource.read_int(iniSection, "sensitivity", m_sensitivity, false);
m_enhancement =
configSource.read_int(iniSection, "enhancement", m_enhancement, false);
m_prediction =
configSource.read_int(iniSection, "prediction", m_prediction, false);
m_useBuffer =
configSource.read_bool(iniSection, "useBuffer", m_useBuffer, false);
// dump parameters to console
cout << "---------------------------" << endl;
cout << "Intersense IMU parameters: " << endl;
cout << "---------------------------" << endl;
cout << "Sensitivity: " << m_sensitivity << endl;
cout << "Enhancement: " << m_enhancement << endl;
cout << "Prediction: " << m_prediction << endl;
cout << "Use buffer: " << m_useBuffer << endl;
cout << m_sensorPose << endl;
cout << "---------------------------" << endl << endl;
}
void CParameterizedTrajectoryGenerator::saveToConfigFile(mrpt::utils::CConfigFileBase &cfg,const std::string &sSection) const
{
MRPT_START
const int WN = 40, WV = 20;
cfg.write(sSection,"num_paths",m_alphaValuesCount, WN,WV, "Number of discrete paths (`resolution`) in the PTG");
cfg.write(sSection,"refDistance",refDistance, WN,WV, "Maximum distance (meters) for building trajectories (visibility range)");
cfg.write(sSection,"score_priority",m_score_priority, WN,WV, "When used in path planning, a multiplying factor (default=1.0) for the scores for this PTG. Assign values <1 to PTGs with low priority.");
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 CIbeoLuxETH::loadConfig_sensorSpecific( const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
bool faillNotFound = false;
pose_x = configSource.read_float(iniSection,"pose_x",0,faillNotFound);
pose_y = configSource.read_float(iniSection,"pose_y",0,faillNotFound);
pose_z = configSource.read_float(iniSection,"pose_z",0,faillNotFound);
pose_yaw = configSource.read_float(iniSection,"pose_yaw",0,faillNotFound);
pose_pitch = configSource.read_float(iniSection,"pose_pitch",0,faillNotFound);
pose_roll = configSource.read_float(iniSection,"pose_roll",0,faillNotFound);
m_sensorPose = CPose3D( pose_x, pose_y, pose_z,
DEG2RAD( pose_yaw ),DEG2RAD( pose_pitch ), DEG2RAD( pose_roll ));
}
void CPTG_RobotShape_Polygonal::loadShapeFromConfigFile(
const mrpt::utils::CConfigFileBase& cfg, const std::string& sSection)
{
bool any_pt = false;
const double BADNUM = std::numeric_limits<double>::max();
for (unsigned int nPt = 0;; ++nPt)
{
const std::string sPtx = mrpt::format("shape_x%u", nPt);
const std::string sPty = mrpt::format("shape_y%u", nPt);
const double ptx = cfg.read_double(sSection, sPtx, BADNUM, false);
const double pty = cfg.read_double(sSection, sPty, BADNUM, false);
if (ptx == BADNUM && pty == BADNUM) break;
ASSERTMSG_(
(ptx != BADNUM && pty != BADNUM),
"Error: mismatch between number of pts in {x,y} defining robot "
"shape");
if (!any_pt)
{
m_robotShape.clear();
any_pt = true;
}
m_robotShape.AddVertex(ptx, pty);
}
if (any_pt) internal_processNewRobotShape();
}
void CPTG_DiffDrive_alpha::saveToConfigFile(
mrpt::utils::CConfigFileBase& cfg, const std::string& sSection) const
{
MRPT_START
const int WN = 25, WV = 30;
CPTG_DiffDrive_CollisionGridBased::saveToConfigFile(cfg, sSection);
cfg.write(
sSection, "cte_a0v_deg", mrpt::utils::RAD2DEG(cte_a0v), WN, WV,
"Contant for vel profile [deg].");
cfg.write(
sSection, "cte_a0w_deg", mrpt::utils::RAD2DEG(cte_a0v), WN, WV,
"Contant for omega profile [deg].");
MRPT_END
}
/*---------------------------------------------------------------
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
}
void CPTG_DiffDrive_CollisionGridBased::saveToConfigFile(mrpt::utils::CConfigFileBase &cfg,const std::string &sSection) const
{
MRPT_START
const int WN = 25, WV = 30;
CParameterizedTrajectoryGenerator::saveToConfigFile(cfg,sSection);
cfg.write(sSection,"resolution",m_resolution, WN,WV, "Resolution of the collision-check look-up-table [m].");
cfg.write(sSection,"v_max_mps",V_MAX, WN,WV, "Maximum linear velocity for trajectories [m/s].");
cfg.write(sSection,"w_max_dps", mrpt::utils::RAD2DEG(W_MAX), WN,WV, "Maximum angular velocity for trajectories [deg/s].");
cfg.write(sSection,"turningRadiusReference",turningRadiusReference, WN,WV, "An approximate dimension of the robot (not a critical parameter) [m].");
CPTG_RobotShape_Polygonal::saveToConfigFile(cfg,sSection);
MRPT_END
}
void CHolonomicND::TOptions::saveToConfigFile(
mrpt::utils::CConfigFileBase& c, const std::string& s) const
{
MRPT_START;
const int WN = mrpt::utils::MRPT_SAVE_NAME_PADDING(),
WV = mrpt::utils::MRPT_SAVE_VALUE_PADDING();
MRPT_SAVE_CONFIG_VAR_COMMENT(WIDE_GAP_SIZE_PERCENT, "");
MRPT_SAVE_CONFIG_VAR_COMMENT(MAX_SECTOR_DIST_FOR_D2_PERCENT, "");
MRPT_SAVE_CONFIG_VAR_COMMENT(RISK_EVALUATION_SECTORS_PERCENT, "");
MRPT_SAVE_CONFIG_VAR_COMMENT(
RISK_EVALUATION_DISTANCE, "In normalized ps-meters [0,1]");
MRPT_SAVE_CONFIG_VAR_COMMENT(TOO_CLOSE_OBSTACLE, "For stopping gradually");
MRPT_SAVE_CONFIG_VAR_COMMENT(
TARGET_SLOW_APPROACHING_DISTANCE, "In normalized ps-meters");
ASSERT_EQUAL_(factorWeights.size(), 4)
c.write(
s, "factorWeights",
mrpt::format(
"%.2f %.2f %.2f %.2f", factorWeights[0], factorWeights[1],
factorWeights[2], factorWeights[3]),
WN, WV,
"[0]=Free space, [1]=Dist. in sectors, [2]=Closer to target "
"(Euclidean), [3]=Hysteresis");
MRPT_END
}
/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CGyroKVHDSP3000::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
m_sensorPose.setFromValues(
configSource.read_float( iniSection, "pose_x", 0, false ),
configSource.read_float( iniSection, "pose_y", 0, false ),
configSource.read_float( iniSection, "pose_z", 0, false ),
DEG2RAD( configSource.read_float( iniSection, "pose_yaw", 0, false ) ),
DEG2RAD( configSource.read_float( iniSection, "pose_pitch", 0, false ) ),
DEG2RAD( configSource.read_float( iniSection, "pose_roll", 0, false ) ) );
string operatingMode = configSource.read_string(iniSection, "operatingMode", "rate", false);
cout << "Operating mode : " << operatingMode << endl;
if(operatingMode == "incremental")
{
m_mode = INCREMENTAL_ANGLE;
cout << "Incremental mode" << endl;
}
else if(operatingMode == "integral")
{
m_mode = INTEGRATED_ANGLE;
cout << "Integrated mode" << endl;
}
else
{
m_mode = RATE;
cout << "Rate mode" << endl;
}
m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false );
}
void CPTG_RobotShape_Polygonal::saveToConfigFile(
mrpt::utils::CConfigFileBase& cfg, const std::string& sSection) const
{
const int WN = 25, WV = 30;
for (unsigned int i = 0; i < m_robotShape.size(); i++)
{
const std::string sPtx = mrpt::format("shape_x%u", i);
const std::string sPty = mrpt::format("shape_y%u", i);
cfg.write(
sSection, sPtx, m_robotShape[i].x, WN, WV,
"Robot polygonal shape, `x` [m].");
cfg.write(
sSection, sPty, m_robotShape[i].y, WN, WV,
"Robot polygonal shape, `y` [m].");
}
}
/** Loads the generic settings common to any sensor (See CGenericSensor), then call to "loadConfig_sensorSpecific"
* \exception This method throws an exception with a descriptive message if some critical parameter is missing or has an invalid value.
*/
void CGenericSensor::loadConfig(
const mrpt::utils::CConfigFileBase &cfg,
const std::string § )
{
MRPT_START
m_process_rate = cfg.read_double(sect,"process_rate",0 ); // Leave it to 0 so rawlog-grabber can detect if it's not set by the user.
m_max_queue_len = static_cast<size_t>(cfg.read_int(sect,"max_queue_len",int(m_max_queue_len)));
m_grab_decimation = static_cast<size_t>(cfg.read_int(sect,"grab_decimation",int(m_grab_decimation)));
m_sensorLabel = cfg.read_string( sect, "sensorLabel", m_sensorLabel );
m_grab_decimation_counter = 0;
loadConfig_sensorSpecific(cfg,sect);
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]) );
}
}
/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CEnoseModular::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);
MRPT_END
}
void CPTG_DiffDrive_CS::saveToConfigFile(mrpt::utils::CConfigFileBase &cfg,const std::string &sSection) const
{
MRPT_START
const int WN = 40, WV = 20;
CPTG_DiffDrive_CollisionGridBased::saveToConfigFile(cfg,sSection);
cfg.write(sSection,"K",K, WN,WV, "K=+1 forward paths; K=-1 for backwards paths.");
MRPT_END
}
void Lidar::loadConfiguration( const mrpt::utils::CConfigFileBase & config, const std::string & sectionName) {
string portName = config.read_string(sectionName, "COM_port_LIN", "/dev/ttyUSB2" );
LOG_LIDAR(DEBUG3) << sectionName << " port name: " << portName << endl;
this->setSerialPort( portName );
int baudRate = config.read_int( sectionName, "COM_baudRate", 38400 );
LOG_LIDAR(DEBUG3) << sectionName << " baudRate: " << baudRate << endl;
this->setBaudRate( baudRate );
int fov = config.read_int( sectionName, "FOV", 180 );
LOG_LIDAR(DEBUG3) << sectionName << " fov: " << fov << endl;
this->setScanFOV( fov );
int res = config.read_int( sectionName, "resolution", 50 );
LOG_LIDAR(DEBUG3) << sectionName << "resolution: " << res / 100. << endl;
this->setScanResolution( config.read_int( sectionName, "resolution", 50 ) ); // 25=0.25deg, 50=0.5deg, 100=1deg
}
void CCascadeClassifierDetection::init(const mrpt::utils::CConfigFileBase &config)
{
#if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM>=0x200
// load configuration values
m_options.cascadeFileName = config.read_string("CascadeClassifier","cascadeFilename","");
m_options.scaleFactor = config.read_double("DetectionOptions","scaleFactor",1.1);
m_options.minNeighbors = config.read_int("DetectionOptions","minNeighbors",3);
m_options.flags = config.read_int("DetectionOptions","flags",0);
m_options.minSize = config.read_int("DetectionOptions","minSize",30);
m_cascade = new CascadeClassifier();
// Load cascade classifier from file
CASCADE->load( m_options.cascadeFileName );
// Check if cascade is empty
if ( CASCADE->empty() )
throw std::runtime_error("Incorrect cascade file.");
#endif
}
/*---------------------------------------------------------------
loadFromConfigFile
---------------------------------------------------------------*/
void CMultiMetricMapPDF::TPredictionParams::loadFromConfigFile(
const mrpt::utils::CConfigFileBase &iniFile,
const std::string §ion)
{
pfOptimalProposal_mapSelection = iniFile.read_int(section,"pfOptimalProposal_mapSelection",pfOptimalProposal_mapSelection, true);
MRPT_LOAD_CONFIG_VAR( ICPGlobalAlign_MinQuality, float, iniFile,section );
KLD_params.loadFromConfigFile(iniFile, section);
icp_params.loadFromConfigFile(iniFile, section);
}
/*---------------------------------------------------------------
loadFromConfigFile
---------------------------------------------------------------*/
void CICP::TConfigParams::loadFromConfigFile(
const mrpt::utils::CConfigFileBase &iniFile,
const std::string §ion)
{
MRPT_LOAD_CONFIG_VAR( maxIterations, int, iniFile, section);
MRPT_LOAD_CONFIG_VAR( minAbsStep_trans, float, iniFile, section);
MRPT_LOAD_CONFIG_VAR( minAbsStep_rot, float, iniFile, section);
ICP_algorithm = iniFile.read_enum<TICPAlgorithm>(section,"ICP_algorithm",ICP_algorithm);
ICP_covariance_method = iniFile.read_enum<TICPCovarianceMethod>(section,"ICP_covariance_method",ICP_covariance_method);
MRPT_LOAD_CONFIG_VAR( thresholdDist, float, iniFile, section);
thresholdAng = DEG2RAD( iniFile.read_float(section.c_str(),"thresholdAng_DEG",RAD2DEG(thresholdAng)) );
MRPT_LOAD_CONFIG_VAR( ALFA, float, iniFile, section);
MRPT_LOAD_CONFIG_VAR( smallestThresholdDist, float, iniFile, section);
MRPT_LOAD_CONFIG_VAR( onlyClosestCorrespondences, bool, iniFile, section);
MRPT_LOAD_CONFIG_VAR( onlyUniqueRobust, bool, iniFile, section);
MRPT_LOAD_CONFIG_VAR( doRANSAC, bool, iniFile, section);
MRPT_LOAD_CONFIG_VAR( covariance_varPoints,float, iniFile, section);
MRPT_LOAD_CONFIG_VAR( ransac_minSetSize, int, iniFile, section);
MRPT_LOAD_CONFIG_VAR( ransac_maxSetSize, int, iniFile, section);
MRPT_LOAD_CONFIG_VAR( ransac_mahalanobisDistanceThreshold, float, iniFile, section);
MRPT_LOAD_CONFIG_VAR( ransac_nSimulations, int, iniFile, section);
MRPT_LOAD_CONFIG_VAR( normalizationStd, float, iniFile, section);
MRPT_LOAD_CONFIG_VAR( ransac_fuseByCorrsMatch, bool, iniFile, section);
MRPT_LOAD_CONFIG_VAR( ransac_fuseMaxDiffXY, float, iniFile, section);
ransac_fuseMaxDiffPhi = DEG2RAD( iniFile.read_float(section.c_str(),"ransac_fuseMaxDiffPhi_DEG",RAD2DEG(ransac_fuseMaxDiffPhi)) );
MRPT_LOAD_CONFIG_VAR( kernel_rho, float, iniFile, section);
MRPT_LOAD_CONFIG_VAR( use_kernel, bool, iniFile, section);
MRPT_LOAD_CONFIG_VAR( Axy_aprox_derivatives, float, iniFile, section);
MRPT_LOAD_CONFIG_VAR( LM_initial_lambda, float, iniFile, section);
MRPT_LOAD_CONFIG_VAR( skip_cov_calculation, bool, iniFile, section);
MRPT_LOAD_CONFIG_VAR( skip_quality_calculation, bool, iniFile, section);
MRPT_LOAD_CONFIG_VAR( corresponding_points_decimation, int, iniFile, section);
}
/** Load all the params from a config source, in the format described in saveToConfigFile()
*/
void TStereoCamera::loadFromConfigFile(const std::string §ion, const mrpt::utils::CConfigFileBase &cfg )
{
// [<SECTION>_LEFT]
// ...
// [<SECTION>_RIGHT]
// ...
// [<SECTION>_LEFT2RIGHT_POSE]
// pose_quaternion = [x y z qr qx qy qz]
leftCamera.loadFromConfigFile(section+string("_LEFT"), cfg);
rightCamera.loadFromConfigFile(section+string("_RIGHT"), cfg);
rightCameraPose.fromString( cfg.read_string(section+string("_LEFT2RIGHT_POSE"), "pose_quaternion","" ) );
}
void CLMS100Eth::loadConfig_sensorSpecific( const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
pose_x = configSource.read_float(iniSection,"pose_x",0,false);
pose_y = configSource.read_float(iniSection,"pose_y",0,false);
pose_z = configSource.read_float(iniSection,"pose_z",0,false);
pose_yaw = configSource.read_float(iniSection,"pose_yaw",0,false);
pose_pitch = configSource.read_float(iniSection,"pose_pitch",0,false);
pose_roll = configSource.read_float(iniSection,"pose_roll",0,false);
m_ip = configSource.read_string(iniSection, "ip_address", "192.168.0.1", false);
m_port = configSource.read_int(iniSection, "TCP_port", 2111, false);
m_process_rate = configSource.read_int(iniSection, string("process_rate"), 10, false);
m_sensorLabel = configSource.read_string(iniSection, "sensorLabel", "SICK", false);
m_sensorPose = CPose3D( pose_x, pose_y, pose_z,
DEG2RAD( pose_yaw ),DEG2RAD( pose_pitch ), DEG2RAD( pose_roll ));
}
/** Save as a config block:
*/
void TStereoCamera::saveToConfigFile(const std::string §ion, mrpt::utils::CConfigFileBase &cfg ) const
{
// [<SECTION>_LEFT]
// ...
// [<SECTION>_RIGHT]
// ...
// [<SECTION>_LEFT2RIGHT_POSE]
// pose_quaternion = [x y z qr qx qy qz]
leftCamera.saveToConfigFile(section+string("_LEFT"), cfg);
rightCamera.saveToConfigFile(section+string("_RIGHT"), cfg);
const CPose3DQuat q = CPose3DQuat(rightCameraPose); // Don't remove this line, it's a safe against future possible bugs if rightCameraPose changes!
cfg.write(section+string("_LEFT2RIGHT_POSE"), "pose_quaternion",q.asString() );
}
void Camera::loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase & config, const std::string & sectionName) {
LOG_CAMERA(DEBUG3) << "Reading config file..." << endl;
GRID_SIZE = config.read_int(sectionName, "grid_size", 10);
PERCENT_FILLED = config.read_double(sectionName, "percent_filled", .3);
THRESHOLD = config.read_int(sectionName, "threshold", 20);
ERODE_AMOUNT = config.read_int(sectionName, "erode_amount", 2);
HSV_VECTOR = config.read_int(sectionName, "hsv_vector", 1);
LOG_CAMERA(DEBUG3) << "Grid size: " << GRID_SIZE << endl;
LOG_CAMERA(DEBUG3) << "Percent filled: " << PERCENT_FILLED << endl;
LOG_CAMERA(DEBUG3) << "Threshold: " << THRESHOLD << endl;
LOG_CAMERA(DEBUG3) << "Erode amount: " << ERODE_AMOUNT << endl;
LOG_CAMERA(DEBUG3) << "HSV vector: " << HSV_VECTOR << endl;
if(GRID_SIZE <= 0) LOG_CAMERA(FATAL) << "Grid size is negative or zero" << endl;
if(PERCENT_FILLED <= 0) LOG_CAMERA(FATAL) << "Percent filled is negative or 0" << endl;
if(PERCENT_FILLED > 1.0) LOG_CAMERA(FATAL) << "Percent filled is greater than 1" << endl;
if(THRESHOLD < 0) LOG_CAMERA(FATAL) << "Threshold is negative, must be between 0 and 255" << endl;
if(THRESHOLD > 255) LOG_CAMERA(FATAL) << "Threshold is too high, must be between 0 and 255" << endl;
if(ERODE_AMOUNT < 0) LOG_CAMERA(FATAL) << "Erode amount must be greater than 0" << endl;
if(HSV_VECTOR < 0) LOG_CAMERA(FATAL) << "hsv vector is negative, must be between 0 and 3" << endl;
if(HSV_VECTOR > 3) LOG_CAMERA(FATAL) << "hsv vector is too high, must be between 0 and 3" << endl;
}