<filename>bin/wifi_im.py<gh_stars>0
#!/usr/bin/env python

# ros
import rospy
import tf
import actionlib
from nav_msgs.msg import OccupancyGrid
from nav_msgs.srv import GetMap
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import PoseWithCovarianceStamped
from tf2_msgs.msg import TFMessage
from move_base_msgs.msg import *
from wifi_sensor.msg import RssiMulti
# other stuff
import numpy as np
from matplotlib import pyplot as plt
import Queue
import random
import math
import cPickle
# scikit learn
from sklearn.svm import SVR
from sklearn.neighbors import KNeighborsRegressor
from sklearn.neighbors import RadiusNeighborsRegressor
from sklearn.linear_model import Ridge
from sklearn.linear_model import RidgeCV
from sklearn.linear_model import SGDRegressor
from sklearn.kernel_approximation import RBFSampler
try:
  from sklearn.preprocessing import StandardScaler
except:
  from sklearn.preprocessing import Scaler as StandardScaler
# pybrain
from pybrain.datasets import SupervisedDataSet
from pybrain.tools.shortcuts import buildNetwork
from pybrain.supervised import BackpropTrainer

class ScaledModel:
  def __init__(self, model):
    self.model = model
  def fit(self, x, y):
    self.x_scaler = StandardScaler().fit(x)
    self.y_scaler = StandardScaler().fit(y)
    self.model.fit(self.x_scaler.transform(x), self.y_scaler.transform(y))
  def predict(self, x):
    return self.y_scaler.inverse_transform(self.model.predict(self.x_scaler.transform(x)))

class KernelModel:
  def __init__(self, model):
    self.model = model
    self.sampler = RBFSampler(gamma=1e-1, n_components=100, random_state=1)
  def fit(self, x, y):
    self.x_scaler = StandardScaler().fit(x)
    self.sampler.fit(self.x_scaler.transform(x))
    self.model.fit(self.sampler.transform(self.x_scaler.transform(x)), y)
  def predict(self, x):
   return self.model.predict(self.sampler.transform(self.x_scaler.transform(x)))

class FNN:
  def __init__(self, n_hidden=[5, 5]):
    self.n_hidden=n_hidden
  def fit(self, x, y):
    if type(self.n_hidden) != list:
      self.nn = buildNetwork(len(x[0]), self.n_hidden, 1, bias=True)
    else:
      if len(self.n_hidden) == 1:
        self.nn = buildNetwork(len(x[0]), self.n_hidden[0], 1, bias=True)
      elif len(self.n_hidden) == 2:
        self.nn = buildNetwork(len(x[0]), self.n_hidden[0], self.n_hidden[1], 1, bias=True)
      elif len(self.n_hidden) == 3:
        self.nn = buildNetwork(len(x[0]), self.n_hidden[0], self.n_hidden[1], self.n_hidden[2], 1, bias=True)
    self.ds = SupervisedDataSet(len(x[0]), 1)
    for xx, yy in zip(x, y):
      self.ds.addSample(xx, [yy])
    self.trainer = BackpropTrainer(self.nn, dataset=self.ds, momentum=0.1)
    self.trainer.trainEpochs(3)
  def predict(self, x):
    if len(x) == 1:
      return self.nn.activate(x[0])
    else:
      return self.nn.activate(x)

class WifiIM:
  def __init__(self):
    rospy.init_node("wifi_im", anonymous=True)
    # parameter
    self.target_mac = rospy.get_param("~target_mac", "")
    # misc variables
    self.pose = Pose()
    # data containers
    self.rssi_log = []
    self.pose_log = []
    self.state_log = []
    self.model_log = []
    self.xyr = []
    self.xyr_r = []
    # msg stuff
    self.msg_queue = Queue.Queue()
    rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.callback_pos)
