171 lines
6.9 KiB
Python
171 lines
6.9 KiB
Python
|
"""Turn the beacon's waypoints into a street-following route for main/route_points.h.
|
||
|
|
|
||
|
|
Asks the OSRM demo server (router.project-osrm.org, OpenStreetMap data) for a driving route that
|
||
|
|
visits WAYPOINTS in order and returns to the first, thins the street geometry out, and writes:
|
||
|
|
|
||
|
|
main/route_points.h the C array the firmware drives (commit this)
|
||
|
|
tools/route_osrm.json the raw OSRM answer, so the header can be regenerated offline (--offline)
|
||
|
|
tools/route_map.html the route over an OpenStreetMap map, to check it before flashing
|
||
|
|
|
||
|
|
Each waypoint also gets a bearing (the direction towards the next waypoint), so OSRM snaps it onto
|
||
|
|
the carriageway going that way. Without it, points on divided roads land on the wrong side and
|
||
|
|
every leg grows a U-turn detour.
|
||
|
|
|
||
|
|
Usage (Python 3.8+, standard library only):
|
||
|
|
py tools/make_route.py fetch from OSRM, then write all three files
|
||
|
|
py -3 tools/make_route.py --offline rebuild from the saved route_osrm.json
|
||
|
|
|
||
|
|
Route data (c) OpenStreetMap contributors, ODbL. Routing by OSRM.
|
||
|
|
"""
|
||
|
|
import argparse
|
||
|
|
import json
|
||
|
|
import math
|
||
|
|
import pathlib
|
||
|
|
import urllib.request
|
||
|
|
|
||
|
|
# (latitude, longitude) in decimal degrees, in driving order. The route closes back to the first.
|
||
|
|
WAYPOINTS = [
|
||
|
|
(53.553309, 10.022043),
|
||
|
|
(53.553611, 10.024146),
|
||
|
|
(53.556164, 10.027121),
|
||
|
|
(53.558310, 10.023362),
|
||
|
|
(53.554062, 10.013515),
|
||
|
|
(53.551540, 10.013398),
|
||
|
|
]
|
||
|
|
BEARING_TOLERANCE_DEG = 60 # how far the road's direction may differ from the waypoint bearing
|
||
|
|
SIMPLIFY_M = 1.5 # drop points that move the line by less than this
|
||
|
|
MAX_POINTS = 512 # must match ROUTE_MAX_POINTS in main/route.h
|
||
|
|
|
||
|
|
HERE = pathlib.Path(__file__).resolve().parent
|
||
|
|
PROJECT = HERE.parent
|
||
|
|
OSRM_JSON = HERE / "route_osrm.json"
|
||
|
|
HEADER = PROJECT / "main" / "route_points.h"
|
||
|
|
MAP_HTML = HERE / "route_map.html"
|
||
|
|
METRES_PER_DEG = 111194.93
|
||
|
|
|
||
|
|
|
||
|
|
def to_xy(lat, lon, lat0):
|
||
|
|
return (lon * METRES_PER_DEG * math.cos(math.radians(lat0)), lat * METRES_PER_DEG)
|
||
|
|
|
||
|
|
|
||
|
|
def bearing(a, b):
|
||
|
|
north = b[0] - a[0]
|
||
|
|
east = (b[1] - a[1]) * math.cos(math.radians(a[0]))
|
||
|
|
return math.degrees(math.atan2(east, north)) % 360
|
||
|
|
|
||
|
|
|
||
|
|
def fetch():
|
||
|
|
n = len(WAYPOINTS)
|
||
|
|
pts = WAYPOINTS + [WAYPOINTS[0]]
|
||
|
|
bearings = [round(bearing(WAYPOINTS[i], WAYPOINTS[(i + 1) % n])) for i in range(n)]
|
||
|
|
bearings.append(bearings[0])
|
||
|
|
url = ("https://router.project-osrm.org/route/v1/driving/"
|
||
|
|
+ ";".join(f"{lon},{lat}" for lat, lon in pts)
|
||
|
|
+ "?overview=full&geometries=geojson&steps=true&bearings="
|
||
|
|
+ ";".join(f"{b},{BEARING_TOLERANCE_DEG}" for b in bearings))
|
||
|
|
req = urllib.request.Request(url, headers={"User-Agent": "MicrOBU-route-tool"})
|
||
|
|
with urllib.request.urlopen(req, timeout=30) as resp:
|
||
|
|
data = json.load(resp)
|
||
|
|
if data.get("code") != "Ok":
|
||
|
|
raise SystemExit(f"OSRM error: {data.get('code')} {data.get('message')}")
|
||
|
|
OSRM_JSON.write_text(json.dumps(data, indent=1), encoding="utf-8")
|
||
|
|
return data
|
||
|
|
|
||
|
|
|
||
|
|
def simplify(xy, tol):
|
||
|
|
"""Douglas-Peucker, iterative. Keeps the first and last point."""
|
||
|
|
keep = [False] * len(xy)
|
||
|
|
keep[0] = keep[-1] = True
|
||
|
|
stack = [(0, len(xy) - 1)]
|
||
|
|
while stack:
|
||
|
|
a, b = stack.pop()
|
||
|
|
(ax, ay), (bx, by) = xy[a], xy[b]
|
||
|
|
dx, dy = bx - ax, by - ay
|
||
|
|
seg2 = dx * dx + dy * dy
|
||
|
|
worst, worst_d = -1, tol
|
||
|
|
for i in range(a + 1, b):
|
||
|
|
px, py = xy[i]
|
||
|
|
if seg2 == 0:
|
||
|
|
d = math.hypot(px - ax, py - ay)
|
||
|
|
else:
|
||
|
|
t = max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / seg2))
|
||
|
|
d = math.hypot(px - ax - t * dx, py - ay - t * dy)
|
||
|
|
if d > worst_d:
|
||
|
|
worst, worst_d = i, d
|
||
|
|
if worst >= 0:
|
||
|
|
keep[worst] = True
|
||
|
|
stack += [(a, worst), (worst, b)]
|
||
|
|
return [i for i, k in enumerate(keep) if k]
|
||
|
|
|
||
|
|
|
||
|
|
def main():
|
||
|
|
ap = argparse.ArgumentParser()
|
||
|
|
ap.add_argument("--offline", action="store_true", help="use the saved route_osrm.json")
|
||
|
|
args = ap.parse_args()
|
||
|
|
data = json.loads(OSRM_JSON.read_text(encoding="utf-8")) if args.offline else fetch()
|
||
|
|
route = data["routes"][0]
|
||
|
|
|
||
|
|
# GeoJSON is [lon, lat]; round to the CAM's 1/10-microdegree grid and drop repeats.
|
||
|
|
raw = []
|
||
|
|
for lon, lat in route["geometry"]["coordinates"]:
|
||
|
|
p = (round(lat * 1e7), round(lon * 1e7))
|
||
|
|
if not raw or p != raw[-1]:
|
||
|
|
raw.append(p)
|
||
|
|
if raw[0] == raw[-1]:
|
||
|
|
raw.pop()
|
||
|
|
closed = raw + [raw[0]]
|
||
|
|
lat0 = closed[0][0] / 1e7
|
||
|
|
xy = [to_xy(p[0] / 1e7, p[1] / 1e7, lat0) for p in closed]
|
||
|
|
pts = [closed[i] for i in simplify(xy, SIMPLIFY_M)][:-1]
|
||
|
|
if len(pts) > MAX_POINTS:
|
||
|
|
raise SystemExit(f"{len(pts)} points is more than MAX_POINTS={MAX_POINTS}; raise SIMPLIFY_M")
|
||
|
|
|
||
|
|
streets = []
|
||
|
|
for leg in route["legs"]:
|
||
|
|
for step in leg["steps"]:
|
||
|
|
if step["name"] and (not streets or streets[-1] != step["name"]):
|
||
|
|
streets.append(step["name"])
|
||
|
|
legs = ", ".join(f"{round(l['distance'])} m" for l in route["legs"])
|
||
|
|
|
||
|
|
lines = [
|
||
|
|
"// GENERATED by tools/make_route.py - do not edit by hand; change WAYPOINTS there and rerun.",
|
||
|
|
"// Route data (c) OpenStreetMap contributors, ODbL. Routing by OSRM.",
|
||
|
|
"//",
|
||
|
|
f"// Driving loop through {len(WAYPOINTS)} waypoints: {round(route['distance'])} m "
|
||
|
|
f"(legs {legs}), {len(pts)} points after",
|
||
|
|
f"// simplifying to {SIMPLIFY_M} m. Streets: {', '.join(streets)}.",
|
||
|
|
"#ifndef ROUTE_POINTS_H",
|
||
|
|
"#define ROUTE_POINTS_H",
|
||
|
|
'#include "route.h"',
|
||
|
|
"",
|
||
|
|
"static const route_point_t route_points[] = {",
|
||
|
|
]
|
||
|
|
lines += [f" {{ {lat}, {lon} }}," for lat, lon in pts]
|
||
|
|
lines += ["};", "", "#endif", ""]
|
||
|
|
HEADER.write_text("\n".join(lines), encoding="utf-8", newline="\n")
|
||
|
|
|
||
|
|
MAP_HTML.write_text(f"""<!doctype html><meta charset="utf-8"><title>Beacon route</title>
|
||
|
|
<link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css">
|
||
|
|
<script src="https://unpkg.com/leaflet@1.9.4/dist/leaflet.js"></script>
|
||
|
|
<style>html,body,#m{{height:100%;margin:0}}</style><div id="m"></div><script>
|
||
|
|
const pts={json.dumps([[p[0] / 1e7, p[1] / 1e7] for p in pts])};
|
||
|
|
const wps={json.dumps(WAYPOINTS)};
|
||
|
|
const m=L.map('m');
|
||
|
|
L.tileLayer('https://tile.openstreetmap.org/{{z}}/{{x}}/{{y}}.png',{{maxZoom:19,
|
||
|
|
attribution:'© OpenStreetMap contributors'}}).addTo(m);
|
||
|
|
const line=L.polyline(pts.concat([pts[0]]),{{color:'#d33',weight:4}}).addTo(m);
|
||
|
|
wps.forEach((w,i)=>L.marker(w,{{title:'wp'+(i+1)}}).bindTooltip('wp'+(i+1),{{permanent:true}}).addTo(m));
|
||
|
|
L.circleMarker(pts[0],{{radius:7,color:'#060'}}).bindTooltip('start').addTo(m);
|
||
|
|
m.fitBounds(line.getBounds(),{{padding:[20,20]}});
|
||
|
|
</script>
|
||
|
|
""", encoding="utf-8")
|
||
|
|
|
||
|
|
print(f"{round(route['distance'])} m, legs {legs}")
|
||
|
|
print(f"{len(route['geometry']['coordinates'])} OSRM points -> {len(pts)} after simplifying")
|
||
|
|
print(f"wrote {HEADER.relative_to(PROJECT)}, {OSRM_JSON.relative_to(PROJECT)}, "
|
||
|
|
f"{MAP_HTML.relative_to(PROJECT)}")
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|