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 ));
}
/*-------------------------------------------------------------
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
-------------------------------------------------------------*/
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 );
}
/* -----------------------------------------------------
loadConfig_sensorSpecific
----------------------------------------------------- */
void CGillAnemometer::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
#ifdef MRPT_OS_WINDOWS
com_port = configSource.read_string(iniSection, "COM_port_WIN", "COM1", true ) ;
#else
com_port = configSource.read_string(iniSection, "COM_port_LIN", "/dev/tty0", true );
#endif
com_bauds = configSource.read_int( iniSection, "COM_baudRate",9600, false );
pose_x = configSource.read_float(iniSection,"pose_x",0,true);
pose_y = configSource.read_float(iniSection,"pose_y",0,true);
pose_z = configSource.read_float(iniSection,"pose_z",0,true);
pose_roll = configSource.read_float(iniSection,"pose_roll",0,true);
pose_pitch = configSource.read_float(iniSection,"pose_pitch",0,true);
pose_yaw = configSource.read_float(iniSection,"pose_yaw",0,true);
}
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 ));
}
/*---------------------------------------------------------------
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);
}
/*-------------------------------------------------------------
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);
}
/** 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 );
m_disable_firmware_timestamp = configSource.read_bool(iniSection, "disable_firmware_timestamp", m_disable_firmware_timestamp);
// Parent options:
C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
}
/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CIMUXSens::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_COMbauds = configSource.read_int(iniSection, "baudRate", m_COMbauds, false );
#ifdef MRPT_OS_WINDOWS
m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port, false );
#else
m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false );
#endif
}
/* -----------------------------------------------------
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 );
}
/* -----------------------------------------------------
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 );
}
/** 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 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
}
/*---------------------------------------------------------------
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
}