CWirelessPower.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"  // Precompiled headers

#include <mrpt/hwdrivers/CWirelessPower.h>
#include <iostream>

#ifdef MRPT_OS_LINUX
#include <iostream>
#include <sstream>
#endif

#ifdef _WIN32
#if defined(__GNUC__)
// MinGW: Nothing to do here (yet)
#else

#include <windows.h>
#include <wlanapi.h>
#include <objbase.h>
#include <wtypes.h>
#pragma comment(lib, "Wlanapi.lib")

#endif

#endif

using namespace mrpt::hwdrivers;
using namespace std;

IMPLEMENTS_GENERIC_SENSOR(CWirelessPower, mrpt::hwdrivers)

CWirelessPower::CWirelessPower() { m_sensorLabel = "WIRELESS_POWER"; }

#ifdef _WIN32
#if defined(__GNUC__)
// MinGW: Nothing to do here (yet)
#else
/*---------------------------------------------------------------
                    ConnectWlanServerW
   Get a connection to the WLAN server
 ---------------------------------------------------------------*/

/** Gets a connection to the server
 * \exception std::exception In case there is a failure (happens when WiFi is
 * not started)
 */

void* ConnectWlanServerW()
{
    DWORD dwMaxClient = 2;
    DWORD dwCurVersion = 0;
    DWORD dwResult = 0;  // Result of the API call
    HANDLE hClient;
    // open connection to server
    dwResult = WlanOpenHandle(dwMaxClient, nullptr, &dwCurVersion, &hClient);
    if (dwResult != ERROR_SUCCESS)
    {
        // if an error ocurred
        std::stringstream excmsg;
        excmsg << "WlanOpenHandle failed with error: " << dwResult << std::endl;

        // You can use FormatMessage here to find out why the function failed
        THROW_EXCEPTION(excmsg.str());
    }
    return (void*)hClient;
}

/*---------------------------------------------------------------
                    ListInterfacesW
   Gets a list of the interfaces (Windows)
 ---------------------------------------------------------------*/

/** Gets a list of the interfaces available in the system (in Windows format)
 * \exception std::exception In case there is a failure
 * \return std::vector returns handles to the available interfaces
 */

std::vector<PWLAN_INTERFACE_INFO> ListInterfacesW(HANDLE hClient)
{
    // Get a list of the available interfaces

    std::vector<PWLAN_INTERFACE_INFO> outputVector;  // start the output vector
    PWLAN_INTERFACE_INFO_LIST pIfList = nullptr;  // list of WLAN interfaces
    PWLAN_INTERFACE_INFO pIfInfo =
        nullptr;  // information element for one interface
    DWORD dwResult = 0;

    int i;

    // Call the interface enumeration function of the API
    dwResult = WlanEnumInterfaces(hClient, nullptr, &pIfList);

    // check result
    if (dwResult != ERROR_SUCCESS)
    {
        // In case of error, raise an exception
        std::stringstream excmsg;
        excmsg << "WlanEnumInterfaces failed with error: " << dwResult
               << std::endl;

        THROW_EXCEPTION(excmsg.str());
        // You can use FormatMessage here to find out why the function failed
    }
    else
    {
        // iterate throught interfaces to add them to the output vector
        for (i = 0; i < (int)pIfList->dwNumberOfItems; i++)
        {
            pIfInfo = (WLAN_INTERFACE_INFO*)&pIfList->InterfaceInfo[i];
            outputVector.push_back(pIfInfo);
        }
    }
    return outputVector;
}

/*---------------------------------------------------------------
                    GUID2Str
   Gets the GUID of a network based on its handler in Windows
 ---------------------------------------------------------------*/
/** Transforms a GUID structure (in Windows format) to a string
 * \exception std::exception In case there is a failure
 * \return std::string returns a string containing the GUID
 */

std::string GUID2Str(const GUID& ifaceGuid)
{
    // Variables
    int iRet;
    errno_t wctostr;
    size_t sizeGUID;

    WCHAR GuidString[39] = {0};
    char GuidChar[100];

    std::string outputString;

    // Call the API function that gets the name of the GUID as a WCHAR[]
    iRet = StringFromGUID2(
        ifaceGuid, (LPOLESTR)&GuidString,
        sizeof(GuidString) / sizeof(*GuidString));
    // For c rather than C++ source code, the above line needs to be
    // iRet = StringFromGUID2(&pIfInfo->InterfaceGuid, (LPOLESTR) &GuidString,
    //     sizeof(GuidString)/sizeof(*GuidString));

    // translate from a WCHAR to string if no error happened
    if (iRet == 0)
    {
        THROW_EXCEPTION("StringFromGUID2 failed\n");
    }
    else
    {
        wctostr = wcstombs_s(&sizeGUID, GuidChar, 100, GuidString, 100);
        if ((wctostr == EINVAL) || (wctostr == ERANGE))
        {
            THROW_EXCEPTION("wcstombs_s failed\n");
        }
        else
        {
            outputString = std::string(GuidChar);
        }
    }

    return outputString;
}

