Ejemplo n.º 1
0
/* -----------------------------------------------------
				loadConfig_sensorSpecific
   ----------------------------------------------------- */
void CGillAnemometer::loadConfig_sensorSpecific(
	const mrpt::config::CConfigFileBase& configSource,
	const std::string& iniSection)
{
#ifdef _WIN32
	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);
}
Ejemplo n.º 2
0
/*-------------------------------------------------------------
					loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CIMUXSens::loadConfig_sensorSpecific(
	const mrpt::config::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 _WIN32
	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
}
Ejemplo n.º 3
0
void CLMS100Eth::loadConfig_sensorSpecific(
	const mrpt::config::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));
}
Ejemplo n.º 4
0
/*-------------------------------------------------------------
						loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CHokuyoURG::loadConfig_sensorSpecific(
	const mrpt::config::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 _WIN32
	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);
	m_intensity = configSource.read_bool(iniSection, "intensity", m_intensity),

	MRPT_LOAD_HERE_CONFIG_VAR(
		scan_interval, int, m_scan_interval, configSource, iniSection);

	// Parent options:
	C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
}
Ejemplo n.º 5
0
void CReactiveNavigationSystem3D::loadConfigFile(
	const mrpt::config::CConfigFileBase& c)
{
	MRPT_START

	m_PTGsMustBeReInitialized = true;

	// 1st: load my own params; at the end, call parent's overriden method:
	const std::string s = "CReactiveNavigationSystem3D";

	unsigned int num_levels;
	vector<float> xaux, yaux;

	// Read config params which describe the robot shape
	num_levels = c.read_int(s, "HEIGHT_LEVELS", 1, true);
	m_robotShape.resize(num_levels);
	for (unsigned int i = 1; i <= num_levels; i++)
	{
		m_robotShape.setHeight(
			i - 1, c.read_float(s, format("LEVEL%d_HEIGHT", i), 1.0, true));
		m_robotShape.setRadius(
			i - 1, c.read_float(s, format("LEVEL%d_RADIUS", i), 0.5, false));
		c.read_vector(
			s, format("LEVEL%d_VECTORX", i), vector<float>(0), xaux, false);
		c.read_vector(
			s, format("LEVEL%d_VECTORY", i), vector<float>(0), yaux, false);
		ASSERT_(xaux.size() == yaux.size());
		for (unsigned int j = 0; j < xaux.size(); j++)
			m_robotShape.polygon(i - 1).AddVertex(xaux[j], yaux[j]);
	}

	// Load PTGs from file:
	// ---------------------------------------------
	// levels = m_robotShape.heights.size()

	unsigned int num_ptgs = c.read_int(s, "PTG_COUNT", 1, true);
	m_ptgmultilevel.resize(num_ptgs);

	// Read each PTG parameters, and generate K x N collisiongrids
	//	K - Number of PTGs
	//	N - Number of height sections
	for (unsigned int j = 1; j <= num_ptgs; j++)
	{
		for (unsigned int i = 1; i <= m_robotShape.size(); i++)
		{
			MRPT_LOG_INFO_FMT(
				"[loadConfigFile] Generating PTG#%u at level %u...", j, i);
			const std::string sPTGName =
				c.read_string(s, format("PTG%d_TYPE", j), "", true);
			CParameterizedTrajectoryGenerator* ptgaux =
				CParameterizedTrajectoryGenerator::CreatePTG(
					sPTGName, c, s, format("PTG%d_", j));
			m_ptgmultilevel[j - 1].PTGs.push_back(ptgaux);
		}
	}

	MRPT_LOG_DEBUG_FMT(
		" Robot height sections = %u\n",
		static_cast<unsigned int>(m_robotShape.size()));

	CAbstractPTGBasedReactive::loadConfigFile(
		c);  // call parent's overriden method:

	MRPT_END
}
Ejemplo n.º 6
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 config::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::config::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::img::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)
			.asTPose();

	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);
}
Ejemplo n.º 7
0
/* -----------------------------------------------------
				loadConfig_sensorSpecific
   ----------------------------------------------------- */
void CGPSInterface::loadConfig_sensorSpecific(
	const mrpt::config::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 _WIN32
	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_sensorLabelAppendMsgType = configSource.read_bool(
		iniSection, "sensor_label_append_msg_type", m_sensorLabelAppendMsgType);

	// 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 CPhidgetInterfaceKitProximitySensors::loadConfig_sensorSpecific(
	const mrpt::config::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] =
				mrpt::poses::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;
	}
#else
	MRPT_UNUSED_PARAM(configSource);
	MRPT_UNUSED_PARAM(iniSection);
#endif
}