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
85 lines
2.5 KiB
8 years ago
|
#!/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
|