/*---------------------------------------------------------------
                    GetInterfaceW
   Gets a handler for the interface (Windows)
 ---------------------------------------------------------------*/

/** Gets a handle to the interface that has been set by setNet() (in Windows
 * format)
 * \exception std::exception In case there is a failure
 * \return PWLAN_INTERFACE_INFO returns a handle to the interface
 */

PWLAN_INTERFACE_INFO GetInterfaceW(std::string guid, HANDLE hClient)
{
    // Get interface given the GUID as a string (by the guid property of the
    // object)

    std::vector<PWLAN_INTERFACE_INFO> ifaceList;  // interface list
    std::vector<PWLAN_INTERFACE_INFO>::iterator ifaceIter;  // iterator
    PWLAN_INTERFACE_INFO output = nullptr;  // interface info element

    // get a list of all the interfaces
    ifaceList = ListInterfacesW(hClient);

    // search for the interface that has the given GUID
    for (ifaceIter = ifaceList.begin(); ifaceIter != ifaceList.end();
         ++ifaceIter)
    {
        if (GUID2Str((*ifaceIter)->InterfaceGuid) == guid)
        {
            output = *ifaceIter;
            break;
        }
    }

    return output;
}

/*---------------------------------------------------------------
                    ListNetworksW
   Gets a list of the networks available for the interface (in Windows)
 ---------------------------------------------------------------*/

/** Gets a list of the networks available for an interface (in Windows format)
 * \exception std::exception In case there is a failure
 * \return std::vector returns handles to the available networks of a given
 * interface
 * \param iface handle to the WiFi interface
 */
std::vector<PWLAN_AVAILABLE_NETWORK> ListNetworksW(
    PWLAN_INTERFACE_INFO iface, HANDLE hClient)
{
    // Start variables

    DWORD dwResult = 0;
    PWLAN_AVAILABLE_NETWORK_LIST pBssList =
        nullptr;  // list of available networks
    PWLAN_AVAILABLE_NETWORK pBssEntry =
        nullptr;  // information element for one interface

    GUID ifaceGuid = iface->InterfaceGuid;  // Get GUID of the interface

    std::vector<PWLAN_AVAILABLE_NETWORK> outputVector;  // output vector

    //  WCHAR GuidString[39] = {0};

    // Force a scan (to obtain new data)
    WLAN_RAW_DATA IeData;
    WlanScan((HANDLE)hClient, &ifaceGuid, nullptr, &IeData, nullptr);

    // Call the Windows API and get a list of the networks available through the
    // interface
    dwResult = WlanGetAvailableNetworkList(
        (HANDLE)hClient, &ifaceGuid, 0, nullptr, &pBssList);

    // Check the result of the call
    if (dwResult != ERROR_SUCCESS)
    {
        // In case an error ocurred
        std::stringstream excmsg;
        excmsg << "WlanGetAvailableNetworkList failed with error: " << dwResult
               << std::endl;
        //  THROW_EXCEPTION(excmsg.str();;
    }
    else
    {
        // for each network, get its info and save it
        for (unsigned int j = 0; j < pBssList->dwNumberOfItems; j++)
        {
            pBssEntry = (WLAN_AVAILABLE_NETWORK*)&pBssList
                            ->Network[j];  // get entry for network
            outputVector.push_back(pBssEntry);  // save entry
        }
    }

    return outputVector;
}

/*---------------------------------------------------------------
                    GetNetworkW
   Gets a handler to a wireless network in Windows
 ---------------------------------------------------------------*/
/** Gets a handle to the network that has been set by setNet() (in Windows
 * format)
 * \exception std::exception In case there is a failure
 * \return PWLAN_AVAILABLE_NETWORK returns a handle to the network
 */
PWLAN_AVAILABLE_NETWORK GetNetworkW(
    HANDLE hClient, const std::string& ssid, const std::string& guid)
{
    // Variables
    PWLAN_INTERFACE_INFO iface;  // interface handler
    PWLAN_AVAILABLE_NETWORK output;  // output network handler

    // Get a handler to the interface
    iface = GetInterfaceW(guid, hClient);

    // Get the list of networks
    std::vector<PWLAN_AVAILABLE_NETWORK> pBssList =
        ListNetworksW(iface, hClient);

    // Iterate through the list and find the network that has the matching SSID
    std::vector<PWLAN_AVAILABLE_NETWORK>::iterator netIter;
    for (netIter = pBssList.begin(); netIter != pBssList.end(); ++netIter)
    {
        if (std::string((char*)((*netIter)->dot11Ssid.ucSSID)) == ssid)
        {
            output = *netIter;
            break;
        }
    }

    return output;
}

