Beispiel #1
0
/* -----------------------------------------------------
                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 );
}
Beispiel #2
0
/*-------------------------------------------------------------
					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 );

}
Beispiel #3
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);
}
Beispiel #4
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);
}
Beispiel #5
0
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
}
Beispiel #6
0
/*-------------------------------------------------------------
					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
}
Beispiel #7
0
/* -----------------------------------------------------
                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 );
}
Beispiel #8
0
/*---------------------------------------------------------------
						loadFromConfigFile
  ---------------------------------------------------------------*/
void  CHMTSLAM::TOptions::loadFromConfigFile(
	const mrpt::utils::CConfigFileBase	&source,
	const std::string		&section)
{
	MRPT_LOAD_CONFIG_VAR( LOG_OUTPUT_DIR, string,    source, section);
	MRPT_LOAD_CONFIG_VAR( LOG_FREQUENCY,  int,     	source, section);

	MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT(SLAM_METHOD, int,  TLSlamMethod,  source, section);

	MRPT_LOAD_CONFIG_VAR( SLAM_MIN_DIST_BETWEEN_OBS, float, 		source, section);
	MRPT_LOAD_CONFIG_VAR_DEGREES( SLAM_MIN_HEADING_BETWEEN_OBS, 	source, section);

	MRPT_LOAD_CONFIG_VAR(MIN_ODOMETRY_STD_XY,float, source,section);
	MRPT_LOAD_CONFIG_VAR_DEGREES( MIN_ODOMETRY_STD_PHI, source,section);

	MRPT_LOAD_CONFIG_VAR( VIEW3D_AREA_SPHERES_HEIGHT, float,  	source, section);
	MRPT_LOAD_CONFIG_VAR( VIEW3D_AREA_SPHERES_RADIUS, float,  	source, section);

	MRPT_LOAD_CONFIG_VAR( random_seed, int, source,section);

	stds_Q_no_odo[2] = RAD2DEG(stds_Q_no_odo[2]);
	source.read_vector(section,"stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo );
	ASSERT_(stds_Q_no_odo.size()==3)

	stds_Q_no_odo[2] = DEG2RAD(stds_Q_no_odo[2]);

	std::string sTLC_detectors = source.read_string(section,"TLC_detectors", "", true );

	mrpt::system::tokenize(sTLC_detectors,", ",TLC_detectors);

	std::cout << "TLC_detectors: " << TLC_detectors.size() << std::endl;

	// load other sub-classes:
	AA_options.loadFromConfigFile(source,section);
}
Beispiel #9
0
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
}
Beispiel #10
0
/* -----------------------------------------------------
				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", ""));
}
Beispiel #11
0
/** 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);
}
Beispiel #12
0
/*-------------------------------------------------------------
					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

}
Beispiel #13
0
/* -----------------------------------------------------
                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);
}
Beispiel #14
0
/*-------------------------------------------------------------
						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 );

}
Beispiel #15
0
/* -----------------------------------------------------
                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]) );
	}
}
Beispiel #16
0
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 ));

}
Beispiel #17
0
/** 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);
	}

}
Beispiel #18
0
/*-------------------------------------------------------------
					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


}
Beispiel #19
0
/**  Load all the params from a config source, in the format described in saveToConfigFile()
  */
void TStereoCamera::loadFromConfigFile(const std::string &section,  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","" ) );
}
Beispiel #20
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);
#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);
}
Beispiel #21
0
/** 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			&sect )
{
	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
}
Beispiel #22
0
/*-------------------------------------------------------------
						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
}
Beispiel #23
0
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
}
Beispiel #25
0
/* -----------------------------------------------------
                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
}