Debian Codegraph @

libs/hwdrivers/src/CWirelessPower.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/hwdrivers/CWirelessPower.h>
13
14 #include <iostream>
15
16 #ifdef MRPT_OS_LINUX
17 #include <iostream>
18 #include <sstream>
19 #endif
20
21 #ifdef _WIN32
22 #if defined(__GNUC__)
23 // MinGW: Nothing to do here (yet)
24 #else
25
26 #define WIN32_LEAN_AND_MEAN
27 #include <windows.h>
28 //
29 #include <objbase.h>
30 #include <wlanapi.h>
31 #include <wtypes.h>
32 #pragma comment(lib, "Wlanapi.lib")
33
34 #endif
35
36 #endif
37
38 using namespace mrpt::hwdrivers;
39 using namespace std;
40
41 IMPLEMENTS_GENERIC_SENSOR(CWirelessPower, mrpt::hwdrivers)
42
43 CWirelessPower::CWirelessPower() { m_sensorLabel = "WIRELESS_POWER"; }
44 #ifdef _WIN32
45 #if defined(__GNUC__)
46 // MinGW: Nothing to do here (yet)
47 #else
48 /*---------------------------------------------------------------
49 ConnectWlanServerW
50 Get a connection to the WLAN server
51 ---------------------------------------------------------------*/
52
53 /** Gets a connection to the server
54 * \exception std::exception In case there is a failure (happens when WiFi is
55 * not started)
56 */
57
58 void* ConnectWlanServerW()
59 {
60 DWORD dwMaxClient = 2;
61 DWORD dwCurVersion = 0;
62 DWORD dwResult = 0; // Result of the API call
63 HANDLE hClient;
64 // open connection to server
65 dwResult = WlanOpenHandle(dwMaxClient, nullptr, &dwCurVersion, &hClient);
66 if (dwResult != ERROR_SUCCESS)
67 {
68 // if an error ocurred
69 std::stringstream excmsg;
70 excmsg << "WlanOpenHandle failed with error: " << dwResult << std::endl;
71
72 // You can use FormatMessage here to find out why the function failed
73 THROW_EXCEPTION(excmsg.str());
74 }
75 return (void*)hClient;
76 }
77
78 /*---------------------------------------------------------------
79 ListInterfacesW
80 Gets a list of the interfaces (Windows)
81 ---------------------------------------------------------------*/
82
83 /** Gets a list of the interfaces available in the system (in Windows format)
84 * \exception std::exception In case there is a failure
85 * \return std::vector returns handles to the available interfaces
86 */
87
88 std::vector<PWLAN_INTERFACE_INFO> ListInterfacesW(HANDLE hClient)
89 {
90 // Get a list of the available interfaces
91
92 std::vector<PWLAN_INTERFACE_INFO> outputVector; // start the output vector
93 PWLAN_INTERFACE_INFO_LIST pIfList = nullptr; // list of WLAN interfaces
94 PWLAN_INTERFACE_INFO pIfInfo = nullptr; // information element for one interface
95 DWORD dwResult = 0;
96
97 int i;
98
99 // Call the interface enumeration function of the API
100 dwResult = WlanEnumInterfaces(hClient, nullptr, &pIfList);
101
102 // check result
103 if (dwResult != ERROR_SUCCESS)
104 {
105 // In case of error, raise an exception
106 std::stringstream excmsg;
107 excmsg << "WlanEnumInterfaces failed with error: " << dwResult << std::endl;
108
109 THROW_EXCEPTION(excmsg.str());
110 // You can use FormatMessage here to find out why the function failed
111 }
112 else
113 {
114 // iterate through interfaces to add them to the output vector
115 for (i = 0; i < (int)pIfList->dwNumberOfItems; i++)
116 {
117 pIfInfo = (WLAN_INTERFACE_INFO*)&pIfList->InterfaceInfo[i];
118 outputVector.push_back(pIfInfo);
119 }
120 }
121 return outputVector;
122 }
123
124 /*---------------------------------------------------------------
125 GUID2Str
126 Gets the GUID of a network based on its handler in Windows
127 ---------------------------------------------------------------*/
128 /** Transforms a GUID structure (in Windows format) to a string
129 * \exception std::exception In case there is a failure
130 * \return std::string returns a string containing the GUID
131 */
132
133 std::string GUID2Str(const GUID& ifaceGuid)
134 {
135 // Variables
136 int iRet;
137 errno_t wctostr;
138 size_t sizeGUID;
139
140 WCHAR GuidString[39] = {0};
141 char GuidChar[100];
142
143 std::string outputString;
144
145 // Call the API function that gets the name of the GUID as a WCHAR[]
146 iRet =
147 StringFromGUID2(ifaceGuid, (LPOLESTR)&GuidString, sizeof(GuidString) / sizeof(*GuidString));
148 // For c rather than C++ source code, the above line needs to be
149 // iRet = StringFromGUID2(&pIfInfo->InterfaceGuid, (LPOLESTR) &GuidString,
150 // sizeof(GuidString)/sizeof(*GuidString));
151
152 // translate from a WCHAR to string if no error happened
153 if (iRet == 0)
154 {
155 THROW_EXCEPTION("StringFromGUID2 failed\n");
156 }
157 else
158 {
159 wctostr = wcstombs_s(&sizeGUID, GuidChar, 100, GuidString, 100);
160 if ((wctostr == EINVAL) || (wctostr == ERANGE))
161 {
162 THROW_EXCEPTION("wcstombs_s failed\n");
163 }
164 else
165 {
166 outputString = std::string(GuidChar);
167 }
168 }
169
170 return outputString;
171 }
172
173 /*---------------------------------------------------------------
174 GetInterfaceW
175 Gets a handler for the interface (Windows)
176 ---------------------------------------------------------------*/
177
178 /** Gets a handle to the interface that has been set by setNet() (in Windows
179 * format)
180 * \exception std::exception In case there is a failure
181 * \return PWLAN_INTERFACE_INFO returns a handle to the interface
182 */
183
184 PWLAN_INTERFACE_INFO GetInterfaceW(std::string guid, HANDLE hClient)
185 {
186 // Get interface given the GUID as a string (by the guid property of the
187 // object)
188
189 std::vector<PWLAN_INTERFACE_INFO> ifaceList; // interface list
190 std::vector<PWLAN_INTERFACE_INFO>::iterator ifaceIter; // iterator
191 PWLAN_INTERFACE_INFO output = nullptr; // interface info element
192
193 // get a list of all the interfaces
194 ifaceList = ListInterfacesW(hClient);
195
196 // search for the interface that has the given GUID
197 for (ifaceIter = ifaceList.begin(); ifaceIter != ifaceList.end(); ++ifaceIter)
198 {
199 if (GUID2Str((*ifaceIter)->InterfaceGuid) == guid)
200 {
201 output = *ifaceIter;
202 break;
203 }
204 }
205
206 return output;
207 }
208
209 /*---------------------------------------------------------------
210 ListNetworksW
211 Gets a list of the networks available for the interface (in Windows)
212 ---------------------------------------------------------------*/
213
214 /** Gets a list of the networks available for an interface (in Windows format)
215 * \exception std::exception In case there is a failure
216 * \return std::vector returns handles to the available networks of a given
217 * interface
218 * \param iface handle to the WiFi interface
219 */
220 std::vector<PWLAN_AVAILABLE_NETWORK> ListNetworksW(PWLAN_INTERFACE_INFO iface, HANDLE hClient)
221 {
222 // Start variables
223
224 DWORD dwResult = 0;
225 PWLAN_AVAILABLE_NETWORK_LIST pBssList = nullptr; // list of available networks
226 PWLAN_AVAILABLE_NETWORK pBssEntry = nullptr; // information element for one interface
227
228 GUID ifaceGuid = iface->InterfaceGuid; // Get GUID of the interface
229
230 std::vector<PWLAN_AVAILABLE_NETWORK> outputVector; // output vector
231
232 // WCHAR GuidString[39] = {0};
233
234 // Force a scan (to obtain new data)
235 WLAN_RAW_DATA IeData;
236 WlanScan((HANDLE)hClient, &ifaceGuid, nullptr, &IeData, nullptr);
237
238 // Call the Windows API and get a list of the networks available through the
239 // interface
240 dwResult = WlanGetAvailableNetworkList((HANDLE)hClient, &ifaceGuid, 0, nullptr, &pBssList);
241
242 // Check the result of the call
243 if (dwResult != ERROR_SUCCESS)
244 {
245 // In case an error ocurred
246 std::stringstream excmsg;
247 excmsg << "WlanGetAvailableNetworkList failed with error: " << dwResult << std::endl;
248 // THROW_EXCEPTION(excmsg.str();;
249 }
250 else
251 {
252 // for each network, get its info and save it
253 for (unsigned int j = 0; j < pBssList->dwNumberOfItems; j++)
254 {
255 pBssEntry = (WLAN_AVAILABLE_NETWORK*)&pBssList->Network[j]; // get entry for network
256 outputVector.push_back(pBssEntry); // save entry
257 }
258 }
259
260 return outputVector;
261 }
262
263 /*---------------------------------------------------------------
264 GetNetworkW
265 Gets a handler to a wireless network in Windows
266 ---------------------------------------------------------------*/
267 /** Gets a handle to the network that has been set by setNet() (in Windows
268 * format)
269 * \exception std::exception In case there is a failure
270 * \return PWLAN_AVAILABLE_NETWORK returns a handle to the network
271 */
272 PWLAN_AVAILABLE_NETWORK GetNetworkW(
273 HANDLE hClient, const std::string& ssid, const std::string& guid)
274 {
275 // Variables
276 PWLAN_INTERFACE_INFO iface; // interface handler
277 PWLAN_AVAILABLE_NETWORK output; // output network handler
278
279 // Get a handler to the interface
280 iface = GetInterfaceW(guid, hClient);
281
282 // Get the list of networks
283 std::vector<PWLAN_AVAILABLE_NETWORK> pBssList = ListNetworksW(iface, hClient);
284
285 // Iterate through the list and find the network that has the matching SSID
286 std::vector<PWLAN_AVAILABLE_NETWORK>::iterator netIter;
287 for (netIter = pBssList.begin(); netIter != pBssList.end(); ++netIter)
288 {
289 if (std::string((char*)((*netIter)->dot11Ssid.ucSSID)) == ssid)
290 {
291 output = *netIter;
292 break;
293 }
294 }
295
296 return output;
297 }
298
299 #endif
300
301 #endif // end of Windows auxiliary functions definition
302
303 /*---------------------------------------------------------------
304 ListInterfaces
305 Gets a list of the interfaces
306 ---------------------------------------------------------------*/
307
308 std::vector<std::string> CWirelessPower::ListInterfaces()
309 {
310 std::vector<std::string> output; // output vector of strings
311
312 #ifdef MRPT_OS_LINUX
313 // in linux, the command line is used to get all the relevant information
314 FILE* cmdoutput; // file handler for the executed command line
315 char ifaceread[256], *netname; // strings used to read the output of the
316 // command line and get the name of each
317 // network
318
319 // Run the command line: get the info frim /proc/net/wireless and cut out
320 // the names of the interfaces
321
322 // commandl << "cat /proc/net/wireless|grep \"wlan\"|cut -d\" \" -f2|cut
323 // -d\":\" -f1";
324 cmdoutput = popen("cat /proc/net/wireless|grep \"wlan\"|cut -d\" \" -f2|cut -d\":\" -f1", "r");
325 if (!fgets(ifaceread, 3, cmdoutput)) // read output
326 THROW_EXCEPTION("Error reading /proc/net/wireless");
327
328 // iterate thrugh list and get each interface as a string
329 netname = ::strtok(ifaceread, "\n");
330 while (netname)
331 {
332 output.emplace_back(netname);
333 netname = ::strtok(nullptr, "\n");
334 }
335 #endif
336
337 #ifdef _WIN32
338 #if defined(__GNUC__)
339 THROW_EXCEPTION("Sorry, method not available for MinGW");
340 #else
341 // In windows, this function is a wrapper to ListInterfacesW
342
343 std::vector<PWLAN_INTERFACE_INFO>
344 ifaces; // vector containing the interface entries (Windows format)
345 std::vector<PWLAN_INTERFACE_INFO>::iterator
346 ifacesIter; // iterator to run through the previous list
347
348 // get the list
349 ifaces = ListInterfacesW(hClient);
350
351 // iterate thrugh list and get each GUID as a string
352 for (ifacesIter = ifaces.begin(); ifacesIter != ifaces.end(); ++ifacesIter)
353 {
354 output.push_back(GUID2Str((*ifacesIter)->InterfaceGuid));
355 }
356 #endif
357 #endif
358
359 return output;
360 }
361
362 /*---------------------------------------------------------------
363 ListNetworks
364 Gets a list of the networks available for the interface
365 ---------------------------------------------------------------*/
366
367 std::vector<std::string> CWirelessPower::ListNetworks()
368 {
369 std::vector<std::string> output; // output vector of strings
370
371 #ifdef MRPT_OS_LINUX
372
373 std::stringstream commandl; // command to be executed
374
375 FILE* cmdoutput;
376 char listread[1024];
377 char* netname;
378
379 // Run command: get a list of networks and cut out their names. Note: this
380 // must be done as a superuser, so the command is executed with sudo.
381 // Usually it should ask for the password only the first time.
382 // To avoid the inconvenience of having to write the password, it would be
383 // useful to configure sudo to allow the user to run this command. See "man
384 // sudoers"
385 commandl << "sudo iwlist "
386 << "wlan0"
387 << " scan|grep ESSID|cut -d\"\\\"\" -f2";
388 cmdoutput = popen(commandl.str().c_str(), "r");
389 if (!fgets(listread, 3, cmdoutput)) THROW_EXCEPTION("Error reading response from iwlist");
390
391 netname = ::strtok(listread, "\n");
392 while (netname)
393 {
394 output.emplace_back(netname);
395 netname = ::strtok(nullptr, "\n");
396 }
397
398 #endif
399
400 #ifdef _WIN32
401 #if defined(__GNUC__)
402 THROW_EXCEPTION("Sorry, method not available for MinGW");
403 #else
404
405 PWLAN_INTERFACE_INFO iface; // Information element for an interface
406
407 iface = GetInterfaceW(m_guid, (HANDLE)hClient); // Get the interface handler
408
409 // Get the list of networks
410 std::vector<PWLAN_AVAILABLE_NETWORK> pBssList = ListNetworksW(iface, (HANDLE)hClient);
411
412 // Iterate through the list and save the names as strings
413 std::vector<PWLAN_AVAILABLE_NETWORK>::iterator netIter;
414 for (netIter = pBssList.begin(); netIter != pBssList.end(); ++netIter)
415 {
416 output.push_back(std::string((char*)((*netIter)->dot11Ssid.ucSSID)));
417 }
418 #endif
419
420 #endif
421
422 return output;
423 }
424
425 /*---------------------------------------------------------------
426 GetPower
427 Gets the power of the network
428 ---------------------------------------------------------------*/
429 int CWirelessPower::GetPower()
430 {
431 #ifdef MRPT_OS_LINUX
432 FILE* cmdoutput;
433 char* powerReadL;
434 std::stringstream commandl;
435 // Run command: get the power of the networks and additional info
436 commandl << "sudo iwlist "
437 << "wlan0"
438 << " scan";
439 cmdoutput = popen(commandl.str().c_str(), "r");
440
441 std::vector<std::string> powerReadV;
442 size_t readBytes;
443
444 powerReadL = (char*)malloc(256);
445 std::stringstream ssidLine;
446
447 ssidLine << "ESSID:\"" << m_ssid << "\"";
448 if (getline(&powerReadL, &readBytes, cmdoutput) < 0)
449 THROW_EXCEPTION("Error reading response from iwlist");
450
451 while (!strstr(powerReadL, ssidLine.str().c_str()))
452 {
453 powerReadV.emplace_back(powerReadL);
454 if (getline(&powerReadL, &readBytes, cmdoutput))
455 THROW_EXCEPTION("Error reading response from iwlist");
456 }
457
458 auto ssiter = powerReadV.end() - 2;
459
460 char powerLine[256];
461
462 // now we have a string per output line of iwlist. We must find the line
463 // containing the desired ESSID and read the power from two lines before
464
465 strcpy(powerLine, (*ssiter).c_str());
466
467 char level[10];
468 // meaning that the ESSID was found
469 // Example pf the value of poerLine: Quality=57/100 Signal level=57/100
470 char* fraction;
471
472 ::strtok(powerLine, "=");
473 ::strtok(nullptr, "=");
474 fraction = ::strtok(nullptr, "=");
475 strcpy(level, ::strtok(fraction, "/"));
476
477 free(powerReadL);
478
479 return atoi(level);
480
481 #elif defined(_WIN32)
482 #if defined(__GNUC__)
483 THROW_EXCEPTION("Sorry, method not available for MinGW");
484 #else
485 PWLAN_AVAILABLE_NETWORK wlan; // handler to the network
486
487 // Get a handler to the network
488 wlan = GetNetworkW((HANDLE)hClient, m_ssid, m_guid);
489
490 return wlan->wlanSignalQuality;
491 #endif
492 #else
493 THROW_EXCEPTION("Method not implemented for this platform/OS!");
494 #endif
495 }
496
497 /*---------------------------------------------------------------
498 getObservation
499 Get the power of a given network as an observation
500 NOTE: Deprecated, use getObservations. Use this class as
501 GenericSensor. See the CGenericSensor documentation
502 ---------------------------------------------------------------*/
503 bool CWirelessPower::getObservation(mrpt::obs::CObservationWirelessPower& outObservation)
504 {
505 try
506 {
507 // outObservation.m_readings.clear();
508 outObservation.power = (float)GetPower();
509
510 outObservation.timestamp = mrpt::Clock::now();
511
512 outObservation.sensorLabel = m_sensorLabel;
513 // std::cout << "mrpt::hwdrivers::CWirelessPower::getObservation() " <<
514 //"\n\tsensorLabel: " << outObservation.sensorLabel << "\n\ttimestamp: "
515 //<< outObservation.timestamp << "\n\tpower: " << outObservation.power
516 //<< std::endl;
517 return true;
518 }
519 catch (exception& e)
520 {
521 cerr << "[CWirelessPower::getObservation] Returning false due to "
522 "exception: "
523 << endl;
524 cerr << e.what() << endl;
525 return false;
526 }
527 }
528
529 void CWirelessPower::doProcess()
530 {
531 // Wrapper to getObservation
532 auto outObservation = mrpt::obs::CObservationWirelessPower::Create();
533 getObservation(*outObservation);
534
535 appendObservation(mrpt::obs::CObservationWirelessPower::Create(*outObservation));
536 }
537
538 void CWirelessPower::loadConfig_sensorSpecific(
539 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
540 {
541 MRPT_START
542 pose_x = configSource.read_float(iniSection, "pose_x", 0, true);
543 pose_y = configSource.read_float(iniSection, "pose_y", 0, true);
544 pose_z = configSource.read_float(iniSection, "pose_z", 0, true);
545 pose_roll = configSource.read_float(iniSection, "pose_roll", 0, true);
546 pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, true);
547 pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, true);
548
549 m_ssid = configSource.read_string(iniSection, "ssid", "", true);
550 m_guid = configSource.read_string(iniSection, "guid", "", true);
551 // in the case of Linux, the "GUID" is the interface name (wlanX)
552
553 #ifdef _WIN32
554 #if defined(__GNUC__)
555 THROW_EXCEPTION("Sorry, method not available for MinGW");
556 #else
557 hClient = ConnectWlanServerW();
558 #endif
559 #endif
560
561 MRPT_END
562 }
563
564 void CWirelessPower::setNet(std::string ssid, std::string guid)
565 {
566 m_ssid = ssid;
567 m_guid = guid;
568 }