FrogPilot structs

frogpilot/frogpilot_process.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 import datetime
+import json
 import time
 
 from cereal import messaging
@@ -7,11 +8,14 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL, Priority, Ratekeeper, config_realtime_process
 from openpilot.common.time_helpers import system_time_valid
 
+from openpilot.frogpilot.controls.frogpilot_planner import FrogPilotPlanner
+
 ASSET_CHECK_RATE = (1 / DT_MDL)
 
 def check_assets(params_memory):
 
-def transition_offroad(time_validated, sm, params):
+def transition_offroad(frogpilot_planner, time_validated, sm, params):
+  params.put("LastGPSPosition", json.dumps(frogpilot_planner.gps_position))
 
 def transition_onroad():
 
@@ -26,9 +30,11 @@ def frogpilot_thread():
 
   config_realtime_process(5, Priority.CTRL_LOW)
 
+  pm = messaging.PubMaster(["frogpilotPlan"])
   sm = messaging.SubMaster(["carControl", "carState", "controlsState", "deviceState", "driverMonitoringState",
                             "gpsLocation", "gpsLocationExternal", "liveParameters", "managerState", "modelV2",
-                            "onroadEvents", "pandaStates", "radarState", "selfdriveState"],
+                            "onroadEvents", "pandaStates", "radarState", "selfdriveState", "frogpilotCarState",
+                            "frogpilotSelfdriveState", "frogpilotModelV2", "frogpilotOnroadEvents"],
                             poll="modelV2")
 
   params = Params(return_defaults=True)
@@ -46,14 +52,20 @@ def frogpilot_thread():
     started = sm["deviceState"].started
 
     if not started and started_previously:
-      transition_offroad(time_validated, sm, params)
+      transition_offroad(frogpilot_planner, time_validated, sm, params)
 
       run_update_checks = True
     elif started and not started_previously:
+      frogpilot_planner = FrogPilotPlanner()
+
       transition_onroad()
 
     if started and sm.updated["modelV2"]:
+      frogpilot_planner.update(now, time_validated, sm)
+      frogpilot_planner.publish(sm, pm)
     elif not started:
+      frogpilot_plan_send = messaging.new_message("frogpilotPlan")
+      pm.send("frogpilotPlan", frogpilot_plan_send)
 
     started_previously = started