diff --git a/ros2-network-analysis/CMakeLists.txt b/ros2-network-analysis/CMakeLists.txt index 8e88771..76c97ae 100644 --- a/ros2-network-analysis/CMakeLists.txt +++ b/ros2-network-analysis/CMakeLists.txt @@ -18,22 +18,16 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(ros2_network_analysis_interface REQUIRED) find_package(ament_cmake_python REQUIRED) ## System dependencies are found with CMake's conventions find_package(Boost REQUIRED COMPONENTS system) -find_package(rosidl_default_generators REQUIRED) +# find_package(rosidl_default_generators REQUIRED) find_package(rclpy REQUIRED) -rosidl_generate_interfaces( ${PROJECT_NAME} - "msg/WirelessLink.msg" - "msg/LinkUtilization.msg" - "msg/NetworkErrors.msg" - "msg/NetworkDelay.msg" - "msg/NetworkAnalysis.msg" - "msg/WirelessLinkVector.msg" - "action/Ping.srv" - DEPENDENCIES builtin_interfaces std_msgs - ) +# rosidl_generate_interfaces( ${PROJECT_NAME} +# DEPENDENCIES builtin_interfaces std_msgs +# ) ament_export_dependencies(rosidl_default_runtime) @@ -43,11 +37,11 @@ ament_export_dependencies(rosidl_default_runtime) add_executable(network_ping_client src/ping_client.cpp) # add_dependencies(${target}${target_suffix} ${PROJECT_NAME}) -ament_target_dependencies(network_ping_client rclcpp std_msgs) +ament_target_dependencies(network_ping_client rclcpp std_msgs ros2_network_analysis_interface) add_executable(network_ping_service src/ping_service.cpp) # add_dependencies(${target}${target_suffix} ${PROJECT_NAME}) -ament_target_dependencies(network_ping_service rclcpp std_msgs) +ament_target_dependencies(network_ping_service rclcpp std_msgs ros2_network_analysis_interface) install(TARGETS @@ -55,11 +49,11 @@ network_ping_client network_ping_service DESTINATION lib/${PROJECT_NAME}) -rosidl_target_interfaces(network_ping_client - ${PROJECT_NAME} "rosidl_typesupport_cpp") +# rosidl_target_interfaces(network_ping_client +# ${PROJECT_NAME} "rosidl_typesupport_cpp") -rosidl_target_interfaces(network_ping_service - ${PROJECT_NAME} "rosidl_typesupport_cpp") +# rosidl_target_interfaces(network_ping_service +# ${PROJECT_NAME} "rosidl_typesupport_cpp") ament_python_install_package(${PROJECT_NAME}) diff --git a/ros2-network-analysis/package.xml b/ros2-network-analysis/package.xml index 243a9c2..364e228 100644 --- a/ros2-network-analysis/package.xml +++ b/ros2-network-analysis/package.xml @@ -13,11 +13,12 @@ rclcpp rclpy std_msgs + ros2_network_analysis_interface - rosidl_default_generators + ament_lint_auto ament_lint_common diff --git a/ros2-network-analysis/scripts/link_utilization.py b/ros2-network-analysis/scripts/link_utilization.py index 62a6b7c..eac614a 100755 --- a/ros2-network-analysis/scripts/link_utilization.py +++ b/ros2-network-analysis/scripts/link_utilization.py @@ -5,7 +5,8 @@ import rclpy from subprocess import Popen, PIPE -from ros2_network_analysis.msg import LinkUtilization +from ros2_network_analysis_interface.msg import LinkUtilization +from rcl_interfaces.msg import ParameterDescriptor, ParameterType import std_msgs.msg import threading @@ -37,17 +38,29 @@ def getparameters(): def linkutilization_publisher(): node = rclpy.create_node('link_utilization') + node.declare_parameter('interface_name', + value="wlan0", + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description="The Wi-Fi interface id (e.g. wlan0)", + ), + ) + node.declare_parameter('update_rate_link_utilization', + value=1.0, + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_DOUBLE, + description="Update frequency in Hz. Note: more than 1 Hz will not be effective in throughput calculation", + ), + ) spin_thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True) spin_thread.start() - interfacename = node.get_parameter_or('~INTERFACE_NAME', 'wlan0') # The Wi-Fi interface id wlan0 - if type(interfacename) == "Parameter": - interfacename = interfacename.value - updaterate = node.get_parameter_or('~update_rate_link_utilization', 1) # Update frequency in Hz. Note: more than 1 Hz will not be effective in throughput calculation. - if type(updaterate) == "Parameter": - updaterate = updaterate.value - if updaterate <= 0: - node.get_logger().error("Update rate should be greater than 0. Setting to 10 hz") - updaterate = 10 + interfacename = node.get_parameter("interface_name").get_parameter_value().string_value # The Wi-Fi interface id wlan0 + node.get_logger().info(f"Wi-Fi Interface Name: {interfacename}") + updaterate = node.get_parameter("update_rate_link_utilization").get_parameter_value().double_value # Update frequency in Hz. Note: more than 1 Hz will not be effective in throughput calculation. + node.get_logger().info(f"Update Frequency: {updaterate} Hz") + if updaterate <= 0: + node.get_logger().error("Update rate should be greater than 0. Setting to 10 hz") + updaterate = 10 global cmd,cmd_netstat_tcp,cmd_netstat_udp,msg cmd ="cat /proc/net/dev | grep " + interfacename cmd_netstat_tcp = "cat /proc/net/snmp | grep Tcp:" @@ -73,7 +86,7 @@ def linkutilization_publisher(): rate.sleep() fout = getparameters() if (fout == 0): - print("The interface %s does not exist or is disconnected",interfacename) + print(f"The interface {interfacename} does not exist or is disconnected") continue msg.total_tx_mbps = (8 * (msg.total_tx_bytes - previous_total_tx_bytes) / float(1000*1000)) msg.total_rx_mbps = (8 * (msg.total_rx_bytes - previous_total_rx_bytes) / float(1000*1000)) diff --git a/ros2-network-analysis/scripts/network_errors.py b/ros2-network-analysis/scripts/network_errors.py index d3e1831..2807974 100755 --- a/ros2-network-analysis/scripts/network_errors.py +++ b/ros2-network-analysis/scripts/network_errors.py @@ -5,27 +5,38 @@ from subprocess import Popen, PIPE import rclpy -from ros2_network_analysis.msg import NetworkErrors +from ros2_network_analysis_interface.msg import NetworkErrors +from rcl_interfaces.msg import ParameterDescriptor, ParameterType import std_msgs.msg import threading def network_errors_publisher(): node = rclpy.create_node('NetworkErrors_publisher') + node.declare_parameter('interface_name', + value="wlan0", + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description="The Wi-Fi interface id (e.g. wlan0)", + ), + ) + node.declare_parameter('update_rate_network_errors', + value=1.0, + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_DOUBLE, + description="Update frequency in Hz. Note: more than 1 Hz will not be effective in throughput calculation", + ), + ) spin_thread = threading.Thread( target=rclpy.spin, args=(node, ), daemon=True) spin_thread.start() - interfacename = node.get_parameter_or( - '~INTERFACE_NAME', 'wlan0') - if type(interfacename) == "Parameter": - interfacename = interfacename.value - # Update frequency in Hz. - updaterate = node.get_parameter_or('~update_rate_network_errors', 1) - if type(updaterate) == "Parameter": - updaterate = updaterate.value - if updaterate <= 0: - node.get_logger().error("Update rate should be greater than 0. Setting to 10 hz") - updaterate = 10 + interfacename = node.get_parameter("interface_name").get_parameter_value().string_value # The Wi-Fi interface id wlan0 + node.get_logger().info(f"Wi-Fi Interface Name: {interfacename}") + updaterate = node.get_parameter("update_rate_network_errors").get_parameter_value().double_value # Update frequency in Hz. Note: more than 1 Hz will not be effective in throughput calculation. + node.get_logger().info(f"Update Frequency: {updaterate} Hz") + if updaterate <= 0: + node.get_logger().error("Update rate should be greater than 0. Setting to 10 hz") + updaterate = 10 pub = node.create_publisher( NetworkErrors, 'network_analysis/network_errors', 10) msg = NetworkErrors() diff --git a/ros2-network-analysis/scripts/wireless_quality.py b/ros2-network-analysis/scripts/wireless_quality.py index 373cd7a..0cbd8a0 100755 --- a/ros2-network-analysis/scripts/wireless_quality.py +++ b/ros2-network-analysis/scripts/wireless_quality.py @@ -5,7 +5,8 @@ import os import rclpy -from ros2_network_analysis.msg import WirelessLink +from ros2_network_analysis_interface.msg import WirelessLink +from rcl_interfaces.msg import ParameterDescriptor, ParameterType import std_msgs.msg import threading @@ -25,9 +26,9 @@ def get_rssi_from_os(interface_name, node): cmd_output = f.read() ans = cmd_output.split() - msg.rssi = int(ans[3]) # there is a problem here when i use my interface (wlp0s20f3). Not sure what it is though + msg.rssi = int(float(ans[3])) # there is a problem here when i use my interface (wlp0s20f3). Not sure what it is though msg.lqi = float(ans[2]) - msg.noise = float(ans[4]) + msg.noise = int(ans[4]) msg.iface = interface_name msg.status = True @@ -42,20 +43,32 @@ def get_rssi_from_os(interface_name, node): if __name__ == '__main__': rclpy.init() node = rclpy.create_node('wireless_quality') + node.declare_parameter('interface_name', + value="wlan0", + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description="The Wi-Fi interface id (e.g. wlan0)", + ), + ) + node.declare_parameter('update_rate_wireless_quality', + value=1.0, + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_DOUBLE, + description="Update frequency in Hz. Note: more than 1 Hz will not be effective in throughput calculation", + ), + ) spin_thread = threading.Thread( target=rclpy.spin, args=(node, ), daemon=True) spin_thread.start() - interfacename = node.get_parameter_or('~INTERFACE_NAME', 'wlan0wlp0s20f3') - if type(interfacename) == "Parameter": - interfacename = interfacename.value - update_rate = node.get_parameter_or( - '~update_rate_wireless_quality', 10) - if type(update_rate) == "Parameter": - update_rate = update_rate.value - if update_rate <= 0: - node.get_logger().error( - "Update rate must be greater than 0. Setting to 10 Hz") - update_rate = 10 + interfacename = node.get_parameter("interface_name").get_parameter_value().string_value # The Wi-Fi interface id wlan0 + node.get_logger().info(f"Wi-Fi Interface Name: {interfacename}") + update_rate = node.get_parameter("update_rate_wireless_quality").get_parameter_value().double_value # Update frequency in Hz. Note: more than 1 Hz will not be effective in throughput calculation. + node.get_logger().info(f"Update Frequency: {update_rate} Hz") + + if update_rate <= 0: + node.get_logger().error( + "Update rate must be greater than 0. Setting to 10 Hz") + update_rate = 10 pub_rssi = node.create_publisher( WirelessLink, 'network_analysis/wireless_quality', 10) diff --git a/ros2-network-analysis/src/ping_client.cpp b/ros2-network-analysis/src/ping_client.cpp index fe0b5c9..7c92805 100644 --- a/ros2-network-analysis/src/ping_client.cpp +++ b/ros2-network-analysis/src/ping_client.cpp @@ -1,7 +1,7 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -30,10 +30,10 @@ int main(int argc, char *argv[]) return 0; } - auto client = node->create_client("network_analysis/ping"); - auto request = std::make_shared(); - ros2_network_analysis::msg::NetworkDelay msg; - auto delayPub = node->create_publisher("network_analysis/network_delay", 10); + auto client = node->create_client("network_analysis/ping"); + auto request = std::make_shared(); + ros2_network_analysis_interface::msg::NetworkDelay msg; + auto delayPub = node->create_publisher("network_analysis/network_delay", 10); request->inp = true; rclcpp::Rate rate(updateRate); while (!client->wait_for_service(std::chrono::seconds(1))) diff --git a/ros2-network-analysis/src/ping_service.cpp b/ros2-network-analysis/src/ping_service.cpp index d9d0c57..8d91cea 100644 --- a/ros2-network-analysis/src/ping_service.cpp +++ b/ros2-network-analysis/src/ping_service.cpp @@ -1,12 +1,12 @@ #include -#include -#include +#include +#include -void respond(std::shared_ptr request, - std::shared_ptr response) +void respond(std::shared_ptr request, + std::shared_ptr response) { RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ping service received request"); if(request->inp) @@ -25,7 +25,7 @@ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("ping_service"); - auto service = node->create_service("network_analysis/ping", &respond); + auto service = node->create_service("network_analysis/ping", &respond); while(rclcpp::ok()){ rclcpp::spin_some(node); } diff --git a/ros2_network_analysis_interface/CMakeLists.txt b/ros2_network_analysis_interface/CMakeLists.txt new file mode 100644 index 0000000..44d24de --- /dev/null +++ b/ros2_network_analysis_interface/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.5) +project(ros2_network_analysis_interface) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(std_msgs REQUIRED) + +find_package(rosidl_default_generators REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/LinkUtilization.msg" + "msg/NetworkAnalysis.msg" + "msg/NetworkDelay.msg" + "msg/NetworkErrors.msg" + "msg/WirelessLink.msg" + "msg/WirelessLinkVector.msg" + "action/Ping.srv" + DEPENDENCIES std_msgs + ) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ros2-network-analysis/action/Ping.srv b/ros2_network_analysis_interface/action/Ping.srv similarity index 100% rename from ros2-network-analysis/action/Ping.srv rename to ros2_network_analysis_interface/action/Ping.srv diff --git a/ros2-network-analysis/msg/LinkUtilization.msg b/ros2_network_analysis_interface/msg/LinkUtilization.msg similarity index 100% rename from ros2-network-analysis/msg/LinkUtilization.msg rename to ros2_network_analysis_interface/msg/LinkUtilization.msg diff --git a/ros2-network-analysis/msg/NetworkAnalysis.msg b/ros2_network_analysis_interface/msg/NetworkAnalysis.msg similarity index 100% rename from ros2-network-analysis/msg/NetworkAnalysis.msg rename to ros2_network_analysis_interface/msg/NetworkAnalysis.msg diff --git a/ros2-network-analysis/msg/NetworkDelay.msg b/ros2_network_analysis_interface/msg/NetworkDelay.msg similarity index 100% rename from ros2-network-analysis/msg/NetworkDelay.msg rename to ros2_network_analysis_interface/msg/NetworkDelay.msg diff --git a/ros2-network-analysis/msg/NetworkErrors.msg b/ros2_network_analysis_interface/msg/NetworkErrors.msg similarity index 100% rename from ros2-network-analysis/msg/NetworkErrors.msg rename to ros2_network_analysis_interface/msg/NetworkErrors.msg diff --git a/ros2-network-analysis/msg/WirelessLink.msg b/ros2_network_analysis_interface/msg/WirelessLink.msg similarity index 100% rename from ros2-network-analysis/msg/WirelessLink.msg rename to ros2_network_analysis_interface/msg/WirelessLink.msg diff --git a/ros2-network-analysis/msg/WirelessLinkVector.msg b/ros2_network_analysis_interface/msg/WirelessLinkVector.msg similarity index 100% rename from ros2-network-analysis/msg/WirelessLinkVector.msg rename to ros2_network_analysis_interface/msg/WirelessLinkVector.msg diff --git a/ros2_network_analysis_interface/package.xml b/ros2_network_analysis_interface/package.xml new file mode 100644 index 0000000..74e7100 --- /dev/null +++ b/ros2_network_analysis_interface/package.xml @@ -0,0 +1,26 @@ + + + + ros2_network_analysis_interface + 0.1.0 + Custom messages for ros2_aruco + Nathan Sprague + TODO: License declaration + + ament_cmake + + std_msgs + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + +