open source driving agent
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

85 lines
2.5 KiB

#!/usr/bin/env python
import os
import zmq
import numpy as np
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from common.realtime import sec_since_boot
from common.params import Params
from selfdrive.swaglog import cloudlog
from cereal import car
from selfdrive.controls.lib.pathplanner import OptPathPlanner
from selfdrive.controls.lib.pathplanner import PathPlanner
from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise
from selfdrive.controls.lib.fcw import ForwardCollisionWarning
_DT = 0.01 # 100Hz
class Planner(object):
def __init__(self, CP):
context = zmq.Context()
self.CP = CP
self.live20 = messaging.sub_sock(context, service_list['live20'].port)
self.model = messaging.sub_sock(context, service_list['model'].port)
self.plan = messaging.pub_sock(context, service_list['plan'].port)
self.last_md_ts = 0
self.last_l20_ts = 0
if os.getenv("OPT") is not None:
self.PP = OptPathPlanner(self.model)
else:
self.PP = PathPlanner()
self.AC = AdaptiveCruise()
self.FCW = ForwardCollisionWarning(_DT)
# this runs whenever we get a packet that can change the plan
def update(self, CS, LoC):
cur_time = sec_since_boot()
md = messaging.recv_sock(self.model)
if md is not None:
self.last_md_ts = md.logMonoTime
l20 = messaging.recv_sock(self.live20)
if l20 is not None:
self.last_l20_ts = l20.logMonoTime
self.PP.update(cur_time, CS.vEgo, md)
# LoC.v_pid -> CS.vEgo
# TODO: is this change okay?
self.AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20)
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
plan_send.plan.mdMonoTime = self.last_md_ts
plan_send.plan.l20MonoTime = self.last_l20_ts
# lateral plan
plan_send.plan.lateralValid = not self.PP.dead
if plan_send.plan.lateralValid:
plan_send.plan.dPoly = map(float, self.PP.d_poly)
# longitudal plan
plan_send.plan.longitudinalValid = not self.AC.dead
if plan_send.plan.longitudinalValid:
plan_send.plan.vTarget = float(self.AC.v_target_lead)
plan_send.plan.aTargetMin = float(self.AC.a_target[0])
plan_send.plan.aTargetMax = float(self.AC.a_target[1])
plan_send.plan.jerkFactor = float(self.AC.jerk_factor)
plan_send.plan.hasLead = self.AC.has_lead
# compute risk of collision events: fcw
self.FCW.process(CS, self.AC)
plan_send.plan.fcw = bool(self.FCW.active)
self.plan.send(plan_send.to_bytes())
return plan_send