/* -----------------------------------------------------
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
}
