def run(self):
     while 1:
         data = self.sock.recv(1024).decode()
         print data
         d = data.find(Dest_string)
         l = data.find(Loc_string)
         if d is not -1:  # found destination substring
             print "Got destination"
             na = data[(len(Dest_string))+1:]  # slising to name of destination only
             print str(na)  # Ensuring got the right name.
             self.route.name = na
             wplist = None
             try:
                 wplist = config.get("routes", self.route.name).split(",")
             except:
                 pass
             self.route.waypoints_list = wp_to_dp(wplist)
         # got name for destination, TODO
         if l is not -1:
             print "Got position\n"
             position = data[(len(Loc_string)):]
             pos_array = position.split(",")
             next_p = self.route.get_next_wp(pos_array)
             # print next_p
             send_location('', 11112, pos_array ,next_p)
         if not data:
             #print "Connection lost"
             self.stop()
             break
DEBUG_LEVEL = config.getint("general", "debug_level")
URL = config.get("general", "url")
CAL_SAVE_PATH = config.get("calibrate_paths", "cal_save")
UPSIDE_DOWN_LIST = config.get("general", "upside_down_list")
CAMERA_LIST = config.get("odroid", "camera_list").split(",")
SECONDS = int(config.get("general", "seconds"))
POSITION_LOG_PATH = config.get("capture_paths", "position_log")
SOCKET_HOST = config.get("socket", "host")
SOCKET_PORT = int(config.get("socket", "port"))
FLOORMAP_PATH = config.get("capture_paths", "floormap")

# Load waypoints from file.
waypoints = pickle.load(open(CAL_SAVE_PATH, "rb"))
floormaps = {}
dest_points = wp_to_dp()  # for demo navigation

for w in waypoints:
    for i in w.keys():
        if w[i].floor not in floormaps.keys():
            floormaps[w[i].floor] = cv2.imread(FLOORMAP_PATH + str(w[i].floor) + ".png")


def send_location(host, port, wp):
    current_pos = "Current point is: " + str(wp.phys_pos[0]) + ", " + str(wp.phys_pos[1]) + ", " + str(wp.floor)
    next_pos = "WayPoint out of the route"
    for i in range(0,len(dest_points)):
        if dest_points[i].wp_id == wp.wp_id:
            if dest_points[i] == dest_points[-1]:  # if it's last one?
                next_pos = "Destination point"
            else: