Debian Codegraph @

libs/hwdrivers/src/CPhidgetInterfaceKitProximitySensors.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/config/CConfigFileBase.h>
13 #include <mrpt/hwdrivers/CPhidgetInterfaceKitProximitySensors.h>
14 #include <mrpt/system/os.h>
15
16 #include <algorithm>
17 #include <iostream>
18
19 #if MRPT_HAS_PHIDGET
20 #include <phidget21.h>
21 #endif
22
23 using namespace mrpt::hwdrivers;
24 using namespace mrpt::obs;
25 using namespace mrpt::system;
26 using namespace std;
27
28 IMPLEMENTS_GENERIC_SENSOR(CPhidgetInterfaceKitProximitySensors, mrpt::hwdrivers)
29
30 /* -----------------------------------------------------
31 Constructor
32 ----------------------------------------------------- */
33 CPhidgetInterfaceKitProximitySensors::CPhidgetInterfaceKitProximitySensors() :
34 mrpt::system::COutputLogger("CPhidgetInterfaceKitProximitySensors")
35
36 {
37 #if MRPT_HAS_PHIDGET
38 m_carteInterfaceKit = new CPhidgetInterfaceKitHandle;
39 *((CPhidgetInterfaceKitHandle*)m_carteInterfaceKit) = 0;
40 m_sensorLabel = "PhidgetInterfaceKit";
41
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);
46 m_sensorType.assign(8, UNPLUGGED);
47
48 #else
49 THROW_EXCEPTION(
50 "MRPT Was compiled without the CPhidget support. Recompile MRPT to use "
51 "this class");
52 #endif
53 }
54
55 /* -----------------------------------------------------
56 loadConfig_sensorSpecific
57 ----------------------------------------------------- */
58 void CPhidgetInterfaceKitProximitySensors::loadConfig_sensorSpecific(
59 [[maybe_unused]] const mrpt::config::CConfigFileBase& configSource,
60 [[maybe_unused]] const std::string& iniSection)
61 {
62 #if MRPT_HAS_PHIDGET
63 if (!configSource.sectionExists(iniSection))
64 THROW_EXCEPTION("Can't find section in configuration file");
65 // looking for the board parameters.
66 // process_rate = 100 // Hz (common to all sensors)
67 // serialNumber = 12345 // The interface kit serial
68 // number.
69 m_process_rate = configSource.read_int(iniSection, string("process_rate"), 50);
70 m_serialNumber = configSource.read_int(iniSection, string("serialNumber"), -1);
71 bool display =
72 configSource.read_bool(iniSection, string("displayRecapitulativeInformations"), false);
73
74 // Looking for each sensor.
75
76 for (int i = 1; i <= 8; i++)
77 {
78 string sensorNKeyName = mrpt::format("sensor%d", i);
79 string sensorType = configSource.read_string(iniSection, sensorNKeyName, string("UNPLUGGED"));
80 if (sensorType != string("UNPLUGGED"))
81 {
82 // the sensor is plugged :
83 // // check if the sensor type is supported.
84 if (sensorType == string("EZ1"))
85 {
86 m_sensorType[i - 1] = EZ1;
87 m_minRange[i - 1] = 0.15; // meters
88 m_maxRange[i - 1] = 6.45; // meters
89 }
90 else if (sensorType == string("SHARP-30cm"))
91 {
92 m_sensorType[i - 1] = SHARP_30cm;
93 m_minRange[i - 1] = 0.04; // meters
94 m_maxRange[i - 1] = 0.3; // meters
95 }
96 else if (sensorType == string("SHARP-80cm"))
97 {
98 m_sensorType[i - 1] = SHARP_80cm;
99 m_minRange[i - 1] = 0.06; // meters
100 m_maxRange[i - 1] = 0.8; // meters
101 }
102 else
103 {
104 string err = mrpt::format("Type of sensor %d is not supported", i);
105 m_state = CGenericSensor::ssError;
106 THROW_EXCEPTION(err);
107 }
108 m_sensorIsPlugged[i - 1] = true;
109 // reading the sensor pose.
110 string sensorNPoseX = mrpt::format("pose%d_x", i);
111 string sensorNPoseY = mrpt::format("pose%d_y", i);
112 string sensorNPoseZ = mrpt::format("pose%d_z", i);
113 string sensorNPoseYaw = mrpt::format("pose%d_yaw", i);
114 string sensorNPosePitch = mrpt::format("pose%d_pitch", i);
115 string sensorNPoseRoll = mrpt::format("pose%d_roll", i);
116
117 float x = configSource.read_float(iniSection, sensorNPoseX, 0.0);
118 float y = configSource.read_float(iniSection, sensorNPoseY, 0.0);
119 float z = configSource.read_float(iniSection, sensorNPoseZ, 0.0);
120 float yaw = configSource.read_float(iniSection, sensorNPoseYaw, 0.0);
121 float pitch = configSource.read_float(iniSection, sensorNPosePitch, 0.0);
122 float roll = configSource.read_float(iniSection, sensorNPoseRoll, 0.0);
123
124 m_sensorPoses[i - 1] = mrpt::poses::CPose3D(x, y, z, yaw, pitch, roll);
125 }
126 }
127 if (display)
128 { // width = 80;
129 cout.fill(' ');
130 cout << "+-------------------------------------------------------------"
131 "-----------------+"
132 << endl;
133 cout.width(79);
134 cout << "| Phidget interfaceKit board number : " << m_serialNumber;
135 cout << "|" << endl;
136 cout << "| Process rate : " << m_process_rate;
137 cout << "|" << endl;
138 cout << "+---------+---------------------+-----------------------------"
139 "-----------------+"
140 << endl;
141 cout << "| # + Sensor type | Sensor 3D pose "
142 " |"
143 << endl;
144 cout << "+---------+---------------------+-----------------------------"
145 "-----------------+"
146 << endl;
147 for (int i = 0; i < 8; i++)
148 {
149 cout << "|";
150 cout.width(9);
151 cout << i + 1;
152 cout << " |";
153 cout.width(19);
154 switch (m_sensorType[i])
155 {
156 case EZ1:
157 cout << "EZ1 |";
158 break;
159 case SHARP_30cm:
160 cout << "SHARP_30cm |";
161 break;
162 case SHARP_80cm:
163 cout << "SHARP_80cm |";
164 break;
165 case UNPLUGGED:
166 cout << "UNPLUGGED |";
167 break;
168 }
169 cout.width(43);
170 cout << m_sensorPoses[i];
171 cout << "|" << endl;
172 }
173 cout << "+-------------------------------------------------------------"
174 "-----------------+"
175 << endl;
176 }
177 #endif
178 }
179
180 /* -----------------------------------------------------
181 Initialize
182 ----------------------------------------------------- */
183 void CPhidgetInterfaceKitProximitySensors::initialize()
184 {
185 #if MRPT_HAS_PHIDGET
186 /*Try to connect to the interface kit board*/
187 CPhidgetInterfaceKit_create((CPhidgetInterfaceKitHandle*)m_carteInterfaceKit);
188 CPhidget_open(*((CPhidgetHandle*)(m_carteInterfaceKit)), m_serialNumber);
189 int err = CPhidget_waitForAttachment(
190 *((CPhidgetHandle*)(m_carteInterfaceKit)),
191 200); // wait 200ms for board attachment.
192 // if an error occur, "err" will be a > 0 value.
193 if (err > 0)
194 {
195 m_state = CGenericSensor::ssError;
196 THROW_EXCEPTION("Can't find Phidget IK card, please check your serial number.");
197 }
198 // set frame rate
199 /*int miliseconds =
200 static_cast<int>(1000./d2f(m_process_rate)); for(int i = 0 ;
201 i < 8 ; i++)
202 {
203 if(m_sensorIsPlugged[i])
204 {
205 int err =
206 CPhidgetInterfaceKit_setDataRate(*((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)),
207 i, miliseconds); if(err > 0)
208 {
209 string error = mrpt::format("Can't set process rate to %d ms on
210 channel %d of the Phidget IK Board.", miliseconds, i); m_state =
211 CGenericSensor::ssError; THROW_EXCEPTION(error);
212 }
213 }
214 }*/ // seems to be used only in the event based programming
215 // way.
216 // compute (min/max) of (min/max) ranges.
217 m_minOfMinRanges = *min_element(m_minRange.begin(), m_minRange.end());
218 m_maxOfMaxRanges = *max_element(m_maxRange.begin(), m_maxRange.end());
219 // driver is ready.
220 m_state = CGenericSensor::ssWorking;
221 #endif
222 }
223
224 /* -----------------------------------------------------
225 Destructor
226 ----------------------------------------------------- */
227 CPhidgetInterfaceKitProximitySensors::~CPhidgetInterfaceKitProximitySensors()
228 {
229 #if MRPT_HAS_PHIDGET
230 if (*((CPhidgetHandle*)m_carteInterfaceKit))
231 {
232 CPhidget_close(*((CPhidgetHandle*)(m_carteInterfaceKit)));
233 CPhidget_delete(*((CPhidgetHandle*)(m_carteInterfaceKit)));
234 }
235 #endif
236 }
237
238 /*-------------------------------------------------------------
239 doProcess
240 -------------------------------------------------------------*/
241 void CPhidgetInterfaceKitProximitySensors::doProcess()
242 {
243 CObservationRange::Ptr obs = std::make_shared<CObservationRange>();
244
245 try
246 {
247 getObservation(*obs);
248 m_state = ssWorking;
249 // if at least one data have been sensed :
250 if (obs->sensedData.size() > 0)
251 {
252 appendObservation(obs);
253 }
254 }
255 catch (...)
256 {
257 m_state = ssError;
258 THROW_EXCEPTION("No observation received from the Phidget board!");
259 }
260 }
261
262 /*-------------------------------------------------------------
263 getObservation
264 -------------------------------------------------------------*/
265 void CPhidgetInterfaceKitProximitySensors::getObservation(
266 [[maybe_unused]] mrpt::obs::CObservationRange& obs)
267 {
268 #if MRPT_HAS_PHIDGET
269 obs.timestamp = mrpt::Clock::now();
270 obs.sensorLabel = m_sensorLabel;
271 obs.minSensorDistance = m_minOfMinRanges;
272 obs.maxSensorDistance = m_maxOfMaxRanges;
273 obs.sensorConeAperture = DEG2RAD(2.0f); // TODO : Adapt to real sensor cone apperture.
274 obs.sensedData.clear();
275
276 int sensorValue;
277 for (int i = 0; i < 8; i++)
278 {
279 if (m_sensorIsPlugged[i])
280 {
281 mrpt::obs::CObservationRange::TMeasurement obsRange;
282 int err = CPhidgetInterfaceKit_getSensorValue(
283 *((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)), i, &sensorValue);
284 if (err > 0)
285 {
286 string error("Error durring acquiering sensor value on channel : %d", i);
287 THROW_EXCEPTION(error);
288 }
289 switch (m_sensorType[i])
290 {
291 case EZ1:
292 // TODO : find the conversion formula.
293 obsRange.sensedDistance = 1.0;
294 break;
295 case SHARP_30cm:
296 obsRange.sensedDistance = 2076. / (d2f(sensorValue) - 11.);
297 break;
298 case SHARP_80cm:
299 obsRange.sensedDistance = 4800. / (d2f(sensorValue) - 16.92);
300 break;
301 default:
302 obsRange.sensedDistance = -1;
303 break;
304 }
305
306 obsRange.sensorID = i;
307 obsRange.sensorPose = m_sensorPoses[i].asTPose(); // The pose of the IR sensor on the robot
308 obs.sensedData.push_back(obsRange);
309 }
310 }
311 #endif
312 }