#    rospy.Subscriber("/tf", TFMessage, self.callback_tf)
    rospy.Subscriber("/rssi", RssiMulti, self.callback_rssi)
    # get map
    print "waiting for map"
    rospy.wait_for_service("/static_map")
    get_map_service  = rospy.ServiceProxy("/static_map", GetMap)
    get_map_response = get_map_service()
    print "got map"
    self.ogrid = np.array(get_map_response.map.data, dtype=np.int8)
    self.ogrid = self.ogrid.reshape((get_map_response.map.info.height, get_map_response.map.info.width))
    self.resolution = get_map_response.map.info.resolution
    self.origin = get_map_response.map.info.origin
    del get_map_service
    del get_map_response
    #  generate walls for plotting
    self.walls = np.argwhere(self.ogrid > 50)
    self.walls = [[x * self.resolution + self.origin.position.x, y * self.resolution + self.origin.position.y] for y, x in self.walls]
    # main loop
    self.run_find_ap()
  def run_find_ap(self):
    # parameters
    self.local_radius = 100.0
    self.t_12 = 5
    self.stepwidth = 1.0
    self.error_thresh = 3.0
    # model
    model = ScaledModel(FNN(100))
    self.model_name = "fnn 100"
#    model = ScaledModel(SVR(C=1e-1))
#    self.model_name = "svr c=1e-1"
#    model = Ridge()
#    self.model_name = "ridge"
#    model = KNeighborsRegressor(n_neighbors=500, weights="distance") 
#    self.model_name = "knn 500 distance"
#    model = KernelModel(Ridge())
#    self.model_name = "kernel ridge"
    selector = RadiusNeighborsRegressor(radius=self.local_radius)
    # plotting
    plt.ion()
    plt.gcf().canvas._tkcanvas.master.geometry("1000x1200")
    n_loop = 0
    # random movement to get some samples
    print "random initial movement"
    self.process_msgs()
    self.state_log.append([rospy.Time.now().to_time(), "start", [self.pose.pose.position.x, self.pose.pose.position.y]])
    candidates = []
    for x in np.linspace(self.pose.pose.position.x - 2.0, self.pose.pose.position.x + 2.0, int(2.0 * 10)):
      for y in np.linspace(self.pose.pose.position.y - 2.0, self.pose.pose.position.y + 2.0, int(2.0 * 10)):
        candidates.append([x, y])
    candidates = [candidate for candidate in candidates if self.test_point_real(candidate[0], candidate[1], 0.25)]
    goal = random.choice(candidates)
    yaw = math.pi-math.atan2(goal[1] - self.pose.pose.position.y, goal[0] - self.pose.pose.position.x)
    self.goto_real(goal[0], goal[1], yaw)
    # main loop
    while not rospy.is_shutdown():
      n_loop = n_loop + 1
      print n_loop, rospy.Time.now().to_time()
      self.state_log.append([rospy.Time.now().to_time(), "loop" + str(n_loop), [self.pose.pose.position.x, self.pose.pose.position.y]])
      # deal with messages
      self.process_msgs()
      print "self.rssi_log", len(self.rssi_log), "self.pose_log", len(self.pose_log)
      # check model predictions
      mean_rssi_rate = len(self.rssi_log) / (self.rssi_log[-1][0] - self.rssi_log[0][0]).to_sec()
      mean_rssi = np.mean([rssi[1] for rssi in self.rssi_log[-(int(1.0*mean_rssi_rate)):]])
      if n_loop == 1 or abs(model.predict([self.pose.pose.position.x, self.pose.pose.position.y]) - mean_rssi) > self.error_thresh:
        print "fitting local model"
        self.state_log.append([rospy.Time.now().to_time(), "fit_model", [self.pose.pose.position.x, self.pose.pose.position.y]])
        xy = [[pose[0].to_time(), pose[1].position.x, pose[1].position.y] for pose in self.pose_log]
        r = [[rssi[0].to_time(), rssi[1]] for rssi in self.rssi_log]
        self.xyr = self.interpolate(xy, r)
        # find relevant data and fit data
        selector.fit([[x, y] for t, x, y, r in self.xyr], [r for t, x, y, r in self.xyr])
        dist, ind = selector.radius_neighbors([self.pose.pose.position.x, self.pose.pose.position.y], radius=self.local_radius)
        self.xyr_r = [self.xyr[idx] for idx in ind[0]]
        # shuffle and only use 10000 samples
        if len(self.xyr_r) < 10000:
          np.random.shuffle(self.xyr_r)
        else:
          xyr_temp = [self.xyr_r[idx] for idx in np.random.choice(len(self.xyr_r), 10000, replace=False)]
          self.xyr_r = xyr_temp
        # fit
        model.fit([[x,y] for t,x,y,r in self.xyr_r], [r for t,x,y,r in self.xyr_r])
        print "xyr", len(self.xyr), "and", len(ind[0]), "used"
      # find new search direction
      # epsilon = 0.9*exp(-alpha*n_loop)+0.1 so that epsilon = 0.5 at self.t_12
      epsilon = 0.9*math.exp((math.log(0.4/0.9) / self.t_12) * n_loop) + 0.1
      print epsilon
      goal = None
      if random.random() > epsilon:
        # follow best, i.e., greedy
        print "greedy"
        self.state_log.append([rospy.Time.now().to_time(), "greedy", [self.pose.pose.position.x, self.pose.pose.position.y]])
        candidates = []
        for x in np.linspace(-12, 2, 96):
          for y in np.linspace(-15, 20, 280):
            if self.test_point_real(x, y, 0.25):
              candidates.append([[x, y], model.predict([x, y])])
        candidates = sorted(candidates, key=lambda x:x[1])
        best = candidates[-1][0]
        curr = [self.pose.pose.position.x, self.pose.pose.position.y]
        goal = [best[0], best[1]]
        # if goal further away than stepwidth, just move stepwidth
        if math.hypot(curr[0] - best[0], curr[1] - best[1]) > self.stepwidth:
          goal = [curr[i] + self.stepwidth*(best[i] - curr[i])/math.hypot(best[0]-curr[0], best[1]-curr[1]) for i in range(2)]
        # if goal not legal, random search of stepwidth
        new_stepwidth = self.stepwidth
        while not self.test_point_real(goal[0], goal[1], 0.25):
          new_stepwidth = new_stepwidth + random.uniform(-0.1, 0.1)
          # don't go backwards or too far
          if new_stepwidth <= 0.0 or new_stepwidth > math.hypot(curr[0] - best[0], curr[1] - best[1]):
            new_stepwidth = self.stepwidth
          goal = [curr[i] + new_stepwidth*(best[i] - curr[i])/math.hypot(best[0]-curr[0], best[1]-curr[1]) for i in range(2)]
        print "stepwidth", new_stepwidth
      else:
        # random search, epsilon
        # TODO: make epsilon only search where you haven't been (maybe look at exploration front)
        print "epsilon", epsilon
        self.state_log.append([rospy.Time.now().to_time(), "epsilon", [self.pose.pose.position.x, self.pose.pose.position.y]])
        candidates = []
        for x in np.linspace(-12, 2, 96):
          for y in np.linspace(-15, 20, 280):
            if self.test_point_real(x, y, 0.25):
              candidates.append([x, y])
        candidates = [candidate for candidate in candidates if len(selector.radius_neighbors(candidate, radius=0.5)[1][0]) == 0]
        probs = [math.exp(-math.hypot(candidate[0]-self.pose.pose.position.x, candidate[1]-self.pose.pose.position.y)) for candidate in candidates]
        probs_norm = np.sum(probs)
        probs = [prob/probs_norm for prob in probs]
        goal = candidates[np.random.choice(len(candidates), p=probs)]
      # generate visualization for model
      xx = np.linspace(-12, 2, 24)
      yy = np.linspace(-15, 20, 70)
      rssi_plot = np.zeros((len(xx), len(yy)))
      for i in range(len(xx)):
        for j in range(len(yy)):
          rssi_plot[i, j] = model.predict([[xx[i], yy[j]]])
      self.model_log.append([rospy.Time.now().to_time(), rssi_plot, [self.pose.pose.position.x, self.pose.pose.position.y]])
      # plotting
      print "plotting"
      plt.clf()
      plt.suptitle(self.target_mac)
      plt.subplot(121)
      plt.title("model")
      plt.pcolormesh(xx, yy, rssi_plot.T, cmap=plt.get_cmap("gnuplot2"), vmin=-80, vmax=-20)
      plt.plot([x for x,y in self.walls], [y for x,y in self.walls], "k,")
      plt.plot([x for t, x, y, r in self.xyr], [y for t, x, y, r in self.xyr], "-", color="#39FF14")
      plt.plot(self.pose.pose.position.x, self.pose.pose.position.y, "r.")
      plt.plot(goal[0], goal[1], "r*")
      plt.xticks([-10, -5, 0])
      plt.gca().set_xlim((min(xx), max(xx)))
      plt.gca().set_xlabel("x [m]")
      plt.gca().set_ylim((min(yy), max(yy)))
      plt.gca().set_ylabel("y [m]")
      cbar = plt.colorbar()
      cbar.set_label("mean rssi [dB]")
      plt.subplot(122)
      plt.title("hexbin")
      plt.hexbin([x for t, x, y, r in self.xyr], [y for t, x, y, r in self.xyr], [r for t, x, y, r in self.xyr], gridsize=20, cmap=plt.get_cmap("gnuplot2"), vmin=-80, vmax=-20, extent=(min(xx), max(xx), min(yy), max(yy)))
      plt.plot([x for x,y in self.walls], [y for x,y in self.walls], "k,")
      plt.plot([x for t, x, y, r in self.xyr], [y for t, x, y, r in self.xyr], "-", color="#39FF14")
      plt.plot(self.pose.pose.position.x, self.pose.pose.position.y, "r.")
      plt.plot(goal[0], goal[1], "r*")
      plt.xticks([-10, -5, 0])
      plt.gca().set_xlim((min(xx), max(xx)))
      plt.gca().set_xlabel("x [m]")
      plt.gca().set_ylim((min(yy), max(yy)))
      plt.gca().set_ylabel("y [m]")
      cbar = plt.colorbar()
      cbar.set_label("mean rssi [dB]")
      plt.draw()
      # moving
      yaw = math.pi-math.atan2(goal[1] - self.pose.pose.position.y, goal[0] - self.pose.pose.position.x)
      self.goto_real(goal[0], goal[1], yaw)
      # logging
      print "logging"
      self.dump_log()
  def process_msgs(self):
    while not self.msg_queue.empty():
      data = self.msg_queue.get()
      if data[0] == "amcl_pose":
        self.pose_log.append([data[1].header.stamp, data[1].pose.pose])
        pose = PoseStamped()
        pose.header = data[1].header
        pose.pose = data[1].pose.pose
        self.pose = pose
      if data[0] == "rssi":
        for rssi in data[1].data:
          if rssi.their_mac_addr == self.target_mac:
            if len([mes for mes in rssi.rssi if mes < 0]) > 0:
              self.rssi_log.append([data[1].header.stamp, np.mean([mes for mes in rssi.rssi if mes < 0])])
  def interpolate(self, xy, rssi):
    xy = sorted(xy, key=lambda x:x[0])
    r = sorted(rssi, key=lambda x:x[0])
    xyr = []
    r_i = 0
    xy_i = 0
    while(xy[0][0] > r[r_i][0]):
      r_i = r_i + 1
    for r_i in range(r_i, len(r)):
      if xy_i+1 >= len(xy):
        break
      while(xy[xy_i+1][0] <= r[r_i][0]):
        xy_i = xy_i + 1
        if xy_i+1 >= len(xy):
          break
      if xy_i+1 >= len(xy):
        break
      x = (xy[xy_i+1][1] - xy[xy_i][1])/(xy[xy_i+1][0] - xy[xy_i][0]) * (r[r_i][0] - xy[xy_i][0]) + xy[xy_i][1]
      y = (xy[xy_i+1][2] - xy[xy_i][2])/(xy[xy_i+1][0] - xy[xy_i][0]) * (r[r_i][0] - xy[xy_i][0]) + xy[xy_i][2]
      xyr.append([r[r_i][0], x, y, r[r_i][1]])
    return xyr
  def goto_real(self, x, y, yaw):
    print "goto", x, y, yaw
    client = actionlib.SimpleActionClient("/move_base", MoveBaseAction)
    client.wait_for_server()
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "/map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.position.y = y
    goal.target_pose.pose.position.z = self.pose.pose.position.z
    quaternion = tf.transformations.quaternion_from_euler(yaw, 0.0, 0.0)
    goal.target_pose.pose.orientation.w = quaternion[0]
    goal.target_pose.pose.orientation.x = quaternion[1]
    goal.target_pose.pose.orientation.y = quaternion[2]
    goal.target_pose.pose.orientation.z = quaternion[3]
    client.send_goal(goal)
    result = client.wait_for_result()
    if not result:
      print "something went wrong while goto_real"
  def goto_map(self, x, y, yaw):
    print "goto", x * self.resolution + self.origin.position.x, y * self.resolution + self.origin.position.y, yaw
    client = actionlib.SimpleActionClient("/move_base", MoveBaseAction)
    client.wait_for_server()
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "/map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = x * self.resolution + self.origin.position.x
    goal.target_pose.pose.position.y = y * self.resolution + self.origin.position.y
    goal.target_pose.pose.position.z = self.pose.pose.position.z
    quaternion = tf.transformations.quaternion_from_euler(yaw, 0.0, 0.0)
    goal.target_pose.pose.orientation.w = quaternion[0]
    goal.target_pose.pose.orientation.x = quaternion[1]
    goal.target_pose.pose.orientation.y = quaternion[2]
    goal.target_pose.pose.orientation.z = quaternion[3]
    client.send_goal(goal)
    result = client.wait_for_result()
    if not result:
      print "something went wrong while goto_map"
  def generate_grid(self, dx, dy):
    grid_goals = []
    window = int(0.5/self.resolution)/2
    for y in np.arange(window, self.ogrid.shape[0]-window, dy/self.resolution):
      for x in np.arange(window, self.ogrid.shape[1]-window, dx/self.resolution):
        if test_point_map(x, y, window):
          grid_goals.append([x, y])
    return grid_goals
  def test_point_real(self, x, y, window):
    return self.test_point_map((x-self.origin.position.x)/self.resolution, (y-self.origin.position.y)/self.resolution, int(window/self.resolution))
  def test_point_map(self, x, y, window):
    if np.mean(self.ogrid[y-window:y+window, x-window:x+window]) == 0:
      return True
    else:
      return False
  def callback_pos(self, data):
    self.msg_queue.put(["amcl_pose", data])
