Vehicle Researcher
6 years ago
parent
fdb04d9f69
commit
0207a97040
55 changed files with 1861 additions and 820 deletions
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1 +1 @@ |
||||
#define COMMA_VERSION "0.5.8-release" |
||||
#define COMMA_VERSION "0.5.9-release" |
||||
|
||||
@ -0,0 +1,65 @@ |
||||
from common.numpy_fast import interp |
||||
from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv |
||||
|
||||
CAMERA_OFFSET = 0.06 # m from center car to camera |
||||
|
||||
|
||||
class ModelParser(object): |
||||
def __init__(self): |
||||
self.d_poly = [0., 0., 0., 0.] |
||||
self.c_poly = [0., 0., 0., 0.] |
||||
self.c_prob = 0. |
||||
self.last_model = 0. |
||||
self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 |
||||
self._path_pinv = compute_path_pinv() |
||||
|
||||
self.lane_width_estimate = 3.7 |
||||
self.lane_width_certainty = 1.0 |
||||
self.lane_width = 3.7 |
||||
self.l_prob = 0. |
||||
self.r_prob = 0. |
||||
|
||||
def update(self, v_ego, md): |
||||
if md is not None: |
||||
p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path |
||||
l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line |
||||
r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line |
||||
|
||||
# only offset left and right lane lines; offsetting p_poly does not make sense |
||||
l_poly[3] += CAMERA_OFFSET |
||||
r_poly[3] += CAMERA_OFFSET |
||||
|
||||
p_prob = 1. # model does not tell this probability yet, so set to 1 for now |
||||
l_prob = md.model.leftLane.prob # left line prob |
||||
r_prob = md.model.rightLane.prob # right line prob |
||||
|
||||
# Find current lanewidth |
||||
lr_prob = l_prob * r_prob |
||||
self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty) |
||||
current_lane_width = abs(l_poly[3] - r_poly[3]) |
||||
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) |
||||
speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8]) |
||||
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ |
||||
(1 - self.lane_width_certainty) * speed_lane_width |
||||
|
||||
lane_width_diff = abs(self.lane_width - current_lane_width) |
||||
lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0]) |
||||
|
||||
r_prob *= lane_r_prob |
||||
|
||||
self.lead_dist = md.model.lead.dist |
||||
self.lead_prob = md.model.lead.prob |
||||
self.lead_var = md.model.lead.std**2 |
||||
|
||||
# compute target path |
||||
self.d_poly, self.c_poly, self.c_prob = calc_desired_path( |
||||
l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width) |
||||
|
||||
self.r_poly = r_poly |
||||
self.r_prob = r_prob |
||||
|
||||
self.l_poly = l_poly |
||||
self.l_prob = l_prob |
||||
|
||||
self.p_poly = p_poly |
||||
self.p_prob = p_prob |
||||
@ -1,64 +1,122 @@ |
||||
from common.numpy_fast import interp |
||||
from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv |
||||
import zmq |
||||
import math |
||||
import numpy as np |
||||
|
||||
from common.realtime import sec_since_boot |
||||
from selfdrive.services import service_list |
||||
from selfdrive.swaglog import cloudlog |
||||
from selfdrive.controls.lib.lateral_mpc import libmpc_py |
||||
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT |
||||
from selfdrive.controls.lib.model_parser import ModelParser |
||||
import selfdrive.messaging as messaging |
||||
|
||||
|
||||
def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): |
||||
states[0].x = v_ego * delay |
||||
states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay |
||||
return states |
||||
|
||||
CAMERA_OFFSET = 0.06 # m from center car to camera |
||||
|
||||
class PathPlanner(object): |
||||
def __init__(self): |
||||
self.d_poly = [0., 0., 0., 0.] |
||||
self.c_poly = [0., 0., 0., 0.] |
||||
self.c_prob = 0. |
||||
self.last_model = 0. |
||||
self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 |
||||
self._path_pinv = compute_path_pinv() |
||||
|
||||
self.lane_width_estimate = 3.7 |
||||
self.lane_width_certainty = 1.0 |
||||
self.lane_width = 3.7 |
||||
self.l_prob = 0. |
||||
self.r_prob = 0. |
||||
|
||||
def update(self, v_ego, md): |
||||
if md is not None: |
||||
p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path |
||||
l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line |
||||
r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line |
||||
|
||||
# only offset left and right lane lines; offsetting p_poly does not make sense |
||||
l_poly[3] += CAMERA_OFFSET |
||||
r_poly[3] += CAMERA_OFFSET |
||||
|
||||
p_prob = 1. # model does not tell this probability yet, so set to 1 for now |
||||
l_prob = md.model.leftLane.prob # left line prob |
||||
r_prob = md.model.rightLane.prob # right line prob |
||||
|
||||
# Find current lanewidth |
||||
lr_prob = l_prob * r_prob |
||||
self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty) |
||||
current_lane_width = abs(l_poly[3] - r_poly[3]) |
||||
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) |
||||
speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8]) |
||||
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ |
||||
(1 - self.lane_width_certainty) * speed_lane_width |
||||
|
||||
lane_width_diff = abs(self.lane_width - current_lane_width) |
||||
lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0]) |
||||
|
||||
r_prob *= lane_r_prob |
||||
|
||||
self.lead_dist = md.model.lead.dist |
||||
self.lead_prob = md.model.lead.prob |
||||
self.lead_var = md.model.lead.std**2 |
||||
|
||||
# compute target path |
||||
self.d_poly, self.c_poly, self.c_prob = calc_desired_path( |
||||
l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width) |
||||
|
||||
self.r_poly = r_poly |
||||
self.r_prob = r_prob |
||||
|
||||
self.l_poly = l_poly |
||||
self.l_prob = l_prob |
||||
|
||||
self.p_poly = p_poly |
||||
self.p_prob = p_prob |
||||
def __init__(self, CP): |
||||
self.MP = ModelParser() |
||||
|
||||
self.last_cloudlog_t = 0 |
||||
|
||||
context = zmq.Context() |
||||
self.plan = messaging.pub_sock(context, service_list['pathPlan'].port) |
||||
self.livempc = messaging.pub_sock(context, service_list['liveMpc'].port) |
||||
|
||||
self.setup_mpc(CP.steerRateCost) |
||||
self.invalid_counter = 0 |
||||
|
||||
def setup_mpc(self, steer_rate_cost): |
||||
self.libmpc = libmpc_py.libmpc |
||||
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost) |
||||
|
||||
self.mpc_solution = libmpc_py.ffi.new("log_t *") |
||||
self.cur_state = libmpc_py.ffi.new("state_t *") |
||||
self.cur_state[0].x = 0.0 |
||||
self.cur_state[0].y = 0.0 |
||||
self.cur_state[0].psi = 0.0 |
||||
self.cur_state[0].delta = 0.0 |
||||
|
||||
self.angle_steers_des = 0.0 |
||||
self.angle_steers_des_mpc = 0.0 |
||||
self.angle_steers_des_prev = 0.0 |
||||
self.angle_steers_des_time = 0.0 |
||||
|
||||
def update(self, CP, VM, CS, md, live100): |
||||
v_ego = CS.carState.vEgo |
||||
angle_steers = CS.carState.steeringAngle |
||||
active = live100.live100.active |
||||
angle_offset = live100.live100.angleOffset |
||||
self.MP.update(v_ego, md) |
||||
|
||||
# Run MPC |
||||
self.angle_steers_des_prev = self.angle_steers_des_mpc |
||||
curvature_factor = VM.curvature_factor(v_ego) |
||||
|
||||
l_poly = libmpc_py.ffi.new("double[4]", list(self.MP.l_poly)) |
||||
r_poly = libmpc_py.ffi.new("double[4]", list(self.MP.r_poly)) |
||||
p_poly = libmpc_py.ffi.new("double[4]", list(self.MP.p_poly)) |
||||
|
||||
# account for actuation delay |
||||
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay) |
||||
|
||||
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed |
||||
self.libmpc.run_mpc(self.cur_state, self.mpc_solution, |
||||
l_poly, r_poly, p_poly, |
||||
self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width) |
||||
|
||||
# reset to current steer angle if not active or overriding |
||||
if active: |
||||
delta_desired = self.mpc_solution[0].delta[1] |
||||
else: |
||||
delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio |
||||
|
||||
self.cur_state[0].delta = delta_desired |
||||
|
||||
self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset) |
||||
|
||||
# Check for infeasable MPC solution |
||||
mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) |
||||
t = sec_since_boot() |
||||
if mpc_nans: |
||||
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) |
||||
self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio |
||||
|
||||
if t > self.last_cloudlog_t + 5.0: |
||||
self.last_cloudlog_t = t |
||||
cloudlog.warning("Lateral mpc - nan: True") |
||||
|
||||
if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge |
||||
self.invalid_counter += 1 |
||||
else: |
||||
self.invalid_counter = 0 |
||||
|
||||
plan_valid = self.invalid_counter < 2 |
||||
|
||||
plan_send = messaging.new_message() |
||||
plan_send.init('pathPlan') |
||||
plan_send.pathPlan.laneWidth = float(self.MP.lane_width) |
||||
plan_send.pathPlan.dPoly = map(float, self.MP.d_poly) |
||||
plan_send.pathPlan.cPoly = map(float, self.MP.c_poly) |
||||
plan_send.pathPlan.cProb = float(self.MP.c_prob) |
||||
plan_send.pathPlan.lPoly = map(float, l_poly) |
||||
plan_send.pathPlan.lProb = float(self.MP.l_prob) |
||||
plan_send.pathPlan.rPoly = map(float, r_poly) |
||||
plan_send.pathPlan.rProb = float(self.MP.r_prob) |
||||
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) |
||||
plan_send.pathPlan.valid = bool(plan_valid) |
||||
|
||||
self.plan.send(plan_send.to_bytes()) |
||||
|
||||
dat = messaging.new_message() |
||||
dat.init('liveMpc') |
||||
dat.liveMpc.x = list(self.mpc_solution[0].x) |
||||
dat.liveMpc.y = list(self.mpc_solution[0].y) |
||||
dat.liveMpc.psi = list(self.mpc_solution[0].psi) |
||||
dat.liveMpc.delta = list(self.mpc_solution[0].delta) |
||||
dat.liveMpc.cost = self.mpc_solution[0].cost |
||||
self.livempc.send(dat.to_bytes()) |
||||
|
||||
@ -0,0 +1,69 @@ |
||||
#!/usr/bin/env python |
||||
import zmq |
||||
|
||||
from cereal import car |
||||
from common.params import Params |
||||
from selfdrive.swaglog import cloudlog |
||||
from selfdrive.services import service_list |
||||
from selfdrive.controls.lib.planner import Planner |
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel |
||||
from selfdrive.controls.lib.pathplanner import PathPlanner |
||||
import selfdrive.messaging as messaging |
||||
|
||||
|
||||
def plannerd_thread(): |
||||
context = zmq.Context() |
||||
params = Params() |
||||
|
||||
# Get FCW toggle from settings |
||||
fcw_enabled = params.get("IsFcwEnabled") == "1" |
||||
|
||||
cloudlog.info("plannerd is waiting for CarParams") |
||||
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) |
||||
cloudlog.info("plannerd got CarParams: %s", CP.carName) |
||||
|
||||
PL = Planner(CP, fcw_enabled) |
||||
PP = PathPlanner(CP) |
||||
|
||||
VM = VehicleModel(CP) |
||||
|
||||
poller = zmq.Poller() |
||||
car_state_sock = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller) |
||||
live100_sock = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller) |
||||
live20_sock = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=poller) |
||||
model_sock = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller) |
||||
live_map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=poller) |
||||
|
||||
car_state = messaging.new_message() |
||||
car_state.init('carState') |
||||
live100 = messaging.new_message() |
||||
live100.init('live100') |
||||
model = messaging.new_message() |
||||
model.init('model') |
||||
live20 = messaging.new_message() |
||||
live20.init('live20') |
||||
live_map_data = messaging.new_message() |
||||
live_map_data.init('liveMapData') |
||||
|
||||
while True: |
||||
for socket, event in poller.poll(): |
||||
if socket is live100_sock: |
||||
live100 = messaging.recv_one(socket) |
||||
elif socket is car_state_sock: |
||||
car_state = messaging.recv_one(socket) |
||||
elif socket is model_sock: |
||||
model = messaging.recv_one(socket) |
||||
PP.update(CP, VM, car_state, model, live100) |
||||
elif socket is live_map_data_sock: |
||||
live_map_data = messaging.recv_one(socket) |
||||
elif socket is live20_sock: |
||||
live20 = messaging.recv_one(socket) |
||||
PL.update(car_state, CP, VM, PP, live20, live100, model, live_map_data) |
||||
|
||||
|
||||
def main(gctx=None): |
||||
plannerd_thread() |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
Binary file not shown.
@ -0,0 +1,105 @@ |
||||
{ |
||||
"_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped", |
||||
"AR:urban": "40", |
||||
"AR:urban:primary": "60", |
||||
"AR:urban:secondary": "60", |
||||
"AR:rural": "110", |
||||
"AT:urban": "50", |
||||
"AT:rural": "100", |
||||
"AT:trunk": "100", |
||||
"AT:motorway": "130", |
||||
"BE:urban": "50", |
||||
"BE-VLG:rural": "70", |
||||
"BE-WAL:rural": "90", |
||||
"BE:trunk": "120", |
||||
"BE:motorway": "120", |
||||
"CH:urban[1]": "50", |
||||
"CH:rural": "80", |
||||
"CH:trunk": "100", |
||||
"CH:motorway": "120", |
||||
"CZ:pedestrian_zone": "20", |
||||
"CZ:living_street": "20", |
||||
"CZ:urban": "50", |
||||
"CZ:urban_trunk": "80", |
||||
"CZ:urban_motorway": "80", |
||||
"CZ:rural": "90", |
||||
"CZ:trunk": "110", |
||||
"CZ:motorway": "130", |
||||
"DK:urban": "50", |
||||
"DK:rural": "80", |
||||
"DK:motorway": "130", |
||||
"DE:living_street": "7", |
||||
"DE:urban": "50", |
||||
"DE:rural": "100", |
||||
"DE:trunk": "none", |
||||
"DE:motorway": "none", |
||||
"FI:urban": "50", |
||||
"FI:rural": "80", |
||||
"FI:trunk": "100", |
||||
"FI:motorway": "120", |
||||
"FR:urban": "50", |
||||
"FR:rural": "80", |
||||
"FR:trunk": "110", |
||||
"FR:motorway": "130", |
||||
"GR:urban": "50", |
||||
"GR:rural": "90", |
||||
"GR:trunk": "110", |
||||
"GR:motorway": "130", |
||||
"HU:urban": "50", |
||||
"HU:rural": "90", |
||||
"HU:trunk": "110", |
||||
"HU:motorway": "130", |
||||
"IT:urban": "50", |
||||
"IT:rural": "90", |
||||
"IT:trunk": "110", |
||||
"IT:motorway": "130", |
||||
"JP:national": "60", |
||||
"JP:motorway": "100", |
||||
"LT:living_street": "20", |
||||
"LT:urban": "50", |
||||
"LT:rural": "90", |
||||
"LT:trunk": "120", |
||||
"LT:motorway": "130", |
||||
"PL:living_street": "20", |
||||
"PL:urban": "50", |
||||
"PL:rural": "90", |
||||
"PL:trunk": "100", |
||||
"PL:motorway": "140", |
||||
"RO:urban": "50", |
||||
"RO:rural": "90", |
||||
"RO:trunk": "100", |
||||
"RO:motorway": "130", |
||||
"RU:living_street": "20", |
||||
"RU:urban": "60", |
||||
"RU:rural": "90", |
||||
"RU:motorway": "110", |
||||
"SK:urban": "50", |
||||
"SK:rural": "90", |
||||
"SK:trunk": "90", |
||||
"SK:motorway": "90", |
||||
"SI:urban": "50", |
||||
"SI:rural": "90", |
||||
"SI:trunk": "110", |
||||
"SI:motorway": "130", |
||||
"ES:living_street": "20", |
||||
"ES:urban": "50", |
||||
"ES:rural": "50", |
||||
"ES:trunk": "90", |
||||
"ES:motorway": "120", |
||||
"SE:urban": "50", |
||||
"SE:rural": "70", |
||||
"SE:trunk": "90", |
||||
"SE:motorway": "110", |
||||
"GB:nsl_restricted": "30 mph", |
||||
"GB:nsl_single": "60 mph", |
||||
"GB:nsl_dual": "70 mph", |
||||
"GB:motorway": "70 mph", |
||||
"UA:urban": "50", |
||||
"UA:rural": "90", |
||||
"UA:trunk": "110", |
||||
"UA:motorway": "130", |
||||
"UZ:living_street": "30", |
||||
"UZ:urban": "70", |
||||
"UZ:rural": "100", |
||||
"UZ:motorway": "110" |
||||
} |
||||
@ -0,0 +1,454 @@ |
||||
{ |
||||
"CA": { |
||||
"Default": [ |
||||
{ |
||||
"speed": "100", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "trunk" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "primary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "secondary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "unclassified" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "40", |
||||
"tags": { |
||||
"highway": "residential" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "40", |
||||
"tags": { |
||||
"highway": "service" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "90", |
||||
"tags": { |
||||
"highway": "motorway_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "trunk_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "primary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "secondary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "20", |
||||
"tags": { |
||||
"highway": "living_street" |
||||
} |
||||
} |
||||
] |
||||
}, |
||||
"DE": { |
||||
"Default": [ |
||||
{ |
||||
"speed": "none", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "10", |
||||
"tags": { |
||||
"highway": "living_street" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "100", |
||||
"tags": { |
||||
"zone:traffic": "DE:rural" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"zone:traffic": "DE:urban" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "30", |
||||
"tags": { |
||||
"bicycle_road": "yes" |
||||
} |
||||
} |
||||
] |
||||
}, |
||||
"AU": { |
||||
"Default": [ |
||||
{ |
||||
"speed": "100", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "trunk" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "primary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "secondary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "unclassified" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "residential" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "40", |
||||
"tags": { |
||||
"highway": "service" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "90", |
||||
"tags": { |
||||
"highway": "motorway_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "trunk_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "primary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "secondary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "30", |
||||
"tags": { |
||||
"highway": "living_street" |
||||
} |
||||
} |
||||
] |
||||
}, |
||||
"US": { |
||||
"South Dakota": [ |
||||
{ |
||||
"speed": "80 mph", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "70 mph", |
||||
"tags": { |
||||
"highway": "trunk" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "65 mph", |
||||
"tags": { |
||||
"highway": "primary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "70 mph", |
||||
"tags": { |
||||
"highway": "trunk_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "65 mph", |
||||
"tags": { |
||||
"highway": "primary_link" |
||||
} |
||||
} |
||||
], |
||||
"Wisconsin": [ |
||||
{ |
||||
"speed": "65 mph", |
||||
"tags": { |
||||
"highway": "trunk" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "35 mph", |
||||
"tags": { |
||||
"highway": "unclassified" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "65 mph", |
||||
"tags": { |
||||
"highway": "trunk_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
} |
||||
], |
||||
"Default": [ |
||||
{ |
||||
"speed": "65 mph", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "trunk" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "primary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "secondary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "35 mph", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "unclassified" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "25 mph", |
||||
"tags": { |
||||
"highway": "residential" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "25 mph", |
||||
"tags": { |
||||
"highway": "service" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "motorway_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "trunk_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "primary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "secondary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "35 mph", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "15 mph", |
||||
"tags": { |
||||
"highway": "living_street" |
||||
} |
||||
} |
||||
], |
||||
"Michigan": [ |
||||
{ |
||||
"speed": "70 mph", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
} |
||||
], |
||||
"Oregon": [ |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "35 mph", |
||||
"tags": { |
||||
"highway": "secondary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "30 mph", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "15 mph", |
||||
"tags": { |
||||
"highway": "service" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "35 mph", |
||||
"tags": { |
||||
"highway": "secondary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "30 mph", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
} |
||||
], |
||||
"New York": [ |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "primary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "secondary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "30 mph", |
||||
"tags": { |
||||
"highway": "residential" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "primary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "secondary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
} |
||||
] |
||||
} |
||||
} |
||||
@ -0,0 +1,209 @@ |
||||
#!/usr/bin/env python |
||||
import json |
||||
|
||||
OUTPUT_FILENAME = "default_speeds_by_region.json" |
||||
|
||||
def main(): |
||||
countries = [] |
||||
|
||||
""" |
||||
-------------------------------------------------- |
||||
US - United State of America |
||||
-------------------------------------------------- |
||||
""" |
||||
US = Country("US") # First step, create the country using the ISO 3166 two letter code |
||||
countries.append(US) # Second step, add the country to countries list |
||||
|
||||
""" Default rules """ |
||||
# Third step, add some default rules for the country |
||||
# Speed limit rules are based on OpenStreetMaps (OSM) tags. |
||||
# The dictionary {...} defines the tag_name: value |
||||
# if a road in OSM has a tag with the name tag_name and this value, the speed limit listed below will be applied. |
||||
# The text at the end is the speed limit (use no unit for km/h) |
||||
# Rules apply in the order in which they are written for each country |
||||
# Rules for specific regions (states) take priority over country rules |
||||
# If you modify existing country rules, you must update all existing states without that rule to use the old rule |
||||
US.add_rule({"highway": "motorway"}, "65 mph") # On US roads with the tag highway and value motorway, the speed limit will default to 65 mph |
||||
US.add_rule({"highway": "trunk"}, "55 mph") |
||||
US.add_rule({"highway": "primary"}, "55 mph") |
||||
US.add_rule({"highway": "secondary"}, "45 mph") |
||||
US.add_rule({"highway": "tertiary"}, "35 mph") |
||||
US.add_rule({"highway": "unclassified"}, "55 mph") |
||||
US.add_rule({"highway": "residential"}, "25 mph") |
||||
US.add_rule({"highway": "service"}, "25 mph") |
||||
US.add_rule({"highway": "motorway_link"}, "55 mph") |
||||
US.add_rule({"highway": "trunk_link"}, "55 mph") |
||||
US.add_rule({"highway": "primary_link"}, "55 mph") |
||||
US.add_rule({"highway": "secondary_link"}, "45 mph") |
||||
US.add_rule({"highway": "tertiary_link"}, "35 mph") |
||||
US.add_rule({"highway": "living_street"}, "15 mph") |
||||
|
||||
""" States """ |
||||
new_york = US.add_region("New York") # Fourth step, add a state/region to country |
||||
new_york.add_rule({"highway": "primary"}, "45 mph") # Fifth step , add rules to the state. See the text above for how to write rules |
||||
new_york.add_rule({"highway": "secondary"}, "55 mph") |
||||
new_york.add_rule({"highway": "tertiary"}, "55 mph") |
||||
new_york.add_rule({"highway": "residential"}, "30 mph") |
||||
new_york.add_rule({"highway": "primary_link"}, "45 mph") |
||||
new_york.add_rule({"highway": "secondary_link"}, "55 mph") |
||||
new_york.add_rule({"highway": "tertiary_link"}, "55 mph") |
||||
# All if not written by the state, the rules will default to the country rules |
||||
|
||||
#california = US.add_region("California") |
||||
# California uses only the default US rules |
||||
|
||||
michigan = US.add_region("Michigan") |
||||
michigan.add_rule({"highway": "motorway"}, "70 mph") |
||||
|
||||
oregon = US.add_region("Oregon") |
||||
oregon.add_rule({"highway": "motorway"}, "55 mph") |
||||
oregon.add_rule({"highway": "secondary"}, "35 mph") |
||||
oregon.add_rule({"highway": "tertiary"}, "30 mph") |
||||
oregon.add_rule({"highway": "service"}, "15 mph") |
||||
oregon.add_rule({"highway": "secondary_link"}, "35 mph") |
||||
oregon.add_rule({"highway": "tertiary_link"}, "30 mph") |
||||
|
||||
south_dakota = US.add_region("South Dakota") |
||||
south_dakota.add_rule({"highway": "motorway"}, "80 mph") |
||||
south_dakota.add_rule({"highway": "trunk"}, "70 mph") |
||||
south_dakota.add_rule({"highway": "primary"}, "65 mph") |
||||
south_dakota.add_rule({"highway": "trunk_link"}, "70 mph") |
||||
south_dakota.add_rule({"highway": "primary_link"}, "65 mph") |
||||
|
||||
wisconsin = US.add_region("Wisconsin") |
||||
wisconsin.add_rule({"highway": "trunk"}, "65 mph") |
||||
wisconsin.add_rule({"highway": "tertiary"}, "45 mph") |
||||
wisconsin.add_rule({"highway": "unclassified"}, "35 mph") |
||||
wisconsin.add_rule({"highway": "trunk_link"}, "65 mph") |
||||
wisconsin.add_rule({"highway": "tertiary_link"}, "45 mph") |
||||
|
||||
""" |
||||
-------------------------------------------------- |
||||
AU - Australia |
||||
-------------------------------------------------- |
||||
""" |
||||
AU = Country("AU") |
||||
countries.append(AU) |
||||
|
||||
""" Default rules """ |
||||
AU.add_rule({"highway": "motorway"}, "100") |
||||
AU.add_rule({"highway": "trunk"}, "80") |
||||
AU.add_rule({"highway": "primary"}, "80") |
||||
AU.add_rule({"highway": "secondary"}, "50") |
||||
AU.add_rule({"highway": "tertiary"}, "50") |
||||
AU.add_rule({"highway": "unclassified"}, "80") |
||||
AU.add_rule({"highway": "residential"}, "50") |
||||
AU.add_rule({"highway": "service"}, "40") |
||||
AU.add_rule({"highway": "motorway_link"}, "90") |
||||
AU.add_rule({"highway": "trunk_link"}, "80") |
||||
AU.add_rule({"highway": "primary_link"}, "80") |
||||
AU.add_rule({"highway": "secondary_link"}, "50") |
||||
AU.add_rule({"highway": "tertiary_link"}, "50") |
||||
AU.add_rule({"highway": "living_street"}, "30") |
||||
|
||||
""" |
||||
-------------------------------------------------- |
||||
CA - Canada |
||||
-------------------------------------------------- |
||||
""" |
||||
CA = Country("CA") |
||||
countries.append(CA) |
||||
|
||||
""" Default rules """ |
||||
CA.add_rule({"highway": "motorway"}, "100") |
||||
CA.add_rule({"highway": "trunk"}, "80") |
||||
CA.add_rule({"highway": "primary"}, "80") |
||||
CA.add_rule({"highway": "secondary"}, "50") |
||||
CA.add_rule({"highway": "tertiary"}, "50") |
||||
CA.add_rule({"highway": "unclassified"}, "80") |
||||
CA.add_rule({"highway": "residential"}, "40") |
||||
CA.add_rule({"highway": "service"}, "40") |
||||
CA.add_rule({"highway": "motorway_link"}, "90") |
||||
CA.add_rule({"highway": "trunk_link"}, "80") |
||||
CA.add_rule({"highway": "primary_link"}, "80") |
||||
CA.add_rule({"highway": "secondary_link"}, "50") |
||||
CA.add_rule({"highway": "tertiary_link"}, "50") |
||||
CA.add_rule({"highway": "living_street"}, "20") |
||||
|
||||
|
||||
""" |
||||
-------------------------------------------------- |
||||
DE - Germany |
||||
-------------------------------------------------- |
||||
""" |
||||
DE = Country("DE") |
||||
countries.append(DE) |
||||
|
||||
""" Default rules """ |
||||
DE.add_rule({"highway": "motorway"}, "none") |
||||
DE.add_rule({"highway": "living_street"}, "10") |
||||
DE.add_rule({"zone:traffic": "DE:rural"}, "100") |
||||
DE.add_rule({"zone:traffic": "DE:urban"}, "50") |
||||
DE.add_rule({"bicycle_road": "yes"}, "30") |
||||
|
||||
""" --- DO NOT MODIFY CODE BELOW THIS LINE --- """ |
||||
""" --- ADD YOUR COUNTRY OR STATE ABOVE --- """ |
||||
|
||||
# Final step |
||||
write_json(countries) |
||||
|
||||
def write_json(countries): |
||||
out_dict = {} |
||||
for country in countries: |
||||
out_dict.update(country.jsonify()) |
||||
json_string = json.dumps(out_dict, indent=2) |
||||
with open(OUTPUT_FILENAME, "wb") as f: |
||||
f.write(json_string) |
||||
|
||||
|
||||
class Region(object): |
||||
ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road"] |
||||
ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"] |
||||
def __init__(self, name): |
||||
self.name = name |
||||
self.rules = [] |
||||
|
||||
def add_rule(self, tag_conditions, speed): |
||||
new_rule = {} |
||||
if not isinstance(tag_conditions, dict): |
||||
raise TypeError("Rule tag conditions must be dictionary") |
||||
if not all(tag_key in self.ALLOWABLE_TAG_KEYS for tag_key in tag_conditions): |
||||
raise ValueError("Rule tag keys must be in allowable tag kesy") # If this is by mistake, please update ALLOWABLE_TAG_KEYS |
||||
if 'highway' in tag_conditions: |
||||
if not tag_conditions['highway'] in self.ALLOWABLE_HIGHWAY_TYPES: |
||||
raise ValueError("Invalid Highway type {}".format(tag_conditions["highway"])) |
||||
new_rule['tags'] = tag_conditions |
||||
try: |
||||
new_rule['speed'] = str(speed) |
||||
except ValueError: |
||||
raise ValueError("Rule speed must be string") |
||||
self.rules.append(new_rule) |
||||
|
||||
def jsonify(self): |
||||
ret_dict = {} |
||||
ret_dict[self.name] = self.rules |
||||
return ret_dict |
||||
|
||||
class Country(Region): |
||||
ALLOWABLE_COUNTRY_CODES = ["AF","AX","AL","DZ","AS","AD","AO","AI","AQ","AG","AR","AM","AW","AU","AT","AZ","BS","BH","BD","BB","BY","BE","BZ","BJ","BM","BT","BO","BQ","BA","BW","BV","BR","IO","BN","BG","BF","BI","KH","CM","CA","CV","KY","CF","TD","CL","CN","CX","CC","CO","KM","CG","CD","CK","CR","CI","HR","CU","CW","CY","CZ","DK","DJ","DM","DO","EC","EG","SV","GQ","ER","EE","ET","FK","FO","FJ","FI","FR","GF","PF","TF","GA","GM","GE","DE","GH","GI","GR","GL","GD","GP","GU","GT","GG","GN","GW","GY","HT","HM","VA","HN","HK","HU","IS","IN","ID","IR","IQ","IE","IM","IL","IT","JM","JP","JE","JO","KZ","KE","KI","KP","KR","KW","KG","LA","LV","LB","LS","LR","LY","LI","LT","LU","MO","MK","MG","MW","MY","MV","ML","MT","MH","MQ","MR","MU","YT","MX","FM","MD","MC","MN","ME","MS","MA","MZ","MM","NA","NR","NP","NL","NC","NZ","NI","NE","NG","NU","NF","MP","NO","OM","PK","PW","PS","PA","PG","PY","PE","PH","PN","PL","PT","PR","QA","RE","RO","RU","RW","BL","SH","KN","LC","MF","PM","VC","WS","SM","ST","SA","SN","RS","SC","SL","SG","SX","SK","SI","SB","SO","ZA","GS","SS","ES","LK","SD","SR","SJ","SZ","SE","CH","SY","TW","TJ","TZ","TH","TL","TG","TK","TO","TT","TN","TR","TM","TC","TV","UG","UA","AE","GB","US","UM","UY","UZ","VU","VE","VN","VG","VI","WF","EH","YE","ZM","ZW"] |
||||
def __init__(self, ISO_3166_alpha_2): |
||||
Region.__init__(self, ISO_3166_alpha_2) |
||||
if ISO_3166_alpha_2 not in self.ALLOWABLE_COUNTRY_CODES: |
||||
raise ValueError("Not valid IOS 3166 country code") |
||||
self.regions = {} |
||||
|
||||
def add_region(self, name): |
||||
self.regions[name] = Region(name) |
||||
return self.regions[name] |
||||
|
||||
def jsonify(self): |
||||
ret_dict = {} |
||||
ret_dict[self.name] = {} |
||||
for r_name, region in self.regions.iteritems(): |
||||
ret_dict[self.name].update(region.jsonify()) |
||||
ret_dict[self.name]['Default'] = self.rules |
||||
return ret_dict |
||||
|
||||
|
||||
if __name__ == '__main__': |
||||
main() |
||||
Binary file not shown.
Binary file not shown.
Loading…
Reference in new issue