From cd4c04e2900428d06e2cfd270fb043f71f2049d5 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 24 Apr 2020 14:05:19 -0700 Subject: [PATCH] No more line following robots old-commit-hash: 42f741d6ffe342b629cdcdf083c4edc77d7f0233 --- selfdrive/controls/gps_plannerd.py | 378 ----------------------------- 1 file changed, 378 deletions(-) delete mode 100755 selfdrive/controls/gps_plannerd.py diff --git a/selfdrive/controls/gps_plannerd.py b/selfdrive/controls/gps_plannerd.py deleted file mode 100755 index 2cca117b09..0000000000 --- a/selfdrive/controls/gps_plannerd.py +++ /dev/null @@ -1,378 +0,0 @@ -#!/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('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('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('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(): - 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()