diff --git a/CMakeLists.txt b/CMakeLists.txt index c1122ce..70dbc4f 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/aux_modules/CMakeLists.txt b/aux_modules/CMakeLists.txt index e7cefc3..cfbe0d2 100644 --- a/aux_modules/CMakeLists.txt +++ b/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() diff --git a/aux_modules/roaming/CMakeLists.txt b/aux_modules/roaming/CMakeLists.txt new file mode 100644 index 0000000..ff2efa9 --- /dev/null +++ b/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() diff --git a/aux_modules/roaming/README.md b/aux_modules/roaming/README.md new file mode 100644 index 0000000..cde9be5 --- /dev/null +++ b/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. diff --git a/aux_modules/roaming/include/INetworkInteraction.h b/aux_modules/roaming/include/INetworkInteraction.h new file mode 100644 index 0000000..a6e355a --- /dev/null +++ b/aux_modules/roaming/include/INetworkInteraction.h @@ -0,0 +1,15 @@ +#ifndef INETWORKINTERACTION_H_ +#define INETWORKINTERACTION_H_ + +#include +#include + +class INetworkInteraction +{ + public: + virtual ~INetworkInteraction() {}; + virtual std::vector getCurrentApName() = 0; + virtual bool roam(const std::string& ap_name) = 0; +}; + +#endif \ No newline at end of file diff --git a/aux_modules/roaming/include/LinuxNetworkInteraction.h b/aux_modules/roaming/include/LinuxNetworkInteraction.h new file mode 100644 index 0000000..8cce87c --- /dev/null +++ b/aux_modules/roaming/include/LinuxNetworkInteraction.h @@ -0,0 +1,15 @@ +#ifndef LINUXNETWORKINTERACTION_H_ +#define LINUXNETWORKINTERACTION_H_ + +#include + +class LinuxNetworkInteraction : public INetworkInteraction +{ + + public: + LinuxNetworkInteraction() {}; + std::vector getCurrentApName() override; + bool roam(const std::string& ap_name) override; +}; + +#endif \ No newline at end of file diff --git a/aux_modules/roaming/include/RoamingClient.h b/aux_modules/roaming/include/RoamingClient.h new file mode 100644 index 0000000..de96cd9 --- /dev/null +++ b/aux_modules/roaming/include/RoamingClient.h @@ -0,0 +1,24 @@ +#include +#include +#include + +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; +}; \ No newline at end of file diff --git a/aux_modules/roaming/include/RoamingServer.h b/aux_modules/roaming/include/RoamingServer.h new file mode 100644 index 0000000..dd8a99c --- /dev/null +++ b/aux_modules/roaming/include/RoamingServer.h @@ -0,0 +1,59 @@ +#include +#include +#include +#include + +#include +#include + +#include + +using LocationApMap = std::map; + +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 getAPList() const; + const std::optional getApPosition(const std::string &ap_name) const; + const std::optional getCurrentApName() const; + const bool isAP(const std::string &location_name) const; + std::pair 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 m_locations_list; + std::map m_location_to_ap; + std::map 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); +}; \ No newline at end of file diff --git a/aux_modules/roaming/include/WpaSupplicantRoaming.h b/aux_modules/roaming/include/WpaSupplicantRoaming.h new file mode 100644 index 0000000..c8db0a6 --- /dev/null +++ b/aux_modules/roaming/include/WpaSupplicantRoaming.h @@ -0,0 +1,10 @@ +#include +#include + +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); +}; \ No newline at end of file diff --git a/aux_modules/roaming/src/LinuxNetworkInteraction.cpp b/aux_modules/roaming/src/LinuxNetworkInteraction.cpp new file mode 100644 index 0000000..ae49082 --- /dev/null +++ b/aux_modules/roaming/src/LinuxNetworkInteraction.cpp @@ -0,0 +1,30 @@ +#include +#include + +std::vector LinuxNetworkInteraction::getCurrentApName() +{ + // Get current connected AP name + std::vector 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; +} \ No newline at end of file diff --git a/aux_modules/roaming/src/RoamingClient.cpp b/aux_modules/roaming/src/RoamingClient.cpp new file mode 100644 index 0000000..4265611 --- /dev/null +++ b/aux_modules/roaming/src/RoamingClient.cpp @@ -0,0 +1,59 @@ +#include +#include +#include +#include + +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; +} \ No newline at end of file diff --git a/aux_modules/roaming/src/RoamingServer.cpp b/aux_modules/roaming/src/RoamingServer.cpp new file mode 100644 index 0000000..629d67a --- /dev/null +++ b/aux_modules/roaming/src/RoamingServer.cpp @@ -0,0 +1,355 @@ +#include +#include +#include +#include +#include +#include +#include + +RoamingServer::RoamingServer(std::string name, INetworkInteraction& inet): + m_name{name}, + m_net{inet} +{ + m_navigation = nullptr; +} + +bool RoamingServer::configure(yarp::os::ResourceFinder &rf) +{ + // Return help if needed + if(rf.check("help") || !rf.check("loc_to_ap_map")) + { + yInfo() << "RoamingServer options:"; + yInfo() << "\t--loc_to_ap_map: file containing location to AP map"; + return false; + } + + m_locations_to_ap_file_name = rf.check("loc_to_ap_map", yarp::os::Value("locations_ap_map.txt")).asString(); + + // Open file + std::ifstream locations_to_ap_map_file(m_locations_to_ap_file_name); + if(!locations_to_ap_map_file.is_open()) + { + yError() << "Failed to open file" << m_locations_to_ap_file_name; + return false; + } + + // Configure interfaces to external services + configureInterfaces(); + + // Get ap and locations maps and list + const auto& [location_to_ap, ap_to_location] = getLocationAPMaps(locations_to_ap_map_file); + m_location_to_ap = location_to_ap; + m_ap_to_location = ap_to_location; + + m_roaming_port_name = "/" + m_name + "/ap:o"; + m_roaming_port.open(m_roaming_port_name); + + yDebug() << "Roaming port opened at" << m_roaming_port_name; + + // Get APs from map + try + { + m_locations_list = getAPList(); + } + catch(const std::runtime_error& e) + { + yError() << e.what(); + return false; + } + + yDebug() << "APs found in map" << m_locations_list.size(); + + if(m_locations_list.size() == 0) { + yError() << "None of the locations defined in the mapping file are found in the map server"; + return false; + } + + // Get robot position + m_navigation->getCurrentPosition(m_robot_position); + m_map_name = m_robot_position.map_id; + + return true; +} + +bool RoamingServer::configureInterfaces() +{ + yarp::os::Property options; + options.put("device", "navigation2D_nwc_yarp"); + options.put("local", "/" + m_name + "/navigation2D_nwc_yarp"); + options.put("navigation_server", "/navigation2D_nws_yarp"); + options.put("map_locations_server", "/map2D_nws_yarp"); + options.put("localization_server", "/localization2D_nws_yarp"); + + if (!m_ddNav.open(options)) + { + yError() << "Failed to open navigation2Dclient device"; + return false; + } + + if (!m_ddNav.view(m_localization)) + { + yError() << "Failed to get ILocalization2D interface"; + return false; + } + + if (!m_ddNav.view(m_navigation)) + { + yError() << "Failed to get INavigation2D interface"; + return false; + } + + return true; + +} + +bool RoamingServer::close() +{ + m_roaming_port.close(); + m_ddNav.close(); + return true; +} + +bool RoamingServer::updateModule() +{ + // Do something with m_navigation + if (checkRoamingCondition()) + { + std::string ap_name = getBestAP(); + if (ap_name != "") + { + yDebug() << "Roaming to AP" << ap_name; + if (!roam(ap_name)) + { + yError() << "Roaming to AP failed"; + } + } + } + + return true; +} + +bool RoamingServer::interruptModule() +{ + m_roaming_port.interrupt(); + return true; +} + +double RoamingServer::getPeriod() +{ + return 1.0; +} + +const double RoamingServer::distanceToAP(const std::string& ap_name) const { + + // Get AP position from map + std::optional ap_position = getApPosition(ap_name); + + // Calculate distance between robot and AP + if(!ap_position.has_value()) { + yError() << "AP" << ap_name << "not found in map"; + return false; + } + + double distance = sqrt(pow(m_robot_position.x - ap_position->x, 2) + pow(m_robot_position.y - ap_position->y, 2)); + yDebug() << "Distance to AP" << ap_name << "is" << distance;\ + + return distance; + +} + +bool RoamingServer::checkRoamingCondition(const double threshold) +{ + // Get current connected AP name + std::optional current_ap_name = getCurrentApName(); + + if(!current_ap_name.has_value()) + { + yInfo() << "No current connected AP. Are you connected to internet?"; + return true; + } + + // Get robot position from localization + m_localization->getCurrentPosition(m_robot_position); + + // Get AP position from map + double distance = distanceToAP(current_ap_name.value()); + + // If distance is greater than threshold, return true + if (distance > threshold) + { + return true; + } + + return false; +} + +const std::string RoamingServer::getBestAP() const { + + std::string best_ap = ""; + + auto current_ap_name = getCurrentApName(); + + double min_dist = -std::numeric_limits::infinity(); + + if(!current_ap_name.has_value()) + { + yWarning() << "No current connected AP. Are you connected to internet?"; + } + else + { + min_dist = distanceToAP(current_ap_name.value()); + } + + for (const auto& [ap_mac_addr,ap_location] : m_ap_to_location) { + if (double dist = distanceToAP(ap_mac_addr) < min_dist) { + best_ap = ap_mac_addr; + min_dist = dist; + } + } + + return best_ap; +} + + +bool RoamingServer::roam(const std::string &ap_name) +{ + if (checkRoamingCondition()) { + + yDebug() << "Roaming to AP" << ap_name; + + // Get next best AP from map + std::string next_ap = getBestAP(); + + // Roam to next AP + yarp::os::Bottle roaming_ap; + roaming_ap.addString(next_ap); + m_roaming_port.write(roaming_ap); + } + + return true; +} + +const std::optional RoamingServer::getApPosition(const std::string &ap_name) const +{ + + yDebug() << "getApPosition" << ap_name; + + // Get AP position from map + std::vector locations; + + if(!m_navigation) + { + yError() << "Navigation interface not available"; + return {}; + } + + for (const auto& [ap_location,ap_mac_address] : m_location_to_ap) + { + yDebug() << "AP location" << ap_location << "AP MAC address" << ap_mac_address; + + if(ap_mac_address == ap_name) + { + yarp::dev::Nav2D::Map2DLocation location; + m_navigation->getLocation(ap_location, location); + if(location.map_id == m_map_name) + { + return location; + } + } + } + + return {}; +} + +const std::optional RoamingServer::getCurrentApName() const +{ + + std::string current_ap_name; + + std::vector lines = m_net.getCurrentApName(); + + for(const auto& line: lines) + { + if(line.find("Access Point: Not-Associated") != std::string::npos) { + yWarning() << "You are not connected to the internet!"; + return {}; + } + if (line.find("Access Point") != std::string::npos) { + std::string::size_type start = line.find("Access Point:") + 14; + std::string::size_type end = line.find(" ", start); + current_ap_name = line.substr(start, end - start); + return current_ap_name; + } + } + + return {}; + +} + +const std::vector RoamingServer::getAPList() const { + + std::vector locations; + std::vector aps; + + // Get all locations from map + if(!m_navigation) + { + yError() << "Navigation interface is not available"; + throw std::runtime_error("Navigation interface is not available"); + } + + m_navigation->getLocationsList(locations); + + // For every location in the mapping, get the mac address and check that it's well defined + for (const auto& [ap_location,ap_mac_add]: m_location_to_ap) + { + if(std::find(locations.begin(),locations.end(),ap_location) != locations.end()) + { + if(this->isAP(ap_mac_add)) + { + aps.push_back(ap_mac_add); + } + } + } + + return aps; +} + +std::pair RoamingServer::getLocationAPMaps(std::ifstream& map_file) const +{ + LocationApMap location_to_ap; + LocationApMap ap_to_location; + + std::string line; + while (std::getline(map_file, line)) + { + std::string location; + std::string ap; + std::istringstream iss(line); + iss >> location >> ap; + location_to_ap.insert_or_assign(location,ap); + ap_to_location.insert_or_assign(ap,location); + } + + return {location_to_ap,ap_to_location}; +} + +const bool RoamingServer::isAP(const std::string &ap_name) const { + + bool is_ap = false; + + // MAC address has 17 characters + if(ap_name.size() != 17) + { + return false; + } + + // If it looks like a MAC address, then it is a MAC address + is_ap = ap_name[2] == ':'; + is_ap = ap_name[5] == ':'; + is_ap = ap_name[8] == ':'; + is_ap = ap_name[11] == ':'; + is_ap = ap_name[14] == ':'; + + return is_ap; +} \ No newline at end of file diff --git a/aux_modules/roaming/src/WpaSupplicantRoaming.cpp b/aux_modules/roaming/src/WpaSupplicantRoaming.cpp new file mode 100644 index 0000000..c3e8831 --- /dev/null +++ b/aux_modules/roaming/src/WpaSupplicantRoaming.cpp @@ -0,0 +1,58 @@ +#include +#include +#include +#include +#include + +bool WpaSupplicantRoaming::roam(const std::string& if_name, const std::string& ssid, const std::string& bssid) +{ + std::string wpa_if = "/var/run/wpa_supplicant/"+if_name; + auto* wpa_ctrl = wpa_ctrl_open(wpa_if.c_str()); + + if(!wpa_ctrl) + { + yError() << "Could not open network interface" << if_name << ", aborting."; + return false; + } + + //TODO: there might be a bug here if ssid contains spaces + const std::string bssid_cmd = "BSSID " + ssid + " " + bssid; + const std::string bssid_reply = wpa_request(wpa_ctrl,bssid_cmd); + + if(bssid_reply != "OK") + { + yError() << "CMD:" << bssid_cmd << "failed. Aborting."; + return false; + } + else + { + yDebug() << "CMD:" << bssid_cmd << "successful."; + } + + const std::string reassociate_cmd = "REASSOCIATE"; + const auto reassociate_reply = wpa_request(wpa_ctrl,reassociate_cmd); + if(reassociate_reply != "OK") + { + yError() << "CMD:" << reassociate_cmd << "failed. Aborting."; + return false; + } + else + { + yDebug() << "CMD:" << reassociate_cmd << "successful."; + } + + return true; +} + +std::string WpaSupplicantRoaming::wpa_request(wpa_ctrl* ctrl, const std::string& cmd) +{ + char buf[4096]; + std::size_t cmd_reply_len = sizeof(buf) - 1; + if(wpa_ctrl_request(ctrl,cmd.c_str(),cmd.size(),buf,&cmd_reply_len,NULL) != 0) + { + yError() << "Failed to send/receivce command to/from wpa_supplicant"; + return ""; + } + std::string cmd_reply(buf,cmd_reply_len-1); + return cmd_reply; +} \ No newline at end of file diff --git a/aux_modules/roaming/src/mainClient.cpp b/aux_modules/roaming/src/mainClient.cpp new file mode 100644 index 0000000..708fe1f --- /dev/null +++ b/aux_modules/roaming/src/mainClient.cpp @@ -0,0 +1,23 @@ +#include +#include +#include +#include + +int main(int argc, char *argv[]) +{ + yarp::os::Network yarp; + + yarp::os::ResourceFinder rf; + rf.configure(argc, argv); + + RoamingClient roaming; + yInfo() << "Configuring and starting module..."; + // This calls configure(rf) and, upon success, the module execution begins with a call to updateModule() + if (!roaming.runModule(rf)) + { + yError() << "Error module did not start!"; + } + + yDebug() << "Main returning..."; + return EXIT_SUCCESS; +} \ No newline at end of file diff --git a/aux_modules/roaming/src/mainServer.cpp b/aux_modules/roaming/src/mainServer.cpp new file mode 100644 index 0000000..2447508 --- /dev/null +++ b/aux_modules/roaming/src/mainServer.cpp @@ -0,0 +1,26 @@ +#include +#include +#include +#include +#include + +int main(int argc, char *argv[]) +{ + yarp::os::Network yarp; + + yarp::os::ResourceFinder rf; + rf.configure(argc, argv); + std::string name = rf.check("name") ? rf.find("name").asString() : "RoamingServer"; + + LinuxNetworkInteraction inet = LinuxNetworkInteraction(); + RoamingServer roaming(name,inet); + yInfo() << "Configuring and starting module..."; + // This calls configure(rf) and, upon success, the module execution begins with a call to updateModule() + if (!roaming.runModule(rf)) + { + yError() << "Error module did not start!"; + } + + yDebug() << "Main returning..."; + return EXIT_SUCCESS; +} \ No newline at end of file diff --git a/aux_modules/roaming/test/CMakeLists.txt b/aux_modules/roaming/test/CMakeLists.txt new file mode 100644 index 0000000..2025348 --- /dev/null +++ b/aux_modules/roaming/test/CMakeLists.txt @@ -0,0 +1,21 @@ +Include(FetchContent) + +FetchContent_Declare( + Catch2 + GIT_REPOSITORY https://github.com/catchorg/Catch2.git + GIT_TAG v3.4.0 # or a later release +) + +FetchContent_MakeAvailable(Catch2) + +project(roaming_test) +add_executable(${PROJECT_NAME}) +target_include_directories(${PROJECT_NAME} PUBLIC ../include) +target_include_directories(${PROJECT_NAME} PUBLIC .) +target_sources(${PROJECT_NAME} PRIVATE + test.cpp + ../src/RoamingServer.cpp + FakeNetworkInteraction.h + ../include/RoamingServer.h + ../include/INetworkInteraction.h) +target_link_libraries(${PROJECT_NAME} PRIVATE ${YARP_LIBRARIES} Catch2::Catch2WithMain) \ No newline at end of file diff --git a/aux_modules/roaming/test/FakeNetworkInteraction.h b/aux_modules/roaming/test/FakeNetworkInteraction.h new file mode 100644 index 0000000..79ece7e --- /dev/null +++ b/aux_modules/roaming/test/FakeNetworkInteraction.h @@ -0,0 +1,35 @@ +#include +#include +#include + +class FakeNetworkInteraction : public INetworkInteraction +{ + public: + FakeNetworkInteraction() {}; + std::vector getCurrentApName() + { + return m_data; + }; + void loadIwConfigDump(const std::string& file_path) + { + std::vector data; + + // Read the file and save it to data + std::ifstream file(file_path); + + std::string line; + while (std::getline(file, line)) + { + data.push_back(line); + } + + m_data = data; + } + bool roam(const std::string& ap_name) { + yInfo() << "Pretending to roam to " << ap_name; + return true; + } + + private: + std::vector m_data; +}; \ No newline at end of file diff --git a/aux_modules/roaming/test/data/iwconfig_dump_connected.txt b/aux_modules/roaming/test/data/iwconfig_dump_connected.txt new file mode 100644 index 0000000..a7a42b9 --- /dev/null +++ b/aux_modules/roaming/test/data/iwconfig_dump_connected.txt @@ -0,0 +1,9 @@ +wlp0s20f3 IEEE 802.11 ESSID:"WhatsGoingOn" + Mode:Managed Frequency:5.5 GHz Access Point: X1:0X:XX:X1:0X:00 + Bit Rate=866.7 Mb/s Tx-Power=20 dBm + Retry short limit:7 RTS thr:off Fragment thr:off + Power Management:on + Link Quality=54/70 Signal level=-56 dBm + Rx invalid nwid:0 Rx invalid crypt:0 Rx invalid frag:0 + Tx excessive retries:0 Invalid misc:2167 Missed beacon:0 + diff --git a/aux_modules/roaming/test/data/iwconfig_no_connection.txt b/aux_modules/roaming/test/data/iwconfig_no_connection.txt new file mode 100644 index 0000000..56c4329 --- /dev/null +++ b/aux_modules/roaming/test/data/iwconfig_no_connection.txt @@ -0,0 +1,5 @@ +wlp0s20f3 IEEE 802.11 ESSID:off/any + Mode:Managed Access Point: Not-Associated Tx-Power=off + Retry short limit:7 RTS thr:off Fragment thr:off + Power Management:on + diff --git a/aux_modules/roaming/test/data/locations_ap_map.txt b/aux_modules/roaming/test/data/locations_ap_map.txt new file mode 100644 index 0000000..4a6ea10 --- /dev/null +++ b/aux_modules/roaming/test/data/locations_ap_map.txt @@ -0,0 +1,2 @@ +area_1 X0:00:XX:X0:0X:00 +area_2 X1:0X:XX:X1:0X:00 \ No newline at end of file diff --git a/aux_modules/roaming/test/test.cpp b/aux_modules/roaming/test/test.cpp new file mode 100644 index 0000000..72151f2 --- /dev/null +++ b/aux_modules/roaming/test/test.cpp @@ -0,0 +1,199 @@ +#include +#include +#include +#include +#include +#include + +// #include + +#include + +#include + +#include + +void increment_tests_skipped(); + +// #define YARP_SKIP_TEST(...) \ +// { \ +// increment_tests_skipped(); \ +// FAIL(__VA_ARGS__); \ +// } + +// #define YARP_REQUIRE_PLUGIN(name, type) \ +// { \ +// bool has_plugin = yarp::os::YarpPluginSelector::checkPlugin(name, type); \ +// if (!has_plugin) { \ +// YARP_SKIP_TEST("Required plugin is missing: " << type << " - " << name); \ +// } \ +// } + +TEST_CASE("Roaming::functionalities", "[Roaming]") +{ + // YARP_REQUIRE_PLUGIN("fakeLocalizer", "device"); + // YARP_REQUIRE_PLUGIN("fakeNavigation", "device"); + // YARP_REQUIRE_PLUGIN("map2DStorage", "device"); + // YARP_REQUIRE_PLUGIN("navigation2D_nwc_yarp", "device"); + + // yarp::os::Network::setLocalMode(true); + + // Open the devices + + // Open fakeLocalizer and localizer nws + yarp::dev::PolyDriver dd_loc; + yarp::dev::PolyDriver dd_loc_nws; + + yarp::os::Property p_loc; + yarp::os::Property p_loc_nws; + p_loc.put("device", "fakeLocalizer"); + p_loc_nws.put("device", "localization2D_nws_yarp"); + + REQUIRE(dd_loc.open(p_loc)); + REQUIRE(dd_loc_nws.open(p_loc_nws)); + + // Attach fakelocalizer to localizer nws + yarp::dev::WrapperSingle* ww_nws_loc; + dd_loc_nws.view(ww_nws_loc); + REQUIRE(ww_nws_loc); + ww_nws_loc->attach(&dd_loc); + + // Open map server + yarp::dev::PolyDriver dd_map; + yarp::os::Property p_map; + p_map.put("device", "map2DStorage"); + REQUIRE(dd_map.open(p_map)); + + yarp::dev::PolyDriver dd_mapserver; + yarp::os::Property p_mapserver; + p_mapserver.put("device", "map2D_nws_yarp"); + REQUIRE(dd_mapserver.open(p_mapserver)); + + yarp::dev::WrapperSingle* ww_nws; + dd_mapserver.view(ww_nws); + REQUIRE(ww_nws); + ww_nws->attach(&dd_map); + + // Open fakeNavigation + yarp::dev::PolyDriver dd_nav; + yarp::os::Property p_nav; + p_nav.put("device", "fakeNavigation"); + REQUIRE(dd_nav.open(p_nav)); + + yarp::dev::PolyDriver dd_navserver; + yarp::os::Property p_navserver; + p_navserver.put("device", "navigation2D_nws_yarp"); + REQUIRE(dd_navserver.open(p_navserver)); + + yarp::dev::WrapperSingle* ww_nws_nav; + dd_navserver.view(ww_nws_nav); + REQUIRE(ww_nws_nav); + ww_nws_nav->attach(&dd_nav); + + // Set up fake AP location + yarp::dev::PolyDriver dd_nav_client; + yarp::os::Property options; + options.put("device", "navigation2D_nwc_yarp"); + options.put("local", "/roaming_tests/navigation2D_nwc_yarp"); + options.put("navigation_server", "/navigation2D_nws_yarp"); + options.put("map_locations_server", "/map2D_nws_yarp"); + options.put("localization_server", "/localization2D_nws_yarp"); + + yarp::dev::Nav2D::INavigation2D *m_navigation; + yarp::dev::Nav2D::ILocalization2D *m_localization; + + REQUIRE(dd_nav_client.open(options)); + + dd_nav_client.view(m_navigation); + dd_nav_client.view(m_localization); + + yarp::dev::Nav2D::Map2DLocation robot_loc; + m_localization->getCurrentPosition(robot_loc); //robot_loc.map_id = test + + std::cout << "fbrand: robot loc x" << robot_loc.x << "\n"; + std::cout << "fbrand: robot loc y" << robot_loc.y << "\n"; + + std::string fake_ap1_location{"area_1"}; //Just a fake MAC address + std::string fake_ap1_mac{"X0:00:XX:X0:0X:00"}; + yarp::dev::Nav2D::Map2DLocation ap_loc(robot_loc.map_id,1.0,0.0,0.0,"ap1"); + m_navigation->storeLocation(fake_ap1_location,ap_loc); //Just a fake MAC address + + std::string fake_ap2_location{"area_2"}; + std::string fake_ap2_mac{"X1:0X:XX:X1:0X:00"}; + yarp::dev::Nav2D::Map2DLocation ap2_loc(robot_loc.map_id,2.0,0.0,0.0,"ap2"); + m_navigation->storeLocation(fake_ap2_location,ap2_loc); + + // Instantiate the FakeNetworkInteraction to read iwconfig dump from files + auto inet = FakeNetworkInteraction(); + RoamingServer roaming("roaming",inet); + + SECTION("Configure roaming module") + { + yarp::os::ResourceFinder rf; + rf.setDefault("loc_to_ap_map","/workspaces/tour-guide-robot/aux_modules/roaming/test/data/locations_ap_map.txt"); + REQUIRE(roaming.configure(rf)); + } + + SECTION("Check isAP") + { + yarp::os::ResourceFinder rf; + rf.setDefault("loc_to_ap_map","/workspaces/tour-guide-robot/aux_modules/roaming/test/data/locations_ap_map.txt"); + REQUIRE(roaming.configure(rf)); + REQUIRE(roaming.isAP(fake_ap1_mac)); + } + + SECTION("Check AP location") + { + yarp::os::ResourceFinder rf; + rf.setDefault("loc_to_ap_map","/workspaces/tour-guide-robot/aux_modules/roaming/test/data/locations_ap_map.txt"); + REQUIRE(roaming.configure(rf)); + auto fake_ap_location = roaming.getApPosition(fake_ap1_mac); + REQUIRE(fake_ap_location.has_value()); + auto fake_ap_location_value = fake_ap_location.value(); + REQUIRE(fake_ap_location_value.x < 1.1); + REQUIRE(fake_ap_location_value.x > 0.9); //float comparison + REQUIRE(fake_ap_location_value.y < 0.1); + REQUIRE(fake_ap_location_value.y > -0.1); //float comparison + } + + SECTION("Check distance to AP") + { + yarp::os::ResourceFinder rf; + rf.setDefault("loc_to_ap_map","/workspaces/tour-guide-robot/aux_modules/roaming/test/data/locations_ap_map.txt"); + REQUIRE(roaming.configure(rf)); + REQUIRE(roaming.distanceToAP(fake_ap1_mac) < 1.1); + REQUIRE(roaming.distanceToAP(fake_ap1_mac) > 0.9); //float comparison + } + + SECTION("Check getCurrentAPName when iwconfig connected") + { + yarp::os::ResourceFinder rf; + rf.setDefault("loc_to_ap_map","/workspaces/tour-guide-robot/aux_modules/roaming/test/data/locations_ap_map.txt"); + REQUIRE(roaming.configure(rf)); + inet.loadIwConfigDump("/workspaces/tour-guide-robot/aux_modules/roaming/test/data/iwconfig_dump_connected.txt"); + bool check{roaming.getCurrentApName() == "X1:0X:XX:X1:0X:00"}; + REQUIRE(check); + } + + SECTION("Check getCurrentAPName when iwconfig disconnected") + { + yarp::os::ResourceFinder rf; + rf.setDefault("loc_to_ap_map","/workspaces/tour-guide-robot/aux_modules/roaming/test/data/locations_ap_map.txt"); + REQUIRE(roaming.configure(rf)); + inet.loadIwConfigDump("/workspaces/tour-guide-robot/aux_modules/roaming/test/data/iwconfig_no_connection.txt"); + bool check{!roaming.getCurrentApName().has_value()}; + REQUIRE(check); + } + + SECTION("Check getBestAp") + { + yarp::os::ResourceFinder rf; + rf.setDefault("loc_to_ap_map","/workspaces/tour-guide-robot/aux_modules/roaming/test/data/locations_ap_map.txt"); + REQUIRE(roaming.configure(rf)); + inet.loadIwConfigDump("/workspaces/tour-guide-robot/aux_modules/roaming/test/data/iwconfig_dump_connected.txt"); + bool check{roaming.getBestAP() == fake_ap1_mac}; + REQUIRE(check); + } + + // yarp::os::Network::setLocalMode(false); +} diff --git a/docker_stuff/docker_tourCore2/Dockerfile b/docker_stuff/docker_tourCore2/Dockerfile index 5451de9..dbebb48 100644 --- a/docker_stuff/docker_tourCore2/Dockerfile +++ b/docker_stuff/docker_tourCore2/Dockerfile @@ -12,7 +12,7 @@ ENV DOCKER_UPDATED_ON=06_06_2024 # Install essentials RUN apt-get update && apt-get install -y apt-utils software-properties-common sudo psmisc lsb-release protobuf-compiler libatlas-base-dev \ tmux nano geany vim wget curl build-essential git gitk cmake cmake-curses-gui autoconf xserver-xorg-video-dummy xserver-xorg-legacy \ - net-tools terminator libjpeg-dev ffmpeg apt-transport-https ca-certificates gnupg libace-dev ycm-cmake-modules locales \ + net-tools terminator libjpeg-dev ffmpeg apt-transport-https ca-certificates gnupg libace-dev ycm-cmake-modules locales wireless-tools \ python3-setuptools python3-pip iproute2 python3-tornado swig lsof iftop iputils-ping gdb bash-completion btop mlocate && rm -rf /var/lib/apt/lists/* RUN pip3 install numpy bpytop