openpilot_comma/selfdrive/controls/gps_plannerd.py

#!/usr/bin/env python
import os
import zmq
import json
import time
import numpy as np
from numpy import linalg as LA
from threading import Thread
from scipy.spatial import cKDTree

from selfdrive.swaglog import cloudlog
from cereal.services import service_list
from common.realtime import Ratekeeper
from common.kalman.ned import geodetic2ecef, NED
import cereal.messaging as messaging
from cereal import log
import warnings
from selfdrive.config import Conversions as CV


if os.getenv('EON_LIVE') == '1':
  _REMOTE_ADDR = "192.168.5.11"
else:
  _REMOTE_ADDR = "127.0.0.1"

LOOP = 'small_loop'

TRACK_SNAP_DIST = 17.  # snap to a track below this distance
TRACK_LOST_DIST = 30.  # lose a track above this distance
INSTRUCTION_APPROACHING_DIST = 200.
INSTRUCTION_ACTIVE_DIST = 20.

ROT_CENTER_TO_LOC = 1.2

class INSTRUCTION_STATE:
  NONE = log.UiNavigationEvent.Status.none
  PASSIVE = log.UiNavigationEvent.Status.passive
  APPROACHING = log.UiNavigationEvent.Status.approaching
  ACTIVE = log.UiNavigationEvent.Status.active


def convert_ecef_to_capnp(points):
  points_capnp = []
  for p in points:
    point = log.ECEFPoint.new_message()
    point.x, point.y, point.z = map(float, p[0:3])
    points_capnp.append(point)
  return points_capnp


def get_spaced_points(track, start_index, cur_ecef, v_ego):
  active_points = []
  look_ahead = 5.0 + 1.5 * v_ego  # 5m + 1.5s

  # forward and backward passes for better poly fit
  for idx_sign in [1, -1]:
    for i in range(0, 1000):
      index = start_index + i * idx_sign
      # loop around 
      p = track[index % len(track)]

      distance = LA.norm(cur_ecef - p[0:3])
      if i > 5 and distance > look_ahead:
        break

      active_points.append([p, index])

  # sort points by index
  active_points = sorted(active_points, key=lambda pt: pt[1])
  active_points = [p[0] for p in active_points]

  return active_points


def fit_poly(points, cur_ecef, cur_heading, ned_converter):
  relative_points = []
  for point in points.points:
    p = np.array([point.x, point.y, point.z])
    relative_points.append(ned_converter.ecef_to_ned_matrix.dot(p - cur_ecef))

  relative_points = np.matrix(np.vstack(relative_points))

  # Calculate relative postions and rotate wrt to heading of car
  c, s = np.cos(-cur_heading), np.sin(-cur_heading)
  R = np.array([[c, -s], [s, c]])

  n, e = relative_points[:, 0], relative_points[:, 1]
  relative_points = np.hstack([e, n])
  rotated_points = relative_points.dot(R)

  rotated_points = np.array(rotated_points)
  x, y = rotated_points[:, 1], -rotated_points[:, 0]

  warnings.filterwarnings('error')

  # delete points that go backward
  max_x = x[0]
  x_new = []
  y_new = []

  for xi, yi in zip(x, y):
    if xi > max_x:
      max_x = xi
      x_new.append(xi)
      y_new.append(yi)

  x = np.array(x_new)
  y = np.array(y_new)

  if len(x) > 10:
    poly = map(float, np.polyfit(x + ROT_CENTER_TO_LOC, y, 3))  # 1.2m in front
  else:
    poly = [0.0, 0.0, 0.0, 0.0]
  return poly, float(max_x + ROT_CENTER_TO_LOC)


def get_closest_track(tracks, track_trees, cur_ecef):

  track_list = [(name, track_trees[name].query(cur_ecef, 1)) for name in track_trees]
  closest_name, [closest_distance, closest_idx] = min(track_list, key=lambda x: x[1][0])

  return {'name': closest_name,
          'distance': closest_distance,
          'idx': closest_idx,
          'speed': tracks[closest_name][closest_idx][3],
          'accel': tracks[closest_name][closest_idx][4]}


def get_track_from_name(tracks, track_trees, track_name, cur_ecef):
  if track_name is None:
    return None
  else:
    track_distance, track_idx = track_trees[track_name].query(cur_ecef, 1)
  return {'name': track_name,
          'distance': track_distance,
          'idx': track_idx,
          'speed': tracks[track_name][track_idx][3],
          'accel': tracks[track_name][track_idx][4]}


