20 #include <phidget21.h>
38 m_carteInterfaceKit =
new CPhidgetInterfaceKitHandle;
39 *((CPhidgetInterfaceKitHandle*)m_carteInterfaceKit) = 0;
40 m_sensorLabel =
"PhidgetInterfaceKit";
42 m_sensorIsPlugged.assign(8,
false);
43 m_minRange.assign(8, 0.1f);
44 m_maxRange.assign(8, 0.8f);
45 m_sensorPoses.resize(8);
50 "MRPT Was compiled without the CPhidget support. Recompile MRPT to use "
60 [[maybe_unused]]
const std::string& iniSection)
63 if (!configSource.sectionExists(iniSection))
70 configSource.read_int(iniSection,
string(
"process_rate"), 50);
72 configSource.read_int(iniSection,
string(
"serialNumber"), -1);
73 bool display = configSource.read_bool(
74 iniSection,
string(
"displayRecapitulativeInformations"),
false);
78 for (
int i = 1; i <= 8; i++)
80 string sensorNKeyName =
format(
"sensor%d", i);
81 string sensorType = configSource.read_string(
82 iniSection, sensorNKeyName,
string(
"UNPLUGGED"));
83 if (sensorType !=
string(
"UNPLUGGED"))
87 if (sensorType ==
string(
"EZ1"))
89 m_sensorType[i - 1] =
EZ1;
90 m_minRange[i - 1] = 0.15;
91 m_maxRange[i - 1] = 6.45;
93 else if (sensorType ==
string(
"SHARP-30cm"))
96 m_minRange[i - 1] = 0.04;
97 m_maxRange[i - 1] = 0.3;
99 else if (sensorType ==
string(
"SHARP-80cm"))
102 m_minRange[i - 1] = 0.06;
103 m_maxRange[i - 1] = 0.8;
107 string err =
format(
"Type of sensor %d is not supported", i);
111 m_sensorIsPlugged[i - 1] =
true;
113 string sensorNPoseX =
format(
"pose%d_x", i);
114 string sensorNPoseY =
format(
"pose%d_y", i);
115 string sensorNPoseZ =
format(
"pose%d_z", i);
116 string sensorNPoseYaw =
format(
"pose%d_yaw", i);
117 string sensorNPosePitch =
format(
"pose%d_pitch", i);
118 string sensorNPoseRoll =
format(
"pose%d_roll", i);
120 float x = configSource.read_float(iniSection, sensorNPoseX, 0.0);
121 float y = configSource.read_float(iniSection, sensorNPoseY, 0.0);
122 float z = configSource.read_float(iniSection, sensorNPoseZ, 0.0);
124 configSource.read_float(iniSection, sensorNPoseYaw, 0.0);
126 configSource.read_float(iniSection, sensorNPosePitch, 0.0);
128 configSource.read_float(iniSection, sensorNPoseRoll, 0.0);
130 m_sensorPoses[i - 1] =
137 cout <<
"+-------------------------------------------------------------"
141 cout <<
"| Phidget interfaceKit board number : " << m_serialNumber;
143 cout <<
"| Process rate : " << m_process_rate;
145 cout <<
"+---------+---------------------+-----------------------------"
148 cout <<
"| # + Sensor type | Sensor 3D pose "
151 cout <<
"+---------+---------------------+-----------------------------"
154 for (
int i = 0; i < 8; i++)
161 switch (m_sensorType[i])
167 cout <<
"SHARP_30cm |";
170 cout <<
"SHARP_80cm |";
173 cout <<
"UNPLUGGED |";
177 cout << m_sensorPoses[i];
180 cout <<
"+-------------------------------------------------------------"
194 CPhidgetInterfaceKit_create(
195 (CPhidgetInterfaceKitHandle*)m_carteInterfaceKit);
196 CPhidget_open(*((CPhidgetHandle*)(m_carteInterfaceKit)), m_serialNumber);
197 int err = CPhidget_waitForAttachment(
198 *((CPhidgetHandle*)(m_carteInterfaceKit)),
205 "Can't find Phidget IK card, please check your serial number.");
226 m_minOfMinRanges = *min_element(m_minRange.begin(), m_minRange.end());
227 m_maxOfMaxRanges = *max_element(m_maxRange.begin(), m_maxRange.end());
239 if (*((CPhidgetHandle*)m_carteInterfaceKit))
241 CPhidget_close(*((CPhidgetHandle*)(m_carteInterfaceKit)));
242 CPhidget_delete(*((CPhidgetHandle*)(m_carteInterfaceKit)));
259 if (obs->sensedData.size() > 0)
261 appendObservation(obs);
279 obs.sensorLabel = m_sensorLabel;
280 obs.minSensorDistance = m_minOfMinRanges;
281 obs.maxSensorDistance = m_maxOfMaxRanges;
282 obs.sensorConeApperture =
284 obs.sensedData.clear();
287 for (
int i = 0; i < 8; i++)
289 if (m_sensorIsPlugged[i])
292 int err = CPhidgetInterfaceKit_getSensorValue(
293 *((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)), i,
298 "Error durring acquiering sensor value on channel : %d", i);
301 switch (m_sensorType[i])
312 4800. / (
d2f(sensorValue) - 16.92);
323 obs.sensedData.push_back(obsRange);