def convertToObstaclePolygons(self, numVertices=8):
'''
Accepts a list of obstacle dictionaries and returns a list of mavsdk compatible
obstacle polygons
'''
# Initialize list to hold obstacle polygons
obstaclePolygonList = []
for obstacle in self.obstacles:
# Initialize list to hold points in each obstacle polygon
obstaclePointList = []
# Capture data from the current obstacle
latitude = obstacle["latitude"]
longitude = obstacle["longitude"]
radius = self.ConvertFootToMeter(obstacle["radius"])
# Initialize ellipsoidal Vincenty point
obstaclePoint = LatLon(latitude, longitude)
# For each vertex point in the polygon that represents the circular obstacle
for index in range(numVertices):
# Calculate the azimuth angle of the vertex
angle = index * 360 / numVertices
# Calculate the gps coordinate of the vertex based off the obstacles
# radius and azimuth angle
vertex = obstaclePoint.destination(radius, angle)
vertexPoint = Point(float(vertex.lat), float(vertex.lon))
obstaclePointList.append(vertexPoint)
# Append the current obstacle polygon to the obstaclePolygonList
obstaclePolygon = Polygon(obstaclePointList,
Polygon.FenceType.EXCLUSION)
obstaclePolygonList.append(obstaclePolygon)
return obstaclePolygonList
def lat_lon_from_reference_given_range_az_degrees(ref_lat, ref_lon,
range_nmi,
az_degrees) -> list:
p = LatLon(ref_lat, ref_lon)
range_meters = range_nmi * constants.NM_TO_METERS
d = p.destination(range_meters, az_degrees)
return (d.lat, d.lon)
def get_gps_of_detected_objects(self, image, position: LatLon,
start_bearing, disparity_map, detections,
scaling_factor_z, scaling_factor_x):
"""
Use the GPS coordinates of the camera (car) and the disparity map to calculate the GPS coordinates of the
detected objects.
:param image: The input image
:param position: LatLon object (latitude, longitude) of the position of the camera (car)
:param start_bearing: The viewing direction of the camera as the deviation from north in degrees
:param disparity_map: The disparity map as an image
:param detections: List of dicts containing the name and the x, y-coordinates of the classified objects
:param scaling_factor_z: A factor which scales x and z distances
:return: The GPS-coordinates of the detected objects
"""
original_width = image.shape[1]
gps = []
for detection in detections:
u, v = detection["coords"]
z = self.basline * self.focal_length / (disparity_map[v, u] *
self.training_width)
z *= scaling_factor_z
x = (u - original_width / 2) * z / (
original_width / self.training_width) / self.focal_length
x *= scaling_factor_x
distance, bearing = self.xz_to_dist_bearing(x, z, start_bearing)
new_gps = position.destination(distance, bearing)
gps.append(new_gps)
self.classified_map_detections.append(
(detection.get("id"), new_gps.lat, new_gps.lon))
return gps
def set_ens(self, ens: Ensemble):
"""
Get the data out of the ensemble and populate
the structure.
Lock and unlock when getting the data to ensure
when new data is received, it is not being used with
old data.
:param ens: Latest ensemble.
:return:
"""
# Lock the object
self.thread_lock.acquire()
# Set Data
if ens.IsNmeaData and ens.NmeaData.GPGGA is not None and ens.IsEarthVelocity:
# Lat and Lon to the arrays
self.lat.append(ens.NmeaData.latitude)
self.lon.append(ens.NmeaData.longitude)
#self.lat_lon_text.append("Lat: " + str(round(ens.NmeaData.latitude, 2)) + " Lon: " + str(round(ens.NmeaData.longitude, 2)))
# Last Lat/Lon to place a marker for end of the path
self.last_lat = ens.NmeaData.latitude
self.last_lon = ens.NmeaData.longitude
# Get the average velocity and direction for the ensemble
avg_mag, avg_dir = ens.EarthVelocity.average_mag_dir()
self.avg_mag.append(avg_mag)
self.avg_dir.append(avg_dir)
# Create the magnitude and direction line
# Use the given Lat/Lon position as the start point
position = LatLon(ens.NmeaData.latitude, ens.NmeaData.longitude)
# Calculate the new position, based on the current lat/lon and the avg mag and dir
# These 2 points will create the quiver that represents the mag and dir for the given
# latitude point
if not math.isnan(avg_mag) and not math.isnan(avg_dir):
avg_position = position.destination(avg_mag * self.mag_scale,
avg_dir)
# Add the points to the arrays
# The start point is the ship track line
# The end point is the magnitude and angle from the start point on the ship track line
self.quiver_x.append(position.lon)
self.quiver_y.append(position.lat)
self.quiver_x.append(avg_position.lon)
self.quiver_y.append(avg_position.lat)
self.quiver_x.append(
None) # Add a None to breakup the lines for each quiver
self.quiver_y.append(
None) # Add a None to breakup the lines for each quiver
self.quiver_text.append("Mag: " + str(round(avg_mag, 2)) +
" Dir: " + str(round(avg_dir, 2)))
# Release the lock
self.thread_lock.release()
def lat_lon_from_reference_given_range_az_degrees(self, ref_lat, ref_lon, range_nmi, az_degrees):
"""
:param ref_lat: in format +/- hh.nnnnnn
:param ref_lon: in format +/- hh.nnnnnn
:param range_nmi:
:param az_degrees:
:return: (+/- hh.nnnn, +/- hh.nnnn)
"""
p = LatLon(ref_lat, ref_lon)
range_meters = range_nmi * self.__meters_in_nmi
d = p.destination(range_meters, az_degrees)
return (d.lat, d.lon)
def loc_at_conflict(self):
# ownship : lon [deg] lat[deg] alt[feet] trk[deg] gs[knots] vs [ft/min]
# traffic : lon [deg] lat[deg] alt[feet] trk[deg] gs[knots] vs [ft/min]
if not self.cd.conflict:
# no conflict
return False
elif not self.gxy:
return False # This method only works with geodesic coordinates
else:
entry_alt_o = round(self.alt_o +
self.vs_o / 60 * self.t_in) # [ft]
exit_alt_o = round(self.alt_o +
self.vs_o / 60 * self.t_out) # [ft]
entry_alt_i = round(self.alt_i +
self.vs_i / 60 * self.t_in) # [ft]
exit_alt_i = round(self.alt_i +
self.vs_i / 60 * self.t_out) # [ft]
dist2entry_o = knots2msec(self.gs_o) * self.t_in # [m]
dist2exit_o = knots2msec(self.gs_o) * self.t_out # [m]
dist2entry_i = knots2msec(self.gs_i) * self.t_in # [m]
dist2exit_i = knots2msec(self.gs_i) * self.t_out # [m]
current_loc_o = LatLon(self.x_o, self.y_o)
current_loc_i = LatLon(self.x_o, self.y_i)
entry_o = current_loc_o.destination(dist2entry_o, self.trk_o)
exit_o = current_loc_o.destination(dist2exit_o, self.trk_o)
entry_i = current_loc_i.destination(dist2entry_i, self.trk_i)
exit_i = current_loc_i.destination(dist2exit_i, self.trk_i)
self.loc_at_entry_o = [entry_o.lat, entry_o.lon, entry_alt_o]
self.loc_at_exit_o = [exit_o.lat, exit_o.lon, exit_alt_o]
self.loc_at_entry_i = [entry_i.lat, entry_i.lon, entry_alt_i]
self.loc_at_exit_i = [exit_i.lat, exit_i.lon, exit_alt_i]
def calculate_bistatic_target_parameters(radar_lat, radar_lon, radar_ele,
radar_bearing, ioo_lat, ioo_lon,
ioo_ele, target_lat, target_lon,
target_ele, target_speed, target_dir,
wavelength):
"""
Description:
------------
This function calculates the bistatic range, bistatic Doppler frequency,
and bearing angle of the observed target using the positions and the
velocities of the target and the location of the radar and the IoO.
Dependencies:
-------------
- PyGeodesy
Parameters:
-----------
:param radar_lat: Radar latitude coordinate [deg]
:param radar_lon: Radar longitude coordinate [deg]
:param radar_ele: Radar elevation [m]
:param radar_bearing: Boresight direction of the surveillance antenna of the radar.
Interpreted as clockwise from the north pole [deg]
:param ioo_lat: Transmitter latitude coordinate [deg]
:param ioo_lon: Transmitter longitude coordinate [deg]
:param ioo_ele: Transmitter elevation [m]. For broadcast towers this is typically
defined as the sum of the ASL and AGL hegihts.
ASL: Above Seal Level
AGL: Above Ground Level
:param target_lat: Target latitude coordinate [deg]
:param target_lon: Target longitude coordinate [deg]
:param target_ele: Target altitude [m]
:param target_speed: Target ground speed [meter per second]
:param target_dir: Target moving direction [deg]
Interpreted as clockwise from the north pole [deg]
:param wavelength: Wavelength of the used IoO [m]
:type radar_lat: float [-90 .. 90]
:type radar_lon: float [-90 .. 90]
:type radar_ele: float
:type radar_bearing: float [0 .. 180]
:type ioo_lat: float [-90 .. 90]
:type ioo_lon: float [-90 .. 90]
:type ioo_ele: float
:type target_lat: float [-90 .. 90]
:type target_lon: float [-90 .. 90]
:type target_ele: float
:type target_speed: flat
:type target_dir: float [0 .. 180]
:type wavelength: float
Return values:
--------------
:return calculated target parameters:
(target bistatic range [m],
target Doppler frequency [Hz]],
target azimuth bearing [deg])
:rtype calculated target parameters: python list with 3 elements
[float, float, float]
"""
ecef_converter = ecef.EcefYou(Datums.Sphere)
# Convert geodetic radar position to ECEF coordinates
radar_ecef = ecef_converter.forward(radar_lat, radar_lon, radar_ele)
radar_ecef = np.array([radar_ecef[0], radar_ecef[1], radar_ecef[2]])
# Convert geodetic IoO position to ECEF coordinates
ioo_ecef = ecef_converter.forward(ioo_lat, ioo_lon, ioo_ele)
ioo_ecef = np.array([ioo_ecef[0], ioo_ecef[1], ioo_ecef[2]])
# Baseline distance
L = np.sqrt(np.sum(np.abs(radar_ecef - ioo_ecef)**2))
# Convert geodetic (lat, lon, elevation) target position to ECEF coordinates
target_ecef = ecef_converter.forward(target_lat, target_lon, target_ele)
target_ecef = np.array([target_ecef[0], target_ecef[1], target_ecef[2]])
# Target to IoO distance
Rt = np.sqrt(np.sum(np.abs(target_ecef - ioo_ecef)**2))
# Target to radar distance
Rr = np.sqrt(np.sum(np.abs(target_ecef - radar_ecef)**2))
# Bistatic distance
Rb = Rt + Rr - L
# Generate speed vector
target_latlon = LatLon(target_lat, target_lon) # Target initial coordinate
speed_vector_latlon = target_latlon.destination(
1, target_dir) # Target coordinate 1m away in the current direction
speed_vector_ecef = ecef_converter.forward(speed_vector_latlon.lat,
speed_vector_latlon.lon,
target_ele)
speed_vector_ecef = np.array(
[speed_vector_ecef[0], speed_vector_ecef[1], speed_vector_ecef[2]])
speed_vector = speed_vector_ecef - target_ecef # Create vector in Cartesian space
speed_vector /= np.sqrt(np.sum(np.abs(speed_vector)**2)) # Normalize
# Generate target to IoO vector
target_to_ioo_vector = (ioo_ecef - target_ecef) / Rt
# Generate target to radar vector
target_to_radar_vector = (radar_ecef - target_ecef) / Rr
# Calculate target Doppler
fD = 1*(target_speed / wavelength) * \
( (np.sum(speed_vector*target_to_ioo_vector)) + (np.sum(speed_vector*target_to_radar_vector)))
# Caculate target azimuth direction
# Formula is originated from: https://www.movable-type.co.uk/scripts/latlong.html
lat1 = np.deg2rad(radar_lat)
lon1 = np.deg2rad(radar_lon)
lat2 = np.deg2rad(target_lat)
lon2 = np.deg2rad(target_lon)
target_doa = np.arctan2(
np.cos(lat1) * np.sin(lat2) -
np.sin(lat1) * np.cos(lat2) * np.cos(lon2 - lon1),
np.sin(lon2 - lon1) * np.cos(lat2))
theta = 90 - np.rad2deg(target_doa) - radar_bearing + 90
return (Rb, fD, theta)
def test1():
p = LatLon(-37.95103, 144.42487)
d = p.destination(54972.271, 306.86816)
print(d.lon, d.lat)
def generateCoOrdinates(latitude, longitude, degrees, secret_key_google,
main_latitude, main_longitude, key):
"""
:param latitude: source latitude
:param longitude: source longitude
:param degrees: directions where co-ordinates should be taken for
:param secret_key_google: secret key of google api
:param main_latitude: main latitude of souce
:param main_longitude: main longitude of source
:param key: Google/Bing key
:return: Dictionary of source and destination coordinates with distance
"""
i = 0
drops = 0
m = 0
hashset = set()
priority_queue = [(0, main_latitude, main_longitude)]
list_queue = [(0, 0, 0, 0)]
distance = 0
geo_distance = {}
maxvalue = -99999.99
final_input = []
count = 0
source_final = []
dest_final = []
dist_final = []
while i < 100000 and len(list_queue) > 0:
if i == 0:
list_queue.pop(0)
else:
maxvalue, distance, latitude, longitude = list_queue.pop(0)
latlong = str(latitude) + "," + str(longitude)
j = 0
for k in range(0, 4):
p = LatLon(latitude, longitude)
latitude1 = np.round(p.destination(10, degrees[j]).lat, 6)
longitude1 = np.round(p.destination(10, degrees[j]).lon, 6)
if maxvalue < 1000:
count += 1
total_distance = checkForPathGoogle(latitude1, longitude1,
latitude, longitude, key)
if (latitude, longitude) in geo_distance:
geo_distance[(latitude, longitude)].update({
(latitude1, longitude1):
total_distance
})
else:
geo_distance[(latitude, longitude)] = {
(latitude1, longitude1): total_distance
}
if (latitude1, longitude1) not in hashset:
maxvalue = haversine(main_latitude, main_longitude,
latitude1, longitude1)
hashset.add((latitude1, longitude1))
list_queue.append(
(maxvalue, total_distance, latitude1, longitude1))
j = (j + 1) % 4
else:
drops += 1
break
i += 1
return geo_distance