nav

2026-09-30 19:33:45 +08:00 · 2026-05-26 11:16:05 -05:00
parent 68d7fd09e2
commit cf506e59b0
119 changed files with 3590 additions and 1357 deletions
@@ -0,0 +1,168 @@
+#!/usr/bin/env python3
+
+import argparse
+import time
+from itertools import cycle
+
+from cereal import log, messaging
+
+
+ROUTE_COORDS = [
+  {"latitude": 0.0, "longitude": 0.0},
+  {"latitude": 0.0, "longitude": 0.01},
+  {"latitude": 0.01, "longitude": 0.02},
+]
+
+
+SCENARIOS = [
+  {
+    "primary": "Turn right onto Main St",
+    "secondary": "Then keep left",
+    "distance": 120.0,
+    "type": "turn",
+    "modifier": "right",
+    "next_type": "fork",
+    "next_modifier": "slightLeft",
+  },
+  {
+    "primary": "Turn left onto Pine Ave",
+    "secondary": "Then merge right",
+    "distance": 180.0,
+    "type": "turn",
+    "modifier": "left",
+    "next_type": "merge",
+    "next_modifier": "right",
+  },
+  {
+    "primary": "Keep right to stay on I-90",
+    "secondary": "Then continue straight",
+    "distance": 260.0,
+    "type": "fork",
+    "modifier": "slightRight",
+    "next_type": "continue",
+    "next_modifier": "straight",
+  },
+  {
+    "primary": "Keep left to stay on I-90",
+    "secondary": "Then take exit 14A",
+    "distance": 210.0,
+    "type": "fork",
+    "modifier": "slightLeft",
+    "next_type": "off ramp",
+    "next_modifier": "right",
+  },
+  {
+    "primary": "Take exit 14A",
+    "secondary": "Then turn sharp right",
+    "distance": 340.0,
+    "type": "off ramp",
+    "modifier": "right",
+    "next_type": "turn",
+    "next_modifier": "sharpRight",
+  },
+  {
+    "primary": "Continue straight for 1 mile",
+    "secondary": "Then enter the roundabout",
+    "distance": 1600.0,
+    "type": "continue",
+    "modifier": "straight",
+    "next_type": "roundabout",
+    "next_modifier": "right",
+  },
+  {
+    "primary": "Enter the roundabout",
+    "secondary": "Take the second exit",
+    "distance": 90.0,
+    "type": "roundabout",
+    "modifier": "right",
+    "next_type": "turn",
+    "next_modifier": "left",
+  },
+  {
+    "primary": "Turn sharp left onto Oak St",
+    "secondary": "Then make a U-turn",
+    "distance": 75.0,
+    "type": "turn",
+    "modifier": "sharpLeft",
+    "next_type": "continue",
+    "next_modifier": "uturn",
+  },
+  {
+    "primary": "Make a U-turn",
+    "secondary": "Then arrive at destination",
+    "distance": 60.0,
+    "type": "continue",
+    "modifier": "uturn",
+    "next_type": "arrive",
+    "next_modifier": "right",
+  },
+  {
+    "primary": "Arrive at destination",
+    "secondary": "Destination is on the right",
+    "distance": 25.0,
+    "type": "arrive",
+    "modifier": "right",
+    "next_type": "turn",
+    "next_modifier": "straight",
+  },
+]
+
+
+def build_instruction(pm: messaging.PubMaster, scenario: dict) -> None:
+  msg = messaging.new_message("navInstruction")
+  msg.valid = True
+
+  nav = msg.navInstruction
+  nav.maneuverPrimaryText = scenario["primary"]
+  nav.maneuverSecondaryText = scenario["secondary"]
+  nav.maneuverDistance = scenario["distance"]
+  nav.maneuverType = scenario["type"]
+  nav.maneuverModifier = scenario["modifier"]
+  nav.distanceRemaining = max(scenario["distance"] * 8, 400.0)
+  nav.timeRemaining = max(scenario["distance"] / 10.0, 20.0)
+  nav.timeRemainingTypical = nav.timeRemaining
+  nav.showFull = True
+  nav.speedLimit = 31.2928
+  nav.speedLimitSign = log.NavInstruction.SpeedLimitSign.mutcd
+  nav.allManeuvers = [
+    {"distance": scenario["distance"], "type": scenario["type"], "modifier": scenario["modifier"]},
+    {"distance": scenario["distance"] + 220.0, "type": scenario["next_type"], "modifier": scenario["next_modifier"]},
+  ]
+
+  pm.send("navInstruction", msg)
+
+
+def build_route(pm: messaging.PubMaster) -> None:
+  msg = messaging.new_message("navRoute")
+  msg.valid = True
+  msg.navRoute.coordinates = ROUTE_COORDS
+  pm.send("navRoute", msg)
+
+
+def main() -> None:
+  parser = argparse.ArgumentParser(description="Publish fake navigation instructions for desktop onroad replay.")
+  parser.add_argument("--hold-seconds", type=float, default=3.0, help="How long each maneuver stays active.")
+  parser.add_argument("--publish-interval", type=float, default=0.25, help="Seconds between navInstruction publishes.")
+  args = parser.parse_args()
+
+  pm = messaging.PubMaster(["navInstruction", "navRoute"])
+
+  scenario_cycle = cycle(SCENARIOS)
+  current = next(scenario_cycle)
+  next_switch = time.monotonic() + args.hold_seconds
+  print(f"showing: {current['type']} / {current['modifier']} -> {current['next_type']} / {current['next_modifier']}", flush=True)
+
+  while True:
+    now = time.monotonic()
+    if now >= next_switch:
+      current = next(scenario_cycle)
+      next_switch = now + args.hold_seconds
+      print(f"showing: {current['type']} / {current['modifier']} -> {current['next_type']} / {current['next_modifier']}", flush=True)
+
+    build_instruction(pm, current)
+    build_route(pm)
+    time.sleep(args.publish_interval)
+
+
+if __name__ == "__main__":
+  main()