12345678910111213141516171819202122232425262728293031323334353637383940 |
- #!/usr/bin/env python3
- from cereal import car
- from openpilot.common.params import Params
- from openpilot.common.realtime import Priority, config_realtime_process
- from openpilot.common.swaglog import cloudlog
- from openpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning
- from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
- import cereal.messaging as messaging
- def main():
- config_realtime_process(5, Priority.CTRL_LOW)
- cloudlog.info("plannerd is waiting for CarParams")
- params = Params()
- CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
- cloudlog.info("plannerd got CarParams: %s", CP.carName)
- ldw = LaneDepartureWarning()
- longitudinal_planner = LongitudinalPlanner(CP)
- pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance'])
- sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState'],
- poll='modelV2', ignore_avg_freq=['radarState'])
- while True:
- sm.update()
- if sm.updated['modelV2']:
- longitudinal_planner.update(sm)
- longitudinal_planner.publish(sm, pm)
- ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl'])
- msg = messaging.new_message('driverAssistance')
- msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2'])
- msg.driverAssistance.leftLaneDeparture = ldw.left
- msg.driverAssistance.rightLaneDeparture = ldw.right
- pm.send('driverAssistance', msg)
- if __name__ == "__main__":
- main()
|