MicrOBU/obu-cam-transmistter/tools/make_route.py

"""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:'&copy; 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()