#endif

#endif  // end of Windows auxiliary functions definition

/*---------------------------------------------------------------
                    ListInterfaces
   Gets a list of the interfaces
 ---------------------------------------------------------------*/

std::vector<std::string> CWirelessPower::ListInterfaces()
{
    std::vector<std::string> output;  // output vector of strings

#ifdef MRPT_OS_LINUX
    // in linux, the command line is used to get all the relevant information
    FILE* cmdoutput;  // file handler for the executed command line
    char ifaceread[256], *netname;  // strings used to read the output of the
    // command line and get the name of each
    // network

    // Run the command line: get the info frim /proc/net/wireless and cut out
    // the names of the interfaces

    // commandl << "cat /proc/net/wireless|grep \"wlan\"|cut -d\" \" -f2|cut
    // -d\":\" -f1";
    cmdoutput = popen(
        "cat /proc/net/wireless|grep \"wlan\"|cut -d\" \" -f2|cut -d\":\" -f1",
        "r");
    if (!fgets(ifaceread, 3, cmdoutput))  // read output
        THROW_EXCEPTION("Error reading /proc/net/wireless");

    // iterate thrugh list and get each interface as a string
    netname = ::strtok(ifaceread, "\n");
    while (netname)
    {
        output.push_back(std::string(netname));
        netname = ::strtok(nullptr, "\n");
    }
#endif

#ifdef _WIN32
#if defined(__GNUC__)
    THROW_EXCEPTION("Sorry, method not available for MinGW");
#else
    // In windows, this function is a wrapper to ListInterfacesW

    std::vector<PWLAN_INTERFACE_INFO>
        ifaces;  // vector containing the interface entries (Windows format)
    std::vector<PWLAN_INTERFACE_INFO>::iterator
        ifacesIter;  // iterator to run through the previous list

    // get the list
    ifaces = ListInterfacesW(hClient);

    // iterate thrugh list and get each GUID as a string
    for (ifacesIter = ifaces.begin(); ifacesIter != ifaces.end(); ++ifacesIter)
    {
        output.push_back(GUID2Str((*ifacesIter)->InterfaceGuid));
    }
#endif
#endif

    return output;
}

/*---------------------------------------------------------------
                    ListNetworks
   Gets a list of the networks available for the interface
 ---------------------------------------------------------------*/

std::vector<std::string> CWirelessPower::ListNetworks()
{
    std::vector<std::string> output;  // output vector of strings

#ifdef MRPT_OS_LINUX

    std::stringstream commandl;  // command to be executed

    FILE* cmdoutput;
    char listread[1024];
    char* netname;

    // Run command: get a list of networks and cut out their names. Note: this
    // must be done as a superuser, so the command is executed with sudo.
    // Usually it should ask for the password only the first time.
    // To avoid the inconvenience of having to write the password, it would be
    // useful to configure sudo to allow the user to run this command. See "man
    // sudoers"
    commandl << "sudo iwlist "
             << "wlan0"
             << " scan|grep ESSID|cut -d\"\\\"\" -f2";
    cmdoutput = popen(commandl.str().c_str(), "r");
    if (!fgets(listread, 3, cmdoutput))
        THROW_EXCEPTION("Error reading response from iwlist");

    netname = ::strtok(listread, "\n");
    while (netname)
    {
        output.push_back(std::string(netname));
        netname = ::strtok(nullptr, "\n");
    }

#endif

#ifdef _WIN32
#if defined(__GNUC__)
    THROW_EXCEPTION("Sorry, method not available for MinGW");
#else

    PWLAN_INTERFACE_INFO iface;  // Information element for an interface

    iface = GetInterfaceW(guid, (HANDLE)hClient);  // Get the interface handler

    // Get the list of networks
    std::vector<PWLAN_AVAILABLE_NETWORK> pBssList =
        ListNetworksW(iface, (HANDLE)hClient);

    // Iterate through the list and save the names as strings
    std::vector<PWLAN_AVAILABLE_NETWORK>::iterator netIter;
    for (netIter = pBssList.begin(); netIter != pBssList.end(); ++netIter)
    {
        output.push_back(std::string((char*)((*netIter)->dot11Ssid.ucSSID)));
    }
#endif

#endif

    return output;
}

