i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-10 05:20:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed Windows build, implemented wifi mapping example in Windows

Browse Source
This commit is contained in:
Mathieu Labbé
2015-03-25 16:30:05 -04:00
parent 19d6da9035
commit 2cc853b507
9 changed files with 208 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/CMakeLists.txt
+3 -3
View File
@@ -4,10 +4,10 @@ ADD_SUBDIRECTORY( BOWMapping )
IF(TARGET rtabmap_gui)
ADD_SUBDIRECTORY( RGBDMapping )
# Only on Linux
IF(NOT WIN32 AND NOT APPLE)
# Only on Linux and Windows
IF(NOT APPLE)
ADD_SUBDIRECTORY( WifiMapping )
ENDIF(NOT WIN32 AND NOT APPLE)
ENDIF(NOT APPLE)
ELSE()
MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping and WifiMapping examples will not be built...")
examples/WifiMapping/MapBuilderWifi.h
+20 -5
View File
@@ -32,6 +32,24 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
using namespace rtabmap;
// A percentage value that represents the signal quality
// of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
// This member contains a value between 0 and 100. A value
// of 0 implies an actual RSSI signal strength of -100 dbm.
// A value of 100 implies an actual RSSI signal strength of -50 dbm.
// You can calculate the RSSI signal strength value for wlanSignalQuality
// values between 1 and 99 using linear interpolation.
inline int dBm2Quality(int dBm)
{
// dBm to Quality:
if(dBm <= -100)
return 0;
else if(dBm >= -50)
return 100;
else
return 2 * (dBm + 100);
}
class MapBuilderWifi : public MapBuilder
{
Q_OBJECT
@@ -124,10 +142,7 @@ protected slots:
else
{
// Make a line with points
int level = -iter->second.first;
level = level < 30 ? 0 : level > 80 ? 50: level - 30; // set between 0 and 50
level/=5; // set between 0 and 10
level = 10 - level; // make higher level to 10
int quality = dBm2Quality(iter->second.first)/10;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
for(int i=0; i<10; ++i)
{
@@ -135,7 +150,7 @@ protected slots:
// the number of points depends on the dBm (which varies from -30 (near) to -80 (far))
pcl::PointXYZRGB pt;
pt.z = float(i+1)*0.02f;
if(i<level)
if(i<quality)
{
// yellow
pt.r = 255;
examples/WifiMapping/WifiThread.h
+127 -12
View File
@@ -28,12 +28,50 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef WIFITHREAD_H_
#define WIFITHREAD_H_
#include <sys/socket.h>
#include <linux/wireless.h>
#include <sys/ioctl.h>
#ifdef _WIN32
#ifndef UNICODE
#define UNICODE
#endif
#include <windows.h>
#include <wlanapi.h>
#include <Windot11.h> // for DOT11_SSID struct
#include <objbase.h>
#include <wtypes.h>
#include <stdio.h>
#include <stdlib.h>
// Need to link with Wlanapi.lib and Ole32.lib
#pragma comment(lib, "wlanapi.lib")
#pragma comment(lib, "ole32.lib")
#else
#include <sys/socket.h>
#include <linux/wireless.h>
#include <sys/ioctl.h>
#endif
#include <rtabmap/core/UserDataEvent.h>
#include <rtabmap/utilite/UTimer.h>
// A percentage value that represents the signal quality
// of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
// This member contains a value between 0 and 100. A value
// of 0 implies an actual RSSI signal strength of -100 dbm.
// A value of 100 implies an actual RSSI signal strength of -50 dbm.
// You can calculate the RSSI signal strength value for wlanSignalQuality
// values between 1 and 99 using linear interpolation.
inline int quality2dBm(int quality)
{
// Quality to dBm:
if(quality <= 0)
return -100;
else if(quality >= 100)
return -50;
else
return (quality / 2) - 100;
}
class WifiThread : public UThread, public UEventsSender
{
public:
@@ -49,6 +87,79 @@ private:
uSleep(1000/rate_);
if(!this->isKilled())
{
int dBm = 0;
#ifdef _WIN32
//From https://msdn.microsoft.com/en-us/library/windows/desktop/ms706765(v=vs.85).aspx
// Declare and initialize variables.
HANDLE hClient = NULL;
DWORD dwMaxClient = 2; //
DWORD dwCurVersion = 0;
DWORD dwResult = 0;
// variables used for WlanEnumInterfaces
PWLAN_INTERFACE_INFO_LIST pIfList = NULL;
PWLAN_INTERFACE_INFO pIfInfo = NULL;
// variables used for WlanQueryInterfaces for opcode = wlan_intf_opcode_current_connection
PWLAN_CONNECTION_ATTRIBUTES pConnectInfo = NULL;
DWORD connectInfoSize = sizeof(WLAN_CONNECTION_ATTRIBUTES);
WLAN_OPCODE_VALUE_TYPE opCode = wlan_opcode_value_type_invalid;
dwResult = WlanOpenHandle(dwMaxClient, NULL, &dwCurVersion, &hClient);
if (dwResult != ERROR_SUCCESS)
{
UERROR("WlanOpenHandle failed with error: %u\n", dwResult);
}
else
{
dwResult = WlanEnumInterfaces(hClient, NULL, &pIfList);
if (dwResult != ERROR_SUCCESS)
{
UERROR("WlanEnumInterfaces failed with error: %u\n", dwResult);
}
else
{
// take the first interface found
int i = 0;
pIfInfo = (WLAN_INTERFACE_INFO *) & pIfList->InterfaceInfo[i];
if(pIfInfo->isState == wlan_interface_state_connected)
{
dwResult = WlanQueryInterface(hClient,
&pIfInfo->InterfaceGuid,
wlan_intf_opcode_current_connection,
NULL,
&connectInfoSize,
(PVOID *) &pConnectInfo,
&opCode);
if (dwResult != ERROR_SUCCESS)
{
UERROR("WlanQueryInterface failed with error: %u\n", dwResult);
}
else
{
int quality = pConnectInfo->wlanAssociationAttributes.wlanSignalQuality;
dBm = quality2dBm(quality);
}
}
else
{
UERROR("Interface not connected!");
}
}
}
if (pConnectInfo != NULL)
{
WlanFreeMemory(pConnectInfo);
pConnectInfo = NULL;
}
if (pIfList != NULL)
{
WlanFreeMemory(pIfList);
pIfList = NULL;
}
#else
// Code inspired from http://blog.ajhodges.com/2011/10/using-ioctl-to-gather-wifi-information.html
//have to use a socket for ioctl
@@ -79,15 +190,7 @@ private:
else if(((iw_statistics *)req.u.data.pointer)->qual.updated & IW_QUAL_DBM)
{
//signal is measured in dBm and is valid for us to use
int level = ((iw_statistics *)req.u.data.pointer)->qual.level - 256;
double stamp = UTimer::now();
// Create user data [level, stamp] with the value (int = 4 bytes) and a timestamp (double = 8 bytes)
std::vector<unsigned char> data(sizeof(int) + sizeof(double));
memcpy(data.data(), &level, sizeof(int));
memcpy(data.data()+sizeof(int), &stamp, sizeof(double));
this->post(new UserDataEvent(data));
//UWARN("posting level %d dBm", level);
dBm = ((iw_statistics *)req.u.data.pointer)->qual.level - 256;
}
else
{
@@ -95,6 +198,18 @@ private:
}
close(sockfd);
#endif
if(dBm != 0)
{
double stamp = UTimer::now();
// Create user data [level, stamp] with the value (int = 4 bytes) and a timestamp (double = 8 bytes)
std::vector<unsigned char> data(sizeof(int) + sizeof(double));
memcpy(data.data(), &dBm, sizeof(int));
memcpy(data.data()+sizeof(int), &stamp, sizeof(double));
this->post(new UserDataEvent(data));
//UWARN("posting level %d dBm", dBm);
}
}
}
examples/WifiMapping/main.cpp
+50 -23
View File
@@ -35,14 +35,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <stdio.h>
#include "MapBuilderWifi.h"
#include "WifiThread.h"
void showUsage()
{
printf("\nUsage:\n"
"rtabmap-wifi_mapping interface_name [driver]\n"
" interface_name Wifi interface name (e.g. \"eth0\")\n"
" driver Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
"rtabmap-wifi_mapping [options]\n"
"Options:\n"
" -i \"name\" Wifi interface name (e.g. \"eth0\"). Only required on Linux.\n"
" -m Enable mirroring of the camera image.\n"
" -d # Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
exit(1);
}
@@ -52,33 +55,52 @@ int main(int argc, char * argv[])
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
std::string interfaceName;
std::string interfaceName = "eth0";
int driver = 0;
if(argc < 2)
bool mirroring = false;
// parse options
for(int i = 1; i<argc; ++i)
{
showUsage();
}
else
{
interfaceName = argv[1];
if(interfaceName.size() == 0)
if(strcmp(argv[i], "-i") == 0)
{
UERROR("Interface name invalid!");
showUsage();
}
else
{
printf("Using Wifi interface \"%s\"\n", interfaceName.c_str());
}
if(argc > 2)
{
driver = atoi(argv[2]);
if(driver < 0 || driver > 4)
++i;
if(i < argc)
{
interfaceName = argv[i];
}
else
{
UERROR("driver should be between 0 and 4.");
showUsage();
}
continue;
}
if(strcmp(argv[i], "-m") == 0)
{
mirroring = true;
continue;
}
if(strcmp(argv[i], "-d") == 0)
{
++i;
if(i < argc)
{
driver = atoi(argv[i]);
if(driver < 0 || driver > 4)
{
UERROR("driver should be between 0 and 4.");
showUsage();
}
}
else
{
showUsage();
}
continue;
}
UERROR("Option \"%s\" not recognized!", argv[i]);
showUsage();
}
// Here is the pipeline that we will use:
@@ -129,6 +151,11 @@ int main(int argc, char * argv[])
camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
}
if(mirroring)
{
camera->setMirroringEnabled(true);
}
CameraThread cameraThread(camera);
if(!cameraThread.init())
{