/* -----------------------------------------------------
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 );
}
/*-------------------------------------------------------------
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 );
}
/*-------------------------------------------------------------
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 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);
}
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
}
/*-------------------------------------------------------------
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
}
/* -----------------------------------------------------
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 );
}
/*---------------------------------------------------------------
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);
}
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
}
/* -----------------------------------------------------
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", ""));
}
/** 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);
}
/*-------------------------------------------------------------
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
}
/* -----------------------------------------------------
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);
}
/*-------------------------------------------------------------
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 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]) );
}
}
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 ));
}
/** 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 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
}
/** 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","" ) );
}
/*-------------------------------------------------------------
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);
}
/** 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 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
}
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
}
/* -----------------------------------------------------
loadConfig_sensorSpecific
----------------------------------------------------- */
void CNTRIPEmitter::loadConfig_sensorSpecific(
const mrpt::utils::CConfigFileBase &configSource,
const std::string &iniSection )
{
#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_com_bauds = configSource.read_int( iniSection, "baudRate",m_com_bauds, true );
m_ntrip_args.mountpoint = mrpt::system::trim( configSource.read_string(iniSection, "mountpoint","",true) );
m_ntrip_args.server = mrpt::system::trim( configSource.read_string(iniSection, "server","",true) );
m_ntrip_args.port = configSource.read_int(iniSection, "port",2101,true);
m_ntrip_args.user = mrpt::system::trim( configSource.read_string(iniSection, "user","") );
m_ntrip_args.password = mrpt::system::trim( configSource.read_string(iniSection, "password","") );
}
/*---------------------------------------------------------------
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
}
/* -----------------------------------------------------
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
}
