No more line following robots
This commit is contained in:
parent
d0d117a999
commit
42f741d6ff
|
@ -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()
|
Loading…
Reference in a new issue