#  def callback_tf(self, data):
#    for msg in data.transforms:
#      if msg.child_frame_id == "odom":
#        self.msg_queue.put(["tf", msg])
  def callback_rssi(self, data):
    self.msg_queue.put(["rssi", data])
  def dump_log(self):
    self.process_msgs()
    data = {}
    data["parameters"] = {}
    data["parameters"]["local_radius"] = self.local_radius
    data["parameters"]["t_12"] = self.t_12
    data["parameters"]["stepwidth"] = self.stepwidth
    data["parameters"]["error_thresh"] = self.error_thresh
    data["parameters"]["model_name"] = self.model_name
    data["pose_log"] = self.pose_log
    data["rssi_log"] = self.rssi_log
    data["state_log"] = self.state_log
    data["model_log"] = self.model_log
    data["walls"] = self.walls
    xy = [[pose[0].to_time(), pose[1].position.x, pose[1].position.y] for pose in self.pose_log]
    r = [[rssi[0].to_time(), rssi[1]] for rssi in self.rssi_log]
    data["xyr"] = self.interpolate(xy, r)
    outname = "log_"+str(int(rospy.Time.now().to_time()))+".pick"
    cPickle.dump(data, open(outname, "wb"), -1)

if __name__ == "__main__":
  try:
    wifi_im = WifiIM()
  except rospy.ROSInterruptException:
    pass
