Drive the bench CAM beacon round a street loop in St. Georg
The bench transmitter sent a parked car: one fixed position, speed 0, no heading, a CAM every second. It now simulates a car driving a loop through six waypoints around Berliner Tor on the real streets, which makes it a moving target for the app's map and the use case detection without taking a car out. The route is generated, not hand-traced. tools/make_route.py asks the OSRM demo server for a driving route through the waypoints and back to the first, thins the 371 street points to 103 (none more than 1.5 m off the line), and writes main/route_points.h. It also saves OSRM's answer (--offline rebuilds from it) and a map page to check the route before flashing. Each waypoint is sent with the direction towards the next one: without it, points on divided roads such as Beim Strohhause snapped to the opposite carriageway and the loop came out at 8.4 km of U-turns. With it the loop is 5.2 km, still including two turn-round detours that OSRM needs to reach the waypoints legally (Borgfelder Strasse / Anckelmannsplatz, and Nagelsweg / Norderstrasse / Repsoldstrasse). Route data (c) OpenStreetMap contributors, ODbL. main/route.c moves the car along the points. It cruises at 50 km/h and limits each bend to the speed that keeps sideways acceleration at 2 m/s^2, so a junction turn is taken at about 15 km/h and a gentle curve barely slows it; braking (2 m/s^2) and acceleration (1.5 m/s^2) are planned across as many points as a bend needs. A simulated lap on the host is 5.16 km in 7.7 min, averaging 40 km/h. CAMs now follow the EN 302 637-2 generation rules instead of a fixed 1 Hz: checked every 100 ms, sent on a heading change over 4 degrees, a move over 4 m, a speed change over 0.5 m/s, or after 1 s - about 3 Hz at 50 km/h. generationDeltaTime is milliseconds since boot. The GeoNetworking source position vector now carries the same speed and heading as the CAM instead of zeros. NOTES.md gains build and flash steps (including reading a board's app descriptor first, since both firmwares name their image obu_firmware.bin) and a section on the simulated drive. The pointer to docs/04-transmit-setup.md is corrected: that file is not in the repo. Flashed to the COM8 board and checked on its console: it starts driving on power-up and sends CAMs with changing position, speed and heading. Not yet received over the air.
This commit is contained in:
Ashin Walpola
@@ -0,0 +1,170 @@
|
||||
"""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()
|
||||
Reference in New Issue
Block a user