sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 67 Packages Projects Releases Wiki Activity

No more line following robots

Browse Source
pull/214/head
Willem Melching 6 years ago
parent d0d117a999
commit 42f741d6ff
1 changed files with 0 additions and 378 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 378
      selfdrive/controls/gps_plannerd.py

378
selfdrive/controls/gps_plannerd.py
Unescape View File

@ -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()
Write Preview
Loading…
Cancel
Save