Adding position-based roaming.

CMakeLists.txt

@@ -6,6 +6,7 @@ set(BTState OFF CACHE BOOL OFF)
 set(CMAKE_CXX_STANDARD 11)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
+set(ENABLE_TESTS OFF CACHE BOOL OFF)
 
 option(BASE_INCOHERENT_TO_FAULT "if enabled, the bt_motorsNotInFault module will consider a fault every pair of different controle modes for the robot base wheels motors" OFF)
 if(BASE_INCOHERENT_TO_FAULT)

aux_modules/CMakeLists.txt

@@ -9,6 +9,7 @@ add_subdirectory(headSynchronizer)
 add_subdirectory(googleLogger)
 add_subdirectory(tourManager)
 add_subdirectory(eyeContactManager)
+add_subdirectory(roaming)
 if(0)
     add_subdirectory(speechProcessing)
 endif()

aux_modules/roaming/CMakeLists.txt

@@ -0,0 +1,43 @@
+project(roaming)
+set(ROAMING_SERVER roaming_server)
+set(ROAMING_CLIENT roaming_client)
+
+## Roaming server configuration
+add_executable(${ROAMING_SERVER})
+target_include_directories(${ROAMING_SERVER} PUBLIC include)
+target_sources(${ROAMING_SERVER} PRIVATE 
+    src/mainServer.cpp
+    src/RoamingServer.cpp 
+    src/LinuxNetworkInteraction.cpp
+    include/LinuxNetworkInteraction.h
+    include/RoamingServer.h)
+target_link_libraries(${ROAMING_SERVER} PRIVATE ${YARP_LIBRARIES})
+if(${ENABLE_TESTS})
+    add_subdirectory(test)
+endif()
+
+
+## Roaming client configuration
+find_library(wpa libwpa_client.so HINTS /opt/wpa_supplicant-2.10/wpa_supplicant)
+if(${wpa} MATCHES wpa-NOTFOUND)
+    message("-- Cannot build roaming client")
+else()
+    add_executable(${ROAMING_CLIENT})
+    cmake_path(GET wpa PARENT_PATH WPA_PATH)
+    cmake_path(GET WPA_PATH PARENT_PATH WPA_PARENT_PATH)
+    target_include_directories(${ROAMING_CLIENT} PUBLIC
+        include
+        ${WPA_PARENT_PATH}/src/common
+        ${WPA_PARENT_PATH}/src/utils
+    )
+    target_sources(${ROAMING_CLIENT} PRIVATE
+        src/mainClient.cpp
+        src/RoamingClient.cpp
+        src/WpaSupplicantRoaming.cpp
+        include/RoamingClient.h
+    )
+    target_link_libraries(${ROAMING_CLIENT} PUBLIC
+        ${YARP_LIBRARIES}
+        ${wpa}
+    )
+endif()

aux_modules/roaming/README.md