def get_tracks_from_instruction(tracks,instruction, track_trees, cur_ecef):
  if instruction is None:
    return None, None
  else:
    source_track = get_track_from_name(tracks, track_trees, instruction['source'], cur_ecef)
    target_track = get_track_from_name(tracks, track_trees, instruction['target'], cur_ecef)
    return source_track, target_track


def get_next_instruction_distance(track, instruction, cur_ecef):
  if instruction is None:
    return None
  else:
    return np.linalg.norm(cur_ecef - track[instruction['start_idx']][0:3])


def update_current_track(tracks, cur_track, cur_ecef, track_trees):

  closest_track = get_closest_track(tracks, track_trees, cur_ecef)

  # have we lost current track?
  if cur_track is not None:
    cur_track = get_track_from_name(tracks, track_trees, cur_track['name'], cur_ecef)
    if cur_track['distance'] > TRACK_LOST_DIST:
      cur_track = None

  # did we snap to a new track?
  if cur_track is None and closest_track['distance'] < TRACK_SNAP_DIST:
    cur_track = closest_track

  return cur_track, closest_track


def update_instruction(instruction, instructions, cur_track, source_track, state, cur_ecef, tracks):

  if state == INSTRUCTION_STATE.ACTIVE:  # instruction frozen, just update distance
    instruction['distance'] = get_next_instruction_distance(tracks[source_track['name']], instruction, cur_ecef)
    return instruction

  elif cur_track is None:
    return None

  else:
    instruction_list = [i for i in instructions[cur_track['name']] if i['start_idx'] > cur_track['idx']]
    if len(instruction_list) > 0:
      next_instruction = min(instruction_list, key=lambda x: x['start_idx'])
      next_instruction['distance'] = get_next_instruction_distance(tracks[cur_track['name']], next_instruction, cur_ecef)
      return next_instruction
    else:
      return None


def calc_instruction_state(state, cur_track, closest_track, source_track, target_track, instruction):

  lost_track_or_instruction = cur_track is None or instruction is None

  if state == INSTRUCTION_STATE.NONE:
    if lost_track_or_instruction:
      pass
    else:
      state = INSTRUCTION_STATE.PASSIVE

  elif state == INSTRUCTION_STATE.PASSIVE:
    if lost_track_or_instruction:
      state = INSTRUCTION_STATE.NONE
    elif instruction['distance'] < INSTRUCTION_APPROACHING_DIST:
      state = INSTRUCTION_STATE.APPROACHING

  elif state == INSTRUCTION_STATE.APPROACHING:
    if lost_track_or_instruction:
      state = INSTRUCTION_STATE.NONE
    elif instruction['distance'] < INSTRUCTION_ACTIVE_DIST:
      state = INSTRUCTION_STATE.ACTIVE

  elif state == INSTRUCTION_STATE.ACTIVE:
    if lost_track_or_instruction:
      state = INSTRUCTION_STATE.NONE
    elif target_track['distance'] < TRACK_SNAP_DIST and \
         source_track['idx'] > instruction['start_idx'] and \
         instruction['distance'] > 10.:
      state = INSTRUCTION_STATE.NONE
      cur_track = target_track

  return state, cur_track


