Jul-17-2022, 02:14 PM
Hi guys, new member here!
Im seeking for some advice regarding my program. What does it do? It reads the output from a USB GPS dongle, and calculate the speed. At the same time it iterates though a list of known "TrafficSafetyCamera" (speeding cameras), and warns when you are within 200 meters of a camera. My program also calculates the bearing you are travelling. Really cool and fun project! It would be cool to compile the code to a arduino or raspberry and have have it mounted in my car, with a small LCD or some other feedback system (but thats another thread lol).
What do i need your help with?
Im seeking for some advice regarding my program. What does it do? It reads the output from a USB GPS dongle, and calculate the speed. At the same time it iterates though a list of known "TrafficSafetyCamera" (speeding cameras), and warns when you are within 200 meters of a camera. My program also calculates the bearing you are travelling. Really cool and fun project! It would be cool to compile the code to a arduino or raspberry and have have it mounted in my car, with a small LCD or some other feedback system (but thats another thread lol).
What do i need your help with?
- I understand if it is hard for you to test the code, if you dont own a GPS USB dongle, or can mimick the output from the GPS. But from my understanding, i have written somewhat of "spaghetti code"? It would be cool to refactor the code so look more professional and maybe consume less resources.
- On line 173, in the
__main__loop, i was trying to clear the terminal so it would be easier to read the output. But when i haveos.system('clear') # Why this doesent work????uncommented, all i get is a blank terminal - how can clear the screen to just have the latest output?
- This one is rather tricky: if you look in the json data, all entities contains
Bearing. I dont want a warning when i approach a camera that is facing the opposite direction. So i was thinking if i could compare theBearingfor the camera and my ownbearing, and if the cardinal is in the same "range" - no warning. But if the cardinal is facing each other - warning. But thinking of that math gives me a headache. Please help :)
"""
Calculate if im moving towards the speed camera, or in the oposite direction?
The camera should have the bearing + a few degrees on left side and + a few degrees on the right side.
If im inside that sphere and moving towards center, send a warning.
Or if "our" (myself and the speed camera) has bearings that are opposite?
https://imgur.com/a/qsKvyK4
Would be cool if it was possible to set the radius depending on the vehicle speed. for example 30-50, 100m. 51-80, 200m. 81-120, 300m.
Reference
https://stackoverflow.com/questions/42686300/how-to-check-if-coordinate-inside-certain-area-python
https://www.egr.msu.edu/classes/ece480/capstone/spring15/group14/uploads/4/2/0/3/42036453/wilsonappnote.pdf
https://github.com/Knio/pynmea2
https://api.trafikinfo.trafikverket.se/
"""
from math import radians, cos, sin, asin, sqrt, atan2, degrees
from turtle import distance
import pynmea2
import serial
import json
import time
import re
import os
data = '{"RESPONSE":{"RESULT":[{"TrafficSafetyCamera":[{"Bearing":90,"Deleted":false,"Geometry":{"WGS84":"POINT (23.512141012281944 65.85875907320121)"},"IconId":"trafficSafetyCamera","Id":"25011020","ModifiedTime":"2022-07-16T01:00:28.773Z","Name":"Sangis östra infarten","RoadNumber":"E4"},{"Bearing":102,"Deleted":false,"Geometry":{"WGS84":"POINT (19.056405807546778 64.39662928870855)"},"IconId":"trafficSafetyCamera","Id":"24011010","ModifiedTime":"2022-07-16T01:00:28.288Z","Name":"Lillsele, västlig riktning","RoadNumber":"E12"},{"Bearing":140,"Deleted":false,"Geometry":{"WGS84":"POINT (12.30632048229635 58.31714553521646)"},"IconId":"trafficSafetyCamera","Id":"14009010","ModifiedTime":"2022-07-16T01:00:06.272Z","Name":"Överby TPL","RoadNumber":"E45"},{"Bearing":100,"Deleted":false,"Geometry":{"WGS84":"POINT (13.067072107941183 56.70782672076204)"},"IconId":"trafficSafetyCamera","Id":"13005040","ModifiedTime":"2022-07-16T01:00:06.319Z","Name":"Skärkered Ö","RoadNumber":"25"},{"Bearing":12,"Deleted":false,"Geometry":{"WGS84":"POINT (13.613975014625842 59.49962007485036)"},"IconId":"trafficSafetyCamera","Id":"17003040","ModifiedTime":"2022-07-16T01:00:31.726Z","Name":"Ökna","RoadNumber":"63"},{"Bearing":184,"Deleted":false,"Geometry":{"WGS84":"POINT (13.617570709661589 59.52313500039571)"},"IconId":"trafficSafetyCamera","Id":"17003050","ModifiedTime":"2022-07-16T01:00:31.726Z","Name":"Svedenäs","RoadNumber":"63"},{"Bearing":14,"Deleted":false,"Geometry":{"WGS84":"POINT (13.618532535235428 59.534192009528454)"},"IconId":"trafficSafetyCamera","Id":"17003060","ModifiedTime":"2022-07-16T01:00:31.726Z","Name":"Hasselbol","RoadNumber":"63"},{"Bearing":162,"Deleted":false,"Geometry":{"WGS84":"POINT (18.44183792545111 57.52301130472474)"},"IconId":"trafficSafetyCamera","Id":"09001020","ModifiedTime":"2022-07-16T01:00:06.351Z","Name":"Roma norrgående körfält","RoadNumber":"143"},{"Bearing":218,"Deleted":false,"Geometry":{"WGS84":"POINT (13.65297532183969 59.54845383986109)"},"IconId":"trafficSafetyCamera","Id":"17003070","ModifiedTime":"2022-07-16T01:00:31.726Z","Name":"Strand","RoadNumber":"63"},{"Bearing":350,"Deleted":false,"Geometry":{"WGS84":"POINT (18.44217434520964 57.521617721193515)"},"IconId":"trafficSafetyCamera","Id":"09001010","ModifiedTime":"2022-07-16T01:00:06.366Z","Name":"Roma södergående körfält","RoadNumber":"143"},{"Bearing":344,"Deleted":false,"Geometry":{"WGS84":"POINT (17.01189040432663 59.49294222471462)"},"IconId":"trafficSafetyCamera","Id":"04001060","ModifiedTime":"2022-07-16T01:00:06.429Z","Name":"Säby","RoadNumber":"55"},{"Bearing":174,"Deleted":false,"Geometry":{"WGS84":"POINT (17.01207008841139 59.49299335496056)"},"IconId":"trafficSafetyCamera","Id":"04001050","ModifiedTime":"2022-07-16T01:00:06.429Z","Name":"Säby","RoadNumber":"55"},{"Bearing":73,"Deleted":false,"Geometry":{"WGS84":"POINT (15.431850074330953 59.15232847147721)"},"IconId":"trafficSafetyCamera","Id":"18008020","ModifiedTime":"2022-07-16T01:00:06.460Z","Name":"Askersby","RoadNumber":"52"},{"Bearing":269,"Deleted":false,"Geometry":{"WGS84":"POINT (15.445158464307017 59.15395393742257)"},"IconId":"trafficSafetyCamera","Id":"18008030","ModifiedTime":"2022-07-16T01:00:06.444Z","Name":"Tybble","RoadNumber":"52"},{"Bearing":220,"Deleted":false,"Geometry":{"WGS84":"POINT (15.402092808949154 59.14480847501118)"},"IconId":"trafficSafetyCamera","Id":"18008010","ModifiedTime":"2022-07-16T01:00:06.460Z","Name":"Norrbyås","RoadNumber":"52"},{"Bearing":106,"Deleted":false,"Geometry":{"WGS84":"POINT (15.52521062939519 59.154588974075914)"},"IconId":"trafficSafetyCamera","Id":"18008050","ModifiedTime":"2022-07-16T01:00:06.491Z","Name":"Odensbacken","RoadNumber":"52"}]}]}}'
#f = open('trafikkameror.json')
#y = json.load(f)
y = json.loads(data)
"""
Convert the ['Geometry']['WGS84'] to readable text
"""
def lattify(string):
x = re.search("\(([^\)]+)\)", string)
x = x[0].replace('(', '').replace(')', '').split(' ')
return float(x[1]), float(x[0])
# x[1] N lat
# x[0] E long
"""
Calculate the distance with haversine
"""
def haversine(lon1, lat1, lon2, lat2):
"""
Calculate the great circle distance between two points
on the earth (specified in decimal degrees)
"""
# convert decimal degrees to radians
lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
# haversine formula
dlon = lon2 - lon1
dlat = lat2 - lat1
a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
c = 2 * asin(sqrt(a))
r = 6371 # Radius of earth in kilometers. Use 3956 for miles
return c * r
"""
TESTING THE HAVERSINE CODE BELOW.
"""
def heading(location):
# TESTING TO ITERATE THE JSON DATA
# Grabbing speedcamera position
for i in y['RESPONSE']['RESULT']:
for b in i['TrafficSafetyCamera']:
# print(b['Name'])
# print(b['Bearing'])
# print(coordinates[0], coordinates[1])
# print('')
coordinates = lattify(b['Geometry']['WGS84'])
# "MY" POSITION
posasdad = location.split(' ')
my_location = [
{'lat': float(posasdad[0]), 'lng': float(posasdad[1])}]
# The static speed camera position
speed_camera_center_point = [
{'lat': coordinates[0], 'lng': coordinates[1]}]
lat1 = speed_camera_center_point[0]['lat']
lon1 = speed_camera_center_point[0]['lng']
lat2 = my_location[0]['lat']
lon2 = my_location[0]['lng']
radius = 0.2 # in kilometer
a = haversine(lon1, lat1, lon2, lat2)
# print('Distance (km) : ', a)
if a <= radius:
# print(b['Name'])
# print('Inside the area')
return True
"""
The GPS functionhere... Should be called inside of __main__
"""
def gps_function():
ser = serial.Serial('/dev/ttyUSB0', 4800, timeout=5)
prev_data = None
while 1:
### Wrpped the readline() in a try. Got a decode error sometimes.
try:
line = ser.readline().decode('UTF-8')
except UnicodeDecodeError as e:
print(e)
continue
splitline = line.split(',')
if splitline[0] == '$GPGGA':
msg = line
data = pynmea2.parse(msg)
if prev_data is not None:
distance = haversine(
data.longitude, data.latitude, prev_data.longitude, prev_data.latitude)
###
#bearing = atan2(sin(prev_data.longitude-data.longitude)*cos(prev_data.latitude), cos(data.latitude)*sin(prev_data.latitude)-sin(data.latitude)*cos(prev_data.latitude)*cos(prev_data.longitude-data.longitude))
bearing = atan2(sin(prev_data.longitude-data.longitude)*cos(data.latitude), cos(prev_data.latitude)*sin(
prev_data.latitude)-sin(prev_data.latitude)*cos(data.latitude)*cos(prev_data.longitude-data.longitude))
bearing = degrees(bearing)
bearing = (bearing + 360) % 360
speed = round(distance*3600, 2)
lat = data.longitude
lng = data.latitude
return distance, speed, bearing, lat, lng
# print('distance', distance)
# print('speed', round(distance*3600, 2))
# print('bearing', bearing)
prev_data = data
if __name__ == '__main__':
try:
while True:
distance, speed, bearing, lat, lng = gps_function()
#print('Distance:', distance)
print('######################')
print('Speed:', speed)
print('Bearing:', bearing)
location = "{} {}".format(lng, lat)
x = heading(location)
if x:
print(
'It seems that you are inside the 200 meter radius of a speed camera!')
# os.system('clear') # Why this doesent work????
except KeyboardInterrupt:
exit()
finally:
print("Exit")I appreciate your input
