This implementation of the Basic Safety Messages to Synthetic Trajectories algorithm uses:
MIN_BSM: Minimum number of BSMs that need to found within the time and distance window
TIME_WINDOW: Initial time, in seconds, that messages are searched for within
DISTANCE_WINDOW: Initial distance, in feet, that messages are searched for within
START_TIME: Epoch timestamp when analysis begins
END_TIME: Epoch timestamp when analysis ends
ID: Used to identify unique trajectories, auto-increments
output_filename: The name of the file where the output should be written to
import pandas as pd
import numpy as np
import geopy
import geopy.distance
import math
MIN_BSM = 1
TIME_WINDOW = 5
DISTANCE_WINDOW = 20
START_TIME = 1479310905
END_TIME = 1479326400
ID = 0
output_filename = "AMCDTrajectories.csv"
For this example, the Socrata API is used to query and access Basic Safety Messages from the Advanced Messaging Concept Development Basic Safety Message dataset available on data.transportation.gov.
Speed is converted from meters per second to feet per second to work with the algorithm standard. Time Received is divided by 1000 to truncate the data at seconds to make it easier to find messages with X seconds.
Routes defines the latitude/longitude pairs making up the critical points of a route on the roadway. Critical points are defined such that the straight line between two critical points follows the same path as the roadway. This can also be a file that is read in to make it easier to edit.
data_source = ("https://data.transportation.gov/resource/5b3h-czfm.csv?"
"$where=time_received%20between%201479310905000%20and%201479326400000"
"&$limit=700000&$$app_token=QL17HswS1IZjgfNJdj9k2ovzG")
col_to_use = ['time_received', 'latitude', 'longitude', 'speed', 'heading', 'elevation']
df_bsms = pd.read_csv(filepath_or_buffer = data_source, header = 0, skipinitialspace = True, usecols = col_to_use)
df_bsms['speed'] = df_bsms['speed'].apply(lambda x: x * 3.28084)
df_bsms['time_received'] = df_bsms['time_received'].apply(lambda x: int(x / 1000))
routes = [[(38.912678, -77.2216596), (38.913925, -77.223756), (38.917122, -77.22882), (38.918959, -77.231256), (38.920387, -77.233466), (38.924985, -77.237457), (38.928508, -77.240858), (38.930185, -77.242462), (38.932781, -77.245117)], [(38.9338843, -77.2465406), (38.929927, -77.242875), (38.928291, -77.241276), (38.926596, -77.239517), (38.924677, -77.237768), (38.921496, -77.234614), (38.920678, -77.233777), (38.919443, -77.232189), (38.9189, -77.231481), (38.91759, -77.229872), (38.91542, -77.226492), (38.913458, -77.223316), (38.911162, -77.219164)], [(38.9291045, -77.2454411), (38.929927, -77.242875), (38.928291, -77.241276), (38.926596, -77.239517), (38.924677, -77.237768), (38.921496, -77.234614), (38.920678, -77.233777), (38.919443, -77.232189), (38.9189, -77.231481), (38.91759, -77.229872), (38.91542, -77.226492), (38.913458, -77.223316), (38.911162, -77.219164)], [(38.912678, -77.2216596), (38.913925, -77.223756), (38.917122, -77.22882), (38.918959, -77.231256), (38.920387, -77.233466), (38.924985, -77.237457), (38.928508, -77.240858), (38.930185, -77.242462), (38.92978, -77.243171), (38.929109, -77.245842)], [(38.9338843, -77.2465406), (38.929927, -77.242875), (38.928291, -77.241276), (38.926596, -77.239517), (38.924677, -77.237768), (38.921496, -77.234614), (38.920678, -77.233777), (38.919443, -77.232189), (38.9189, -77.231481), (38.918499, -77.231138), (38.917894, -77.231288), (38.917719, -77.231883), (38.918065, -77.232404), (38.918495, -77.23228), (38.918883, -77.231299), (38.919142, -77.23022), (38.919342, -77.229013), (38.919522, -77.22778)], [(38.9184973, -77.2257953), (38.917936, -77.225473), (38.917343, -77.225403), (38.916863, -77.225521), (38.915912, -77.22625), (38.915857, -77.226782), (38.917122, -77.22882), (38.918959, -77.231256), (38.920387, -77.233466), (38.924985, -77.237457), (38.928508, -77.240858), (38.930185, -77.242462), (38.932781, -77.245117)], [(38.9338843, -77.2465406), (38.929927, -77.242875), (38.928291, -77.241276), (38.926596, -77.239517), (38.924677, -77.237768), (38.921496, -77.234614), (38.920678, -77.233777), (38.919443, -77.232189), (38.9189, -77.231481), (38.91759, -77.229872), (38.91542, -77.226492), (38.916405, -77.225644), (38.914106, -77.225247), (38.917765, -77.225237), (38.918554, -77.225618)]]
rsulocations = [(38.930045,-77.24315),(38.928128,-77.241327),(38.923859,-77.236135),(38.920883,-77.234304),(38.918416,-77.230494),(38.915165,-77.226364)]
df_bsms.head()
Algorithm Step 3: Initialize location l_pof a hypothetical connected vehicle as the start of the route, L_O^R. Initialize time t_pwhen hypothetical connected vehicle is at location l_p as t ̅. Write Tid and l_p to output file.
def getId():
global ID
ID += 1
return ID
def initialize_vehicle(route, time):
scaled_time = (time - START_TIME)/(END_TIME - START_TIME)
altitude = scaled_time * 1000
vehicle = {
'time': time,
'lat': route[0][0],
'lon': route[0][1],
'heading': find_heading(route[0],route[1]),
'id': getId(),
'link': 0,
'altitude': altitude,
'inRange':False,
'end': False
}
return vehicle
Algorithm Step 4: Calculate the hypothetical connected vehicle’s heading h_p as the bearing between two points L and L+1, such that L ∈ (L_O^R,L_D^R) for the current roadway segment the hypothetical connected vehicle is on
def find_heading(pointA, pointB):
lat1 = math.radians(pointA[0])
lat2 = math.radians(pointB[0])
diffLong = math.radians(pointB[1] - pointA[1])
x = math.sin(diffLong) * math.cos(lat2)
y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1)
* math.cos(lat2) * math.cos(diffLong))
initial_bearing = math.atan2(x, y)
initial_bearing = math.degrees(initial_bearing)
compass_bearing = (initial_bearing + 360) % 360
return compass_bearing
Algorithm Step 5: Search for all messages that are generated on the route within a pre-defined time-space region from l_p.
def get_bsms(vehicle, df_heading, route, time_max = TIME_WINDOW, distance_max = DISTANCE_WINDOW):
df_time = df_heading[(abs(vehicle['time'] - df_heading['time_received']) <= time_max)]
df_dist = df_time[df_time['distance'] <= distance_max]
if len(df_dist) >= MIN_BSM:
return df_dist
# Maximum search time to prevent an infinte loop in the case that no messages are found
elif time_max > 600:
return None
else:
return get_bsms(vehicle, df_heading, route, time_max + TIME_WINDOW, distance_max + DISTANCE_WINDOW)
Algorithm Step 7: For each message, i from Step 5, assign a weight:
w_i= 1⁄√((t_p-t_i )^2+(DIS(i,l_p )/v_i )^2 )
def calculateWeight(vehicle,time,distance,speed):
time_diff = (vehicle['time'] - time)**2
if distance == 0.0:
distance = 0.0001
if speed != 0.0:
dis_diff = (distance/speed)**2
else:
dis_diff = (distance/0.0001)**2
message_weight = 1/(time_diff + distance**2)**.5
return message_weight
Algorithm Step 10: Move the hypothetical connected vehicle to a new location along the route, which is the distance (in feet) it can travel in 4 seconds at a speed of v_p fps
def move_vehicle(vehicle, new_speed, distance_to_move, route):
distance_remaining = geopy.distance.vincenty((vehicle['lat'],vehicle['lon']),route[vehicle['link'] + 1]).feet
if distance_to_move > distance_remaining:
distance_to_move = distance_to_move - distance_remaining
if not (vehicle['link'] + 2) == len(route):
vehicle["link"] = vehicle['link'] + 1
vehicle['lat'] = route[vehicle['link']][0]
vehicle['lon'] = route[vehicle['link']][1]
vehicle['heading'] = find_heading(route[vehicle['link']],route[vehicle['link'] + 1])
move_vehicle(vehicle, new_speed, distance_to_move, route)
else:
finishTrip(vehicle, route, distance_remaining, new_speed)
else:
new_point = travelDistance((vehicle["lat"], vehicle["lon"]), vehicle['heading'], distance_to_move)
vehicle['lat'] = float(new_point[0])
vehicle['lon'] = float(new_point[1])
def travelDistance(origin, bearing, distance):
return geopy.distance.vincenty(feet = distance).destination(point = origin, bearing = bearing).format_decimal().split(',')
Algorithm Step 11: Calculate the vehicles “altitude” in time using the following formula:
a_p=10000*((t_p-T_S)/(T_F- T_S ))+e_p
def calculateAltitude(vehicle, elevation):
scaled_time = (vehicle['time'] - START_TIME)/(END_TIME - START_TIME)
altitude = scaled_time * 1000
vehicle['altitude'] = altitude + elevation
Algorithm Step 12: Determine whether the position l_p is within 300 meters of the position of a roadside unit, r_i in the set of r
def inRange(vehicle,rsulocations):
for rsu in rsulocations:
if geopy.distance.vincenty(rsu, (vehicle['lat'],vehicle['lon'])).meters <= 300:
vehicle['inRange'] = True
return
vehicle['inRange'] = False
Algorithm Step 14: Check if new location from Step 9 is greater than or equal to L_D^R. If position is greater, reset the position and time to L_D^R and set vehicle['end'] to true.
def finishTrip(vehicle, route, distance, new_speed):
vehicle["link"] = vehicle['link'] + 1
vehicle['lat'] = route[vehicle['link']][0]
vehicle['lon'] = route[vehicle['link']][1]
vehicle['time'] = vehicle['time'] - (distance/new_speed)
vehicle['end'] = True
Calculates the Great Circle distance between two series of points at once using numpy. Helper method for Step 5, finding messages within the distance window.
def haversine(lat1,lon1,lat2,lon2):
latRad1 = np.deg2rad(lat1)
latRad2 = np.deg2rad(lat2)
deltaLat = np.deg2rad(lat2 - lat1)
deltaLon = np.deg2rad(lon2-lon1)
a = np.sin(deltaLat/2)**2 + np.cos(latRad1) * np.cos(latRad2) * np.sin(deltaLon/2)**2
c = 2 * np.arcsin(np.sqrt(a))
return 3959 * 5280 * c
Algorithm Step 13: Write the trip id Tid, position l_p, time t_p, altitude a_p, speed v_p and heading h_p to the trajectories output file
def write_output(out_f, output):
for row in output:
out_f.write("{},{},{},{},{},{},{},{}\n".format(row[0], row[1], row[2], row[3], row[4], row[5], row[6],row[7]))
The main function. Calls above functions to process algorithm steps. Also performs the following steps:
Algorithm Step 1: For each Route, R in the network, find L_O^R and L_D^R. Do steps 2-12.
Algorithm Step 2: For time t ̅∈T ̅, do steps 3-12.
Algortihm Step 8: Rank order all BSMs identified in Step 5 in descending order based on the weights from Step 7. Let m be the total number of BSMs that lie within the pre-specified time-space region.
Algorithm Step 9: Calculate the speed and elevation of the hypothetical connected vehicle that starts its trip at time t ̅ as follows:
v_p= (∑(i=1)^(Min(m,8))(w_i v_i))/(∑(i=1)^(Min(m,8))w_i)
e_p= (∑(i=1)^(Min(m,8))(w_i a_i))/(∑(i=1)^(Min(m,8))w_i)
For each time step, it limits the number of BSMs to search by vehicle heading, then calculates the distances between the messages and current hypothetical vehicle position before calling get_bsms.
Note: This process takes around an hour to run and possibly more depending on the BSM file used
#Used to hide unneeded warning messages
import warnings
warnings.filterwarnings('ignore')
def process_bsms(df_bsms, routes, rsulocations, output_filename):
output = []
with open(output_filename, 'w') as out_f:
out_f.write('id,lat,long,tic,alt,speed,heading,inrangeofrsu\n')
for route in routes:
not_complete = 0
complete = 0
for clocktime in range(START_TIME,END_TIME,300):
output = []
vehicle = initialize_vehicle(route, clocktime)
output.append((vehicle['id'], vehicle['lat'], vehicle['lon'], vehicle['time'], vehicle['altitude'],0, vehicle['heading'],vehicle['inRange']))
while not vehicle['end']:
df_onRoute = df_bsms[(abs(vehicle['heading'] - df_bsms['heading']) <= 22.5)]
df_onRoute['distance'] = haversine(vehicle['lat'],vehicle['lon'],df_onRoute['latitude'],df_onRoute['longitude'])
df_messages = get_bsms(vehicle, df_onRoute, route)
if df_messages is None:
not_complete += 1
break
df_messages['message_weight'] = df_messages.apply(lambda x: calculateWeight(vehicle,x['time_received'],x['distance'],x['speed']),axis=1)
df_messages = df_messages.sort_values(by=['message_weight'],ascending=False)
df_messages = df_messages[:min(len(df_messages),8)]
df_messages['weightedSpeed'] = df_messages['message_weight'] * df_messages['speed']
df_messages['weightedElevation'] = df_messages['message_weight'] * df_messages['elevation']
new_speed_numerator = df_messages['weightedSpeed'].sum()
elevation_numerator = df_messages['weightedElevation'].sum()
denominator = df_messages['message_weight'].sum()
new_speed = new_speed_numerator/denominator
elevation = elevation_numerator/denominator
distance_to_travel = new_speed * 4
vehicle['time'] += 4
move_vehicle(vehicle, new_speed, distance_to_travel, route)
calculateAltitude(vehicle, elevation)
inRange(vehicle, rsulocations)
output.append((vehicle['id'], vehicle['lat'], vehicle['lon'], vehicle['time'], vehicle['altitude'],new_speed, vehicle['heading'],vehicle['inRange']))
if vehicle['end']:
complete += 1
write_output(out_f,output)
print("{} out of {} trajectories completed for route {}".format(complete,complete+not_complete,routes.index(route)))
process_bsms(df_bsms,routes,rsulocations,output_filename)