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