/*---------------------------------------------------------------
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
}
/** Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see utils::CConfigFileBase and derived classes)
* \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value.
*/
void COpenNI2_RGBD360::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
cout << "COpenNI2_RGBD360::loadConfig_sensorSpecific...\n";
m_sensorPoseOnRobot.setFromValues(
configSource.read_float(iniSection,"pose_x",0),
configSource.read_float(iniSection,"pose_y",0),
configSource.read_float(iniSection,"pose_z",0),
DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
);
m_preview_window = configSource.read_bool(iniSection,"preview_window",m_preview_window);
m_width = configSource.read_int(iniSection,"width",0);
m_height = configSource.read_int(iniSection,"height",0);
m_fps = configSource.read_float(iniSection,"fps",0);
std::cout << "width " << m_width << " height " << m_height << " fps " << m_fps << endl;
m_grab_rgb = configSource.read_bool(iniSection,"grab_image",m_grab_rgb);
m_grab_depth = configSource.read_bool(iniSection,"grab_depth",m_grab_depth);
m_grab_3D_points = configSource.read_bool(iniSection,"grab_3D_points",m_grab_3D_points);
// m_num_sensors = configSource.read_int(iniSection,"m_num_sensors",0);
}
/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CHokuyoURG::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
m_reduced_fov = DEG2RAD( configSource.read_float(iniSection,"reduced_fov",0) ),
m_motorSpeed_rpm = configSource.read_int(iniSection,"HOKUYO_motorSpeed_rpm",0);
m_sensorPose.setFromValues(
configSource.read_float(iniSection,"pose_x",0),
configSource.read_float(iniSection,"pose_y",0),
configSource.read_float(iniSection,"pose_z",0),
DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
);
m_highSensMode = configSource.read_bool(iniSection,"HOKUYO_HS_mode",m_highSensMode);
#ifdef MRPT_OS_WINDOWS
m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true );
#else
m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true );
#endif
m_ip_dir = configSource.read_string(iniSection, "IP_DIR", m_ip_dir );
m_port_dir = configSource.read_int(iniSection, "PORT_DIR", m_port_dir );
// FAMD
m_showPreview = configSource.read_bool(iniSection, "preview", false );
// Parent options:
this->loadExclusionAreas(configSource,iniSection);
}
/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CHokuyoURG::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
m_reduced_fov = DEG2RAD( configSource.read_float(iniSection,"reduced_fov",0) ),
m_motorSpeed_rpm = configSource.read_int(iniSection,"HOKUYO_motorSpeed_rpm",0);
m_sensorPose.setFromValues(
configSource.read_float(iniSection,"pose_x",0),
configSource.read_float(iniSection,"pose_y",0),
configSource.read_float(iniSection,"pose_z",0),
DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
);
m_highSensMode = configSource.read_bool(iniSection,"HOKUYO_HS_mode",m_highSensMode);
#ifdef MRPT_OS_WINDOWS
m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true );
#else
m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true );
#endif
m_ip_dir = configSource.read_string(iniSection, "IP_DIR", m_ip_dir );
m_port_dir = configSource.read_int(iniSection, "PORT_DIR", m_port_dir );
m_disable_firmware_timestamp = configSource.read_bool(iniSection, "disable_firmware_timestamp", m_disable_firmware_timestamp);
// Parent options:
C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
}
/* -----------------------------------------------------
loadConfig_sensorSpecific
----------------------------------------------------- */
void CGPSInterface::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
m_parser = configSource.read_enum<CGPSInterface::PARSERS>(iniSection,"parser",m_parser,false /*Allow default values*/);
m_raw_dump_file_prefix = configSource.read_string(iniSection,"raw_dump_file_prefix",m_raw_dump_file_prefix,false /*Allow default values*/);
#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 );
// legacy custom cmds:
m_customInit = configSource.read_string( iniSection, "customInit", m_customInit, false );
// new custom cmds:
m_custom_cmds_delay = configSource.read_float( iniSection, "custom_cmds_delay",m_custom_cmds_delay );
m_custom_cmds_append_CRLF = configSource.read_bool( iniSection, "custom_cmds_append_CRLF",m_custom_cmds_append_CRLF);
// Load as many strings as found on the way:
m_setup_cmds.clear();
for (int i=1; true; i++)
{
std::string sLine = configSource.read_string(iniSection, mrpt::format("setup_cmd%i",i),std::string() );
sLine = mrpt::system::trim( sLine );
if (sLine.empty())
break;
m_setup_cmds.push_back(sLine);
}
m_shutdown_cmds.clear();
for (int i=1; true; i++)
{
std::string sLine = configSource.read_string(iniSection, mrpt::format("shutdown_cmd%i",i),std::string() );
sLine = mrpt::system::trim( sLine );
if (sLine.empty())
break;
m_shutdown_cmds.push_back(sLine);
}
m_sensorPose.setFromValues(
configSource.read_float(iniSection,"pose_x",0),
configSource.read_float(iniSection,"pose_y",0),
configSource.read_float(iniSection,"pose_z",0),
DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
);
m_JAVAD_rtk_src_port = configSource.read_string(iniSection, "JAVAD_rtk_src_port",m_JAVAD_rtk_src_port );
m_JAVAD_rtk_src_baud = configSource.read_int(iniSection, "JAVAD_rtk_src_baud",m_JAVAD_rtk_src_baud );
m_JAVAD_rtk_format = configSource.read_string(iniSection,"JAVAD_rtk_format", m_JAVAD_rtk_format );
m_topcon_useAIMMode = configSource.read_bool( iniSection,"JAVAD_useAIMMode", m_topcon_useAIMMode );
m_topcon_data_period = 1.0/configSource.read_double( iniSection,"outputRate", m_topcon_data_period );
}
/** Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see utils::CConfigFileBase and derived classes)
* \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value.
*/
void CKinect::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
m_sensorPoseOnRobot.setFromValues(
configSource.read_float(iniSection,"pose_x",0),
configSource.read_float(iniSection,"pose_y",0),
configSource.read_float(iniSection,"pose_z",0),
DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
);
m_preview_window = configSource.read_bool(iniSection,"preview_window",m_preview_window);
// "Stereo" calibration data:
// [<SECTION>_LEFT] // Depth
// ...
// [<SECTION>_RIGHT] // RGB
// ...
// [<SECTION>_LEFT2RIGHT_POSE]
// pose_quaternion = [x y z qr qx qy qz]
const mrpt::poses::CPose3D twist(0,0,0,DEG2RAD(-90),DEG2RAD(0),DEG2RAD(-90));
mrpt::utils::TStereoCamera sc;
sc.leftCamera = m_cameraParamsDepth; // Load default values so that if we fail to load from cfg at least we have some reasonable numbers.
sc.rightCamera = m_cameraParamsRGB;
sc.rightCameraPose = mrpt::poses::CPose3DQuat(m_relativePoseIntensityWRTDepth - twist);
try {
sc.loadFromConfigFile(iniSection,configSource);
} catch (std::exception &e) {
std::cout << "[CKinect::loadConfig_sensorSpecific] Warning: Ignoring error loading calibration parameters:\n" << e.what();
}
m_cameraParamsDepth = sc.leftCamera;
m_cameraParamsRGB = sc.rightCamera;
m_relativePoseIntensityWRTDepth = twist + mrpt::poses::CPose3D(sc.rightCameraPose);
// Id:
m_user_device_number = configSource.read_int(iniSection,"device_number",m_user_device_number );
m_grab_image = configSource.read_bool(iniSection,"grab_image",m_grab_image);
m_grab_depth = configSource.read_bool(iniSection,"grab_depth",m_grab_depth);
m_grab_3D_points = configSource.read_bool(iniSection,"grab_3D_points",m_grab_3D_points);
m_grab_IMU = configSource.read_bool(iniSection,"grab_IMU",m_grab_IMU );
m_video_channel = configSource.read_enum<TVideoChannel>(iniSection,"video_channel",m_video_channel);
{
std::string s = configSource.read_string(iniSection,"relativePoseIntensityWRTDepth","");
if (!s.empty())
m_relativePoseIntensityWRTDepth.fromString(s);
}
m_initial_tilt_angle = configSource.read_int(iniSection,"initial_tilt_angle",m_initial_tilt_angle);
}
/** Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see utils::CConfigFileBase and derived classes)
* \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value.
*/
void COpenNI2Sensor::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
cout << "COpenNI2Sensor::loadConfig_sensorSpecific...\n";
m_sensorPoseOnRobot.setFromValues(
configSource.read_float(iniSection,"pose_x",0),
configSource.read_float(iniSection,"pose_y",0),
configSource.read_float(iniSection,"pose_z",0),
DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
);
m_preview_window = configSource.read_bool(iniSection,"preview_window",m_preview_window);
width = configSource.read_int(iniSection,"width",0);
height = configSource.read_int(iniSection,"height",0);
fps = configSource.read_float(iniSection,"fps",0);
std::cout << "width " << width << " height " << height << " fps " << fps << endl;
const mrpt::poses::CPose3D twist(0,0,0,DEG2RAD(-90),DEG2RAD(0),DEG2RAD(-90));
mrpt::utils::TStereoCamera sc;
sc.leftCamera = m_cameraParamsDepth; // Load default values so that if we fail to load from cfg at least we have some reasonable numbers.
sc.rightCamera = m_cameraParamsRGB;
sc.rightCameraPose = mrpt::poses::CPose3DQuat(m_relativePoseIntensityWRTDepth - twist);
try {
sc.loadFromConfigFile(iniSection,configSource);
} catch (std::exception &e) {
std::cout << "[COpenNI2Sensor::loadConfig_sensorSpecific] Warning: Ignoring error loading calibration parameters:\n" << e.what();
}
m_cameraParamsDepth = sc.leftCamera;
m_cameraParamsRGB = sc.rightCamera;
m_relativePoseIntensityWRTDepth = twist + mrpt::poses::CPose3D(sc.rightCameraPose);
// Id:
m_user_device_number = configSource.read_int(iniSection,"device_number",m_user_device_number );
cout << "LOAD m_user_device_number " << m_user_device_number << endl;
m_grab_image = configSource.read_bool(iniSection,"grab_image",m_grab_image);
m_grab_depth = configSource.read_bool(iniSection,"grab_depth",m_grab_depth);
m_grab_3D_points = configSource.read_bool(iniSection,"grab_3D_points",m_grab_3D_points);
{
std::string s = configSource.read_string(iniSection,"relativePoseIntensityWRTDepth","");
if (!s.empty())
m_relativePoseIntensityWRTDepth.fromString(s);
}
}
/* -----------------------------------------------------
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", ""));
}
/* -----------------------------------------------------
loadConfig_sensorSpecific
----------------------------------------------------- */
void CGPSInterface::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 );
m_customInit = configSource.read_string( iniSection, "customInit", m_customInit, false );
m_sensorPose.x( configSource.read_float( iniSection, "pose_x",0, false ) );
m_sensorPose.y( configSource.read_float( iniSection, "pose_y",0, false ) );
m_sensorPose.z( configSource.read_float( iniSection, "pose_z",0, false ) );
m_JAVAD_rtk_src_port = configSource.read_string(iniSection, "JAVAD_rtk_src_port",m_JAVAD_rtk_src_port );
m_JAVAD_rtk_src_baud = configSource.read_int(iniSection, "JAVAD_rtk_src_baud",m_JAVAD_rtk_src_baud );
m_JAVAD_rtk_format = configSource.read_string(iniSection,"JAVAD_rtk_format", m_JAVAD_rtk_format );
m_useAIMMode = configSource.read_bool( iniSection,"JAVAD_useAIMMode", m_useAIMMode );
m_data_period = 1.0/configSource.read_double( iniSection,"outputRate", m_data_period );
}
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);
}
/*-------------------------------------------------------------
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;
}
/*-------------------------------------------------------------
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;
}
}
/*-------------------------------------------------------------
loadConfig
-------------------------------------------------------------*/
void CActivMediaRobotBase::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
#ifdef MRPT_OS_WINDOWS
m_com_port = configSource.read_string(iniSection,"robotPort_WIN",m_com_port,true);
#else
m_com_port = configSource.read_string(iniSection,"robotPort_LIN",m_com_port,true);
#endif
m_robotBaud = configSource.read_int(iniSection, "robotBaud", m_robotBaud, true );
m_enableSonars = configSource.read_bool(iniSection, "enableSonars", m_enableSonars );
m_enableJoyControl = configSource.read_bool(iniSection, "joystick_control", m_enableJoyControl );
m_joy_max_v = configSource.read_float(iniSection, "joystick_max_v", m_joy_max_v );
m_joy_max_w = DEG2RAD( configSource.read_float(iniSection, "joystick_max_w_degps", RAD2DEG(m_joy_max_w) ) );
m_capture_rate = configSource.read_double(iniSection, "capture_rate", m_capture_rate );
}
/** Load all the params from a config source, in the format described in saveToConfigFile()
*/
void TMultiResDescMatchOptions::loadFromConfigFile( const mrpt::utils::CConfigFileBase &cfg, const std::string §ion )
{
useOriFilter = cfg.read_bool(section,"useOriFilter",true,false);
oriThreshold = cfg.read_double(section,"oriThreshold",0.2,false);
lastSeenThreshold = cfg.read_int(section,"lastSeenThreshold",10,false);
timesSeenThreshold = cfg.read_int(section,"timesSeenThreshold",5,false);
minFeaturesToFind = cfg.read_int(section,"minFeaturesToFind",5,false);
minFeaturesToBeLost = cfg.read_int(section,"minFeaturesToBeLost",5,false);
useDepthFilter = cfg.read_bool(section,"useDepthFilter",true,false);
matchingThreshold = cfg.read_double(section,"matchingThreshold",1e4,false);
matchingRatioThreshold = cfg.read_double(section,"matchingRatioThreshold",0.5,false);
lowScl1 = cfg.read_int(section,"lowScl1",0,false);
lowScl2 = cfg.read_int(section,"lowScl1",0,false);
highScl1 = cfg.read_int(section,"highScl1",6,false);
highScl2 = cfg.read_int(section,"highScl2",6,false);
searchAreaSize = cfg.read_double(section,"searchAreaSize",20,false);
}
/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CHokuyoURG::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
m_reduced_fov = DEG2RAD( configSource.read_float(iniSection,"reduced_fov",0) ),
m_motorSpeed_rpm = configSource.read_int(iniSection,"HOKUYO_motorSpeed_rpm",0);
m_sensorPose.setFromValues(
configSource.read_float(iniSection,"pose_x",0),
configSource.read_float(iniSection,"pose_y",0),
configSource.read_float(iniSection,"pose_z",0),
DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
);
m_highSensMode = configSource.read_bool(iniSection,"HOKUYO_HS_mode",m_highSensMode);
#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_ip_dir = configSource.read_string(iniSection, "IP_DIR", m_ip_dir );
m_port_dir = configSource.read_int(iniSection, "PORT_DIR", m_port_dir );
ASSERTMSG_(!m_com_port.empty() || !m_ip_dir.empty(), "Either COM_port or IP_DIR must be defined in the configuration file!");
ASSERTMSG_(m_com_port.empty() || m_ip_dir.empty(), "Both COM_port and IP_DIR set! Please, define only one of them.");
if (!m_ip_dir.empty()) { ASSERTMSG_(m_port_dir,"A TCP/IP port number `PORT_DIR` must be specified for Ethernet connection"); }
m_disable_firmware_timestamp = configSource.read_bool(iniSection, "disable_firmware_timestamp", m_disable_firmware_timestamp);
// Parent options:
C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
}
/* -----------------------------------------------------
loadConfig_sensorSpecific
----------------------------------------------------- */
void CPhidgetInterfaceKitProximitySensors::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
#if MRPT_HAS_PHIDGET
if(!configSource.sectionExists(iniSection)) THROW_EXCEPTION("Can't find section in configuration file");
// looking for the board parameters.
// process_rate = 100 // Hz (common to all sensors)
// serialNumber = 12345 // The interface kit serial number.
m_process_rate = configSource.read_int(iniSection, string("process_rate"), 50);
m_serialNumber = configSource.read_int(iniSection, string("serialNumber"), -1);
bool display = configSource.read_bool(iniSection, string("displayRecapitulativeInformations"), false);
// Looking for each sensor.
for(int i = 1 ; i <= 8 ; i++)
{
string sensorNKeyName = format("sensor%d", i);
string sensorType = configSource.read_string(iniSection, sensorNKeyName, string("UNPLUGGED"));
if(sensorType != string("UNPLUGGED"))
{
// the sensor is plugged :
// // check if the sensor type is supported.
if(sensorType == string("EZ1"))
{
m_sensorType[i-1] = EZ1;
m_minRange[i-1] = 0.15; // meters
m_maxRange[i-1] = 6.45; // meters
}else if(sensorType == string("SHARP-30cm"))
{
m_sensorType[i-1] = SHARP_30cm;
m_minRange[i-1] = 0.04; // meters
m_maxRange[i-1] = 0.3; // meters
}else if(sensorType == string("SHARP-80cm"))
{
m_sensorType[i-1] = SHARP_80cm;
m_minRange[i-1] = 0.06; // meters
m_maxRange[i-1] = 0.8; // meters
}else
{
string err = format("Type of sensor %d is not supported", i);
m_state = CGenericSensor::ssError;
THROW_EXCEPTION(err);
}
m_sensorIsPlugged[i-1] = true;
// reading the sensor pose.
string sensorNPoseX = format("pose%d_x", i);
string sensorNPoseY = format("pose%d_y", i);
string sensorNPoseZ = format("pose%d_z", i);
string sensorNPoseYaw = format("pose%d_yaw", i);
string sensorNPosePitch = format("pose%d_pitch", i);
string sensorNPoseRoll = format("pose%d_roll", i);
float x = configSource.read_float(iniSection, sensorNPoseX, 0.0);
float y = configSource.read_float(iniSection, sensorNPoseY, 0.0);
float z = configSource.read_float(iniSection, sensorNPoseZ, 0.0);
float yaw = configSource.read_float(iniSection, sensorNPoseYaw, 0.0);
float pitch = configSource.read_float(iniSection, sensorNPosePitch, 0.0);
float roll = configSource.read_float(iniSection, sensorNPoseRoll, 0.0);
m_sensorPoses[i-1] = CPose3D(x,y,z,yaw,pitch,roll);
}
}
if(display)
{ // width = 80;
cout.fill(' ');
cout << "+------------------------------------------------------------------------------+" << endl;
cout.width(79);
cout << "| Phidget interfaceKit board number : " << m_serialNumber;
cout << "|" << endl;
cout << "| Process rate : " << m_process_rate;
cout << "|" << endl;
cout << "+---------+---------------------+----------------------------------------------+" << endl;
cout << "| # + Sensor type | Sensor 3D pose |" << endl;
cout << "+---------+---------------------+----------------------------------------------+" << endl;
for(int i = 0 ; i < 8 ; i++)
{
cout << "|";
cout.width(9);
cout << i+1;
cout << " |";
cout.width(19);
switch (m_sensorType[i])
{
case EZ1 :
cout << "EZ1 |";
break;
case SHARP_30cm :
cout << "SHARP_30cm |";
break;
case SHARP_80cm :
cout << "SHARP_80cm |";
break;
case UNPLUGGED :
cout << "UNPLUGGED |";
break;
}
cout.width(43);
cout << m_sensorPoses[i];
cout << "|" << endl;
}
cout << "+------------------------------------------------------------------------------+" << endl;
}
#endif
}
