def get_traj_bearing(traj, airport_lat, airport_lon, airport_altitude, ind_f, ind_l):
lat_f, lon_f, _ = pm.enu2geodetic(traj[ind_f,1], traj[ind_f,2], traj[ind_f,3]/10, airport_lat, airport_lon, airport_altitude)
lat_l, lon_l, _ = pm.enu2geodetic(traj[ind_l,1], traj[ind_l,2], traj[ind_l,3]/10, airport_lat, airport_lon, airport_altitude)
lat_f, lon_f = radians(lat_f), radians(lon_f)
lat_l, lon_l = radians(lat_l), radians(lon_l)
dLon = lon_l - lon_f
y = sin(dLon) * cos(lat_l)
x = cos(lat_f) * sin(lat_l) - sin(lat_f) * cos(lat_l) * cos(dLon)
traj_bearing = (np.rad2deg(atan2(y, x)) + 360) % 360
return traj_bearing
def test_ned():
xyz = pm.aer2ecef(*aer0, *lla0)
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
lla = pm.aer2geodetic(*aer0, *lla0)
assert pm.aer2ned(*aer0) == approx(ned0)
with pytest.raises(ValueError):
pm.aer2ned(aer0[0], aer0[1], -1)
assert pm.enu2aer(*enu) == approx(aer0)
assert pm.enu2aer(*enu, deg=False) == approx(raer0)
assert pm.ned2aer(*ned) == approx(aer0)
assert pm.ecef2ned(*xyz, *lla0) == approx(ned)
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
# %%
assert pm.ecef2enuv(vx, vy, vz, *lla0[:2]) == approx((ve, vn, vu))
assert pm.ecef2nedv(vx, vy, vz, *lla0[:2]) == approx((vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
assert pm.enu2geodetic(*enu3, *lla0) == approx(lla)
assert pm.ned2geodetic(*ned3, *lla0) == approx(lla)
def test_ned():
xyz = pm.aer2ecef(*aer0, *lla0)
enu = pm.aer2enu(*aer0)
ned = (enu[1], enu[0], -enu[2])
lla = pm.aer2geodetic(*aer0, *lla0)
assert pm.aer2ned(*aer0) == approx(ned0)
with pytest.raises(ValueError):
pm.aer2ned(aer0[0], aer0[1], -1)
assert pm.enu2aer(*enu) == approx(aer0)
assert pm.enu2aer(*enu, deg=False) == approx(raer0)
assert pm.ned2aer(*ned) == approx(aer0)
assert pm.ecef2ned(*xyz, *lla0) == approx(ned)
assert pm.ned2ecef(*ned, *lla0) == approx(xyz)
# %%
assert pm.ecef2enuv(vx, vy, vz, *lla0[:2]) == approx((ve, vn, vu))
assert pm.ecef2nedv(vx, vy, vz, *lla0[:2]) == approx((vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
assert pm.enu2geodetic(*enu3, *lla0) == approx(lla)
assert pm.ned2geodetic(*ned3, *lla0) == approx(lla)
def cal_ex_ey(x, y, yaw, lat0, lon0, count, remarks, color, kml_color, shape):
h = 2.5
yaw = float(yaw)
#c = float(yaw*math.pi/180)
c = float(yaw * math.pi /
180) + math.pi # require to add PI to correct the orientation
#print(-y*math.pi/pano_image_height + math.pi/2)
z = -h / math.tan(-y * math.pi / pano_image_height + math.pi / 2)
ex = math.sin(x * 2 * math.pi / pano_image_width - math.pi + c) * z
ey = math.cos(x * 2 * math.pi / pano_image_width - math.pi + c) * z
#print(x,y,c,z,ex,ey)
u = z
e = ex
n = ey
h0 = 2.5
#try:
r = pymap3d.enu2geodetic(e, n, u, float(lat0), float(lon0), h0)
# http://www.copypastemap.com/
distance = cal_distance_latlon((lat0, lon0), (r[0], r[1]))
angle = angle_from_coordinate((lat0, lon0), (r[0], r[1]))
#print(angle)
return distance, angle, r[0], r[1]
def updateLatlonelFromENU(data, verbose=True, decimals=8):
'''
Given data, this will take the ENU data and use it to update the latlonel data in a deep copy of the original dict.
Parameters
----------
data : dict
The dict generated when loading a configuration from a json file.
'''
data = copy.deepcopy(data) #So as to not edit the original.
origin = data['origin'][
'latlonel'] #Lat Lon Elevation, use for generating ENU from other latlonel values.
for mast in range(4):
for key in ['physical', 'hpol', 'vpol']:
if numpy.asarray(
data['antennas']['ant%i' % mast][key]['enu']).size > 0:
data['antennas'][
'ant%i' %
mast][key]['latlonel'] = numpy.round(numpy.asarray(
pm.enu2geodetic(
data['antennas']['ant%i' % mast][key]['enu'][0],
data['antennas']['ant%i' % mast][key]['enu'][1],
data['antennas']['ant%i' % mast][key]['enu'][2],
origin[0], origin[1], origin[2])),
decimals=decimals)
else:
if verbose:
print(key + ' does not contain ENU data for mast %i %s.' %
(mast, key))
return data
def c(points):
lat, lon, alt = p3d.enu2geodetic(points[:, 0], points[:,
1],
points[:, 2], fromCrs.lat,
fromCrs.lon, fromCrs.alt)
return CoordinateConverter.__getPyprojFunc(
Geodetic().getProjStr(),
toCrs.getProjStr())(np.column_stack((lon, lat, alt)))
def enu2geo(self, x, y):
lat, lon, _ = p3d.enu2geodetic(x,
y,
0,
self.lat0,
self.lon0,
0,
deg=True,
ell=p3d.Ellipsoid('wgs84'))
return lat, lon
def desiredENU2geo(self, x_L, y_L, z): """Converts from local ENU grid coordinates to geoditic LLA. It requires local_rot, origin LLA """ x = cos(self.local_rot)*x_L - sin(self.local_rot)*y_L y = sin(self.local_rot)*x_L + cos(self.local_rot)*y_L lat0 = self.origin[0] lon0 = self.origin[1] lat, lon, alt = pm.enu2geodetic(x, y, z, lat0, lon0, self.h0) return lat, lon, alt
def extend_flight_path_earth(self, launch_point_LLH):
lat, lon, h = pm.enu2geodetic(self.pos_ENU_log[:self.index_landing, 0],
self.pos_ENU_log[:self.index_landing, 1],
self.pos_ENU_log[:self.index_landing, 2],
launch_point_LLH[0], launch_point_LLH[1],
launch_point_LLH[2]) # lat, lon, h
self.pos_LLH_log = np.c_[lat, lon, h]
ecef_x, ecef_y, ecef_z = pm.enu2ecef(
self.pos_ENU_log[:self.index_landing,
0], self.pos_ENU_log[:self.index_landing, 1],
self.pos_ENU_log[:self.index_landing, 2], launch_point_LLH[0],
launch_point_LLH[1], launch_point_LLH[2])
self.pos_ECEF_log = np.c_[ecef_x, ecef_y, ecef_z]
plt.figure()
plt.plot(self.time_log[:self.index_landing],
self.pos_ECEF_log[:self.index_landing, 0] / 1e3,
label='X')
plt.plot(self.time_log[:self.index_landing],
self.pos_ECEF_log[:self.index_landing, 1] / 1e3,
label='Y')
plt.plot(self.time_log[:self.index_landing],
self.pos_ECEF_log[:self.index_landing, 2] / 1e3,
label='Z')
plt.xlabel('Time [s]')
plt.ylabel('Position ECEF [km]')
plt.grid()
plt.legend()
plt.savefig(self.result_name + '_Pos_ECEF.png')
kml = simplekml.Kml(open=1)
Log_LLH = []
for i in range(len(self.pos_LLH_log[:, 0])):
if 0 == i % 10:
Log_LLH.append([
self.pos_LLH_log[i, 1], self.pos_LLH_log[i, 0],
self.pos_LLH_log[i, 2]
])
line = kml.newlinestring()
line.style.linestyle.width = 4
line.style.linestyle.color = simplekml.Color.red
line.extrude = 1
line.altitudemode = simplekml.AltitudeMode.absolute
line.coords = Log_LLH
line.style.linestyle.colormode = simplekml.ColorMode.random
kml.save(self.result_name + '_trajectory.kml')
def test_ned_geodetic():
lla = pm.aer2geodetic(*aer0, *lla0)
enu3 = pm.geodetic2enu(*lla, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(*lla, *lla0) == approx(ned3)
lat, lon, alt = pm.enu2geodetic(*enu3, *lla0)
assert lat == approx(lla[0])
assert lon == approx(lla[1])
assert alt == approx(lla[2])
lat, lon, alt = pm.ned2geodetic(*ned3, *lla0)
assert lat == approx(lla[0])
assert lon == approx(lla[1])
assert alt == approx(lla[2])
def get_box_coordinates(self, box):
local_x = (self.get_box_x(box)[0] / 1e3 +
self.get_box_x(box)[1] / 1e3) / 2
local_y = self.real_y / 1e3
local_z = self.real_z / 1e3
width = np.abs(
self.get_box_x(box)[1] / 1e3 - self.get_box_x(box)[0] / 1e3)
east = np.cos(np.radians(self.origin[3])) * local_x + np.sin(
np.radians(self.origin[3])) * local_y
north = -np.sin(np.radians(self.origin[3])) * local_x + np.cos(
np.radians(self.origin[3])) * local_y
# Global
lat, long, alt = pm.enu2geodetic(east, north, local_z, self.origin[0],
self.origin[1], self.origin[2])
output = [lat, long, alt, width]
return output
def to_geodetic(self, point):
"""
ENU to Azimuth, Elevation, Range
Parameters
----------
point : collection
ENU point (meters, meters, meters)
Results
-------
WGS84 point (lon, lat, alt)
"""
return numpy.array([
x for x in xyz_to_yxz(
pymap3d.enu2geodetic(*point, *xyz_to_yxz(self.geodeticPoint)))
])
def test_ned_geodetic():
lat1, lon1, alt1 = pm.aer2geodetic(*aer0, *lla0)
enu3 = pm.geodetic2enu(lat1, lon1, alt1, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert pm.geodetic2ned(lat1, lon1, alt1, *lla0) == approx(ned3)
lat, lon, alt = pm.enu2geodetic(*enu3, *lla0)
assert lat == approx(lat1)
assert lon == approx(lon1)
assert alt == approx(alt1)
assert isinstance(lat, float)
assert isinstance(lon, float)
assert isinstance(alt, float)
lat, lon, alt = pm.ned2geodetic(*ned3, *lla0)
assert lat == approx(lat1)
assert lon == approx(lon1)
assert alt == approx(alt1)
assert isinstance(lat, float)
assert isinstance(lon, float)
assert isinstance(alt, float)
for t in t_range:
for i, traj in enumerate(syn_trajs): #for each traj.
ind = np.where(traj[1:, 0] == t)[0]
if ind != None:
point = {}
curr_t = start_t + datetime.timedelta(seconds=t.tolist() - min_t)
point['time'] = curr_t.strftime('%Y-%m-%dT%H:%M:%S')
# aircraft icon image with rotating headings
curr_xEast = traj[ind, 1] #(m)
curr_yNorth = traj[ind, 2] #(m)
curr_zUp = traj[ind, 3] / 10 #(m)
curr_lat, curr_lon, curr_alt = pm.enu2geodetic(
curr_xEast, curr_yNorth, curr_zUp, airport_lat, airport_lon,
airport_altitude)
curr_alt *= M_TO_FT
point['coordinates'] = [curr_lon[0], curr_lat[0]]
prev_xEast = traj[ind - 1, 1] #(m)
prev_yNorth = traj[ind - 1, 2] #(m)
prev_zUp = traj[ind - 1, 3] / 10 #(m)
prev_lat, prev_lon, prev_alt = pm.enu2geodetic(
prev_xEast, prev_yNorth, prev_zUp, airport_lat, airport_lon,
airport_altitude)
prev_alt *= M_TO_FT
heading = np.arctan2(curr_lat - prev_lat,
curr_lon - prev_lon) * 180 / np.pi
rotated_image = image.rotate(heading)
def test_ecefenu():
assert_allclose(pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt),
(a2x, a2y, a2z),
rtol=0.01,
err_msg='aer2ecef: {}'.format(
pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt)))
assert_allclose(pm.aer2enu(taz, tel, tsrange), (a2e, a2n, a2u),
rtol=0.01,
err_msg='aer2enu: ' + str(pm.aer2enu(taz, tel, tsrange)))
assert_allclose(pm.aer2ned(taz, tel, tsrange), (a2n, a2e, -a2u),
rtol=0.01,
err_msg='aer2ned: ' + str(pm.aer2ned(taz, tel, tsrange)))
assert_allclose(pm.ecef2enu(tx, ty, tz, tlat, tlon, talt), (e2e, e2n, e2u),
rtol=0.01,
err_msg='ecef2enu: {}'.format(
pm.ecef2enu(tx, ty, tz, tlat, tlon, talt)))
assert_allclose(pm.ecef2enuv(vx, vy, vz, tlat, tlon), (ve, vn, vu))
assert_allclose(pm.ecef2ned(tx, ty, tz, tlat, tlon, talt),
(e2n, e2e, -e2u),
rtol=0.01,
err_msg='ecef2ned: {}'.format(
pm.ecef2enu(tx, ty, tz, tlat, tlon, talt)))
assert_allclose(pm.ecef2nedv(vx, vy, vz, tlat, tlon), (vn, ve, -vu))
assert_allclose(pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt),
(a2la, a2lo, a2a),
rtol=0.01,
err_msg='aer2geodetic {}'.format(
pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt)))
assert_allclose(pm.ecef2aer(tx, ty, tz, tlat, tlon, talt),
(ec2az, ec2el, ec2rn),
rtol=0.01,
err_msg='ecef2aer {}'.format(
pm.ecef2aer(a2x, a2y, a2z, tlat, tlon, talt)))
#%%
assert_allclose(pm.enu2aer(te, tn, tu), (e2az, e2el, e2rn),
rtol=0.01,
err_msg='enu2aer: ' + str(pm.enu2aer(te, tn, tu)))
assert_allclose(pm.ned2aer(tn, te, -tu), (e2az, e2el, e2rn),
rtol=0.01,
err_msg='enu2aer: ' + str(pm.enu2aer(te, tn, tu)))
assert_allclose(pm.enu2geodetic(te, tn, tu, tlat, tlon,
talt), (lat2, lon2, alt2),
rtol=0.01,
err_msg='enu2geodetic: ' +
str(pm.enu2geodetic(te, tn, tu, tlat, tlon, talt)))
assert_allclose(pm.ned2geodetic(tn, te, -tu, tlat, tlon,
talt), (lat2, lon2, alt2),
rtol=0.01,
err_msg='enu2geodetic: ' +
str(pm.enu2geodetic(te, tn, tu, tlat, tlon, talt)))
assert_allclose(pm.enu2ecef(te, tn, tu, tlat, tlon, talt), (e2x, e2y, e2z),
rtol=0.01,
err_msg='enu2ecef: ' +
str(pm.enu2ecef(te, tn, tu, tlat, tlon, talt)))
assert_allclose(
pm.ned2ecef(tn, te, -tu, tlat, tlon, talt), (e2x, e2y, e2z),
rtol=0.01,
err_msg='ned2ecef: ' + str(pm.ned2ecef(tn, te, -tu, tlat, tlon, talt)))
def gpp2lpp(velocity, current_lat, current_lon, light, ALL_STOP):
# Initialize---------------------------------------------------------------
global Out_of_speed
global Out_of_accel
global Out_of_curvature
global Warning_obstacle
global MAX_LEFT_WIDTH
global MAX_RIGHT_WIDTH
global show_animation
matplotlib.use('TkAgg')
location = './'
Directory = os.listdir(location) # 경로에 있는 모든 파일들을 불러온다
channel_num = find_channel(2) # For Sending information to Control System
Channel = setUpChannel(channel_num)
Coordination = []
stop_line = []
HD_LIST = []
# HD Map 정보가 담겨져 있는 csv 파일을 가져온다
for name in Directory:
file_name = name.split('.')
if len(file_name) > 1:
if file_name[1] == 'csv':
HD_LIST.append(name)
for name in HD_LIST:
with open(name, 'r', encoding='UTF8') as f:
# Necessary----------------------------------------------
if name == "A2_LINK.csv": # 이 파일은 HD Map의 모든 노드 데이터들이 있는 파일이다. (필수 데이터)
all_data = csv.reader(f)
coord = []
for line in all_data:
if not line == []:
temp = [float(x) for x in line]
coord.append(temp)
Coordination = np.array(coord)
# Necessary----------------------------------------------
# OPTIONS-------------------------------------------------
elif name == "A2_STOP.csv": # 이 파일은 HD Map의 Stop Line 데이터들이 있는 파일이다. (옵션 데이터)
all_data = csv.reader(f)
tt = []
for line in all_data:
if not line == []:
temp = [float(x) for x in line]
tt.append(temp)
stop_line = np.array(tt)
# OPTIONS-------------------------------------------------
router = Router("car", "A2_LINK_OUT.osm") # HD Map이 있는 모든 데이터들을 가져온다. Readme.md 설명 참조
# Initialize---------------------------------------------------------------
# 차량의 위도 및 경도를 받았을 때 while문을 나온다
while True:
if current_lat.value > 0 and current_lon.value > 0:
lat = current_lat.value
lon = current_lon.value
break
# Start -----------------------------------------------------------------
start = router.findNode(lat, lon) # 나의 좌표
end = router.findNode(37.1111111, 126.11111111) # 목표 지점 좌표
status, route = router.doRoute(start, end) # Find the route - a list of OSM nodes (Global Path Planning)
if status == 'success':
routeLatLons = list(map(router.nodeLatLon, route)) # Get actual route coordinates
routeLatLons = np.array(routeLatLons)
# Essential (Change list)
result_path = []
for latlon in routeLatLons:
result_path.append([float(latlon[0]), float(latlon[1])])
result_path = np.array(result_path)
# plotting Coordination------------------------------------------ option
plt.title("Global Path Planning")
plt.plot(Coordination[:, 1], Coordination[:, 0], '*b', label="HD Map")
plt.plot(result_path[:, 1], result_path[:, 0], '--r', linewidth=3, label='Generated Path Trajectory')
plt.plot(result_path[0, 1], result_path[0, 0], 'Xm', markersize=10, label="Start Point")
plt.plot(result_path[-1, 1], result_path[-1, 0], 'Xg', markersize=10, label="End Point")
plt.plot(stop_line[:, 1], stop_line[:, 0], 'ok', markersize=5, label="Stop Lines")
plt.legend()
plt.show()
# plotting Coordination------------------------------------------ option
center = sum(Coordination) / len(Coordination) # Calculate ENU Center For Convert other LLH Data to ENU
# Create all lane------------------------- option
ENU_all = []
for llh in Coordination:
e, n, u = pm.geodetic2enu(llh[0], llh[1], llh[2], center[0], center[1], center[2])
ENU_all.append([e, n])
ENU_all = np.array(ENU_all)
# ------------------------------------------ option
# Create enu pqosition of the GPP result
temp_result = []
for llh in result_path:
e, n, u = pm.geodetic2enu(llh[0], llh[1], center[2], center[0], center[1], center[2])
temp_result.append([e, n])
result_path = np.array(temp_result)
# Create Stop Line by me
traffic_all = []
for llh in stop_line:
e, n, u = pm.geodetic2enu(llh[0], llh[1], llh[2], center[0], center[1], center[2])
traffic_all.append([e, n])
traffic_all = np.array(traffic_all)
# This is Local Path Planning----------------------------------
dx, dy, dyaw, dk, s = get_course(result_path[:, 0], result_path[:, 1]) # X, Y Yaw, Curvature, Index
# Initial Parameters ------------------------------------
# For LPP
c_d = 0.0 # current lateral position [m]
c_d_d = 0.0 # current lateral speed [m/s]
c_d_dd = 0.0 # current lateral acceleration [m/s]
# s0 = 0.0 # current course position
area = 50.0 # animation area length [m]
# For LPP
# For Comparing
heading = 0
time = 0.0
toggle = False
temp_light = 0
# For Comparing
# ----------------------------------------------
lists_dict = {}
for index, lists in enumerate(result_path):
lists_dict[index] = lists
start_time = datetime.now() # For Compare Times
obstacle = [[0, 0], [1, 1]] #example Obstacle
# LOCAL PATH PLANNING Start!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
while True:
if ALL_STOP.value == 1: # IF all stop toggle on, All Codes Stop
break
# Change my Coordinate to ENU
comp_x, comp_y, comp_z = pm.geodetic2enu(current_lat.value, current_lon.value, center[2], center[0], center[1], center[2])
# Compare my Coordination--------------------------
temp_dict = {}
for key, value in lists_dict.items():
point_position = np.hypot(value[0] - comp_x, value[1] - comp_y)
temp_dict[key] = point_position
POINT = min(temp_dict.keys(), key=lambda k: temp_dict[k])
s0 = s.s[POINT]
# For Car's left right light----------------------
# 차선 바꾸고 나서 가고 있는데 장애물 회피해야 됨, 그러면 이거
if Warning_obstacle == True:
temp_light = 0
# 깜빡이 처리============================================================
end_time = datetime.now()
SEC = (end_time - start_time).seconds # Second
if SEC > 3: # 만약 3초 동안 깜빡이 없으면
toggle = False
if temp_light == 0: # 이전에 켜진 깜박이가 없거나, 옆으로 갔다가 다시 돌아올 때
# 정밀지도 상에서 헤딩에 따라 피하고 난 다음 위치 선정 -> 헤딩이 일정 구간 안에 있으면 왼쪽 or 오른쪽
if heading > 150 and heading < 250: # 왼쪽으로 피함
MAX_LEFT_WIDTH = -LARGE
MAX_RIGHT_WIDTH = 1
elif heading > 300 or heading < 50: # 오른쪽으로 피함
MAX_LEFT_WIDTH = -1
MAX_RIGHT_WIDTH = LARGE
elif temp_light < 0: # 오른쪽으로만 계속 주행할 수 있도록 설정
MAX_LEFT_WIDTH = -LARGE * abs(temp_light)
MAX_RIGHT_WIDTH = -SHORTE * abs(temp_light)
elif temp_light > 0: # 왼쪽으로만 계속 주행할 수 있도록 설정
MAX_LEFT_WIDTH = SHORTE * abs(temp_light)
MAX_RIGHT_WIDTH = LARGE * abs(temp_light)
if light.value == -1: # 오른쪽 신호를 받았을 때
start_time = datetime.now()
if toggle == False: # 한번만 실행되게 만드는것
idx = np.abs(np.array(dx) - path.x[-1]).argmin() # 맨 앞의 LPP Position 값이 전체 구역에서 몇번째 index 가지고 있는지 추출 (X)
idy = np.abs(np.array(dy) - path.y[-1]).argmin() # 맨 앞의 LPP Position 값이 전체 구역에서 몇번째 index 가지고 있는지 추출 (Y)
obstacle.append([dx[idx], dy[idy]]) # 자리 이동할 수 있도록 하나의 장애물을 위에 추가
temp_light += light.value # 만약 오른쪽 신호를 받고 그 쪽으로 갔는데, 또 오른쪽 신호를 받으면 피할 ROI를 더 증가하도록 하기 위한 것
toggle = True # 한번 신호 받으면 그 다음 이 if 문으로 안 들어 오게 하는 toggle
if temp_light == 0:
MAX_LEFT_WIDTH = -SHORTE
MAX_RIGHT_WIDTH = SHORTE
elif temp_light < 0:
MAX_LEFT_WIDTH = LARGE * temp_light # 오른쪽으로 피함
MAX_RIGHT_WIDTH = SHORTE * temp_light
else:
MAX_LEFT_WIDTH = SHORTE * temp_light # 왼쪽으로 피함
MAX_RIGHT_WIDTH = LARGE * temp_light
if light.value == 1: # 왼쪽 신호를 받았을 때
start_time = datetime.now()
if toggle == False:
idx = np.abs(np.array(dx) - path.x[-1]).argmin() # 맨 앞의 LPP Position 값이 전체 구역에서 몇번째 index 가지고 있는지 추출 (X)
idy = np.abs(np.array(dy) - path.y[-1]).argmin() # 맨 앞의 LPP Position 값이 전체 구역에서 몇번째 index 가지고 있는지 추출 (Y)
obstacle.append([dx[idx], dy[idy]]) # 자리 이동할 수 있도록 하나의 장애물을 위에 추가
temp_light += light.value
toggle = True # 한번 신호 받으면 그 다음 이 if 문으로 안 들어 오게 하는 toggle
if temp_light == 0:
MAX_LEFT_WIDTH = -SHORTE
MAX_RIGHT_WIDTH = SHORTE
elif temp_light < 0:
MAX_LEFT_WIDTH = LARGE * temp_light # 오른쪽으로 피함
MAX_RIGHT_WIDTH = SHORTE * temp_light
else:
MAX_LEFT_WIDTH = SHORTE * temp_light # 왼쪽으로 피함
MAX_RIGHT_WIDTH = LARGE * temp_light
# 깜빡이 처리============================================================
# For Car's left right light----------------------
obstacle = np.array(obstacle) # Relate Coordination
path = frenet_optimal_planning(s, s0, velocity.value, c_d, c_d_d, c_d_dd, obstacle)
c_d = path.d[1]
c_d_d = path.d_d[1]
c_d_dd = path.d_dd[1]
time = time + DT
heading = math.atan2(path.x[1] - path.x[0], path.y[1] - path.y[0]) * 180 / math.pi + 180 # 현재 값과 다음 값의 방향을 비교해서 Heading값 추출하기
# update my coordination ---------------------------------
# stop line 찾는 법은 간단하게 코사인 유사도 거리 방법 및 유클라디안 거리 검출 사용함====================
INDEXS = {}
point = np.array([path.x[0], path.y[0]])
last_point = np.array([path.x[int(len(path.x) / 2)], path.y[int(len(path.y) / 2)]])
comp_heading = math.atan2(last_point[0] - point[0], last_point[1] - point[1]) * 180 / math.pi + 180
for i in range(len(traffic_all)):
comp = np.array(traffic_all[i])
point_position = np.array(comp - point)
# Heading ----------->
heading_traffic = math.atan2(point_position[0], point_position[1]) * 180 / math.pi + 180
point_pos = np.sqrt((point_position[0] ** 2) + (point_position[1] ** 2)) # L2 norm vector -> 유클라디안 거리 검출임
if abs(abs(heading_traffic) - abs(comp_heading)) < 10: # 대충 Cosine 유사도 거리 검출임
INDEXS[i] = point_pos
if len(INDEXS) > 0:
POINT = min(INDEXS.keys(), key=lambda k: INDEXS[k])
else:
POINT = 0
stop_lat, stop_lon, stop_h = pm.enu2geodetic(traffic_all[POINT][0], traffic_all[POINT][1], center[2],
center[0], center[1], center[2])
# stop line 찾는 법은 간단하게 코사인 유사도 거리 방법 및 유클라디안 거리 검출 사용함====================
# Find next Coordination based on Lookahead=====================================================================
lookahead = velocity.value * 0.2 + 4.5
num = 0
for i in range(len(path.x)):
distance = np.hypot(path.x[i] - path.x[0], path.y[i] - path.y[0])
dists = distance - lookahead
if dists > 0:
num = i
break
# ENU to LLH
next_lat, next_lon, next_alt = pm.enu2geodetic(path.x[num], path.y[num], center[2], center[0], center[1], center[2])
# Find next Coordination based on Lookahead=====================================================================
# tx버퍼 클리어 구문
# 실제 실험할 때 키는 구문 (제어 프로세스에 송신)==============================
canlib.IOControl(Channel).flush_tx_buffer()
Wave_Path_Next_Lat_sig.Wave_Path_Next_Lat.phys = round(float(next_lat), 8)
Channel.write(Wave_Path_Next_Lat_sig._frame)
Wave_Path_Next_Long_sig.Wave_Path_Next_Long.phys = round(float(next_lon), 7)
Channel.write(Wave_Path_Next_Long_sig._frame)
Wave_Path_Next_Alt_Yaw_sig.Wave_Path_Next_Alt.phys = round(float(next_alt), 2)
Channel.write(Wave_Path_Next_Alt_Yaw_sig._frame)
Wave_StopLine_Lat_sig.Wave_StopLine_Lat.phys = float(stop_lat)
Channel.write(Wave_StopLine_Lat_sig._frame)
Wave_StopLine_Long_sig.Wave_StopLine_Long.phys = float(stop_lon)
Channel.write(Wave_StopLine_Long_sig._frame)
# 실제 실험할 때 키는 구문 (제어 프로세스에 송신)==============================
# Warning Alert=================================
if Out_of_speed == True:
print("[Speed] -> Out of the Max")
Out_of_speed = False
if Out_of_accel == True:
print("[Accel] -> Out of the Max")
Out_of_accel = False
if Out_of_curvature == True:
print("[Curvature] -> Out of the Max")
Out_of_curvature = False
if Warning_obstacle == True:
print("[WARNING] -> Obstacle")
Warning_obstacle = False
# Warning Alert=================================
# 자동차 제동 거리 공식
break_distance = round(0.0005 * math.pow(velocity.value * 3.6, 2) + 0.2 * (velocity * 3.6), 3)
comp_end = np.hypot(path.x[1] - dx[-1], path.y[1] - dy[-1])
# 자동차 제동 거리와 비교해서 다달으면 모든 코드 break
if comp_end <= break_distance:
print("Goal!")
ALL_STOP.value = 1
break
if show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
# line of the world
plt.plot(ENU_all[:, 0], ENU_all[:, 1], '*b')
# line of the GPP
plt.plot(dx, dy, '--k', linewidth=3)
if len(obstacle) > 0:
plt.plot(obstacle[:, 0], obstacle[:, 1], "or", markersize=6)
# line of the LPP
plt.plot(path.x[1:], path.y[1:], "-og", linewidth=2)
plot_car(path.x[1], path.y[1], radian)
plt.plot(traffic_all[:, 0], traffic_all[:, 1], '*y')
plt.plot(traffic_all[POINT][0], traffic_all[POINT][1], 'Xm', markersize=10)
# ROI
plt.xlim(path.x[1] - area, path.x[1] + area)
plt.ylim(path.y[1] - area, path.y[1] + area)
text = "Time: " + str(round(time, 2)) + " / Velocity: " + str(round(velocity * 3.6, 2)) + "km/h"
plt.title(text)
plt.grid(True)
plt.pause(0.0001)
print("Finish")
else:
print("Position not found. Tray again...")
print("GPP=LPP FINISH")
def test_geodetic(self):
if pyproj:
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
x1, y1, z1 = pm.geodetic2ecef(tlat, tlon, talt)
assert_allclose(
pm.geodetic2ecef(radians(tlat), radians(tlon), talt, deg=False),
(x1, y1, z1))
assert_allclose((x1, y1, z1), (x0, y0, z0), err_msg='geodetic2ecef')
assert_allclose(pm.ecef2geodetic(x1, y1, z1), (tlat, tlon, talt),
err_msg='ecef2geodetic')
if pyproj:
assert_allclose(pyproj.transform(lla, ecef, tlon, tlat, talt),
(x1, y1, z1))
assert_allclose(pyproj.transform(ecef, lla, x1, y1, z1),
(tlon, tlat, talt))
lat2, lon2, alt2 = pm.aer2geodetic(taz, tel, tsrange, tlat, tlon, talt)
assert_allclose((lat2, lon2, alt2), (lat1, lon1, alt1),
err_msg='aer2geodetic')
assert_allclose(pm.geodetic2aer(lat2, lon2, alt2, tlat, tlon, talt),
(taz, tel, tsrange),
err_msg='geodetic2aer')
x2, y2, z2 = pm.aer2ecef(taz, tel, tsrange, tlat, tlon, talt)
assert_allclose(
pm.aer2ecef(radians(taz),
radians(tel),
tsrange,
radians(tlat),
radians(tlon),
talt,
deg=False), (a2x, a2y, a2z))
assert_allclose((x2, y2, z2), (a2x, a2y, a2z), err_msg='aer2ecef')
assert_allclose(pm.ecef2aer(x2, y2, z2, tlat, tlon, talt),
(taz, tel, tsrange),
err_msg='ecef2aer')
e1, n1, u1 = pm.aer2enu(taz, tel, tsrange)
assert_allclose((e1, n1, u1), (e0, n0, u0), err_msg='aer2enu')
assert_allclose(pm.aer2ned(taz, tel, tsrange), (n0, e0, -u0),
err_msg='aer2ned')
assert_allclose(pm.enu2aer(e1, n1, u1), (taz, tel, tsrange),
err_msg='enu2aer')
assert_allclose(pm.ned2aer(n1, e1, -u1), (taz, tel, tsrange),
err_msg='ned2aer')
assert_allclose(pm.enu2ecef(e1, n1, u1, tlat, tlon, talt),
(x2, y2, z2),
err_msg='enu2ecef')
assert_allclose(pm.ecef2enu(x2, y2, z2, tlat, tlon, talt),
(e1, n1, u1),
err_msg='ecef2enu')
assert_allclose(pm.ecef2ned(x2, y2, z2, tlat, tlon, talt),
(n1, e1, -u1),
err_msg='ecef2ned')
assert_allclose(pm.ned2ecef(n1, e1, -u1, tlat, tlon, talt),
(x2, y2, z2),
err_msg='ned2ecef')
# %%
assert_allclose(pm.ecef2enuv(vx, vy, vz, tlat, tlon), (ve, vn, vu))
assert_allclose(pm.ecef2nedv(vx, vy, vz, tlat, tlon), (vn, ve, -vu))
#%%
e3, n3, u3 = pm.geodetic2enu(lat2, lon2, alt2, tlat, tlon, talt)
assert_allclose(pm.geodetic2ned(lat2, lon2, alt2, tlat, tlon, talt),
(n3, e3, -u3))
assert_allclose(pm.enu2geodetic(e3, n3, u3, tlat, tlon, talt),
(lat2, lon2, alt2),
err_msg='enu2geodetic')
assert_allclose(pm.ned2geodetic(n3, e3, -u3, tlat, tlon, talt),
(lat2, lon2, alt2),
err_msg='ned2geodetic')
def enu_to_latlonalt(e, n, u, lat0, lon0, alt0):
lat, lon, alt = pymap3d.enu2geodetic(e, n, u, lat0, lon0, alt0)
return lat, lon, alt
def main(args):
results = {'x': [], 'y': [], 'z': [], "lat": [], "lon": [], "alt": []}
for bagfile in args.bags:
with Bag(bagfile, 'r') as input_bag:
if args.datum or args.datum_topic:
datum_lat, datum_lon, datum_alt = get_datum(
input_bag, args.datum, args.datum_topic)
else:
datum_lat, datum_lon, datum_alt = None, None, None
for topic, msg, time in input_bag.read_messages(
topics=[args.topic]):
if msg._type == "sensor_msgs/NavSatFix":
lat = msg.latitude
lon = msg.longitude
alt = msg.altitude
if datum_lat is None:
datum_lat = lat
datum_lon = lon
datum_alt = alt
x, y, z, = pymap3d.geodetic2enu(lat, lon, alt, datum_lat,
datum_lon, datum_alt)
elif msg._type == "nav_msgs/Odometry":
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
z = msg.pose.pose.position.z
if not args.local:
lat, lon, alt = pymap3d.enu2geodetic(
x, y, z, datum_lat, datum_lon, datum_alt)
else:
lat, lon, alt = None, None, None
elif msg._type == "geometry_msgs/Pose":
x = msg.position.x
y = msg.position.y
z = msg.position.z
if datum_lat is not None:
lat, lon, alt = pymap3d.enu2geodetic(
x, y, z, datum_lat, datum_lon, datum_alt)
else:
lat, lon, alt = None, None, None
elif msg._type == "geometry_msgs/PoseStamped":
x = msg.pose.position.x
y = msg.pose.position.y
z = msg.pose.position.z
if datum_lat is not None:
lat, lon, alt = pymap3d.enu2geodetic(
x, y, z, datum_lat, datum_lon, datum_alt)
else:
lat, lon, alt = None, None, None
elif msg._type == "geometry_msgs/PoseWithCovariance":
x = msg.pose.position.x
y = msg.pose.position.y
z = msg.pose.position.z
if datum_lat is not None:
lat, lon, alt = pymap3d.enu2geodetic(
x, y, z, datum_lat, datum_lon, datum_alt)
else:
lat, lon, alt = None, None, None
elif msg._type == "geometry_msgs/PoseWithCovarianceStamped":
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
z = msg.pose.pose.position.z
if datum_lat is not None:
lat, lon, alt = pymap3d.enu2geodetic(
x, y, z, datum_lat, datum_lon, datum_alt)
else:
lat, lon, alt = None, None, None
results['lat'].append(lat)
results['lon'].append(lon)
results['alt'].append(alt)
results['x'].append(x)
results['y'].append(y)
results['z'].append(z)
if args.output.endswith('.csv'):
df = pd.DataFrame.from_dict(results)
df.to_csv(args.output, index=False)
else:
raise ValueError("Output format %s not supported" %
os.path.splitext(args.output)[1])
% (numpy.rad2deg(numpy.arctan(
fit[0])), numpy.rad2deg(numpy.arctan(fit[1]))))
altitude_offset_ft = 8
print(
'\nAdding additional offset of %i ft to altitude to account for offset between my phone height and mast height'
% altitude_offset_ft)
plane = lambda E, N: fit[0] * E + fit[1] * N + fit[
2] + altitude_offset_ft * 0.3048
print(
'\nUsing input GPS positions of antennas, the new coordinates (with modified altitude) are:'
)
for i, a in antennas_physical.items():
lat, lon, alt = pm.enu2geodetic(a[0], a[1], plane(a[0], a[1]),
origin[0], origin[1],
origin[2])
print('Antenna %i\n%s : (%f,%f,%f)\n%s : (%f,%f,%f)' %
(i, '{:15}'.format('(E, N, U)'), a[0], a[1],
plane(a[0], a[1]), '{:15}'.format('(lat, lon, alt)'),
lat, lon, alt))
ax.scatter(a[0],
a[1],
plane(a[0], a[1]),
marker='+',
s=50,
color=colors[i],
label='%i New Initial Position' % (i),
alpha=1.0)
ax.set_xlabel('E (m)')
def xy2latlon(x, y, lat0, lon0): lat, lon, elv = pm.enu2geodetic(e=x, n=y, u=0, lat0=lat0, lon0=lon0, h0=0) return lat, lon
radar,
grid_shape = (1, 401, 401),
grid_limits = ((0, 2000), (-radarRange, radarRange), (-radarRange, radarRange)))
# fields
rho = grid.fields['cross_correlation_ratio']['data']
ref = grid.fields['reflectivity']['data']
vel = grid.fields['dealiased_velocity']['data']
lat = grid.point_latitude['data']
lon = grid.point_longitude['data']
xpts = 2000*np.arange(-400,401,1); ypts = 2000*np.arange(-400,401,1); zpts = np.zeros([1,801]); # array for interpolation
#center_lon_rad, center_lat_rad = np.deg2rad([center_lon, center_lat]) # convert center lat/lon to radians
[lat_list, lon_list, alt_list] = pm.enu2geodetic(xpts, ypts, zpts, center_lat, center_lon, center_alt)
[lon_grid, lat_grid] = np.meshgrid(lon_list, lat_list)
rho_grid = griddata((lon.flatten(), lat.flatten()), rho.flatten(), (lon_grid, lat_grid), method='linear')
ref_grid = griddata((lon.flatten(), lat.flatten()), ref.flatten(), (lon_grid, lat_grid), method='linear')
vel_grid = griddata((lon.flatten(), lat.flatten()), vel.flatten(), (lon_grid, lat_grid), method='linear')
df = pd.DataFrame({'lat':lat_grid.flatten(), 'lon':lon_grid.flatten(), 'ref':ref_grid.flatten(), 'rho':rho_grid.flatten(), 'vel':vel_grid.flatten()})
#df = df.dropna(axis=0, how='all', subset=['ref', 'rho', 'vel'])
#savepath = "G:\\My Drive\\phd\\plotly\\data\\pd_interp\\" + iradar + '\\' + date + '\\' # personal computer
savepath = "Q:\\My Drive\\phd\\plotly\\data\\pd_interp\\" + iradar + '\\' + date + '\\' # work computer
if not os.path.exists(savepath):
os.makedirs(savepath)
def test_geodetic(self):
if pyproj:
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
xyz1 = pm.geodetic2ecef(*lla0)
assert_allclose(pm.geodetic2ecef(*rlla0, deg=False),
xyz1,
err_msg='geodetic2ecef: rad')
assert_allclose(xyz1, xyz0, err_msg='geodetic2ecef: deg')
assert_allclose(pm.ecef2geodetic(*xyz1),
lla0,
err_msg='ecef2geodetic: deg')
assert_allclose(pm.ecef2geodetic(*xyz1, deg=False),
rlla0,
err_msg='ecef2geodetic: rad')
if pyproj:
assert_allclose(
pyproj.transform(lla, ecef, lla0[1], lla0[0], lla0[2]), xyz1)
assert_allclose(pyproj.transform(ecef, lla, *xyz1),
(lla0[1], lla0[0], lla0[2]))
lla2 = pm.aer2geodetic(*aer0, *lla0)
rlla2 = pm.aer2geodetic(*raer0, *rlla0, deg=False)
assert_allclose(lla2, lla1, err_msg='aer2geodetic: deg')
assert_allclose(rlla2, rlla1, err_msg='aer2geodetic:rad')
assert_allclose(pm.geodetic2aer(*lla2, *lla0),
aer0,
err_msg='geodetic2aer: deg')
assert_allclose(pm.geodetic2aer(*rlla2, *rlla0, deg=False),
raer0,
err_msg='geodetic2aer: rad')
# %% aer-ecef
xyz2 = pm.aer2ecef(*aer0, *lla0)
assert_allclose(pm.aer2ecef(*raer0, *rlla0, deg=False),
axyz0,
err_msg='aer2ecef:rad')
assert_allclose(xyz2, axyz0, err_msg='aer2ecef: deg')
assert_allclose(pm.ecef2aer(*xyz2, *lla0),
aer0,
err_msg='ecef2aer:deg')
assert_allclose(pm.ecef2aer(*xyz2, *rlla0, deg=False),
raer0,
err_msg='ecef2aer:rad')
# %% aer-enu
enu1 = pm.aer2enu(*aer0)
ned1 = (enu1[1], enu1[0], -enu1[2])
assert_allclose(enu1, enu0, err_msg='aer2enu: deg')
assert_allclose(pm.aer2enu(*raer0, deg=False),
enu0,
err_msg='aer2enu: rad')
assert_allclose(pm.aer2ned(*aer0), ned0, err_msg='aer2ned')
assert_allclose(pm.enu2aer(*enu1), aer0, err_msg='enu2aer: deg')
assert_allclose(pm.enu2aer(*enu1, deg=False),
raer0,
err_msg='enu2aer: rad')
assert_allclose(pm.ned2aer(*ned1), aer0, err_msg='ned2aer')
# %% enu-ecef
assert_allclose(pm.enu2ecef(*enu1, *lla0),
xyz2,
err_msg='enu2ecef: deg')
assert_allclose(pm.enu2ecef(*enu1, *rlla0, deg=False),
xyz2,
err_msg='enu2ecef: rad')
assert_allclose(pm.ecef2enu(*xyz2, *lla0),
enu1,
err_msg='ecef2enu:deg')
assert_allclose(pm.ecef2enu(*xyz2, *rlla0, deg=False),
enu1,
err_msg='ecef2enu:rad')
assert_allclose(pm.ecef2ned(*xyz2, *lla0), ned1, err_msg='ecef2ned')
assert_allclose(pm.ned2ecef(*ned1, *lla0), xyz2, err_msg='ned2ecef')
# %%
assert_allclose(pm.ecef2enuv(vx, vy, vz, *lla0[:2]), (ve, vn, vu))
assert_allclose(pm.ecef2nedv(vx, vy, vz, *lla0[:2]), (vn, ve, -vu))
# %%
enu3 = pm.geodetic2enu(*lla2, *lla0)
ned3 = (enu3[1], enu3[0], -enu3[2])
assert_allclose(pm.geodetic2ned(*lla2, *lla0),
ned3,
err_msg='geodetic2ned: deg')
assert_allclose(pm.enu2geodetic(*enu3, *lla0),
lla2,
err_msg='enu2geodetic')
assert_allclose(pm.ned2geodetic(*ned3, *lla0),
lla2,
err_msg='ned2geodetic')
def publish_topics(dictionary_observables: dict) -> object:
name_stages = []
people_distrib_per_stage = []
lat_stages = []
lon_stages = []
cov_stages = []
if not PermanentSettings.containerized:
device_number = Settings.device_number
stage_number = Settings.stage_number
name_stages = Settings.name_stages
people_distrib_per_stage = Settings.people_distrib_per_stage
lat_stages = Settings.lat_stages
lon_stages = Settings.lon_stages
cov_stages = Settings.cov_stages
else:
stage_number = 4
try:
device_number = int(os.environ[constants.DEVICE_NUMBER_KEY])
for i in range(1, stage_number + 1):
stage_id = "_" + str(i)
name_stages.append(os.environ[constants.STAGE_NAME_KEY +
stage_id])
people_distrib_per_stage.append(
float(os.environ[constants.DISTR_STAGE_KEY +
stage_id]))
lat_stages.append(
float(os.environ[constants.LAT_STAGE_KEY + stage_id]))
lon_stages.append(
float(os.environ[constants.LON_STAGE_KEY + stage_id]))
cov_stages.append([[
int(os.environ[constants.SIGMA_N_S_KEY + stage_id]), 0
], [
0,
int(os.environ[constants.SIGMA_E_O_KEY + stage_id])
]])
except KeyError as ke:
logging.critical("Missing environmental variable: " + str(ke))
exit(1)
try:
if not dictionary_observables:
return
# create an ellipsoid object
ell_wgs84 = pymap3d.Ellipsoid('wgs84')
counter_message_sent = 0
num_people = []
for percentage in people_distrib_per_stage:
num_people.append(round(percentage / 100 * device_number))
logging.debug("Total number of people: " + str(sum(num_people)))
count = num_people[0]
thresholds = [num_people[0]]
for i in range(1, stage_number):
thresholds.append(num_people[i] + count)
count = thresholds[i]
index_stage = 0
current_threshold = thresholds[index_stage]
for iot_id in dictionary_observables:
list_topic_tag_id = dictionary_observables[iot_id]
if len(list_topic_tag_id) < 2:
logging.debug("Element ignored: " + str(list_topic_tag_id))
continue
topic = list_topic_tag_id[0]
tag_id = list_topic_tag_id[1]
cov = cov_stages[index_stage]
# cov = [[100, 0], [0, 100]] # diagonal covariance
mean = [0, 0]
num_samples = 1
e1, n1 = np.random.multivariate_normal(mean, cov,
num_samples).T
u1 = 0
lat0 = lat_stages[index_stage]
lon0 = lon_stages[index_stage]
h0 = 0
# lat0, lon0, h0 = 5.0, 48.0, 10.0 # origin of ENU, (h is height above ellipsoid)
# e1, n1, u1 = 0.0, 0.0, 0.0 # ENU coordinates of test point, `point_1`
# From ENU to geodetic computation
lat1, lon1, h1 = pymap3d.enu2geodetic(
e1[0], n1[0], u1, lat0, lon0, h0, ell=ell_wgs84,
deg=True) # use wgs84 ellisoid
localization = Localization(tag_id=tag_id,
iot_id=iot_id,
lat=lat1,
lon=lon1,
area_id=name_stages[index_stage])
payload = localization.to_dictionary()
correctly_sent = ServerMQTT.publish(topic=topic,
dictionary=payload)
if correctly_sent:
logging.debug('On topic: "' + topic +
'" was sent payload: \n' + str(payload))
else:
logging.error("Error sending on topic: '" + topic +
"' payload: \n" + str(payload))
counter_message_sent += 1
if counter_message_sent >= current_threshold:
index_stage = index_stage + 1
if index_stage < stage_number:
current_threshold = thresholds[index_stage]
else:
index_stage = index_stage - 1
# go back -- do not increase this index larger than index_stage - 1
if (counter_message_sent % 100) == 0:
logging.info('MQTT Publish Messages: {}'.format(
counter_message_sent))
logging.info('MQTT Publish Messages Completed: {}'.format(
counter_message_sent))
except Exception as ex:
logging.error('Exception publish_topics: {}'.format(ex))
def c(points):
lat, lon, alt = p3d.enu2geodetic(points[:, 0], points[:,
1],
points[:, 2], fromCrs.lat,
fromCrs.lon, fromCrs.alt)
return np.column_stack((lon, lat, alt))
def localCartesianToGps(x,y,lat0,lon0): ret=pymap3d.enu2geodetic(x,y,0,lat0,lon0,0)[0:2] return [it if isinstance(it, (int,float)) else it.item(0) for it in ret]
