def query_thread(): global last_query_result, last_query_pos, cache_valid api = overpy.Overpass(url=OVERPASS_API_URL, headers=OVERPASS_HEADERS, timeout=10.) while True: time.sleep(1) if last_gps is not None: fix_ok = last_gps.flags & 1 if not fix_ok: continue if last_query_pos is not None: cur_ecef = geodetic2ecef( (last_gps.latitude, last_gps.longitude, last_gps.altitude)) prev_ecef = geodetic2ecef( (last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude)) dist = np.linalg.norm(cur_ecef - prev_ecef) if dist < 1000: continue if dist > 3000: cache_valid = False q = build_way_query(last_gps.latitude, last_gps.longitude, radius=3000) try: new_result = api.query(q) # Build kd-tree nodes = [] real_nodes = [] node_to_way = defaultdict(list) for n in new_result.nodes: nodes.append((float(n.lat), float(n.lon), 0)) real_nodes.append(n) for way in new_result.ways: for n in way.nodes: node_to_way[n.id].append(way) nodes = np.asarray(nodes) nodes = geodetic2ecef(nodes) tree = spatial.cKDTree(nodes) query_lock.acquire() last_query_result = new_result, tree, real_nodes, node_to_way last_query_pos = last_gps cache_valid = True query_lock.release() except Exception as e: print e query_lock.acquire() last_query_result = None query_lock.release()
def query_thread(): global last_query_result, last_query_pos, cache_valid api = overpy.Overpass(url=OVERPASS_API_URL, headers=OVERPASS_HEADERS, timeout=10.) while True: time.sleep(1) if last_gps is not None: fix_ok = last_gps.flags & 1 if not fix_ok: continue if last_query_pos is not None: cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude)) prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude)) dist = np.linalg.norm(cur_ecef - prev_ecef) if dist < 1000: #updated when we are 1km from the edge of the downloaded circle continue if dist > 3000: cache_valid = False q = build_way_query(last_gps.latitude, last_gps.longitude, radius=3000) try: new_result = api.query(q) # Build kd-tree nodes = [] real_nodes = [] node_to_way = defaultdict(list) location_info = {} for n in new_result.nodes: nodes.append((float(n.lat), float(n.lon), 0)) real_nodes.append(n) for way in new_result.ways: for n in way.nodes: node_to_way[n.id].append(way) for area in new_result.areas: if area.tags.get('admin_level', '') == "2": location_info['country'] = area.tags.get('ISO3166-1:alpha2', '') if area.tags.get('admin_level', '') == "4": location_info['region'] = area.tags.get('name', '') nodes = np.asarray(nodes) nodes = geodetic2ecef(nodes) tree = spatial.cKDTree(nodes) query_lock.acquire() last_query_result = new_result, tree, real_nodes, node_to_way, location_info last_query_pos = last_gps cache_valid = True query_lock.release() except Exception as e: print(e) query_lock.acquire() last_query_result = None query_lock.release()
def test_ecef_geodetic(self): # testing single np.testing.assert_allclose(ecef_positions[0], coord.geodetic2ecef(geodetic_positions[0]), rtol=1e-9) np.testing.assert_allclose(geodetic_positions[0,:2], coord.ecef2geodetic(ecef_positions[0])[:2], rtol=1e-9) np.testing.assert_allclose(geodetic_positions[0,2], coord.ecef2geodetic(ecef_positions[0])[2], rtol=1e-9, atol=1e-4) np.testing.assert_allclose(geodetic_positions[:,:2], coord.ecef2geodetic(ecef_positions)[:,:2], rtol=1e-9) np.testing.assert_allclose(geodetic_positions[:,2], coord.ecef2geodetic(ecef_positions)[:,2], rtol=1e-9, atol=1e-4) np.testing.assert_allclose(ecef_positions, coord.geodetic2ecef(geodetic_positions), rtol=1e-9) np.testing.assert_allclose(geodetic_positions_radians[0], coord.ecef2geodetic(ecef_positions[0], radians=True), rtol=1e-5) np.testing.assert_allclose(geodetic_positions_radians[:,:2], coord.ecef2geodetic(ecef_positions, radians=True)[:,:2], rtol=1e-7) np.testing.assert_allclose(geodetic_positions_radians[:,2], coord.ecef2geodetic(ecef_positions, radians=True)[:,2], rtol=1e-7, atol=1e-4)
def closest(cls, query_results, lat, lon, heading, prev_way=None): results, tree, real_nodes, node_to_way, location_info = query_results cur_pos = geodetic2ecef((lat, lon, 0)) nodes = tree.query_ball_point(cur_pos, 500) # If no nodes within 500m, choose closest one if not nodes: nodes = [tree.query(cur_pos)[1]] ways = [] for n in nodes: real_node = real_nodes[n] ways += node_to_way[real_node.id] ways = set(ways) closest_way = None best_score = None for way in ways: way = Way(way, query_results) points = way.points_in_car_frame(lat, lon, heading) on_way = way.on_way(lat, lon, heading, points) if not on_way: continue # Create mask of points in front and behind x = points[:, 0] y = points[:, 1] angles = np.arctan2(y, x) front = np.logical_and((-np.pi / 2) < angles, angles < (np.pi / 2)) behind = np.logical_not(front) dists = np.linalg.norm(points, axis=1) # Get closest point behind the car dists_behind = np.copy(dists) dists_behind[front] = np.NaN closest_behind = points[np.nanargmin(dists_behind)] # Get closest point in front of the car dists_front = np.copy(dists) dists_front[behind] = np.NaN closest_front = points[np.nanargmin(dists_front)] # fit line: y = a*x + b x1, y1, _ = closest_behind x2, y2, _ = closest_front a = (y2 - y1) / max((x2 - x1), 1e-5) b = y1 - a * x1 # With a factor of 60 a 20m offset causes the same error as a 20 degree heading error # (A 20 degree heading offset results in an a of about 1/3) score = abs(a) * 60. + abs(b) # Prefer same type of road if prev_way is not None: if way.way.tags.get('highway', '') == prev_way.way.tags.get( 'highway', ''): score *= 0.5 if closest_way is None or score < best_score: closest_way = way best_score = score # Normal score is < 5 if best_score > 50: return None return closest_way
def main(gctx=None): context = zmq.Context() poller = zmq.Poller() gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, poller) gps_ext_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, poller) app_sock = messaging.sub_sock(context, service_list['applanixLocation'].port, poller) loc_sock = messaging.pub_sock(context, service_list['liveLocation'].port) last_ext, last_gps, last_app = -1, -1, -1 # 5 sec max_gap = 5*1e9 preferred_type = None while 1: for sock, event in poller.poll(500): if sock is app_sock: msg = messaging.recv_one(sock) last_app = msg.logMonoTime this_type = 'app' if sock is gps_sock: msg = messaging.recv_one(sock) gps_pkt = msg.gpsLocation last_gps = msg.logMonoTime this_type = 'gps' if sock is gps_ext_sock: msg = messaging.recv_one(sock) gps_pkt = msg.gpsLocationExternal last_ext = msg.logMonoTime this_type = 'ext' last = max(last_gps, last_ext, last_app) if last_app > last - max_gap: new_preferred_type = 'app' elif last_ext > last - max_gap: new_preferred_type = 'ext' else: new_preferred_type = 'gps' if preferred_type != new_preferred_type: print "switching from %s to %s" % (preferred_type, new_preferred_type) preferred_type = new_preferred_type if this_type == preferred_type: new_msg = messaging.new_message() if this_type == 'app': # straight proxy the applanix new_msg.init('liveLocation') new_msg.liveLocation = copy(msg.applanixLocation) else: new_msg.logMonoTime = msg.logMonoTime new_msg.init('liveLocation') pkt = new_msg.liveLocation pkt.lat = gps_pkt.latitude pkt.lon = gps_pkt.longitude pkt.alt = gps_pkt.altitude pkt.speed = gps_pkt.speed pkt.heading = gps_pkt.bearing pkt.positionECEF = [float(x) for x in geodetic2ecef([pkt.lat, pkt.lon, pkt.alt])] pkt.source = log.LiveLocationData.SensorSource.dummy loc_sock.send(new_msg.to_bytes())
def closest(cls, query_results, lat, lon, heading, prev_way=None): if query_results is None: return None else: # if prev_way is not None and len(prev_way.way.nodes) < 10: # if prev_way.on_way(lat, lon, heading): # return prev_way # else: # way = prev_way.next_way(heading) # if way is not None and way.on_way(lat, lon, heading): # return way results, tree, real_nodes, node_to_way, location_info = query_results cur_pos = geodetic2ecef((lat, lon, 0)) nodes = tree.query_ball_point(cur_pos, 150) # If no nodes within 150m, choose closest one if not nodes: nodes = [tree.query(cur_pos)[1]] ways = [] for n in nodes: real_node = real_nodes[n] ways += node_to_way[real_node.id] ways = set(ways) closest_way = None best_score = None for way in ways: way = Way(way, query_results) # don't consider backward facing roads if 'oneway' in way.way.tags and way.way.tags['oneway'] == 'yes': angle=heading - math.atan2(way.way.nodes[0].lon-way.way.nodes[-1].lon,way.way.nodes[0].lat-way.way.nodes[-1].lat)*180/3.14159265358979 - 180 if angle < -180: angle = angle + 360 if angle > 180: angle = angle - 360 backwards = abs(angle) > 90 if backwards: continue points = way.points_in_car_frame(lat, lon, heading, True) on_way = way.on_way(lat, lon, heading, points) if not on_way: continue # Create mask of points in front and behind x = points[:, 0] y = points[:, 1] angles = np.arctan2(y, x) front = np.logical_and((-np.pi / 2) < angles, angles < (np.pi / 2)) if all(front): angles[angles==0] = np.pi front = np.logical_and((-np.pi / 2) < angles,angles < (np.pi / 2)) behind = np.logical_not(front) dists = np.linalg.norm(points, axis=1) # Get closest point behind the car dists_behind = np.copy(dists) dists_behind[front] = np.NaN closest_behind = points[np.nanargmin(dists_behind)] # Get closest point in front of the car dists_front = np.copy(dists) dists_front[behind] = np.NaN closest_front = points[np.nanargmin(dists_front)] # fit line: y = a*x + b x1, y1, _ = closest_behind x2, y2, _ = closest_front a = (y2 - y1) / max((x2 - x1), 1e-5) b = y1 - a * x1 # With a factor of 60 a 20m offset causes the same error as a 20 degree heading error # (A 20 degree heading offset results in an a of about 1/3) score = abs(a) * (abs(b) + 1) * 3. + abs(b) # Prefer same type of road if prev_way is not None: if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''): score *= 0.5 if closest_way is None or score < best_score: closest_way = way best_score = score if best_score is None: return None # Normal score is < 5 if best_score > 50: return None return closest_way
def run(self): self.logger.debug("run method started for thread %s" % self.name) # for now we follow old logic, will be optimized later start = time.time() radius = 3000 while True: if time.time() - start > 2.5: print("Mapd QueryThread lagging by: %s" % str(time.time() - start - 1.0)) if time.time() - start < 1.5: time.sleep(1.0) continue else: start = time.time() self.logger.debug("Starting after sleeping for 1 second ...") last_gps = self.sharedParams.get('last_gps', None) self.logger.debug("last_gps = %s" % str(last_gps)) if last_gps is not None: fix_ok = last_gps.flags & 1 if not fix_ok: continue else: continue last_query_pos = self.sharedParams.get('last_query_pos', None) if last_query_pos is not None: cur_ecef = geodetic2ecef( (last_gps.latitude, last_gps.longitude, last_gps.altitude)) if self.prev_ecef is None: self.prev_ecef = geodetic2ecef( (last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude)) dist = np.linalg.norm(cur_ecef - self.prev_ecef) if dist < radius - self.distance_to_edge: #updated when we are close to the edge of the downloaded circle continue self.logger.debug( "parameters, cur_ecef = %s, prev_ecef = %s, dist=%s" % (str(cur_ecef), str(self.prev_ecef), str(dist))) if dist > radius: query_lock = self.sharedParams.get('query_lock', None) if query_lock is not None: query_lock.acquire() self.sharedParams['cache_valid'] = False query_lock.release() else: self.logger.error("There is no query_lock") if last_gps is not None and last_gps.accuracy < 5.0: q, lat, lon = self.build_way_query(last_gps.latitude, last_gps.longitude, last_gps.bearing, radius=radius) try: if self.is_connected_to_local(): api = overpy.Overpass(url=self.OVERPASS_API_LOCAL) api.timeout = 15.0 self.distance_to_edge = radius * 3 / 8 elif self.is_connected_to_internet(): api = overpy.Overpass(url=self.OVERPASS_API_URL) self.logger.error("Using origional Server") self.distance_to_edge = radius / 4 elif self.is_connected_to_internet2(): api = overpy.Overpass(url=self.OVERPASS_API_URL2) api.timeout = 10.0 self.logger.error("Using backup Server") self.distance_to_edge = radius / 4 else: continue new_result = api.query(q) self.logger.debug("new_result = %s" % str(new_result)) # Build kd-tree nodes = [] real_nodes = [] node_to_way = defaultdict(list) location_info = {} for n in new_result.nodes: nodes.append((float(n.lat), float(n.lon), 0)) real_nodes.append(n) for way in new_result.ways: for n in way.nodes: node_to_way[n.id].append(way) for area in new_result.areas: if area.tags.get('admin_level', '') == "2": location_info['country'] = area.tags.get( 'ISO3166-1:alpha2', '') elif area.tags.get('admin_level', '') == "4": location_info['region'] = area.tags.get('name', '') nodes = np.asarray(nodes) nodes = geodetic2ecef(nodes) tree = spatial.KDTree(nodes) self.logger.debug("query thread, ... %s %s" % (str(nodes), str(tree))) # write result query_lock = self.sharedParams.get('query_lock', None) if query_lock is not None: query_lock.acquire() last_gps_mod = last_gps.as_builder() last_gps_mod.latitude = lat last_gps_mod.longitude = lon last_gps = last_gps_mod.as_reader() self.sharedParams[ 'last_query_result'] = new_result, tree, real_nodes, node_to_way, location_info self.prev_ecef = geodetic2ecef( (last_gps.latitude, last_gps.longitude, last_gps.altitude)) self.sharedParams['last_query_pos'] = last_gps self.sharedParams['cache_valid'] = True query_lock.release() else: self.logger.error("There is not query_lock") except Exception as e: self.logger.error("ERROR :" + str(e)) print(str(e)) query_lock = self.sharedParams.get('query_lock', None) query_lock.acquire() self.sharedParams['last_query_result'] = None query_lock.release() else: query_lock = self.sharedParams.get('query_lock', None) query_lock.acquire() self.sharedParams['last_query_result'] = None query_lock.release() self.logger.debug("end of one cycle in endless loop ...")