def ned2ecef(self, ned, origin):
"""
Converts ned local tangent plane coordinates into ecef coordinates
using origin as the ecef point of tangency.
Input: ned - (north, east, down) in (m, m, m)
origin - (x0, y0, z0) in (m, m, m)
Output: ecef - (x, y, z) in (m, m, m)
"""
llaOrigin = self.ecef2lla(origin)
lat = geo.deg2rad(llaOrigin[0])
lon = geo.deg2rad(llaOrigin[1])
Rt2e = array([[-sin(lat)*cos(lon), -sin(lon), -cos(lat)*cos(lon)],
[-sin(lat)*sin(lon), cos(lon), -cos(lat)*sin(lon)],
[cos(lat), 0., -sin(lat)]])
return list(dot(Rt2e, array(ned)) + array(origin))
def ned2ecef(self, ned, origin):
"""
Converts ned local tangent plane coordinates into ecef coordinates
using origin as the ecef point of tangency.
Input: ned - (north, east, down) in (m, m, m)
origin - (x0, y0, z0) in (m, m, m)
Output: ecef - (x, y, z) in (m, m, m)
"""
llaOrigin = self.ecef2lla(origin)
lat = geo.deg2rad(llaOrigin[0])
lon = geo.deg2rad(llaOrigin[1])
Rt2e = array([[-sin(lat) * cos(lon), -sin(lon), -cos(lat) * cos(lon)],
[-sin(lat) * sin(lon),
cos(lon), -cos(lat) * sin(lon)],
[cos(lat), 0., -sin(lat)]])
return list(dot(Rt2e, array(ned)) + array(origin))
def lla2ecef(self, lla):
"""
Convert lat, lon, alt to Earth-centered, Earth-fixed coordinates.
Input: lla - (lat, lon, alt) in (decimal degrees, decimal degees, m)
Output: ecef - (x, y, z) in (m, m, m)
"""
# Decompose the input
lat = geo.deg2rad(lla[0])
lon = geo.deg2rad(lla[1])
alt = lla[2]
# Calculate length of the normal to the ellipsoid
N = self.a / sqrt(1 - (self.e * sin(lat))**2)
# Calculate ecef coordinates
x = (N + alt) * cos(lat) * cos(lon)
y = (N + alt) * cos(lat) * sin(lon)
z = (N * (1 - self.e**2) + alt) * sin(lat)
# Return the ecef coordinates
return (x, y, z)
def lla2ecef(self, lla):
"""
Convert lat, lon, alt to Earth-centered, Earth-fixed coordinates.
Input: lla - (lat, lon, alt) in (decimal degrees, decimal degees, m)
Output: ecef - (x, y, z) in (m, m, m)
"""
# Decompose the input
lat = geo.deg2rad(lla[0])
lon = geo.deg2rad(lla[1])
alt = lla[2]
# Calculate length of the normal to the ellipsoid
N = self.a / sqrt(1 - (self.e * sin(lat))**2)
# Calculate ecef coordinates
x = (N + alt) * cos(lat) * cos(lon)
y = (N + alt) * cos(lat) * sin(lon)
z = (N * (1 - self.e**2) + alt) * sin(lat)
# Return the ecef coordinates
return (x, y, z)
def ecef2ned(self, ecef, origin):
"""
Converts ecef coordinates into local tangent plane where the
origin is the origin in ecef coordinates.
Input: ecef - (x, y, z) in (m, m, m)
origin - (x0, y0, z0) in (m, m, m)
Output: ned - (north, east, down) in (m, m, m)
"""
# print('ecef = ', ecef)
# print('origin = ', origin)
llaOrigin = self.ecef2lla(origin)
lat = geo.deg2rad(llaOrigin[0])
lon = geo.deg2rad(llaOrigin[1])
Re2t = array([[-sin(lat)*cos(lon), -sin(lat)*sin(lon), cos(lat)],
[-sin(lon), cos(lon), 0],
[-cos(lat)*cos(lon), -cos(lat)*sin(lon), -sin(lat)]])
# print('type ecef = %s' % type(ecef))
# print('type origin = %s' % type(origin))
return list(dot(Re2t, array(ecef) - array(origin)))
def ecef2ned(self, ecef, origin):
"""
Converts ecef coordinates into local tangent plane where the
origin is the origin in ecef coordinates.
Input: ecef - (x, y, z) in (m, m, m)
origin - (x0, y0, z0) in (m, m, m)
Output: ned - (north, east, down) in (m, m, m)
"""
# print('ecef = ', ecef)
# print('origin = ', origin)
llaOrigin = self.ecef2lla(origin)
lat = geo.deg2rad(llaOrigin[0])
lon = geo.deg2rad(llaOrigin[1])
Re2t = array([[-sin(lat) * cos(lon), -sin(lat) * sin(lon),
cos(lat)], [-sin(lon), cos(lon), 0],
[-cos(lat) * cos(lon), -cos(lat) * sin(lon), -sin(lat)]])
# print('type ecef = %s' % type(ecef))
# print('type origin = %s' % type(origin))
return list(dot(Re2t, array(ecef) - array(origin)))
def get_variables(data):
"""
выдает основные переменные
"""
x, y = data['x'], data['y']
s, d = data['s'], data['d']
end_path_s, end_path_d = data['end_path_s'], data['end_path_d']
prev_x, prev_y = data['previous_path_x'], data['previous_path_y']
if len(prev_x) == 0:
end_path_s, end_path_d = s, d
ref_yaw = deg2rad(data['yaw'])
other_cars = np.array([[a[-2], a[-1],
sqrt(a[-3]**2 + a[-4]**2)]
for a in data['sensor_fusion']])
#добавляем к s скорость * время
for i in range(len(other_cars)):
other_cars[i][0] += other_cars[i][2] * len(prev_x) * 0.02
#last_s,last_d=data['end_path_s'], data['end_path_d']
while len(last_coords) > len(prev_x):
last_coords.pop(0)
if len(last_coords) == 0:
last_coords.append((s, d))
#todo - здесь должен быть cos/sin
if len(prev_x) == 0:
prev_x, prev_y = [x - 0.001, x], [y, y]
if len(prev_x) == 1:
prev_x, prev_y = [prev_x[0] - 0.001, prev_x[0]], [prev_y[0], prev_y[0]]
car_speed = distance(prev_x[-1], prev_y[-1], prev_x[-2], prev_y[-2]) / 0.02
return prev_x, prev_y, end_path_s, end_path_d, car_speed, ref_yaw, other_cars
def lla2utm(self, lla):
"""
Converts lat, lon, alt to Universal Transverse Mercator coordinates
Input: lla - (lat, lon, alt) in (decimal degrees, decimal degrees, m)
Output: utm - (easting, northing, upping) in (m, m, m)
info - (zone, scale factor)
Algorithm from:
Snyder, J. P., Map Projections-A Working Manual, U.S. Geol. Surv.
Prof. Pap., 1395, 1987
Code segments from pygps project, Russ Nelson
"""
# Decompose lla
lat = lla[0]
lon = lla[1]
alt = lla[2]
# Determine the zone number
zoneNumber = int((lon+180.)/6) + 1
# Special zone for Norway
if (56. <= lat < 64.) and (3. <= lon < 12.):
zoneNumber = 32
# Special zones for Svalbard
if 72. <= lat < 84.:
if 0. <= lon < 9.:
zoneNumber = 31
elif 9. <= lon < 21.:
zoneNumber = 33
elif 21. <= lon < 33.:
zoneNumber = 35
elif 33. <= lon < 42.:
zoneNumber = 37
# Format the zone
zone = "%d%c" % (zoneNumber, self.utmLetterDesignator(lat))
# Determine longitude origin
lonOrigin = (zoneNumber - 1) * 6 - 180 + 3
# Convert to radians
latRad = geo.deg2rad(lat)
lonRad = geo.deg2rad(lon)
lonOriginRad = geo.deg2rad(lonOrigin)
# Conversion constants
k0 = 0.9996
eSquared = self.e**2
ePrimeSquared = eSquared/(1.-eSquared)
N = self.a/sqrt(1.-eSquared*sin(latRad)**2)
T = tan(latRad)**2
C = ePrimeSquared*cos(latRad)**2
A = (lonRad - lonOriginRad)*cos(latRad)
M = self.a*(
(1. -
eSquared/4. -
3.*eSquared**2/64. -
5.*eSquared**3/256)*latRad -
(3.*eSquared/8. +
3.*eSquared**2/32. +
45.*eSquared**3/1024.)*sin(2.*latRad) +
(15.*eSquared**2/256. +
45.*eSquared**3/1024.)*sin(4.*latRad) -
(35.*eSquared**3/3072.)*sin(6.*latRad))
M0 = 0.
# Calculate coordinates
x = k0*N*(
A+(1-T+C)*A**3/6. +
(5.-18.*T+T**2+72.*C-58.*ePrimeSquared)*A**5/120.) + 500000.
y = k0*(
M-M0+N*tan(latRad)*(
A**2/2. +
(5.-T+9.*C+4.*C**2)*A**4/24. +
(61.-58.*T+T**2+600.*C-330.*ePrimeSquared)*A**6/720.))
# Calculate scale factor
k = k0*(1 +
(1+C)*A**2/2. +
(5.-4.*T+42.*C+13.*C**2-28.*ePrimeSquared)*A**4/24. +
(61.-148.*T+16.*T**2)*A**6/720.)
utm = [x, y, alt]
info = [zone, k]
return utm, info
from geo import calc_dist, deg2rad, is_prop_latlon
if __name__ == '__main__':
"""Start of program execution."""
# Read configuration file...
config = configparser.ConfigParser()
config.read('config.ini')
# Read customer records from file...
cust_records = read(
join(config['DEFAULT']['DATA_DIR'], config['DEFAULT']['GIST_FILE']))
# Convert intercom office coordinates from degrees to radians...
off_lat_deg = config['INPUTS']['OFFICE_LAT']
if is_prop_latlon(off_lat_deg, True):
off_lat = deg2rad(float(off_lat_deg))
else:
print("Intercom Office latitude is not in a proper float format.")
print("Exitiing application.")
exit()
off_lon_deg = config['INPUTS']['OFFICE_LON']
if is_prop_latlon(off_lon_deg, False):
off_lon = deg2rad(float(off_lon_deg))
else:
print("Intercom Office longitude is not in a proper float format.")
print("Exitiing application.")
exit()
# Loop through all records, find distance from intercom office,
# and store the customer records in a dictionary who is within 100 kms...
def lla2utm(self, lla):
"""
Converts lat, lon, alt to Universal Transverse Mercator coordinates
Input: lla - (lat, lon, alt) in (decimal degrees, decimal degrees, m)
Output: utm - (easting, northing, upping) in (m, m, m)
info - (zone, scale factor)
Algorithm from:
Snyder, J. P., Map Projections-A Working Manual, U.S. Geol. Surv.
Prof. Pap., 1395, 1987
Code segments from pygps project, Russ Nelson
"""
# Decompose lla
lat = lla[0]
lon = lla[1]
alt = lla[2]
# Determine the zone number
zoneNumber = int((lon + 180.) / 6) + 1
# Special zone for Norway
if (56. <= lat < 64.) and (3. <= lon < 12.):
zoneNumber = 32
# Special zones for Svalbard
if 72. <= lat < 84.:
if 0. <= lon < 9.:
zoneNumber = 31
elif 9. <= lon < 21.:
zoneNumber = 33
elif 21. <= lon < 33.:
zoneNumber = 35
elif 33. <= lon < 42.:
zoneNumber = 37
# Format the zone
zone = "%d%c" % (zoneNumber, self.utmLetterDesignator(lat))
# Determine longitude origin
lonOrigin = (zoneNumber - 1) * 6 - 180 + 3
# Convert to radians
latRad = geo.deg2rad(lat)
lonRad = geo.deg2rad(lon)
lonOriginRad = geo.deg2rad(lonOrigin)
# Conversion constants
k0 = 0.9996
eSquared = self.e**2
ePrimeSquared = eSquared / (1. - eSquared)
N = self.a / sqrt(1. - eSquared * sin(latRad)**2)
T = tan(latRad)**2
C = ePrimeSquared * cos(latRad)**2
A = (lonRad - lonOriginRad) * cos(latRad)
M = self.a * ((1. - eSquared / 4. - 3. * eSquared**2 / 64. -
5. * eSquared**3 / 256) * latRad -
(3. * eSquared / 8. + 3. * eSquared**2 / 32. +
45. * eSquared**3 / 1024.) * sin(2. * latRad) +
(15. * eSquared**2 / 256. + 45. * eSquared**3 / 1024.) *
sin(4. * latRad) -
(35. * eSquared**3 / 3072.) * sin(6. * latRad))
M0 = 0.
# Calculate coordinates
x = k0 * N * (A + (1 - T + C) * A**3 / 6. +
(5. - 18. * T + T**2 + 72. * C - 58. * ePrimeSquared) *
A**5 / 120.) + 500000.
y = k0 * (M - M0 + N * tan(latRad) *
(A**2 / 2. + (5. - T + 9. * C + 4. * C**2) * A**4 / 24. +
(61. - 58. * T + T**2 + 600. * C - 330. * ePrimeSquared) *
A**6 / 720.))
# Calculate scale factor
k = k0 * (1 + (1 + C) * A**2 / 2. +
(5. - 4. * T + 42. * C + 13. * C**2 - 28. * ePrimeSquared) *
A**4 / 24. + (61. - 148. * T + 16. * T**2) * A**6 / 720.)
utm = [x, y, alt]
info = [zone, k]
return utm, info