@@ -0,0 +1,24 @@
+## This is a work in progress, such definition may change in the future once the functionality is fully implemented.
+
+## Position based roaming
+
+The modules in this folder allow the robot to roam between Access Points (AP) of which the position is physically known.
+
+### Requirements
+
+#### Wpa supplicant
+- *On your host machine* You need a copy of `wpa_supplicant` (you can find the latest release [here](https://w1.fi/wpa_supplicant/)). Unzip the archive and put the source code in the `/opt` folder.
+- `cd /opt/wpa_supplicant-2.* && make -C wpa_supplicant libwpa_client.so`
+
+#### YARP
+- Make sure YARP>3.9 is installed on your host machine
+
+### Setup
+
+- Before running your nav2 stack, make sure to include the positions of the APs in the locations.ini file. You can give the location any name that you want. Then you need to map the name used in the locations.ini file to the name of the mac address of the AP. You need to do this in a txt file of the style of `locations_ap_map.txt`. You can then run the `roaming_server` module with the `--loc_to_ap_map` option.
+
+### Usage
+
+- Run the `roaming_server` module on your preferred machine (the advice is to run it within the tour-guide-robot docker on the robot). This module is responsible for deciding whether to roam based on the position of the robot compared to the known access points. For now it uses the Euclidean distance to decide whether to roam or not.
+
+- Run the `roaming_client` module on the host machine of the robot. This module is responsible for receiving the AP to roam to from the `roaming_server` and then connecting to the AP.

aux_modules/roaming/include/INetworkInteraction.h

@@ -0,0 +1,15 @@
+#ifndef INETWORKINTERACTION_H_
+#define INETWORKINTERACTION_H_
+
+#include <string>
+#include <vector>
+
+class INetworkInteraction
+{
+    public:
+        virtual ~INetworkInteraction() {};
+        virtual std::vector<std::string> getCurrentApName() = 0;
+        virtual bool roam(const std::string& ap_name) = 0;
+};
+
+#endif

aux_modules/roaming/include/LinuxNetworkInteraction.h

@@ -0,0 +1,15 @@
+#ifndef LINUXNETWORKINTERACTION_H_
+#define LINUXNETWORKINTERACTION_H_
+
+#include <INetworkInteraction.h>
+
+class LinuxNetworkInteraction : public INetworkInteraction 
+{
+
+    public:
+        LinuxNetworkInteraction() {};
+        std::vector<std::string> getCurrentApName() override;
+        bool roam(const std::string& ap_name) override;
+};
+
+#endif

aux_modules/roaming/include/RoamingClient.h

@@ -0,0 +1,24 @@
+#include <yarp/os/RFModule.h>
+#include <yarp/os/Port.h>
+#include <WpaSupplicantRoaming.h>
+
+class RoamingClient: public yarp::os::RFModule
+{
+    public:
+        RoamingClient();
+        bool configure(yarp::os::ResourceFinder& rf) override;
+        bool updateModule() override;
+        bool interruptModule() override;
+        double getPeriod() override;
+        bool close() override;
+
+    private:
+        std::string m_name;
+        std::string m_port_ap_name;
+        std::string m_interface_name;
+        std::string m_ssid;
+
+        yarp::os::Port m_port_ap;
+
+        WpaSupplicantRoaming roaming;
+};

aux_modules/roaming/include/RoamingServer.h

@@ -0,0 +1,59 @@
+#include <yarp/os/RFModule.h>
+#include <yarp/dev/INavigation2D.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/os/Port.h>
+
+#include <optional>
+#include <map>
+
+#include <INetworkInteraction.h>
+
+using LocationApMap = std::map<std::string,std::string>;
+
+class RoamingServer : public yarp::os::RFModule
+{
+public:
+    RoamingServer(std::string name);
+    RoamingServer(std::string name, INetworkInteraction& inet);
+    virtual bool configure(yarp::os::ResourceFinder &rf);
+    virtual double getPeriod();
+    virtual bool updateModule();
+    virtual bool close();
+    virtual bool interruptModule();
+
+    bool roam(const std::string &ap_name);
+
+    // APs
+    const std::vector<std::string> getAPList() const;
+    const std::optional<yarp::dev::Nav2D::Map2DLocation> getApPosition(const std::string &ap_name) const;
+    const std::optional<std::string> getCurrentApName() const;
+    const bool isAP(const std::string &location_name) const;
+    std::pair<LocationApMap,LocationApMap> getLocationAPMaps(std::ifstream& map_file) const;
+    const double distanceToAP(const std::string& ap_name) const;
+    const std::string getBestAP() const;
+
+private:
+    std::string m_name;
+    std::string m_roaming_port_name;
+    std::string m_locations_to_ap_file_name;
+    yarp::os::Port m_roaming_port;
+
+    yarp::dev::Nav2D::INavigation2D *m_navigation;
+    yarp::dev::Nav2D::ILocalization2D *m_localization;
+    yarp::dev::Nav2D::IMap2D *m_map;
+    yarp::dev::PolyDriver m_ddNav;
+
+    INetworkInteraction& m_net;
+
+    std::vector<std::string> m_locations_list;
+    std::map<std::string,std::string> m_location_to_ap;
+    std::map<std::string,std::string> m_ap_to_location;
+    std::string m_map_name;
+
+    // Configuration
+    bool configureInterfaces();
+
+    // Roaming 
+    yarp::dev::Nav2D::Map2DLocation m_robot_position;
+    bool checkRoamingCondition(const double threshold = 5.0);
+};

aux_modules/roaming/include/WpaSupplicantRoaming.h

@@ -0,0 +1,10 @@
+#include <string>
+#include <wpa_ctrl.h>
+
+class WpaSupplicantRoaming
+{
+    public:
+        bool roam(const std::string& if_name, const std::string& ssid, const std::string& ap_name);
+    private:
+        std::string wpa_request(wpa_ctrl* ctrl, const std::string& cmd);
+};

aux_modules/roaming/src/LinuxNetworkInteraction.cpp

@@ -0,0 +1,30 @@
+#include <LinuxNetworkInteraction.h>
+#include <yarp/os/LogStream.h>
+
+std::vector<std::string> LinuxNetworkInteraction::getCurrentApName()
+{
+    // Get current connected AP name
+    std::vector<std::string> iw_config_out;
+
+    // Get iwconfig output
+    FILE* pipe = popen("iwconfig 2>&1", "r");
+    if(!pipe) {
+        yError() << "Failed to execute iwconfig. Is iwconfig installed? Please run sudo apt update; sudo apt install wireless-tools";
+        return iw_config_out;
+    }
+
+    // Parse iwconfig output to get Access Point name
+    char buffer[128];
+    while(fgets(buffer, 128, pipe) != NULL) {
+        std::string line(buffer);
+        iw_config_out.push_back(line);
+    }
+
+    return iw_config_out;
+}
+
+bool LinuxNetworkInteraction::roam(const std::string& ap_name)
+{
+    yWarning() << "Roaming capability is not implemented in LinuxNetworkInteraction";
+    return true;
+}

aux_modules/roaming/src/RoamingClient.cpp

@@ -0,0 +1,59 @@
+#include <RoamingClient.h>
+#include <yarp/os/Bottle.h>
+#include <yarp/os/LogStream.h>
+#include <algorithm>
+
+RoamingClient::RoamingClient():
+    roaming{WpaSupplicantRoaming()}
+    {}
+
+bool RoamingClient::configure(yarp::os::ResourceFinder& rf)
+{
+    m_name = rf.check("name",yarp::os::Value("RoamingClient")).asString();
+    m_interface_name = rf.check("interface",yarp::os::Value("wlp0s20f3")).asString();
+    m_ssid = rf.check("ssid",yarp::os::Value("r1_wifi")).asString();
+
+    yDebug() << "Parameter name set to:" << m_name;
+    yDebug() << "Parameter interface set to:" << m_interface_name;
+    yDebug() << "Parameter ssid set to:" << m_ssid;
+
+    m_port_ap_name = "/" + m_name + "/ap:i";
+    m_port_ap.open(m_port_ap_name);
+
+    return true;
+}
+
+bool RoamingClient::updateModule()
+{
+    yarp::os::Bottle ap_name_bot;
+    if(m_port_ap.read(ap_name_bot))
+    {
+        std::string ap_name = ap_name_bot.toString();
+
+        // Trimming the \" at the start and end of string introduced by the bottle
+        ap_name.erase(std::remove(ap_name.begin(),ap_name.end(),'\"'),ap_name.end());
+        if(!roaming.roam(m_interface_name,m_ssid,ap_name))
+        {
+            yError() << "Roaming to ap: " << ap_name << " has failed";
+        }
+    }
+
+    return true;
+}
+
+bool RoamingClient::interruptModule()
+{
+    m_port_ap.interrupt();
+    return true;
+}
+
+bool RoamingClient::close()
+{
+    m_port_ap.close();
+    return true;
+}
+
+double RoamingClient::getPeriod()
+{
+    return 1;
+}