Pulse desire (#1172)
* initial * compilation fixes * add desire * pulse desire * 0d5728f6-a918-40a6-bce3-a9d0f1238180/20 * cleaner * inject model * inject model * test runs * ah already done I see * remove useless * update * updates, doesnt work * fix generating new references Co-authored-by: Willem Melching <willem.melching@gmail.com>pull/1202/head
HaraldSchafer
GitHub
parent
bae7816999
commit
63d3bb517f
BIN
models/supercombo.dlc (Stored with Git LFS)
BIN
models/supercombo.dlc (Stored with Git LFS)
Binary file not shown.
BIN
models/supercombo.keras (Stored with Git LFS)
BIN
models/supercombo.keras (Stored with Git LFS)
Binary file not shown.
|
|
@ -61,6 +61,8 @@ def events_to_bytes(events):
|
|||
for e in events:
|
||||
if isinstance(e, capnp.lib.capnp._DynamicStructReader):
|
||||
e = e.as_builder()
|
||||
if not e.is_root:
|
||||
e = e.copy()
|
||||
ret.append(e.to_bytes())
|
||||
return ret
|
||||
|
||||
|
|
|
|||
|
|
@ -65,9 +65,9 @@ class LanePlanner():
|
|||
self.l_prob = md.leftLane.prob # left line prob
|
||||
self.r_prob = md.rightLane.prob # right line prob
|
||||
|
||||
if len(md.meta.desirePrediction):
|
||||
self.l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1]
|
||||
self.r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1]
|
||||
if len(md.meta.desireState):
|
||||
self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft - 1]
|
||||
self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight - 1]
|
||||
|
||||
def update_d_poly(self, v_ego):
|
||||
# only offset left and right lane lines; offsetting p_poly does not make sense
|
||||
|
|
|
|||
|
|
@ -22,19 +22,16 @@ DESIRES = {
|
|||
LaneChangeDirection.none: {
|
||||
LaneChangeState.off: log.PathPlan.Desire.none,
|
||||
LaneChangeState.preLaneChange: log.PathPlan.Desire.none,
|
||||
LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none,
|
||||
LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none,
|
||||
},
|
||||
LaneChangeDirection.left: {
|
||||
LaneChangeState.off: log.PathPlan.Desire.none,
|
||||
LaneChangeState.preLaneChange: log.PathPlan.Desire.none,
|
||||
LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft,
|
||||
LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft,
|
||||
},
|
||||
LaneChangeDirection.right: {
|
||||
LaneChangeState.off: log.PathPlan.Desire.none,
|
||||
LaneChangeState.preLaneChange: log.PathPlan.Desire.none,
|
||||
LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight,
|
||||
LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight,
|
||||
},
|
||||
}
|
||||
|
|
@ -119,14 +116,10 @@ class PathPlanner():
|
|||
if not one_blinker or below_lane_change_speed:
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
elif torque_applied:
|
||||
self.lane_change_state = LaneChangeState.laneChangeStarting
|
||||
|
||||
# starting
|
||||
elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5:
|
||||
self.lane_change_state = LaneChangeState.laneChangeFinishing
|
||||
self.lane_change_state = LaneChangeState.laneChangeFinishing
|
||||
|
||||
# finishing
|
||||
elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2:
|
||||
elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.5:
|
||||
if one_blinker:
|
||||
self.lane_change_state = LaneChangeState.preLaneChange
|
||||
else:
|
||||
|
|
|
|||
|
|
@ -10,10 +10,11 @@
|
|||
#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2 + 1
|
||||
#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 2
|
||||
#define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 2
|
||||
#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION
|
||||
#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION
|
||||
#define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2
|
||||
#define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2
|
||||
#define META_IDX LONG_A_IDX + TIME_DISTANCE*2
|
||||
#define DESIRE_STATE_IDX LONG_A_IDX + TIME_DISTANCE*2
|
||||
#define META_IDX DESIRE_STATE_IDX + DESIRE_LEN
|
||||
#define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE
|
||||
#define OUTPUT_SIZE POSE_IDX + POSE_SIZE
|
||||
#ifdef TEMPORAL
|
||||
|
|
@ -43,7 +44,9 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
|
|||
#ifdef DESIRE
|
||||
s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float));
|
||||
for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0;
|
||||
s->m->addDesire(s->desire, DESIRE_SIZE);
|
||||
s->pulse_desire = (float*)malloc(DESIRE_SIZE * sizeof(float));
|
||||
for (int i = 0; i < DESIRE_SIZE; i++) s->pulse_desire[i] = 0.0;
|
||||
s->m->addDesire(s->pulse_desire, DESIRE_SIZE);
|
||||
#endif
|
||||
|
||||
// Build Vandermonde matrix
|
||||
|
|
@ -61,7 +64,16 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
|
|||
mat3 transform, void* sock, float *desire_in) {
|
||||
#ifdef DESIRE
|
||||
if (desire_in != NULL) {
|
||||
for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = desire_in[i];
|
||||
for (int i = 0; i < DESIRE_SIZE; i++) {
|
||||
// Model decides when action is completed
|
||||
// so desire input is just a pulse triggered on rising edge
|
||||
if (desire_in[i] - s->desire[i] == 1) {
|
||||
s->pulse_desire[i] = desire_in[i];
|
||||
} else {
|
||||
s->pulse_desire[i] = 0.0;
|
||||
}
|
||||
s->desire[i] = desire_in[i];
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -88,7 +100,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
|
|||
net_outputs.long_x = &s->output[LONG_X_IDX];
|
||||
net_outputs.long_v = &s->output[LONG_V_IDX];
|
||||
net_outputs.long_a = &s->output[LONG_A_IDX];
|
||||
net_outputs.meta = &s->output[META_IDX];
|
||||
net_outputs.meta = &s->output[DESIRE_STATE_IDX];
|
||||
net_outputs.pose = &s->output[POSE_IDX];
|
||||
return net_outputs;
|
||||
}
|
||||
|
|
@ -183,11 +195,13 @@ void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, in
|
|||
}
|
||||
|
||||
void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) {
|
||||
meta.setEngagedProb(meta_data[0]);
|
||||
meta.setGasDisengageProb(meta_data[1]);
|
||||
meta.setBrakeDisengageProb(meta_data[2]);
|
||||
meta.setSteerOverrideProb(meta_data[3]);
|
||||
kj::ArrayPtr<const float> desire_pred(&meta_data[OTHER_META_SIZE], DESIRE_PRED_SIZE);
|
||||
kj::ArrayPtr<const float> desire_state(&meta_data[0], DESIRE_LEN);
|
||||
meta.setDesireState(desire_state);
|
||||
meta.setEngagedProb(meta_data[DESIRE_LEN]);
|
||||
meta.setGasDisengageProb(meta_data[DESIRE_LEN + 1]);
|
||||
meta.setBrakeDisengageProb(meta_data[DESIRE_LEN + 2]);
|
||||
meta.setSteerOverrideProb(meta_data[DESIRE_LEN + 3]);
|
||||
kj::ArrayPtr<const float> desire_pred(&meta_data[DESIRE_LEN + OTHER_META_SIZE], DESIRE_PRED_SIZE);
|
||||
meta.setDesirePrediction(desire_pred);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@
|
|||
#define MODEL_PATH_DISTANCE 192
|
||||
#define POLYFIT_DEGREE 4
|
||||
#define SPEED_PERCENTILES 10
|
||||
#define DESIRE_LEN 8
|
||||
#define DESIRE_PRED_SIZE 32
|
||||
#define OTHER_META_SIZE 4
|
||||
#define LEAD_MDN_N 5 // probs for 5 groups
|
||||
|
|
@ -51,6 +52,7 @@ struct ModelDataRaw {
|
|||
float *long_x;
|
||||
float *long_v;
|
||||
float *long_a;
|
||||
float *desire_state;
|
||||
float *meta;
|
||||
float *pose;
|
||||
};
|
||||
|
|
@ -63,6 +65,7 @@ typedef struct ModelState {
|
|||
RunModel *m;
|
||||
#ifdef DESIRE
|
||||
float *desire;
|
||||
float *pulse_desire;
|
||||
#endif
|
||||
} ModelState;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,92 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
import time
|
||||
|
||||
|
||||
from tqdm import tqdm
|
||||
from cereal.messaging import PubMaster, recv_one, sub_sock
|
||||
from tools.lib.framereader import FrameReader
|
||||
import subprocess
|
||||
import selfdrive.manager as manager
|
||||
|
||||
|
||||
def rreplace(s, old, new, occurrence):
|
||||
li = s.rsplit(old, occurrence)
|
||||
return new.join(li)
|
||||
|
||||
|
||||
def regen_model(msgs, pm, frame_reader, model_sock):
|
||||
# Send some livecalibration messages to initalize visiond
|
||||
for msg in msgs:
|
||||
if msg.which() == 'liveCalibration':
|
||||
pm.send('liveCalibration', msg.as_builder())
|
||||
|
||||
|
||||
out_msgs = []
|
||||
fidx = 0
|
||||
for msg in tqdm(msgs):
|
||||
w = msg.which()
|
||||
|
||||
if w == 'frame':
|
||||
msg = msg.as_builder()
|
||||
|
||||
img = frame_reader.get(fidx, pix_fmt="rgb24")[0][:,::-1]
|
||||
|
||||
msg.frame.image = img.flatten().tobytes()
|
||||
|
||||
pm.send(w, msg)
|
||||
model = recv_one(model_sock)
|
||||
fidx += 1
|
||||
out_msgs.append(model)
|
||||
elif w == 'liveCalibration':
|
||||
pm.send(w, msg.as_builder())
|
||||
|
||||
return out_msgs
|
||||
|
||||
|
||||
def inject_model(msgs, segment_name):
|
||||
if segment_name.count('--') == 2:
|
||||
segment_name = rreplace(segment_name, '--', '/', 1)
|
||||
frame_reader = FrameReader('cd:/'+segment_name.replace("|", "/") + "/fcamera.hevc")
|
||||
|
||||
manager.start_managed_process('camerad')
|
||||
manager.start_managed_process('modeld')
|
||||
# TODO do better than just wait for modeld to boot
|
||||
time.sleep(5)
|
||||
|
||||
pm = PubMaster(['liveCalibration', 'frame'])
|
||||
model_sock = sub_sock('model')
|
||||
try:
|
||||
out_msgs = regen_model(msgs, pm, frame_reader, model_sock)
|
||||
except (KeyboardInterrupt, SystemExit, Exception) as e:
|
||||
manager.kill_managed_process('modeld')
|
||||
time.sleep(2)
|
||||
manager.kill_managed_process('camerad')
|
||||
raise e
|
||||
manager.kill_managed_process('modeld')
|
||||
time.sleep(2)
|
||||
manager.kill_managed_process('camerad')
|
||||
|
||||
|
||||
new_msgs = []
|
||||
midx = 0
|
||||
for msg in msgs:
|
||||
if (msg.which() == 'model') and (midx < len(out_msgs)):
|
||||
model = out_msgs[midx].as_builder()
|
||||
model.logMonoTime = msg.logMonoTime
|
||||
model = model.as_reader()
|
||||
new_msgs.append(model)
|
||||
midx += 1
|
||||
else:
|
||||
new_msgs.append(msg)
|
||||
|
||||
print(len(new_msgs), len(list(msgs)))
|
||||
assert abs(len(new_msgs) - len(list(msgs))) < 2
|
||||
|
||||
return new_msgs
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
inject_model("0375fdf7b1ce594d|2019-06-13--08-32-25/3")
|
||||
|
|
@ -0,0 +1 @@
|
|||
95638846316e5de7f0314ed2330b01428792c889
|
||||
|
|
@ -1 +1 @@
|
|||
a8f2dbe727e8b999a4e1df024abf919c35b1ac7d
|
||||
43b4e291bb92ee02066c59a13ef28aa900a3f092
|
||||
|
|
|
|||
|
|
@ -6,19 +6,23 @@ import sys
|
|||
import tempfile
|
||||
|
||||
from selfdrive.car.car_helpers import interface_names
|
||||
from selfdrive.test.process_replay.compare_logs import compare_logs
|
||||
from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS
|
||||
from selfdrive.test.process_replay.compare_logs import compare_logs
|
||||
from tools.lib.logreader import LogReader
|
||||
|
||||
|
||||
INJECT_MODEL = 0
|
||||
|
||||
segments = [
|
||||
("HONDA", "0375fdf7b1ce594d|2019-06-13--08-32-25--3"), # HONDA.ACCORD
|
||||
("HONDA", "99c94dc769b5d96e|2019-08-03--14-19-59--2"), # HONDA.CIVIC
|
||||
("TOYOTA", "cce908f7eb8db67d|2019-08-02--15-09-51--3"), # TOYOTA.COROLLA_TSS2
|
||||
("GM", "7ad88f53d406b787|2019-07-09--10-18-56--8"), # GM.VOLT
|
||||
("HYUNDAI", "704b2230eb5190d6|2019-07-06--19-29-10--0"), # HYUNDAI.KIA_SORENTO
|
||||
("CHRYSLER", "b6e1317e1bfbefa6|2019-07-06--04-05-26--5"), # CHRYSLER.JEEP_CHEROKEE
|
||||
("TOYOTA", "77611a1fac303767|2020-02-29--13-29-33--3"), # TOYOTA.COROLLA_TSS2
|
||||
("GM", "7cc2a8365b4dd8a9|2018-12-02--12-10-44--2"), # GM.ACADIA
|
||||
("CHRYSLER", "b6849f5cf2c926b1|2020-02-28--07-29-48--13"), # CHRYSLER.PACIFICA
|
||||
("HYUNDAI", "38bfd238edecbcd7|2018-08-29--22-02-15--4"), # HYUNDAI.SANTA_FE
|
||||
#("CHRYSLER", "b6e1317e1bfbefa6|2020-03-04--13-11-40"), # CHRYSLER.JEEP_CHEROKEE
|
||||
("SUBARU", "7873afaf022d36e2|2019-07-03--18-46-44--0"), # SUBARU.IMPREZA
|
||||
("VOLKSWAGEN", "b0c9d2329ad1606b|2020-02-19--16-29-36--7"), # VW.GOLF
|
||||
("VOLKSWAGEN", "b0c9d2329ad1606b|2020-02-19--16-29-36--7"), # VW.GOLF
|
||||
]
|
||||
|
||||
# ford doesn't need to be tested until a full port is done
|
||||
|
|
@ -29,9 +33,14 @@ BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
|
|||
# run the full test (including checks) when no args given
|
||||
FULL_TEST = len(sys.argv) <= 1
|
||||
|
||||
def get_segment(segment_name):
|
||||
def get_segment(segment_name, original=True):
|
||||
route_name, segment_num = segment_name.rsplit("--", 1)
|
||||
rlog_url = BASE_URL + "%s/%s/rlog.bz2" % (route_name.replace("|", "/"), segment_num)
|
||||
if original:
|
||||
rlog_url = BASE_URL + "%s/%s/rlog.bz2" % (route_name.replace("|", "/"), segment_num)
|
||||
else:
|
||||
process_replay_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
model_ref_commit = open(os.path.join(process_replay_dir, "model_ref_commit")).read().strip()
|
||||
rlog_url = BASE_URL + "%s/%s/rlog_%s.bz2" % (route_name.replace("|", "/"), segment_num, model_ref_commit)
|
||||
req = requests.get(rlog_url)
|
||||
assert req.status_code == 200, ("Failed to download log for %s" % segment_name)
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,45 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
|
||||
from selfdrive.test.openpilotci_upload import upload_file
|
||||
from selfdrive.test.process_replay.compare_logs import save_log
|
||||
from selfdrive.test.process_replay.test_processes import segments, get_segment
|
||||
from selfdrive.version import get_git_commit
|
||||
from tools.lib.logreader import LogReader
|
||||
from inject_model import inject_model
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
no_upload = "--no-upload" in sys.argv
|
||||
|
||||
process_replay_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
ref_commit_fn = os.path.join(process_replay_dir, "model_ref_commit")
|
||||
|
||||
ref_commit = get_git_commit()
|
||||
with open(ref_commit_fn, "w") as f:
|
||||
f.write(ref_commit)
|
||||
|
||||
for car_brand, segment in segments:
|
||||
rlog_fn = get_segment(segment, original=True)
|
||||
|
||||
if rlog_fn is None:
|
||||
print("failed to get segment %s" % segment)
|
||||
sys.exit(1)
|
||||
|
||||
lr = LogReader(rlog_fn)
|
||||
print('injecting model into % s' % segment)
|
||||
lr = inject_model(lr, segment)
|
||||
|
||||
route_name, segment_num = segment.rsplit("--", 1)
|
||||
log_fn = "%s/%s/rlog_%s.bz2" % (route_name.replace("|", "/"), segment_num, ref_commit)
|
||||
tmp_name = 'tmp_%s_%s' % (route_name, segment_num)
|
||||
save_log(tmp_name, lr)
|
||||
|
||||
if not no_upload:
|
||||
upload_file(tmp_name, log_fn)
|
||||
print('uploaded %s', log_fn)
|
||||
os.remove(tmp_name)
|
||||
os.remove(rlog_fn)
|
||||
|
||||
print("done")
|
||||
Loading…
Reference in New Issue