diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 0000000..bd5588b --- /dev/null +++ b/.github/CODEOWNERS @@ -0,0 +1,2 @@ +# Default all changes will request review from: +* @clearpathrobotics/clearpath-platform-team diff --git a/.github/ISSUE_TEMPLATE/bug.md b/.github/ISSUE_TEMPLATE/bug.md new file mode 100644 index 0000000..9b69753 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug.md @@ -0,0 +1,31 @@ +--- +name: Bug Report +about: Provide a report for that the issue is +title: '' +labels: bug +assignees: clearpathrobotics/clearpath-platform-team + +--- + +**Please provide the following information:** + - OS: (e.g. Ubuntu 24.04) + - ROS 2 Distro: (e.g. Jazzy) + - Built from source or installed: + - Package version: (if from repository, give version from `sudo dpkg -s ros-$ROS_VERSION-wireless-watcher`, if from source, give commit hash) + + + **Expected behaviour** + A clear and concise description of what you expected to happen. + + **Actual behaviour** + A clear and concise description of what you encountered. + +**To Reproduce** +Provide the steps to reproduce: +1. run something +2. launch something else +3. see the error + + +**Other notes** +Add anything else you thing is important. diff --git a/.github/ISSUE_TEMPLATE/feature.md b/.github/ISSUE_TEMPLATE/feature.md new file mode 100644 index 0000000..f2d6d35 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature.md @@ -0,0 +1,14 @@ +--- +name: Feature request +about: Provide context for the feature you are requesting +title: '' +labels: enhancement +assignees: clearpathrobotics/clearpath-platform-team + +--- + +**Describe the the feature you would like** +A clear and concise description of what you want to happen. + +**Other notes** +Add anything else you thing is important. diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..aaa96d6 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,103 @@ +name: wireless_watcher_ci + +on: + push: + pull_request: + schedule: + - cron: "0 0 * * *" # every day at midnight + +jobs: + wireless_watcher_osrf_industrial_ci_humble: + name: Humble OSRF Industrial + strategy: + matrix: + env: + - {ROS_REPO: testing, ROS_DISTRO: humble} + - {ROS_REPO: main, ROS_DISTRO: humble} + fail-fast: false + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} + wireless_watcher_cpr_ci_humble: + name: Humble Clearpath Release + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: humble + - name: clearpath-package-server + run: | + sudo apt install wget + wget https://packages.clearpathrobotics.com/public.key -O - | sudo apt-key add - + sudo sh -c 'echo "deb https://packages.clearpathrobotics.com/stable/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/clearpath-latest.list' + sudo apt-get update + - uses: ros-tooling/action-ros-ci@v0.3 + id: action_ros_ci_step + with: + target-ros2-distro: humble + package-name: | + wireless_watcher + wireless_watcher_src_ci_humble: + name: Humble Clearpath Source + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: humble + - uses: ros-tooling/action-ros-ci@v0.3 + id: action_ros_ci_step + with: + target-ros2-distro: humble + package-name: | + wireless_watcher + wireless_watcher_osrf_industrial_ci_jazzy: + name: Jazzy OSRF Industrial + strategy: + matrix: + env: + - {ROS_REPO: testing, ROS_DISTRO: jazzy} + - {ROS_REPO: main, ROS_DISTRO: jazzy} + fail-fast: false + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v3 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} + wireless_watcher_cpr_ci_jazzy: + name: Jazzy Clearpath Release + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: jazzy + - name: clearpath-package-server + run: | + sudo apt install wget + wget https://packages.clearpathrobotics.com/public.key -O - | sudo apt-key add - + sudo sh -c 'echo "deb https://packages.clearpathrobotics.com/stable/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/clearpath-latest.list' + sudo apt-get update + - uses: ros-tooling/action-ros-ci@v0.3 + id: action_ros_ci_step + with: + target-ros2-distro: jazzy + package-name: | + wireless_watcher + wireless_watcher_src_ci_jazzy: + name: Jazzy Clearpath Source + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: jazzy + - uses: ros-tooling/action-ros-ci@v0.3 + id: action_ros_ci_step + with: + target-ros2-distro: jazzy + package-name: | + wireless_watcher diff --git a/wireless_msgs/package.xml b/wireless_msgs/package.xml index dcb6965..e53efab 100644 --- a/wireless_msgs/package.xml +++ b/wireless_msgs/package.xml @@ -5,13 +5,13 @@ 1.1.4 Messages for describing a wireless network such as bitrate, essid, and link quality. - Mike Purvis - Roni Kreinin Tony Baltovski - + BSD + Mike Purvis + ament_cmake std_msgs diff --git a/wireless_watcher/CHANGELOG.rst b/wireless_watcher/CHANGELOG.rst index dc1525a..4a5a484 100644 --- a/wireless_watcher/CHANGELOG.rst +++ b/wireless_watcher/CHANGELOG.rst @@ -105,4 +105,4 @@ Changelog for package wireless_watcher 0.0.1 (2013-10-17) ------------------ -* Catkinize wireless_watcher \ No newline at end of file +* Catkinize wireless_watcher diff --git a/wireless_watcher/CMakeLists.txt b/wireless_watcher/CMakeLists.txt index dd28399..861a7ce 100644 --- a/wireless_watcher/CMakeLists.txt +++ b/wireless_watcher/CMakeLists.txt @@ -45,5 +45,15 @@ install(FILES package.xml DESTINATION share/${PROJECT_NAME} ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_copyright + ament_cmake_cpplint + ament_cmake_uncrustify + ) + ament_lint_auto_find_test_dependencies() +endif() + # Specify install targets ament_package() diff --git a/wireless_watcher/include/wireless_watcher/wireless_watcher.hpp b/wireless_watcher/include/wireless_watcher/wireless_watcher.hpp index 0012928..5b3e951 100644 --- a/wireless_watcher/include/wireless_watcher/wireless_watcher.hpp +++ b/wireless_watcher/include/wireless_watcher/wireless_watcher.hpp @@ -33,12 +33,13 @@ * */ -#ifndef WIRELESS_WATCHER_HPP -#define WIRELESS_WATCHER_HPP +#ifndef WIRELESS_WATCHER__WIRELESS_WATCHER_HPP_ +#define WIRELESS_WATCHER__WIRELESS_WATCHER_HPP_ #include #include +#include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" #include "wireless_msgs/msg/connection.hpp" @@ -48,31 +49,32 @@ #define SIGNAL_STRENGTH_WEAK -67 #define SIGNAL_STRENGTH_VERY_WEAK -75 -class WirelessWatcher : public rclcpp::Node { +class WirelessWatcher : public rclcpp::Node +{ public: - WirelessWatcher(); + WirelessWatcher(); private: - // Parameters - double hz; - std::string dev; - std::string connected_topic; - std::string connection_topic; + // Parameters + double hz; + std::string dev; + std::string connected_topic; + std::string connection_topic; - // Other Variables - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr connected_pub_; - rclcpp::Publisher::SharedPtr connection_pub_; - std_msgs::msg::Bool connected_msg_; - wireless_msgs::msg::Connection connection_msg_; - diagnostic_updater::Updater updater_; + // Other Variables + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr connected_pub_; + rclcpp::Publisher::SharedPtr connection_pub_; + std_msgs::msg::Bool connected_msg_; + wireless_msgs::msg::Connection connection_msg_; + diagnostic_updater::Updater updater_; - // Methods - void timer_callback(); - std::string exec_cmd(const std::string& cmd); - std::vector split(const std::string& s, const std::string& delimiter); - void diagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat); - void ip_address_diag(std::string dev, diagnostic_updater::DiagnosticStatusWrapper & stat); + // Methods + void timer_callback(); + std::string exec_cmd(const std::string& cmd); + std::vector split(const std::string& s, const std::string& delimiter); + void diagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat); + void ip_address_diag(std::string dev, diagnostic_updater::DiagnosticStatusWrapper & stat); }; -#endif // WIRELESS_WATCHER_HPP \ No newline at end of file +#endif // WIRELESS_WATCHER__WIRELESS_WATCHER_HPP_ diff --git a/wireless_watcher/launch/watcher.launch.py b/wireless_watcher/launch/watcher.launch.py index 40b497f..c56748b 100644 --- a/wireless_watcher/launch/watcher.launch.py +++ b/wireless_watcher/launch/watcher.launch.py @@ -1,26 +1,29 @@ -# Copyright 2022 Clearpath Robotics, Inc. -# # Software License Agreement (BSD) # # @author Roni Kreinin # @copyright (c) 2022, Clearpath Robotics, Inc., All rights reserved. # -# Redistribution and use in source and binary forms, with or without modification, are permitted provided that -# the following conditions are met: -# * Redistributions of source code must retain the above copyright notice, this list of conditions and the -# following disclaimer. -# * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the -# following disclaimer in the documentation and/or other materials provided with the distribution. -# * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or -# promote products derived from this software without specific prior written permission. +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Clearpath Robotics nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. # -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED -# WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A -# PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR -# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -# TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -# NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. @@ -31,34 +34,35 @@ from launch_ros.actions import Node ARGUMENTS = [ - DeclareLaunchArgument('hz', default_value='1.0', - description='Update frequency'), - DeclareLaunchArgument('dev', default_value="''", - description='Wireless device'), - DeclareLaunchArgument('connected_topic', default_value='connected', - description='Connected status topic'), - DeclareLaunchArgument('connection_topic', default_value='connection', - description='Connection information topic'), - DeclareLaunchArgument('namespace', default_value='', - description='Namespace'), + DeclareLaunchArgument('hz', default_value='1.0', description='Update frequency'), + DeclareLaunchArgument('dev', default_value='', description='Wireless device'), + DeclareLaunchArgument( + 'connected_topic', default_value='connected', description='Connected status topic' + ), + DeclareLaunchArgument( + 'connection_topic', default_value='connection', description='Connection information topic' + ), + DeclareLaunchArgument('namespace', default_value='', description='Namespace'), ] def generate_launch_description(): watcher = Node( - package='wireless_watcher', - executable='wireless_watcher', - name='wireless_watcher', - namespace=LaunchConfiguration('namespace'), - output='screen', - parameters=[{ - 'hz': LaunchConfiguration('hz'), - 'dev': LaunchConfiguration('dev'), - 'connected_topic': LaunchConfiguration('connected_topic'), - 'connection_topic': LaunchConfiguration('connection_topic') - }], - ) + package='wireless_watcher', + executable='wireless_watcher', + name='wireless_watcher', + namespace=LaunchConfiguration('namespace'), + output='screen', + parameters=[ + { + 'hz': LaunchConfiguration('hz'), + 'dev': LaunchConfiguration('dev'), + 'connected_topic': LaunchConfiguration('connected_topic'), + 'connection_topic': LaunchConfiguration('connection_topic'), + } + ], + ) ld = LaunchDescription(ARGUMENTS) ld.add_action(watcher) diff --git a/wireless_watcher/package.xml b/wireless_watcher/package.xml index c4bc810..3c0c332 100644 --- a/wireless_watcher/package.xml +++ b/wireless_watcher/package.xml @@ -4,19 +4,22 @@ wireless_watcher 1.1.4 A node which publishes connection information about a linux wireless interface. - - Mike Purvis - - rkreinin + + Roni Kreinin Tony Baltovski BSD + Mike Purvis + diagnostic_updater rclcpp wireless_msgs wireless-tools + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/wireless_watcher/src/wireless_watcher.cpp b/wireless_watcher/src/wireless_watcher.cpp index e114d60..5f06f27 100644 --- a/wireless_watcher/src/wireless_watcher.cpp +++ b/wireless_watcher/src/wireless_watcher.cpp @@ -33,6 +33,8 @@ * */ +#include "wireless_watcher/wireless_watcher.hpp" + #include #include #include @@ -50,243 +52,279 @@ #include "diagnostic_updater/diagnostic_updater.hpp" -#include "wireless_watcher/wireless_watcher.hpp" - using namespace std::chrono_literals; -WirelessWatcher::WirelessWatcher() : rclcpp::Node("wireless_watcher"), updater_(this) { - this->declare_parameter("hz", 1.0); - this->declare_parameter("dev", ""); - this->declare_parameter("connected_topic", "connected"); - this->declare_parameter("connection_topic", "connection"); - - hz = this->get_parameter("hz").as_double(); - dev = this->get_parameter("dev").as_string(); - connected_topic = this->get_parameter("connected_topic").as_string(); - connection_topic = this->get_parameter("connection_topic").as_string(); - - if (dev.empty()) { - std::vector wldevs; - DIR *dir; - struct dirent *ent; - if ((dir = opendir(SYS_NET_PATH)) != NULL) { - while ((ent = readdir(dir)) != NULL) { - std::string dev_name = ent->d_name; - if (dev_name.compare(0, 2, "wl") == 0 || dev_name.compare(0, 4, "wifi") == 0) { - wldevs.push_back(dev_name); - } - } - closedir(dir); - } else { - RCLCPP_FATAL(this->get_logger(), "Failed to open directory: %s", SYS_NET_PATH); - return; - } - if (!wldevs.empty()) { - dev = wldevs[0]; - } else { - RCLCPP_FATAL(this->get_logger(), "No wireless device found to monitor."); - return; +WirelessWatcher::WirelessWatcher() : rclcpp::Node("wireless_watcher"), updater_(this) +{ + this->declare_parameter("hz", 1.0); + this->declare_parameter("dev", ""); + this->declare_parameter("connected_topic", "connected"); + this->declare_parameter("connection_topic", "connection"); + + hz = this->get_parameter("hz").as_double(); + dev = this->get_parameter("dev").as_string(); + connected_topic = this->get_parameter("connected_topic").as_string(); + connection_topic = this->get_parameter("connection_topic").as_string(); + + if (dev.empty()) + { + std::vector wldevs; + DIR *dir; + struct dirent *ent; + if ((dir = opendir(SYS_NET_PATH)) != NULL) + { + while ((ent = readdir(dir)) != NULL) + { + std::string dev_name = ent->d_name; + if (dev_name.compare(0, 2, "wl") == 0 || dev_name.compare(0, 4, "wifi") == 0) + { + wldevs.push_back(dev_name); } + } + closedir(dir); } + else + { + RCLCPP_FATAL(this->get_logger(), "Failed to open directory: %s", SYS_NET_PATH); + return; + } + if (!wldevs.empty()) + { + dev = wldevs[0]; + } + else + { + RCLCPP_FATAL(this->get_logger(), "No wireless device found to monitor."); + return; + } + } - RCLCPP_INFO(this->get_logger(), "Monitoring %s", dev.c_str()); + RCLCPP_INFO(this->get_logger(), "Monitoring %s", dev.c_str()); - connected_pub_ = this->create_publisher(connected_topic, rclcpp::SensorDataQoS()); - connection_pub_ = this->create_publisher(connection_topic, rclcpp::SensorDataQoS()); + connected_pub_ = this->create_publisher(connected_topic, rclcpp::SensorDataQoS()); + connection_pub_ = this->create_publisher(connection_topic, rclcpp::SensorDataQoS()); - timer_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(1000.0 / hz)), std::bind(&WirelessWatcher::timer_callback, this)); + timer_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(1000.0 / hz)), std::bind(&WirelessWatcher::timer_callback, this)); - updater_.setHardwareID(dev); - updater_.add("Wi-Fi Monitor", this, &WirelessWatcher::diagnostic); + updater_.setHardwareID(dev); + updater_.add("Wi-Fi Monitor", this, &WirelessWatcher::diagnostic); } -void WirelessWatcher::timer_callback() { - try { - std::string operstate_filepath = std::string(SYS_NET_PATH); - operstate_filepath += "/"; - operstate_filepath += dev; - operstate_filepath += "/operstate"; - std::ifstream operstate_file(operstate_filepath.c_str()); - std::string operstate; - operstate_file >> operstate; - connected_msg_.data = operstate == "up"; - } catch (const std::exception& e) { - connected_msg_.data = false; +void WirelessWatcher::timer_callback() +{ + try + { + std::string operstate_filepath = std::string(SYS_NET_PATH); + operstate_filepath += "/"; + operstate_filepath += dev; + operstate_filepath += "/operstate"; + std::ifstream operstate_file(operstate_filepath.c_str()); + std::string operstate; + operstate_file >> operstate; + connected_msg_.data = operstate == "up"; + } + catch (const std::exception& e) + { + connected_msg_.data = false; + } + connected_pub_->publish(connected_msg_); + + if (!connected_msg_.data) + { + return; + } + + std::string iwconfig_output = exec_cmd("iwconfig " + dev); + std::vector fields_str = split(iwconfig_output, "\\s\\s+"); + + std::unordered_map fields_dict; + fields_dict["dev"] = fields_str[0]; + fields_dict["type"] = fields_str[1]; + fields_dict["Access Point"] = split(fields_str[5], " ").back(); + + for (size_t i = 2; i < fields_str.size(); ++i) + { + std::vector field = split(fields_str[i], "[:=]"); + if (field.size() == 2) + { + fields_dict[field[0]] = field[1]; } - connected_pub_->publish(connected_msg_); + } - if (!connected_msg_.data) { - return; + if (fields_dict["Access Point"].find("Not-Associated") == std::string::npos) + { + try + { + connection_msg_.bitrate = std::stof(split(fields_dict["Bit Rate"], " ")[0]); } - - std::string iwconfig_output = exec_cmd("iwconfig " + dev); - std::vector fields_str = split(iwconfig_output, "\\s\\s+"); - - std::unordered_map fields_dict; - fields_dict["dev"] = fields_str[0]; - fields_dict["type"] = fields_str[1]; - fields_dict["Access Point"] = split(fields_str[5], " ").back(); - - for (size_t i = 2; i < fields_str.size(); ++i) { - std::vector field = split(fields_str[i], "[:=]"); - if (field.size() == 2) { - fields_dict[field[0]] = field[1]; - } + catch (std::invalid_argument) + { + connection_msg_.bitrate = std::numeric_limits::quiet_NaN(); } - if (fields_dict["Access Point"].find("Not-Associated") == std::string::npos) { - try - { - connection_msg_.bitrate = std::stof(split(fields_dict["Bit Rate"], " ")[0]); - } - catch(std::invalid_argument) - { - connection_msg_.bitrate = std::numeric_limits::quiet_NaN(); - } - - connection_msg_.txpower = std::stoi(split(fields_dict["Tx-Power"], " ")[0]); - connection_msg_.signal_level = std::stoi(split(fields_dict["Signal level"], " ")[0]); + connection_msg_.txpower = std::stoi(split(fields_dict["Tx-Power"], " ")[0]); + connection_msg_.signal_level = std::stoi(split(fields_dict["Signal level"], " ")[0]); - // Strip quotations from ESSID - std::string essid = fields_dict["ESSID"]; - essid.erase(std::remove(essid.begin(), essid.end(), '\"'), essid.end()); - connection_msg_.essid = essid; + // Strip quotations from ESSID + std::string essid = fields_dict["ESSID"]; + essid.erase(std::remove(essid.begin(), essid.end(), '\"'), essid.end()); + connection_msg_.essid = essid; - try - { - connection_msg_.frequency = std::stof(split(fields_dict["Frequency"], " ")[0]); - } - catch(std::invalid_argument) - { - connection_msg_.frequency = std::numeric_limits::quiet_NaN(); - } - - connection_msg_.bssid = fields_dict["Access Point"]; - - // Calculate link_quality from Link Quality - std::string link_quality_str = fields_dict["Link Quality"]; - connection_msg_.link_quality_raw = link_quality_str; - size_t delimiter_pos = link_quality_str.find("/"); - if (delimiter_pos != std::string::npos) { - int num = std::stoi(link_quality_str.substr(0, delimiter_pos)); - int den = std::stoi(link_quality_str.substr(delimiter_pos + 1)); - connection_msg_.link_quality = static_cast(num) / den; - } + try + { + connection_msg_.frequency = std::stof(split(fields_dict["Frequency"], " ")[0]); + } + catch (std::invalid_argument) + { + connection_msg_.frequency = std::numeric_limits::quiet_NaN(); + } - connection_pub_->publish(connection_msg_); + connection_msg_.bssid = fields_dict["Access Point"]; + + // Calculate link_quality from Link Quality + std::string link_quality_str = fields_dict["Link Quality"]; + connection_msg_.link_quality_raw = link_quality_str; + size_t delimiter_pos = link_quality_str.find("/"); + if (delimiter_pos != std::string::npos) + { + int num = std::stoi(link_quality_str.substr(0, delimiter_pos)); + int den = std::stoi(link_quality_str.substr(delimiter_pos + 1)); + connection_msg_.link_quality = static_cast(num) / den; } + + connection_pub_->publish(connection_msg_); + } } -std::string WirelessWatcher::exec_cmd(const std::string& cmd) { - std::array buffer; - std::string result; - std::unique_ptr pipe(popen(cmd.c_str(), "r"), pclose); - if (!pipe) { - throw std::runtime_error("popen() failed!"); - } - while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) { - result += buffer.data(); - } - return result; +std::string WirelessWatcher::exec_cmd(const std::string& cmd) +{ + std::array buffer; + std::string result; + std::unique_ptr pipe(popen(cmd.c_str(), "r"), pclose); + if (!pipe) + { + throw std::runtime_error("popen() failed!"); + } + while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) + { + result += buffer.data(); + } + return result; } -std::vector WirelessWatcher::split(const std::string& s, const std::string& delimiter) { - std::regex regex(delimiter); - std::sregex_token_iterator it(s.begin(), s.end(), regex, -1); - std::vector tokens{it, {}}; - return tokens; +std::vector WirelessWatcher::split(const std::string& s, const std::string& delimiter) +{ + std::regex regex(delimiter); + std::sregex_token_iterator it(s.begin(), s.end(), regex, -1); + std::vector tokens{it, {}}; + return tokens; } -void WirelessWatcher::diagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat) { - stat.add("Wireless Network Interface", dev); - stat.add("Wi-Fi Connected", connected_msg_.data ? "True" : "False"); +void WirelessWatcher::diagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.add("Wireless Network Interface", dev); + stat.add("Wi-Fi Connected", connected_msg_.data ? "True" : "False"); - if (!connected_msg_.data) { - stat.summaryf(diagnostic_updater::DiagnosticStatusWrapper::WARN, "%s Disconnected", dev.c_str()); - return; - } - stat.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "OK"); - - ip_address_diag(dev, stat); - stat.add("Frequency (GHz)", connection_msg_.frequency); - stat.add("ESSID", connection_msg_.essid); - stat.add("BSSID", connection_msg_.bssid); - stat.add("Transmit Power (dBm)", connection_msg_.txpower); - stat.add("Theoretical Max Bitrate (Mbps)", connection_msg_.bitrate); - stat.add("Link Quality Raw", connection_msg_.link_quality_raw); - stat.addf("Link Quality (%)", "%.1f", connection_msg_.link_quality * 100); - stat.add("Signal Strength (dBm)", connection_msg_.signal_level); - - if (connection_msg_.signal_level < SIGNAL_STRENGTH_VERY_WEAK) { - stat.mergeSummaryf(diagnostic_updater::DiagnosticStatusWrapper::WARN, - "Very Poor Signal Strength (%d dBm)", connection_msg_.signal_level); - } else if (connection_msg_.signal_level < SIGNAL_STRENGTH_WEAK) { - stat.mergeSummaryf(diagnostic_updater::DiagnosticStatusWrapper::WARN, - "Poor Signal Strength (%d dBm)", connection_msg_.signal_level); - } + if (!connected_msg_.data) + { + stat.summaryf(diagnostic_updater::DiagnosticStatusWrapper::WARN, "%s Disconnected", dev.c_str()); + return; + } + stat.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "OK"); + + ip_address_diag(dev, stat); + stat.add("Frequency (GHz)", connection_msg_.frequency); + stat.add("ESSID", connection_msg_.essid); + stat.add("BSSID", connection_msg_.bssid); + stat.add("Transmit Power (dBm)", connection_msg_.txpower); + stat.add("Theoretical Max Bitrate (Mbps)", connection_msg_.bitrate); + stat.add("Link Quality Raw", connection_msg_.link_quality_raw); + stat.addf("Link Quality (%)", "%.1f", connection_msg_.link_quality * 100); + stat.add("Signal Strength (dBm)", connection_msg_.signal_level); + + if (connection_msg_.signal_level < SIGNAL_STRENGTH_VERY_WEAK) + { + stat.mergeSummaryf(diagnostic_updater::DiagnosticStatusWrapper::WARN, + "Very Poor Signal Strength (%d dBm)", connection_msg_.signal_level); + } + else if (connection_msg_.signal_level < SIGNAL_STRENGTH_WEAK) + { + stat.mergeSummaryf(diagnostic_updater::DiagnosticStatusWrapper::WARN, + "Poor Signal Strength (%d dBm)", connection_msg_.signal_level); + } } void WirelessWatcher::ip_address_diag( - std::string dev, diagnostic_updater::DiagnosticStatusWrapper & stat) { - - struct ifaddrs *ptr_ifaddrs = nullptr, *entry; - - if (getifaddrs(&ptr_ifaddrs) == 0) { - for (entry = ptr_ifaddrs; entry != nullptr; entry = entry->ifa_next) { - // Find the requested interface and ensure it has an address - if (std::string(entry->ifa_name) != dev || entry->ifa_addr == nullptr) { - continue; - } - - sa_family_t address_family = entry->ifa_addr->sa_family; - // Skip if the address is not IPv4 - if (address_family != AF_INET) { - continue; - } - char buffer[INET_ADDRSTRLEN] = {}; - inet_ntop( - address_family, - &((struct sockaddr_in*)(entry->ifa_addr))->sin_addr, - buffer, - INET_ADDRSTRLEN - ); - - stat.add("IP Address", std::string(buffer)); - - if (entry->ifa_netmask != nullptr) { - char buffer[INET_ADDRSTRLEN] = {0, }; - inet_ntop( - address_family, - &((struct sockaddr_in*)(entry->ifa_netmask))->sin_addr, - buffer, - INET_ADDRSTRLEN - ); - - stat.add("Netmask", std::string(buffer)); - } else { - stat.add("Netmask", "Not found"); - } - - freeifaddrs(ptr_ifaddrs); - return; - } + std::string dev, diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + + struct ifaddrs *ptr_ifaddrs = nullptr, *entry; + + if (getifaddrs(&ptr_ifaddrs) == 0) + { + for (entry = ptr_ifaddrs; entry != nullptr; entry = entry->ifa_next) + { + // Find the requested interface and ensure it has an address + if (std::string(entry->ifa_name) != dev || entry->ifa_addr == nullptr) + { + continue; + } + + sa_family_t address_family = entry->ifa_addr->sa_family; + // Skip if the address is not IPv4 + if (address_family != AF_INET) + { + continue; + } + char buffer[INET_ADDRSTRLEN] = {}; + inet_ntop( + address_family, + &((struct sockaddr_in*)(entry->ifa_addr))->sin_addr, + buffer, + INET_ADDRSTRLEN + ); + + stat.add("IP Address", std::string(buffer)); + + if (entry->ifa_netmask != nullptr) + { + char buffer[INET_ADDRSTRLEN] = {0, }; + inet_ntop( + address_family, + &((struct sockaddr_in*)(entry->ifa_netmask))->sin_addr, + buffer, + INET_ADDRSTRLEN + ); + + stat.add("Netmask", std::string(buffer)); + } + else + { + stat.add("Netmask", "Not found"); + } + + freeifaddrs(ptr_ifaddrs); + return; } - stat.mergeSummaryf(diagnostic_updater::DiagnosticStatusWrapper::WARN, - "Failed to get IP addresses for %s", dev.c_str()); - stat.add("IP Address", "Not found"); - stat.add("Netmask", "Not found"); - - if (ptr_ifaddrs != nullptr) { - freeifaddrs(ptr_ifaddrs); - } - return; + } + stat.mergeSummaryf(diagnostic_updater::DiagnosticStatusWrapper::WARN, + "Failed to get IP addresses for %s", dev.c_str()); + stat.add("IP Address", "Not found"); + stat.add("Netmask", "Not found"); + + if (ptr_ifaddrs != nullptr) + { + freeifaddrs(ptr_ifaddrs); + } + return; } -int main(int argc, char * argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; }