def run(self):
self.logger.debug("Entered run method for thread :" + str(self.name))
cur_way = None
curvature_valid = False
curvature = None
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
max_speed = None
max_speed_ahead = None
max_speed_ahead_dist = None
max_speed_prev = 0
start = time.time()
while True:
if time.time() - start > 2.5:
print("Mapd MapsdThread lagging by: %s" %
str(time.time() - start - 0.1))
if time.time() - start < 1.5:
time.sleep(1.0)
continue
else:
start = time.time()
self.logger.debug("starting new cycle in endless loop")
query_lock = self.sharedParams.get('query_lock', None)
query_lock.acquire()
gps = self.sharedParams['last_gps']
query_lock.release()
if gps is None:
continue
fix_ok = gps.flags & 1
self.logger.debug("fix_ok = %s" % str(fix_ok))
if not fix_ok or self.sharedParams[
'last_query_result'] is None or not self.sharedParams[
'cache_valid']:
self.logger.debug("fix_ok %s" % fix_ok)
self.logger.error("Error in fix_ok logic")
cur_way = None
curvature = None
max_speed_ahead = None
max_speed_ahead_dist = None
curvature_valid = False
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
map_valid = False
else:
map_valid = True
lat = gps.latitude
lon = gps.longitude
heading = gps.bearing
speed = gps.speed
query_lock.acquire()
cur_way = Way.closest(self.sharedParams['last_query_result'],
lat, lon, heading, cur_way)
query_lock.release()
if cur_way is not None:
self.logger.debug("cur_way is not None ...")
pnts, curvature_valid = cur_way.get_lookahead(
lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
if pnts is not None:
xs = pnts[:, 0]
ys = pnts[:, 1]
road_points = [float(x)
for x in xs], [float(y) for y in ys]
if speed < 5:
curvature_valid = False
if curvature_valid and pnts.shape[0] <= 3:
curvature_valid = False
else:
curvature_valid = False
upcoming_curvature = 0.
curvature = None
dist_to_turn = 0.
# The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found
if curvature_valid:
# Compute the curvature for each point
with np.errstate(divide='ignore'):
circles = [
circle_through_points(*p, direction=True)
for p in zip(pnts, pnts[1:], pnts[2:])
]
circles = np.asarray(circles)
radii = np.nan_to_num(circles[:, 2])
radii[abs(radii) < 15.] = 10000
if cur_way.way.tags[
'highway'] == 'trunk_link' or cur_way.way.tags[
'highway'] == 'motorway_link':
radii = radii * 1.6 # https://media.springernature.com/lw785/springer-static/image/chp%3A10.1007%2F978-3-658-01689-0_21/MediaObjects/298553_35_De_21_Fig65_HTML.gif
elif cur_way.way.tags['highway'] == 'motorway':
radii = radii * 2.8
curvature = 1. / radii
rate = [
rate_curvature_points(*p)
for p in zip(pnts[1:], pnts[2:], curvature[0:],
curvature[1:])
]
rate = ([0] + rate)
curvature = np.abs(curvature)
curvature = np.multiply(
np.minimum(np.multiply(rate, 4000) + 0.7, 1.1),
curvature)
# Index of closest point
closest = np.argmin(np.linalg.norm(pnts, axis=1))
dist_to_closest = pnts[
closest,
0] # We can use x distance here since it should be close
# Compute distance along path
dists = list()
dists.append(0)
for p, p_prev in zip(pnts, pnts[1:, :]):
dists.append(dists[-1] +
np.linalg.norm(p - p_prev))
dists = np.asarray(dists)
dists = dists - dists[closest] + dist_to_closest
dists = dists[1:-1]
close_idx = np.logical_and(dists > 0, dists < 500)
dists = dists[close_idx]
curvature = curvature[close_idx]
if len(curvature):
curvature = np.nan_to_num(curvature)
upcoming_curvature = np.amax(curvature)
dist_to_turn = np.amin(dists[np.logical_and(
curvature >= upcoming_curvature,
curvature <= upcoming_curvature)])
else:
upcoming_curvature = 0.
dist_to_turn = 999
dat = messaging.new_message()
dat.init('liveMapData')
last_gps = self.sharedParams.get('last_gps', None)
if last_gps is not None:
dat.liveMapData.lastGps = last_gps
if cur_way is not None:
dat.liveMapData.wayId = cur_way.id
self.sharedParams['osm_way_id'] = cur_way.id
# Speed limit
max_speed = cur_way.max_speed(heading)
max_speed_ahead = None
max_speed_ahead_dist = None
if max_speed is not None:
max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(
max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
else:
max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(
speed * 1.1, lat, lon, heading,
MAPS_LOOKAHEAD_DISTANCE)
# TODO: anticipate T junctions and right and left hand turns based on indicator
if max_speed_ahead is not None and max_speed_ahead_dist is not None:
dat.liveMapData.speedLimitAheadValid = True
dat.liveMapData.speedLimitAhead = float(max_speed_ahead)
dat.liveMapData.speedLimitAheadDistance = float(
max_speed_ahead_dist)
if max_speed is not None:
if abs(max_speed - max_speed_prev) > 0.1:
max_speed_prev = max_speed
# Curvature
dat.liveMapData.curvatureValid = curvature_valid
dat.liveMapData.curvature = float(upcoming_curvature)
dat.liveMapData.distToTurn = float(dist_to_turn)
if road_points is not None:
dat.liveMapData.roadX, dat.liveMapData.roadY = road_points
if curvature is not None:
dat.liveMapData.roadCurvatureX = [float(x) for x in dists]
dat.liveMapData.roadCurvature = [
float(x) for x in curvature
]
else:
self.sharedParams['osm_way_id'] = 0
if max_speed is not None and map_valid:
dat.liveMapData.speedLimitValid = True
dat.liveMapData.speedLimit = max_speed
dat.liveMapData.mapValid = map_valid
self.logger.debug("Sending ... liveMapData ... %s", str(dat))
self.pm.send('liveMapData', dat)
def mapsd_thread():
global last_gps
context = zmq.Context()
poller = zmq.Poller()
gps_external_sock = messaging.sub_sock(context, 8032, poller, conflate=True)
map_data_sock = messaging.pub_sock(context, 8065)
traffic_data_sock = messaging.sub_sock(context, 8208, poller, conflate=True)
cur_way = None
curvature_valid = False
curvature = None
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
speedLimittraffic = 0
speedLimittraffic_prev = 0
max_speed = None
max_speed_ahead = None
max_speed_ahead_dist = None
max_speed_prev = 0
speedLimittrafficvalid = False
while True:
gps_ext = None
traffic = None
for socket, event in poller.poll(0):
if socket is gps_external_sock:
gps_ext = messaging.recv_one(socket)
elif socket is traffic_data_sock:
traffic = arne182.LiveTrafficData.from_bytes(socket.recv())
if traffic is not None:
if traffic.speedLimitValid:
speedLimittraffic = traffic.speedLimit
if abs(speedLimittraffic_prev - speedLimittraffic) > 0.1:
speedLimittrafficvalid = True
speedLimittraffic_prev = speedLimittraffic
if traffic.speedAdvisoryValid:
speedLimittrafficAdvisory = traffic.speedAdvisory
speedLimittrafficAdvisoryvalid = True
else:
speedLimittrafficAdvisoryvalid = False
else:
speedLimittrafficAdvisoryvalid = False
speedLimittrafficvalid = False
if gps_ext is not None:
gps = gps_ext.gpsLocationExternal
else:
continue
save_gps_data(gps)
last_gps = gps
fix_ok = gps.flags & 1
if gps.accuracy > 2.0 and not speedLimittrafficvalid:
fix_ok = False
if not fix_ok or last_query_result is None or not cache_valid:
cur_way = None
curvature = None
max_speed_ahead = None
max_speed_ahead_dist = None
curvature_valid = False
upcoming_curvature = 0.
dist_to_turn = 0.
road_points = None
map_valid = False
else:
map_valid = True
lat = gps.latitude
lon = gps.longitude
heading = gps.bearing
speed = gps.speed
query_lock.acquire()
cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way)
if cur_way is not None:
pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
if pnts is not None:
xs = pnts[:, 0]
ys = pnts[:, 1]
road_points = [float(x) for x in xs], [float(y) for y in ys]
if speed < 5:
curvature_valid = False
if curvature_valid and pnts.shape[0] <= 3:
curvature_valid = False
else:
curvature_valid = False
upcoming_curvature = 0.
curvature = None
dist_to_turn = 0.
# The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found
if curvature_valid:
# Compute the curvature for each point
with np.errstate(divide='ignore'):
circles = [circle_through_points(*p, direction=True) for p in zip(pnts, pnts[1:], pnts[2:])]
circles = np.asarray(circles)
radii = np.nan_to_num(circles[:, 2])
radii[radii < 15.] = 10000
if cur_way.way.tags['highway'] == 'trunk':
radii = radii*1.6 # https://media.springernature.com/lw785/springer-static/image/chp%3A10.1007%2F978-3-658-01689-0_21/MediaObjects/298553_35_De_21_Fig65_HTML.gif
if cur_way.way.tags['highway'] == 'motorway' or cur_way.way.tags['highway'] == 'motorway_link':
radii = radii*2.8
curvature = 1. / radii
rate = [rate_curvature_points(*p) for p in zip(pnts[1:], pnts[2:],curvature[0:],curvature[1:])]
rate = ([0] + rate)
curvature = np.multiply(np.minimum(np.multiply(rate,4000)+0.7,1.1),curvature)
# Index of closest point
closest = np.argmin(np.linalg.norm(pnts, axis=1))
dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close
# Compute distance along path
dists = list()
dists.append(0)
for p, p_prev in zip(pnts, pnts[1:, :]):
dists.append(dists[-1] + np.linalg.norm(p - p_prev))
dists = np.asarray(dists)
dists = dists - dists[closest] + dist_to_closest
dists = dists[1:-1]
close_idx = np.logical_and(dists > 0, dists < 500)
dists = dists[close_idx]
curvature = curvature[close_idx]
if len(curvature):
curvature = np.nan_to_num(curvature)
upcoming_curvature = np.amax(curvature)
dist_to_turn =np.amin(dists[np.logical_and(curvature >= np.amax(curvature), curvature <= np.amax(curvature))])
else:
upcoming_curvature = 0.
dist_to_turn = 999
query_lock.release()
dat = messaging.new_message()
dat.init('liveMapData')
if last_gps is not None:
dat.liveMapData.lastGps = last_gps
if cur_way is not None:
dat.liveMapData.wayId = cur_way.id
# Speed limit
max_speed = cur_way.max_speed(heading)
max_speed_ahead = None
max_speed_ahead_dist = None
if max_speed is not None:
max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
else:
max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(speed*1.1, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
# TODO: anticipate T junctions and right and left hand turns based on indicator
if max_speed_ahead is not None and max_speed_ahead_dist is not None:
dat.liveMapData.speedLimitAheadValid = True
dat.liveMapData.speedLimitAhead = float(max_speed_ahead)
dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist)
if max_speed is not None:
if abs(max_speed - max_speed_prev) > 0.1:
speedLimittrafficvalid = False
max_speed_prev = max_speed
advisory_max_speed = cur_way.advisory_max_speed()
if speedLimittrafficAdvisoryvalid:
dat.liveMapData.speedAdvisoryValid = True
dat.liveMapData.speedAdvisory = speedLimittrafficAdvisory / 3.6
else:
if advisory_max_speed is not None:
dat.liveMapData.speedAdvisoryValid = True
dat.liveMapData.speedAdvisory = advisory_max_speed
# Curvature
dat.liveMapData.curvatureValid = curvature_valid
dat.liveMapData.curvature = float(upcoming_curvature)
dat.liveMapData.distToTurn = float(dist_to_turn)
if road_points is not None:
dat.liveMapData.roadX, dat.liveMapData.roadY = road_points
if curvature is not None:
dat.liveMapData.roadCurvatureX = [float(x) for x in dists]
dat.liveMapData.roadCurvature = [float(x) for x in curvature]
if speedLimittrafficvalid:
if speedLimittraffic > 0.1:
dat.liveMapData.speedLimitValid = True
dat.liveMapData.speedLimit = speedLimittraffic / 3.6
map_valid = False
else:
speedLimittrafficvalid = False
else:
if max_speed is not None and map_valid:
dat.liveMapData.speedLimitValid = True
dat.liveMapData.speedLimit = max_speed
dat.liveMapData.mapValid = map_valid
map_data_sock.send(dat.to_bytes())