extract_wifi_data.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "ename": "ModuleNotFoundError",
     "evalue": "No module named 'rospy'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
      "Input \u001b[0;32mIn [1]\u001b[0m, in \u001b[0;36m<cell line: 4>\u001b[0;34m()\u001b[0m\n\u001b[1;32m      1\u001b[0m get_ipython()\u001b[38;5;241m.\u001b[39mrun_line_magic(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mmatplotlib\u001b[39m\u001b[38;5;124m'\u001b[39m, \u001b[38;5;124m'\u001b[39m\u001b[38;5;124minline\u001b[39m\u001b[38;5;124m'\u001b[39m)\n\u001b[1;32m      2\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01m__future__\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m print_function\n\u001b[0;32m----> 4\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mrospy\u001b[39;00m\n\u001b[1;32m      5\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mrosbag\u001b[39;00m\n\u001b[1;32m      6\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mitertools\u001b[39;00m\n",
      "\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'rospy'"
     ]
    }
   ],
   "source": [
    "%matplotlib inline\n",
    "from __future__ import print_function\n",
    "\n",
    "import rospy\n",
    "import rosbag\n",
    "import itertools\n",
    "from  tqdm import tqdm\n",
    "from geometry_msgs.msg import PoseWithCovarianceStamped\n",
    "import tf\n",
    "import numpy as np\n",
    "import time\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "import os\n",
    "\n",
    "%load_ext autoreload\n",
    "%autoreload 2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Create ground truth"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def create_gt(map_name):\n",
    "    gt_bag_filename = '{0:s}/{1:s}/rosbag/{1:s}_pointcloud_odom_gt.bag'.format(root_folder,map_name)\n",
    "    if not os.path.isfile(gt_bag_filename):\n",
    "        gt_bag_filename = '{0:s}/{1:s}/rosbag/{1:s}_pointcloud_gt.bag'.format(root_folder,map_name)\n",
    "    if not os.path.isfile(gt_bag_filename):\n",
    "        print('[Error] _gt.bag not found')\n",
    "        return None\n",
    "\n",
    "    #gt_bag_filename = '{0:s}/{1:s}/{1:s}_pointcloud_odom_gt.bag'.format(root_folder,map_name)\n",
    "    #sensors_bag_filename = '{0:s}/{1:s}/{1:s}_sensors.bag'.format(root_folder,map_name)\n",
    "\n",
    "    #odom_tf_bag_filename = '/tmp/odom_tf.bag'\n",
    "    #odom_nav_bag_filename = '/tmp/odom_msg.bag'\n",
    "    print('Reading {:60s}'.format(gt_bag_filename),end='')\n",
    "    try: \n",
    "        gt_bag = rosbag.Bag(gt_bag_filename)\n",
    "    except:\n",
    "        gt_bag = rosbag.Bag(gt_bag_filename2)\n",
    "    print('\\t Done')\n",
    "    time.sleep(0.2)\n",
    "    \n",
    "    t_offset = None\n",
    "\n",
    "    Pose = list()\n",
    "    for topic, msg, t in tqdm(gt_bag.read_messages(topics=['/carto_pose',])):\n",
    "    #for topic, msg, t in bag.read_messages(topics=['/amcl_pose',]):\n",
    "        #yaw angle\n",
    "        rpy = tf.transformations.euler_from_quaternion((msg.pose.pose.orientation.x,\n",
    "                                                        msg.pose.pose.orientation.y,\n",
    "                                                        msg.pose.pose.orientation.z,\n",
    "                                                        msg.pose.pose.orientation.w,\n",
    "                                                        'xyzs'))\n",
    "        if t_offset is None: t_offset = msg.header.stamp.secs - 300 # -5 minutes\n",
    "        ti = msg.header.stamp.secs-t_offset + 1e-9*msg.header.stamp.nsecs\n",
    "        Pose.append([ti,\n",
    "                    msg.pose.pose.position.x,\n",
    "                    msg.pose.pose.position.y,\n",
    "                    np.cos(rpy[2]),\n",
    "                    np.sin(rpy[2])])\n",
    "\n",
    "    Pose = np.asarray(Pose)\n",
    "\n",
    "    # Smooth traj\n",
    "    eps_time = 1e-2#1e-2 #0.01\n",
    "    del_sum = 0\n",
    "\n",
    "    while(True):\n",
    "        Diff = list()\n",
    "        del_ind = list()\n",
    "        min_time_jump = 1000\n",
    "        for i in range(1,len(Pose)):\n",
    "            dif = Pose[i,0]-Pose[i-1,0]\n",
    "            Diff.append(dif)\n",
    "            if dif < min_time_jump: min_time_jump = dif\n",
    "            if dif <= eps_time: \n",
    "                del_ind.append(i)\n",
    "                del_sum+=1;\n",
    "\n",
    "    #    time_pose = np.delete(time_pose,del_ind)\n",
    "        Pose = np.delete(Pose,del_ind,axis=0)\n",
    "        print(min_time_jump)\n",
    "        if (len(del_ind)==0): break\n",
    "    \n",
    "    return Pose, t_offset"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Extract Wifi data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "import rosbag\n",
    "import tf\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "\n",
    "from rf_msgs.msg import Rss\n",
    "\"\"\"\n",
    "load all rss info from rosbag\n",
    "\"\"\"\n",
    "\n",
    "def get_wifi(map_name,Pose,t_offset,macdict=None,reverse_macdict=None):\n",
    "\n",
    "    sensors_bag_filename = '{0:s}/{1:s}/rosbag/{1:s}_sensors.bag'.format(root_folder,map_name)\n",
    "    print('Reading {:60s}'.format(sensors_bag_filename),end='')\n",
    "    sensors_bag = rosbag.Bag(sensors_bag_filename)\n",
    "    print('\\t Done')\n",
    "\n",
    "    \n",
    "    if macdict is None: \n",
    "        print('No macdict')\n",
    "        macdict = {}\n",
    "        reverse_macdict = {}\n",
    "        update_macdict = True\n",
    "    else:\n",
    "        print('Using macdict')\n",
    "        update_macdict = False\n",
    "\n",
    "    sensors_topics = ['/rss{:d}'.format(i) for i in range(1,9)]\n",
    "    print('Topics: ',sensors_topics)\n",
    "\n",
    "    sensors_bag = rosbag.Bag(sensors_bag_filename)\n",
    "    sensors = {topic:list() for topic in sensors_topics}\n",
    "\n",
    "    Ti,X,Y,Yaw,Topic,Mac,Freq,Data = [list() for i in range(8)]\n",
    "\n",
    "    for topic, msg, t in tqdm(sensors_bag.read_messages(topics=sensors_topics)):\n",
    "        ti = msg.header.stamp.secs-t_offset + 1e-9*msg.header.stamp.nsecs\n",
    "        # interpolate pose\n",
    "        yaw_c = np.interp(ti,Pose[:,0],Pose[:,3])\n",
    "        yaw_s = np.interp(ti,Pose[:,0],Pose[:,4])\n",
    "        try:\n",
    "            macindex = macdict[msg.mac_address]\n",
    "        except:\n",
    "            if update_macdict:\n",
    "                macindex = int(len(macdict))\n",
    "                macdict[msg.mac_address] = macindex\n",
    "                reverse_macdict[macindex] = msg.mac_address\n",
    "            else:\n",
    "                macindex = -1\n",
    "                continue\n",
    "\n",
    "\n",
    "        # add datapoint\n",
    "        Ti.append(ti)\n",
    "        X.append(np.interp(ti,Pose[:,0],Pose[:,1]))\n",
    "        Y.append(np.interp(ti,Pose[:,0],Pose[:,2]))\n",
    "        Yaw.append(np.arctan2(yaw_s,yaw_c))\n",
    "        Topic.append(topic)\n",
    "        Mac.append(macindex)\n",
    "        Freq.append(int(msg.freq))\n",
    "        Data.append(msg.data[0]/95.+1)\n",
    "    \n",
    "    return  pd.DataFrame.from_dict({\n",
    "            'time':Ti,\n",
    "            'x':X,\n",
    "            'y':Y,\n",
    "            'yaw':Yaw,\n",
    "            'topic':Topic,\n",
    "            'mac_address':Mac,\n",
    "            'freq':Freq,\n",
    "            'data':Data\n",
    "            }), macdict, reverse_macdict\n",
    "\n",
    "def filter_wifi(df):\n",
    "    # create time-slices\n",
    "    ts = 1\n",
    "    time_slices = np.arange(df[\"time\"].min(),df[\"time\"].max(),ts)\n",
    "    tmp = df.groupby(pd.cut(df[\"time\"],time_slices))\n",
    "\n",
    "    df_list_tmp = [group for name, group in tmp]\n",
    "    df_list = list()\n",
    "    for di in df_list_tmp:\n",
    "        if len(di)>0: df_list.append(di)\n",
    "            \n",
    "    # distance filter\n",
    "    d_thr = 0.5 # merge if less than 50cm\n",
    "    Dxy = list()\n",
    "    i = 0\n",
    "    for c in tqdm(range(len(df_list))):\n",
    "        dxy = (df_list[i]['x'].max()-df_list[i]['x'].min())**2+(df_list[i]['y'].max()-df_list[i]['y'].min())**2\n",
    "        if dxy < d_thr**2:\n",
    "            try:\n",
    "                df_list[i] = pd.concat([df_list[i],df_list.pop(i+1)])\n",
    "            except:\n",
    "                pass\n",
    "        else:\n",
    "            i +=1\n",
    "            Dxy.append(dxy**.5)\n",
    "\n",
    "    print(len(df_list))\n",
    "    return df_list\n",
    "\n",
    "\n",
    "def create_wifi_dataset(df_list,macdict,reverse_macdict):\n",
    "    XY = np.asarray([[df['x'].mean(), df['y'].mean()] for df in df_list])\n",
    "    Wifi = np.zeros((XY.shape[0],len(macdict)))\n",
    "    for xi,df in enumerate(df_list):\n",
    "        for macindex, data in df.groupby(['mac_address'])['data'].mean().iteritems():\n",
    "            Wifi[xi,macindex] = data\n",
    "    print(Wifi.shape)\n",
    "    \n",
    "    # delete AP that appear less than x points\n",
    "    ncounts = np.zeros(Wifi.shape[1])\n",
    "    for i,wifi in enumerate(Wifi.T):\n",
    "        ncounts[i] = np.count_nonzero(wifi)\n",
    "    nthr = 5\n",
    "    time.sleep(0.2)\n",
    "    print('APs with more than {:d} points: {:d}'.format(nthr,np.sum(ncounts > nthr)))\n",
    "    \n",
    "    filtered_dict = {}\n",
    "    del_id = list()\n",
    "\n",
    "    for i,nc in enumerate(ncounts):\n",
    "        if nc > 5: # if more than 5 occurrences. the ap is saved\n",
    "            new_index = len(filtered_dict)\n",
    "            filtered_dict[reverse_macdict[i]] = new_index\n",
    "\n",
    "    print(len(macdict),'->',len(filtered_dict))\n",
    "    if len(macdict) != len(filtered_dict): macdict = filtered_dict\n",
    "    return XY,Wifi,macdict"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Example use"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "####################\n",
      "  map: udai-01 \n",
      "####################\n",
      "Reading /mnt/matrix/rosbag/processed_data/nict/udai-01/rosbag/udai-01_pointcloud_odom_gt.bag\t Done\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "7666it [00:00, 20854.75it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.002025550000325893\n",
      "0.9940555800003494\n",
      "Pose: 4681\n",
      ">> Get Wifi\n",
      "Reading /mnt/matrix/rosbag/processed_data/nict/udai-01/rosbag/udai-01_sensors.bag\t Done\n",
      "No macdict\n",
      "Topics:  ['/rss1', '/rss2', '/rss3', '/rss4', '/rss5', '/rss6', '/rss7', '/rss8']\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "826254it [00:51, 15900.05it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      ">> Filter time/distance\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 4829/4829 [00:03<00:00, 1293.55it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "2288\n",
      ">> Filter mac\n",
      "(2288, 343)\n",
      "APs with more than 5 points: 270\n",
      "343 -> 270\n",
      ">> Recompute Wifi\n",
      "Reading /mnt/matrix/rosbag/processed_data/nict/udai-01/rosbag/udai-01_sensors.bag\t Done\n",
      "Using macdict\n",
      "Topics:  ['/rss1', '/rss2', '/rss3', '/rss4', '/rss5', '/rss6', '/rss7', '/rss8']\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "826254it [00:45, 18012.49it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      ">> Re-Filter time/distance\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 4829/4829 [00:03<00:00, 1293.39it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "2288\n",
      ">> Compute Wifi dataset\n",
      "(2288, 270)\n",
      "APs with more than 5 points: 270\n",
      "270 -> 270\n",
      "Wifi  :  (2288, 270) \n",
      "XY    :  (2288, 2)\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7f92a5023590>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "root_folder = '/mnt/matrix/rosbag/processed_data/nict'\n",
    "map_name = 'udai-01'\n",
    "\n",
    "print('\\n####################\\n  map: {:s} \\n####################'.format(map_name)); time.sleep(0.2)\n",
    "Pose, t_offset = create_gt(map_name)\n",
    "print('Pose: {:d}'.format(int(Pose.shape[0])))\n",
    "print('>> Get Wifi'); time.sleep(0.2)\n",
    "df, macdict, reverse_macdict = get_wifi(map_name,Pose,t_offset,macdict=None)\n",
    "print('>> Filter time/distance'); time.sleep(0.2)\n",
    "df_list = filter_wifi(df)\n",
    "print('>> Filter mac'); time.sleep(0.2)\n",
    "_,_,macdict = create_wifi_dataset(df_list,macdict,reverse_macdict)\n",
    "#recompute using filtered macdict\n",
    "print('>> Recompute Wifi'); time.sleep(0.2)\n",
    "df, macdict, reverse_macdict = get_wifi(map_name,Pose,t_offset,macdict=macdict,reverse_macdict=reverse_macdict)\n",
    "print('>> Re-Filter time/distance'); time.sleep(0.2)\n",
    "df_list = filter_wifi(df)\n",
    "print('>> Compute Wifi dataset'); time.sleep(0.2)\n",
    "XY,Wifi,macdict = create_wifi_dataset(df_list,macdict,reverse_macdict)\n",
    "\n",
    "print('Wifi  : ',Wifi.shape,\n",
    "      '\\nXY    : ',XY.shape)\n",
    "f,ax = plt.subplots()\n",
    "ax.scatter(XY[:,0],XY[:,1])\n",
    "ax.set_aspect('equal')\n",
    "\n",
    "# Save data\n",
    "#try:\n",
    "#    os.mkdir('{0:s}/{1:s}/invisible-maps/'.format(root_folder,map_name))\n",
    "#except:\n",
    "#    pass\n",
    "#save_filename = '{0:s}/{1:s}/invisible-maps/{1:s}_WifiData.npz'.format(root_folder,map_name)\n",
    "#np.savez_compressed(save_filename,XY=XY,Wifi=Wifi,macdict=macdict)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}