82 lines
2.2 KiB
C++
82 lines
2.2 KiB
C++
#include "ros/ros.h"
|
|
#include "wlan_pioneer/WlanSignalMsg.h"
|
|
#include <iostream>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <linux/wireless.h>
|
|
#include <sys/types.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/socket.h>
|
|
#include <cstring>
|
|
|
|
using namespace std;
|
|
using namespace ros;
|
|
using namespace wlan_pioneer;
|
|
|
|
|
|
int64_t getWlanSignalStrength(const string &interface) {
|
|
int sockfd;
|
|
struct iw_statistics stats;
|
|
struct iwreq req;
|
|
memset(&stats, 0, sizeof(stats));
|
|
memset(&req, 0, sizeof(iwreq));
|
|
strncpy(req.ifr_name, interface.c_str(), 16);
|
|
req.u.data.pointer = &stats;
|
|
req.u.data.length = sizeof(iw_statistics);
|
|
#ifdef CLEAR_UPDATED
|
|
req.u.data.flags = 1;
|
|
#endif
|
|
|
|
if((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
|
|
ROS_ERROR("Could not create simple datagram socket");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
if(ioctl(sockfd, SIOCGIWSTATS, &req) == -1) {
|
|
ROS_ERROR("Error performing SIOCGIWSTATS");
|
|
close(sockfd);
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
close(sockfd);
|
|
return (int8_t)stats.qual.level;
|
|
}
|
|
|
|
|
|
int main(int argc, char **argv) {
|
|
init(argc, argv, "wlanSignal");
|
|
NodeHandle node;
|
|
string wlanInterface24G;
|
|
string wlanInterface5G;
|
|
|
|
if (node.param<string>("wlan_interface_24G", wlanInterface24G, "wlan0")) {
|
|
ROS_INFO("Param wlan_interface_24G: %s", wlanInterface24G.c_str());
|
|
}
|
|
else {
|
|
ROS_INFO("Param 'wlan_interface_24G' not set. Using 'wlan0'");
|
|
}
|
|
|
|
if (node.param<string>("wlan_interface_5G", wlanInterface5G, "wlan0")) {
|
|
ROS_INFO("Param wlan_interface_5G: %s", wlanInterface5G.c_str());
|
|
}
|
|
else {
|
|
ROS_INFO("Param 'wlan_interface_5G' not set. Using 'wlan0'");
|
|
}
|
|
|
|
// topic: wlan_signal
|
|
Publisher publisher = node.advertise<WlanSignalMsg>("wlan_signal", 1000);
|
|
Rate loopRate(1); // every second
|
|
|
|
while (ok()) {
|
|
WlanSignalMsg msg;
|
|
msg.timestamp = Time::now();
|
|
msg.level_24G = getWlanSignalStrength(wlanInterface24G);
|
|
msg.level_5G = getWlanSignalStrength(wlanInterface5G);
|
|
|
|
ROS_INFO("Signal strength 2.4G: %li Signal strength 5G: %li", msg.level_24G, msg.level_5G);
|
|
|
|
publisher.publish(msg);
|
|
spinOnce();
|
|
loopRate.sleep();
|
|
}
|
|
return 0;
|
|
}
|