From 69e2ea816b7264263c068cb9d697025cba4aec8c Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 18 Nov 2020 00:12:48 -0600 Subject: [PATCH 1/7] add debugging --- long_debug.py | 33 +++++++++++++++++++++++++++++++++ selfdrive/debug/uiview.py | 14 +++++++++----- selfdrive/manager.py | 6 +++--- 3 files changed, 45 insertions(+), 8 deletions(-) create mode 100644 long_debug.py diff --git a/long_debug.py b/long_debug.py new file mode 100644 index 00000000000000..05ae69d159f1a3 --- /dev/null +++ b/long_debug.py @@ -0,0 +1,33 @@ +from cereal.messaging import SubMaster + +sm = SubMaster(['modelV2'], poll=['modelV2']) +model_t = [0, 0.009765625, 0.0390625, 0.087890625, 0.15625, 0.24414062, 0.3515625, 0.47851562, 0.625, 0.79101562, 0.9765625, 1.1816406, 1.40625, 1.6503906, 1.9140625, 2.1972656, 2.5, 2.8222656, 3.1640625, 3.5253906, 3.90625, 4.3066406, 4.7265625, 5.1660156, 5.625, 6.1035156, 6.6015625, 7.1191406, 7.65625, 8.2128906, 8.7890625, 9.3847656, 10] +mpc_idxs = list(range(10)) + +model_t_idx = [sorted(range(len(model_t)), key=[abs(idx - t) for t in model_t].__getitem__)[0] for idx in mpc_idxs] # matches 0 to 9 interval to idx from t +speed_curr_idx = sorted(range(len(model_t)), key=[abs(t - .1) for t in model_t].__getitem__)[0] # idx used for current speed, position still uses model_t_idx + + +while 1: + sm.update() + + modelV2 = sm['modelV2'] + if not sm.updated['modelV2'] or len(modelV2.position.x) == 0: + continue + + distances = [] # everything is derived from x position since velocity is outputting weird values + speeds = [] + accelerations = [0] + for t in model_t_idx: + distances.append(modelV2.position.x[t]) + + if t == 0: + speeds.append(modelV2.position.x[speed_curr_idx] / model_t[speed_curr_idx]) + else: + speeds.append(modelV2.position.x[t] / model_t[t]) + + if model_t_idx.index(t) > 0: # skip first since we can't calculate (and don't want to use v_ego) + accelerations.append((speeds[-1] - speeds[-2]) / model_t[t]) + + accelerations[0] = accelerations[1] - (accelerations[2] - accelerations[1]) # extrapolate back first accel from second and third, less weight + print(distances[:5]) diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py index e31d6cf3c399cc..0b6ba5ce5acf5e 100755 --- a/selfdrive/debug/uiview.py +++ b/selfdrive/debug/uiview.py @@ -1,11 +1,15 @@ #!/usr/bin/env python3 +import os import time +import signal +import subprocess import cereal.messaging as messaging -from selfdrive.manager import start_managed_process, kill_managed_process +from common.basedir import BASEDIR services = ['controlsState', 'thermal', 'radarState'] # the services needed to be spoofed to start ui offroad -procs = ['camerad', 'ui', 'modeld', 'calibrationd'] -[start_managed_process(p) for p in procs] # start needed processes +procs = {'camerad': 'selfdrive/camerad/camerad', 'ui': 'selfdrive/ui/ui', + 'modeld': 'selfdrive/modeld/modeld', 'calibrationd': 'selfdrive/locationd/calibrationd.py'} +started_procs = [subprocess.Popen(os.path.join(BASEDIR, procs[p]), cwd=os.path.join(BASEDIR, os.path.dirname(procs[p]))) for p in procs] # start needed processes pm = messaging.PubMaster(services) dat_cs, dat_thermal, dat_radar = [messaging.new_message(s) for s in services] @@ -17,6 +21,6 @@ pm.send('controlsState', dat_cs) pm.send('thermal', dat_thermal) pm.send('radarState', dat_radar) - time.sleep(1 / 100) # continually send, rate doesn't matter for thermal + time.sleep(1 / 100) # continually send, rate doesn't matter except KeyboardInterrupt: - [kill_managed_process(p) for p in procs] + [p.send_signal(signal.SIGINT) for p in started_procs] diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 29b8835fe3ab89..571186e42912f1 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -89,7 +89,7 @@ def unblock_stdout(): env['SCONS_CACHE'] = "1" nproc = os.cpu_count() - j_flag = "" if nproc is None else "-j%d" % (nproc - 1) + j_flag = "" if nproc is None else "-j8" scons = subprocess.Popen(["scons", j_flag], cwd=BASEDIR, env=env, stderr=subprocess.PIPE) compile_output = [] @@ -124,8 +124,8 @@ def unblock_stdout(): for i in range(3, -1, -1): print("....%d" % i) time.sleep(1) - subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env) - shutil.rmtree("/tmp/scons_cache") + # subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env) + # shutil.rmtree("/tmp/scons_cache") else: print("scons build failed after retry") sys.exit(1) From a987d75b79ed804514171f0170c3e513b9a76e56 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 18 Nov 2020 00:18:37 -0600 Subject: [PATCH 2/7] print speed --- long_debug.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/long_debug.py b/long_debug.py index 05ae69d159f1a3..e117f0111758da 100644 --- a/long_debug.py +++ b/long_debug.py @@ -5,7 +5,7 @@ mpc_idxs = list(range(10)) model_t_idx = [sorted(range(len(model_t)), key=[abs(idx - t) for t in model_t].__getitem__)[0] for idx in mpc_idxs] # matches 0 to 9 interval to idx from t -speed_curr_idx = sorted(range(len(model_t)), key=[abs(t - .1) for t in model_t].__getitem__)[0] # idx used for current speed, position still uses model_t_idx +# speed_curr_idx = sorted(range(len(model_t)), key=[abs(t - .1) for t in model_t].__getitem__)[0] # idx used for current speed, position still uses model_t_idx while 1: @@ -21,13 +21,14 @@ for t in model_t_idx: distances.append(modelV2.position.x[t]) - if t == 0: - speeds.append(modelV2.position.x[speed_curr_idx] / model_t[speed_curr_idx]) - else: - speeds.append(modelV2.position.x[t] / model_t[t]) + # if t == 0: + # speeds.append(modelV2.position.x[speed_curr_idx] / model_t[speed_curr_idx]) + # else: + # speeds.append(modelV2.position.x[t] / model_t[t]) + speeds.append(modelV2.speeds.x[t]) if model_t_idx.index(t) > 0: # skip first since we can't calculate (and don't want to use v_ego) accelerations.append((speeds[-1] - speeds[-2]) / model_t[t]) accelerations[0] = accelerations[1] - (accelerations[2] - accelerations[1]) # extrapolate back first accel from second and third, less weight - print(distances[:5]) + print(speeds[:5]) From f118a1bdfccffda0312f6ecbc8cc7d0005970d2d Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 17 Nov 2020 20:04:32 -0800 Subject: [PATCH 3/7] Speed idx fix --- selfdrive/modeld/models/driving.cc | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 9af34fb8ea63d7..301e7fafb3ec5a 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -325,14 +325,6 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, plan_mhp_max_idx = i; } } - float valid_len = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*32]; - valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len)); - int valid_len_idx = 0; - for (int i=1; i= X_IDXS[valid_len_idx]){ - valid_len_idx = i; - } - } float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)]; float plan_t_arr[TRAJECTORY_SIZE]; @@ -342,12 +334,12 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, auto position = framed.initPosition(); fill_xyzt(position, best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr); - auto orientation = framed.initOrientation(); - fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr); auto velocity = framed.initVelocity(); - fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 6, plan_t_arr); + fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr); + auto orientation = framed.initOrientation(); + fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr); auto orientation_rate = framed.initOrientationRate(); - fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr); + fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr); // lane lines auto lane_lines = framed.initLaneLines(4); @@ -411,9 +403,17 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, plan_mhp_max_idx = i; } } + // x pos at 10s is a good valid_len - float valid_len = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*32]; - // clamp to 5 and MODEL_PATH_DISTANCE + float valid_len = 0; + float valid_len_candidate; + for (int i=1; i= valid_len){ + valid_len = valid_len_candidate; + } + } + // clamp to 10 and MODEL_PATH_DISTANCE valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len)); int valid_len_idx = 0; for (int i=1; i Date: Wed, 18 Nov 2020 00:29:34 -0600 Subject: [PATCH 4/7] fix --- long_debug.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/long_debug.py b/long_debug.py index e117f0111758da..c2ef1b1bbb27cd 100644 --- a/long_debug.py +++ b/long_debug.py @@ -25,7 +25,7 @@ # speeds.append(modelV2.position.x[speed_curr_idx] / model_t[speed_curr_idx]) # else: # speeds.append(modelV2.position.x[t] / model_t[t]) - speeds.append(modelV2.speeds.x[t]) + speeds.append(modelV2.velocity.x[t]) if model_t_idx.index(t) > 0: # skip first since we can't calculate (and don't want to use v_ego) accelerations.append((speeds[-1] - speeds[-2]) / model_t[t]) From 12bf424e8f4585f19fa057aa81a4c6304d71ab14 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 18 Nov 2020 00:31:39 -0600 Subject: [PATCH 5/7] fix --- long_debug.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/long_debug.py b/long_debug.py index c2ef1b1bbb27cd..020f951c8481ec 100644 --- a/long_debug.py +++ b/long_debug.py @@ -31,4 +31,4 @@ accelerations.append((speeds[-1] - speeds[-2]) / model_t[t]) accelerations[0] = accelerations[1] - (accelerations[2] - accelerations[1]) # extrapolate back first accel from second and third, less weight - print(speeds[:5]) + print([round(i * 2.23694, 2) for i in speeds[:5]]) From bd77cfb9e1e1aa4fd90c3bc4343284bdcea127b2 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 18 Nov 2020 00:46:58 -0600 Subject: [PATCH 6/7] Revamp planner e2e long code. Use velocity predictions instead of deriving them. Acceleration is still calculated (not from model) --- long_debug.py | 14 +++----------- selfdrive/controls/lib/planner.py | 19 +++++-------------- 2 files changed, 8 insertions(+), 25 deletions(-) diff --git a/long_debug.py b/long_debug.py index 020f951c8481ec..b693d503c4f35c 100644 --- a/long_debug.py +++ b/long_debug.py @@ -15,20 +15,12 @@ if not sm.updated['modelV2'] or len(modelV2.position.x) == 0: continue - distances = [] # everything is derived from x position since velocity is outputting weird values - speeds = [] - accelerations = [0] + distances, speeds, accelerations = [], [], [] # everything is derived from x position since velocity is outputting weird values for t in model_t_idx: - distances.append(modelV2.position.x[t]) - - # if t == 0: - # speeds.append(modelV2.position.x[speed_curr_idx] / model_t[speed_curr_idx]) - # else: - # speeds.append(modelV2.position.x[t] / model_t[t]) speeds.append(modelV2.velocity.x[t]) - + distances.append(modelV2.position.x[t]) if model_t_idx.index(t) > 0: # skip first since we can't calculate (and don't want to use v_ego) accelerations.append((speeds[-1] - speeds[-2]) / model_t[t]) - accelerations[0] = accelerations[1] - (accelerations[2] - accelerations[1]) # extrapolate back first accel from second and third, less weight + accelerations.insert(0, accelerations[1] - (accelerations[2] - accelerations[1])) # extrapolate back first accel from second and third, less weight print([round(i * 2.23694, 2) for i in speeds[:5]]) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 56ad7515502d52..68f57b3ee0a1bb 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -118,27 +118,18 @@ def parse_modelV2_data(self, sm): return distances, speeds, accelerations model_t = modelV2.position.t - mpc_times = [0.009765625] + list(range(1, 10)) + mpc_times = list(range(0, 10)) model_t_idx = [sorted(range(len(model_t)), key=[abs(idx - t) for t in model_t].__getitem__)[0] for idx in mpc_times] # matches 0 to 9 interval to idx from t - speed_curr_idx = sorted(range(len(model_t)), key=[abs(t - .1) for t in model_t].__getitem__)[0] # idx used for current speed, position still uses model_t_idx - distances = [] # everything is derived from x position since velocity is outputting weird values - speeds = [] - accelerations = [0] - for t in model_t_idx: + for t in model_t_idx: # everything is derived from x position since velocity is outputting weird values + speeds.append(modelV2.velocity.x[t]) distances.append(modelV2.position.x[t]) - - if t == 0: - speeds.append(modelV2.position.x[speed_curr_idx] / model_t[speed_curr_idx]) - else: - speeds.append(modelV2.position.x[t] / model_t[t]) - if model_t_idx.index(t) > 0: # skip first since we can't calculate (and don't want to use v_ego) accelerations.append((speeds[-1] - speeds[-2]) / model_t[t]) - accelerations[0] = accelerations[1] - (accelerations[2] - accelerations[1]) # extrapolate back first accel from second and third, less weight - return distances, speeds, accelerations # hope this works + accelerations.insert(0, accelerations[1] - (accelerations[2] - accelerations[1])) # extrapolate back first accel from second and third, less weight + return distances, speeds, accelerations def update(self, sm, pm, CP, VM, PP): """Gets called when new radarState is available""" From b672005be031056e650dfdd7353a7cde88154135 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 18 Nov 2020 00:52:05 -0600 Subject: [PATCH 7/7] unnecessary --- selfdrive/controls/lib/planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 68f57b3ee0a1bb..c1820dd7a8de99 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -118,7 +118,7 @@ def parse_modelV2_data(self, sm): return distances, speeds, accelerations model_t = modelV2.position.t - mpc_times = list(range(0, 10)) + mpc_times = list(range(10)) model_t_idx = [sorted(range(len(model_t)), key=[abs(idx - t) for t in model_t].__getitem__)[0] for idx in mpc_times] # matches 0 to 9 interval to idx from t