def gps_planner_point_selection():

  DECIMATION = 1

  cloudlog.info("Starting gps_plannerd point selection")

  rk = Ratekeeper(10.0, print_delay_threshold=np.inf)

  context = zmq.Context()
  live_location = messaging.sub_sock(context, 'liveLocation', conflate=True, addr=_REMOTE_ADDR)
  car_state = messaging.sub_sock(context, 'carState', conflate=True)
  gps_planner_points = messaging.pub_sock(context, 'gpsPlannerPoints')
  ui_navigation_event = messaging.pub_sock(context, 'uiNavigationEvent')

  # Load tracks and instructions from disk
  basedir = os.environ['BASEDIR']
  tracks = np.load(os.path.join(basedir, 'selfdrive/controls/tracks/%s.npy' % LOOP)).item()
  instructions = json.loads(open(os.path.join(basedir, 'selfdrive/controls/tracks/instructions_%s.json' % LOOP)).read())

  # Put tracks into KD-trees
  track_trees = {}
  for name in tracks:
    tracks[name] = tracks[name][::DECIMATION]
    track_trees[name] = cKDTree(tracks[name][:,0:3]) # xyz
  cur_track = None
  source_track = None
  target_track = None
  instruction = None
  v_ego = 0.
  state = INSTRUCTION_STATE.NONE

  counter = 0

  while True:
    counter += 1
    ll = messaging.recv_one(live_location)
    ll = ll.liveLocation
    cur_ecef = geodetic2ecef((ll.lat, ll.lon, ll.alt))
    cs = messaging.recv_one_or_none(car_state)
    if cs is not None:
      v_ego = cs.carState.vEgo

    cur_track, closest_track = update_current_track(tracks, cur_track, cur_ecef, track_trees)
    #print cur_track

    instruction = update_instruction(instruction, instructions, cur_track, source_track, state, cur_ecef, tracks)

    source_track, target_track = get_tracks_from_instruction(tracks, instruction, track_trees, cur_ecef)

    state, cur_track = calc_instruction_state(state, cur_track, closest_track, source_track, target_track, instruction)

    active_points = []

    # Make list of points used by gpsPlannerPlan
    if cur_track is not None:
      active_points = get_spaced_points(tracks[cur_track['name']], cur_track['idx'], cur_ecef, v_ego)

    cur_pos = log.ECEFPoint.new_message()
    cur_pos.x, cur_pos.y, cur_pos.z = map(float, cur_ecef)
    m = messaging.new_message()
    m.init('gpsPlannerPoints')
    m.gpsPlannerPoints.curPos = cur_pos
    m.gpsPlannerPoints.points = convert_ecef_to_capnp(active_points)
    m.gpsPlannerPoints.valid = len(active_points) > 10
    m.gpsPlannerPoints.trackName = "none" if cur_track is None else cur_track['name']
    m.gpsPlannerPoints.speedLimit = 100. if cur_track is None else float(cur_track['speed'])
    m.gpsPlannerPoints.accelTarget = 0. if cur_track is None else float(cur_track['accel'])
    gps_planner_points.send(m.to_bytes())

    m = messaging.new_message()
    m.init('uiNavigationEvent')
    m.uiNavigationEvent.status = state
    m.uiNavigationEvent.type = "none" if instruction is None else instruction['type']
    m.uiNavigationEvent.distanceTo = 0. if instruction is None else float(instruction['distance'])
    endRoadPoint = log.ECEFPoint.new_message()
    m.uiNavigationEvent.endRoadPoint = endRoadPoint
    ui_navigation_event.send(m.to_bytes())

    rk.keep_time()


def gps_planner_plan():

  context = zmq.Context()

  live_location = messaging.sub_sock(context, 'liveLocation', conflate=True, addr=_REMOTE_ADDR)
  gps_planner_points = messaging.sub_sock(context, 'gpsPlannerPoints', conflate=True)
  gps_planner_plan = messaging.pub_sock(context, 'gpsPlannerPlan')

  points = messaging.recv_one(gps_planner_points).gpsPlannerPoints

  target_speed = 100. * CV.MPH_TO_MS
  target_accel = 0.

  last_ecef = np.array([0., 0., 0.])

  while True:
    ll = messaging.recv_one(live_location)
    ll = ll.liveLocation
    p = messaging.recv_one_or_none(gps_planner_points)
    if p is not None:
      points = p.gpsPlannerPoints
      target_speed = p.gpsPlannerPoints.speedLimit
      target_accel = p.gpsPlannerPoints.accelTarget

    cur_ecef = geodetic2ecef((ll.lat, ll.lon, ll.alt))

    # TODO: make NED initialization much faster so we can run this every time step
    if np.linalg.norm(last_ecef - cur_ecef) > 200.:
      ned_converter = NED(ll.lat, ll.lon, ll.alt)
      last_ecef = cur_ecef

    cur_heading = np.radians(ll.heading)

    if points.valid:
      poly, x_lookahead = fit_poly(points, cur_ecef, cur_heading, ned_converter)
    else:
      poly, x_lookahead = [0.0, 0.0, 0.0, 0.0], 0.

    valid = points.valid

    m = messaging.new_message()
    m.init('gpsPlannerPlan')
    m.gpsPlannerPlan.valid = valid
    m.gpsPlannerPlan.poly = poly
    m.gpsPlannerPlan.trackName = points.trackName
    r = []
    for p in points.points:
      point = log.ECEFPoint.new_message()
      point.x, point.y, point.z = p.x, p.y, p.z
      r.append(point)
    m.gpsPlannerPlan.points = r
    m.gpsPlannerPlan.speed = target_speed
    m.gpsPlannerPlan.acceleration = target_accel
    m.gpsPlannerPlan.xLookahead = x_lookahead
    gps_planner_plan.send(m.to_bytes())


def main(gctx=None):
  cloudlog.info("Starting gps_plannerd main thread")

  point_thread = Thread(target=gps_planner_point_selection)
  point_thread.daemon = True
  control_thread = Thread(target=gps_planner_plan)
  control_thread.daemon = True

  point_thread.start()
  control_thread.start()

  while True:
    time.sleep(1)


if __name__ == "__main__":
  main()