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 ...")