/*---------------------------------------------------------------
                    GetPower
   Gets the power of the network
 ---------------------------------------------------------------*/
int CWirelessPower::GetPower()
{
#ifdef MRPT_OS_LINUX
    FILE* cmdoutput;
    char* powerReadL;
    std::stringstream commandl;
    // Run command: get the power of the networks and additional info
    commandl << "sudo iwlist "
             << "wlan0"
             << " scan";
    cmdoutput = popen(commandl.str().c_str(), "r");

    std::vector<std::string> powerReadV;
    size_t readBytes;

    powerReadL = (char*)malloc(256);
    std::stringstream ssidLine;

    ssidLine << "ESSID:\"" << ssid << "\"";
    if (getline(&powerReadL, &readBytes, cmdoutput) < 0)
        THROW_EXCEPTION("Error reading response from iwlist");

    while (!strstr(powerReadL, ssidLine.str().c_str()))
    {
        powerReadV.push_back(std::string(powerReadL));
        if (getline(&powerReadL, &readBytes, cmdoutput))
            THROW_EXCEPTION("Error reading response from iwlist");
    }

    std::vector<std::string>::iterator ssiter = powerReadV.end() - 2;

    char powerLine[256];

    // now we have a string per output line of iwlist. We must find the line
    // containing the desired ESSID and read the power from two lines before

    strcpy(powerLine, (*ssiter).c_str());

    char level[10];
    // meaning that the ESSID was found
    // Example pf the value of poerLine: Quality=57/100  Signal level=57/100
    char* fraction;

    ::strtok(powerLine, "=");
    ::strtok(nullptr, "=");
    fraction = ::strtok(nullptr, "=");
    strcpy(level, ::strtok(fraction, "/"));

    free(powerReadL);

    return atoi(level);

#elif defined(_WIN32)
#if defined(__GNUC__)
    THROW_EXCEPTION("Sorry, method not available for MinGW");
#else
    PWLAN_AVAILABLE_NETWORK wlan;  // handler to the network

    // Get a handler to the network
    wlan = GetNetworkW((HANDLE)hClient, ssid, guid);

    return wlan->wlanSignalQuality;
#endif
#else
    THROW_EXCEPTION("Method not implemented for this platform/OS!");
#endif
}

/*---------------------------------------------------------------
                    getObservation
    Get the power of a given network as an observation
    NOTE: Deprecated, use getObservations. Use this class as
          GenericSensor. See the CGenericSensor documentation
 ---------------------------------------------------------------*/
bool CWirelessPower::getObservation(
    mrpt::obs::CObservationWirelessPower& outObservation)
{
    try
    {
        //  outObservation.m_readings.clear();
        outObservation.power = (float)GetPower();

        outObservation.timestamp = mrpt::system::getCurrentTime();

        outObservation.sensorLabel = m_sensorLabel;
        //  std::cout << "mrpt::hwdrivers::CWirelessPower::getObservation() " <<
        //"\n\tsensorLabel: " << outObservation.sensorLabel << "\n\ttimestamp: "
        //<< outObservation.timestamp << "\n\tpower: " << outObservation.power
        //<< std::endl;
        return true;
    }
    catch (exception& e)
    {
        cerr << "[CWirelessPower::getObservation] Returning false due to "
                "exception: "
             << endl;
        cerr << e.what() << endl;
        return false;
    }
}

void CWirelessPower::doProcess()
{
    // Wrapper to getObservation
    mrpt::obs::CObservationWirelessPower::Ptr outObservation =
        mrpt::make_aligned_shared<mrpt::obs::CObservationWirelessPower>();
    getObservation(*outObservation);

    appendObservation(mrpt::obs::CObservationWirelessPower::Ptr(
        new mrpt::obs::CObservationWirelessPower(*outObservation)));
}

void CWirelessPower::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& configSource,
    const std::string& iniSection)
{
    MRPT_START
    pose_x = configSource.read_float(iniSection, "pose_x", 0, true);
    pose_y = configSource.read_float(iniSection, "pose_y", 0, true);
    pose_z = configSource.read_float(iniSection, "pose_z", 0, true);
    pose_roll = configSource.read_float(iniSection, "pose_roll", 0, true);
    pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, true);
    pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, true);

    ssid = configSource.read_string(iniSection, "ssid", "", true);
    guid = configSource.read_string(
        iniSection, "guid", "", true);  // in the case of Linux, the "GUID" is
    // the interface name (wlanX)

#ifdef _WIN32
#if defined(__GNUC__)
    THROW_EXCEPTION("Sorry, method not available for MinGW");
#else
    hClient = ConnectWlanServerW();
#endif
#endif

    MRPT_END
}