ZwX1616
3 years ago
commit
79623f1da0
91 changed files with 987 additions and 728 deletions
@ -1 +1 @@ |
|||||||
Subproject commit 242d6bd0b08784a325febae4362d2fe8b048daab |
Subproject commit 8669b82f5fbe0b21a51e018ead0d6d20cb090e0a |
||||||
@ -1 +1 @@ |
|||||||
Subproject commit 919154efe2bd07c4dd124d7e6a11a4afc8685f9d |
Subproject commit 9564b74d80525c9f289b730febbb2348c529c9cc |
||||||
@ -1 +1 @@ |
|||||||
Subproject commit eb662e4e5014a3fc3c04512d708f61080c7707c1 |
Subproject commit d5bd81e5b517c79e164d87b96355e6bc75915da0 |
||||||
@ -1,81 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
# type: ignore |
|
||||||
|
|
||||||
import os |
|
||||||
import argparse |
|
||||||
import signal |
|
||||||
from collections import defaultdict |
|
||||||
|
|
||||||
import cereal.messaging as messaging |
|
||||||
|
|
||||||
def sigint_handler(signal, frame): |
|
||||||
print("handler!") |
|
||||||
exit(0) |
|
||||||
signal.signal(signal.SIGINT, sigint_handler) |
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
|
|
||||||
parser = argparse.ArgumentParser(description='Sniff a communication socket') |
|
||||||
parser.add_argument('--addr', default='127.0.0.1') |
|
||||||
args = parser.parse_args() |
|
||||||
|
|
||||||
if args.addr != "127.0.0.1": |
|
||||||
os.environ["ZMQ"] = "1" |
|
||||||
messaging.context = messaging.Context() |
|
||||||
|
|
||||||
carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True) |
|
||||||
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState'], addr=args.addr) |
|
||||||
|
|
||||||
msg_cnt = 0 |
|
||||||
stats = defaultdict(lambda: {'err': 0, "cnt": 0, "=": 0, "+": 0, "-": 0}) |
|
||||||
cnt = 0 |
|
||||||
total_error = 0 |
|
||||||
|
|
||||||
while messaging.recv_one(carControl): |
|
||||||
sm.update() |
|
||||||
msg_cnt += 1 |
|
||||||
|
|
||||||
actual_speed = sm['carState'].vEgo |
|
||||||
enabled = sm['controlsState'].enabled |
|
||||||
steer_override = sm['controlsState'].steerOverride |
|
||||||
|
|
||||||
# must be above 10 m/s, engaged and not overriding steering |
|
||||||
if actual_speed > 10.0 and enabled and not steer_override: |
|
||||||
cnt += 1 |
|
||||||
|
|
||||||
# wait 5 seconds after engage/override |
|
||||||
if cnt >= 500: |
|
||||||
# calculate error before rounding |
|
||||||
actual_angle = sm['controlsState'].angleSteers |
|
||||||
desired_angle = sm['carControl'].actuators.steeringAngleDeg |
|
||||||
angle_error = abs(desired_angle - actual_angle) |
|
||||||
|
|
||||||
# round numbers |
|
||||||
actual_angle = round(actual_angle, 1) |
|
||||||
desired_angle = round(desired_angle, 1) |
|
||||||
angle_error = round(angle_error, 2) |
|
||||||
angle_abs = int(abs(round(desired_angle, 0))) |
|
||||||
|
|
||||||
# collect stats |
|
||||||
stats[angle_abs]["err"] += angle_error |
|
||||||
stats[angle_abs]["cnt"] += 1 |
|
||||||
if actual_angle == desired_angle: |
|
||||||
stats[angle_abs]["="] += 1 |
|
||||||
else: |
|
||||||
if desired_angle == 0.: |
|
||||||
overshoot = True |
|
||||||
else: |
|
||||||
overshoot = desired_angle < actual_angle if desired_angle > 0. else desired_angle > actual_angle |
|
||||||
stats[angle_abs]["+" if overshoot else "-"] += 1 |
|
||||||
else: |
|
||||||
cnt = 0 |
|
||||||
|
|
||||||
if msg_cnt % 100 == 0: |
|
||||||
print(chr(27) + "[2J") |
|
||||||
if cnt != 0: |
|
||||||
print("COLLECTING ...") |
|
||||||
else: |
|
||||||
print("DISABLED (speed too low, not engaged, or steer override)") |
|
||||||
for k in sorted(stats.keys()): |
|
||||||
v = stats[k] |
|
||||||
print(f'angle: {k:#2} | error: {round(v["err"] / v["cnt"], 2):2.2f} | =:{int(v["="] / v["cnt"] * 100):#3}% | +:{int(v["+"] / v["cnt"] * 100):#4}% | -:{int(v["-"] / v["cnt"] * 100):#3}% | count: {v["cnt"]:#4}') |
|
||||||
@ -0,0 +1,23 @@ |
|||||||
|
#!/usr/bin/bash |
||||||
|
set -e |
||||||
|
|
||||||
|
cd /sys/kernel/tracing |
||||||
|
|
||||||
|
echo 1 > tracing_on |
||||||
|
echo boot > trace_clock |
||||||
|
echo 1000 > buffer_size_kb |
||||||
|
|
||||||
|
# /sys/kernel/tracing/available_events |
||||||
|
echo 1 > events/irq/enable |
||||||
|
echo 1 > events/sched/enable |
||||||
|
echo 1 > events/kgsl/enable |
||||||
|
echo 1 > events/camera/enable |
||||||
|
echo 1 > events/workqueue/enable |
||||||
|
|
||||||
|
echo > trace |
||||||
|
sleep 5 |
||||||
|
echo 0 > tracing_on |
||||||
|
|
||||||
|
cp trace /tmp/trace |
||||||
|
chown comma: /tmp/trace |
||||||
|
echo /tmp/trace |
||||||
@ -1,13 +0,0 @@ |
|||||||
#pragma once |
|
||||||
|
|
||||||
#include <cstdint> |
|
||||||
#include "cereal/visionipc/visionipc.h" |
|
||||||
|
|
||||||
class VideoEncoder { |
|
||||||
public: |
|
||||||
virtual ~VideoEncoder() {} |
|
||||||
virtual int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, |
|
||||||
int in_width, int in_height, VisionIpcBufExtra *extra) = 0; |
|
||||||
virtual void encoder_open(const char* path) = 0; |
|
||||||
virtual void encoder_close() = 0; |
|
||||||
}; |
|
||||||
@ -0,0 +1,79 @@ |
|||||||
|
#include <cassert> |
||||||
|
#include "selfdrive/loggerd/encoder/encoder.h" |
||||||
|
|
||||||
|
VideoEncoder::~VideoEncoder() {} |
||||||
|
|
||||||
|
void VideoEncoder::publisher_init() { |
||||||
|
// publish
|
||||||
|
service_name = this->type == DriverCam ? "driverEncodeData" : |
||||||
|
(this->type == WideRoadCam ? "wideRoadEncodeData" : |
||||||
|
(this->in_width == this->out_width ? "roadEncodeData" : "qRoadEncodeData")); |
||||||
|
pm.reset(new PubMaster({service_name})); |
||||||
|
} |
||||||
|
|
||||||
|
void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, |
||||||
|
unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat) { |
||||||
|
// broadcast packet
|
||||||
|
MessageBuilder msg; |
||||||
|
auto event = msg.initEvent(true); |
||||||
|
auto edat = (e->type == DriverCam) ? event.initDriverEncodeData() : |
||||||
|
((e->type == WideRoadCam) ? event.initWideRoadEncodeData() : |
||||||
|
(e->in_width == e->out_width ? event.initRoadEncodeData() : event.initQRoadEncodeData())); |
||||||
|
auto edata = edat.initIdx(); |
||||||
|
edata.setFrameId(extra.frame_id); |
||||||
|
edata.setTimestampSof(extra.timestamp_sof); |
||||||
|
edata.setTimestampEof(extra.timestamp_eof); |
||||||
|
edata.setType(e->codec); |
||||||
|
edata.setEncodeId(idx); |
||||||
|
edata.setSegmentNum(segment_num); |
||||||
|
edata.setSegmentId(idx); |
||||||
|
edata.setFlags(flags); |
||||||
|
edata.setLen(dat.size()); |
||||||
|
edat.setData(dat); |
||||||
|
if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header); |
||||||
|
|
||||||
|
auto words = new kj::Array<capnp::word>(capnp::messageToFlatArray(msg)); |
||||||
|
auto bytes = words->asBytes(); |
||||||
|
e->pm->send(e->service_name, bytes.begin(), bytes.size()); |
||||||
|
if (e->write) { |
||||||
|
e->to_write.push(words); |
||||||
|
} else { |
||||||
|
delete words; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// TODO: writing should be moved to loggerd
|
||||||
|
void VideoEncoder::write_handler(VideoEncoder *e, const char *path) { |
||||||
|
VideoWriter writer(path, e->filename, e->codec != cereal::EncodeIndex::Type::FULL_H_E_V_C, e->out_width, e->out_height, e->fps, e->codec); |
||||||
|
|
||||||
|
bool first = true; |
||||||
|
kj::Array<capnp::word>* out_buf; |
||||||
|
while ((out_buf = e->to_write.pop())) { |
||||||
|
capnp::FlatArrayMessageReader cmsg(*out_buf); |
||||||
|
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>(); |
||||||
|
|
||||||
|
auto edata = (e->type == DriverCam) ? event.getDriverEncodeData() : |
||||||
|
((e->type == WideRoadCam) ? event.getWideRoadEncodeData() : |
||||||
|
(e->in_width == e->out_width ? event.getRoadEncodeData() : event.getQRoadEncodeData())); |
||||||
|
auto idx = edata.getIdx(); |
||||||
|
auto flags = idx.getFlags(); |
||||||
|
|
||||||
|
if (first) { |
||||||
|
assert(flags & V4L2_BUF_FLAG_KEYFRAME); |
||||||
|
auto header = edata.getHeader(); |
||||||
|
writer.write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof()/1000, true, false); |
||||||
|
first = false; |
||||||
|
} |
||||||
|
|
||||||
|
// dangerous cast from const, but should be fine
|
||||||
|
auto data = edata.getData(); |
||||||
|
if (data.size() > 0) { |
||||||
|
writer.write((uint8_t *)data.begin(), data.size(), idx.getTimestampEof()/1000, false, flags & V4L2_BUF_FLAG_KEYFRAME); |
||||||
|
} |
||||||
|
|
||||||
|
// free the data
|
||||||
|
delete out_buf; |
||||||
|
} |
||||||
|
|
||||||
|
// VideoWriter is freed on out of scope
|
||||||
|
} |
||||||
@ -0,0 +1,60 @@ |
|||||||
|
#pragma once |
||||||
|
|
||||||
|
#include <cassert> |
||||||
|
#include <cstdint> |
||||||
|
#include <thread> |
||||||
|
|
||||||
|
#include "cereal/messaging/messaging.h" |
||||||
|
#include "cereal/visionipc/visionipc.h" |
||||||
|
#include "selfdrive/common/queue.h" |
||||||
|
#include "selfdrive/loggerd/encoder/video_writer.h" |
||||||
|
#include "selfdrive/camerad/cameras/camera_common.h" |
||||||
|
|
||||||
|
#define V4L2_BUF_FLAG_KEYFRAME 8 |
||||||
|
|
||||||
|
class VideoEncoder { |
||||||
|
public: |
||||||
|
VideoEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps, |
||||||
|
int bitrate, cereal::EncodeIndex::Type codec, int out_width, int out_height, bool write) |
||||||
|
: filename(filename), type(type), in_width(in_width), in_height(in_height), fps(fps), |
||||||
|
bitrate(bitrate), codec(codec), out_width(out_width), out_height(out_height), write(write) { } |
||||||
|
virtual ~VideoEncoder(); |
||||||
|
virtual int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, |
||||||
|
int in_width, int in_height, VisionIpcBufExtra *extra) = 0; |
||||||
|
virtual void encoder_open(const char* path) = 0; |
||||||
|
virtual void encoder_close() = 0; |
||||||
|
|
||||||
|
void publisher_init(); |
||||||
|
static void publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat); |
||||||
|
|
||||||
|
void writer_open(const char* path) { |
||||||
|
if (this->write) write_handler_thread = std::thread(VideoEncoder::write_handler, this, path); |
||||||
|
} |
||||||
|
|
||||||
|
void writer_close() { |
||||||
|
if (this->write) { |
||||||
|
to_write.push(NULL); |
||||||
|
write_handler_thread.join(); |
||||||
|
} |
||||||
|
assert(to_write.empty()); |
||||||
|
} |
||||||
|
|
||||||
|
protected: |
||||||
|
bool write; |
||||||
|
const char* filename; |
||||||
|
int in_width, in_height; |
||||||
|
int out_width, out_height, fps; |
||||||
|
int bitrate; |
||||||
|
cereal::EncodeIndex::Type codec; |
||||||
|
CameraType type; |
||||||
|
|
||||||
|
private: |
||||||
|
// publishing
|
||||||
|
std::unique_ptr<PubMaster> pm; |
||||||
|
const char *service_name; |
||||||
|
|
||||||
|
// writing support
|
||||||
|
static void write_handler(VideoEncoder *e, const char *path); |
||||||
|
std::thread write_handler_thread; |
||||||
|
SafeQueue<kj::Array<capnp::word>* > to_write; |
||||||
|
}; |
||||||
@ -0,0 +1,38 @@ |
|||||||
|
#pragma once |
||||||
|
|
||||||
|
#include <cstdio> |
||||||
|
#include <cstdlib> |
||||||
|
#include <string> |
||||||
|
#include <vector> |
||||||
|
|
||||||
|
extern "C" { |
||||||
|
#include <libavcodec/avcodec.h> |
||||||
|
#include <libavformat/avformat.h> |
||||||
|
#include <libavutil/imgutils.h> |
||||||
|
} |
||||||
|
|
||||||
|
#include "selfdrive/loggerd/encoder/encoder.h" |
||||||
|
#include "selfdrive/loggerd/loggerd.h" |
||||||
|
#include "selfdrive/loggerd/encoder/video_writer.h" |
||||||
|
|
||||||
|
class FfmpegEncoder : public VideoEncoder { |
||||||
|
public: |
||||||
|
FfmpegEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps, |
||||||
|
int bitrate, cereal::EncodeIndex::Type codec, int out_width, int out_height, bool write) : |
||||||
|
VideoEncoder(filename, type, in_width, in_height, fps, bitrate, cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS, out_width, out_height, write) { encoder_init(); } |
||||||
|
~FfmpegEncoder(); |
||||||
|
void encoder_init(); |
||||||
|
int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, |
||||||
|
int in_width_, int in_height_, VisionIpcBufExtra *extra); |
||||||
|
void encoder_open(const char* path); |
||||||
|
void encoder_close(); |
||||||
|
|
||||||
|
private: |
||||||
|
int segment_num = -1; |
||||||
|
int counter = 0; |
||||||
|
bool is_open = false; |
||||||
|
|
||||||
|
AVCodecContext *codec_ctx; |
||||||
|
AVFrame *frame = NULL; |
||||||
|
std::vector<uint8_t> downscale_buf; |
||||||
|
}; |
||||||
@ -1,41 +0,0 @@ |
|||||||
#pragma once |
|
||||||
|
|
||||||
#include <cstdio> |
|
||||||
#include <cstdlib> |
|
||||||
#include <string> |
|
||||||
#include <vector> |
|
||||||
|
|
||||||
extern "C" { |
|
||||||
#include <libavcodec/avcodec.h> |
|
||||||
#include <libavformat/avformat.h> |
|
||||||
#include <libavutil/imgutils.h> |
|
||||||
} |
|
||||||
|
|
||||||
#include "selfdrive/loggerd/encoder.h" |
|
||||||
#include "selfdrive/loggerd/loggerd.h" |
|
||||||
#include "selfdrive/loggerd/video_writer.h" |
|
||||||
|
|
||||||
class RawLogger : public VideoEncoder { |
|
||||||
public: |
|
||||||
RawLogger(const char* filename, CameraType type, int in_width, int in_height, int fps, |
|
||||||
int bitrate, bool h265, int out_width, int out_height, bool write = true); |
|
||||||
~RawLogger(); |
|
||||||
int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, |
|
||||||
int in_width, int in_height, VisionIpcBufExtra *extra); |
|
||||||
void encoder_open(const char* path); |
|
||||||
void encoder_close(); |
|
||||||
|
|
||||||
private: |
|
||||||
const char* filename; |
|
||||||
//bool write;
|
|
||||||
int fps; |
|
||||||
int counter = 0; |
|
||||||
bool is_open = false; |
|
||||||
|
|
||||||
int in_width_, in_height_; |
|
||||||
|
|
||||||
AVFrame *frame = NULL; |
|
||||||
std::vector<uint8_t> downscale_buf; |
|
||||||
|
|
||||||
VideoWriter *writer = NULL; |
|
||||||
}; |
|
||||||
@ -1 +1 @@ |
|||||||
14f411de8085d1cc9e467592c90bcaf95447a467 |
01ab99d416ffa6fb5f96ebd2091be94a3dc6c6bf |
||||||
@ -1,38 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
import os |
|
||||||
import sys |
|
||||||
|
|
||||||
from selfdrive.test.openpilotci import upload_file, get_url |
|
||||||
from selfdrive.test.process_replay.compare_logs import save_log |
|
||||||
from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS |
|
||||||
from selfdrive.test.process_replay.test_processes import segments |
|
||||||
from selfdrive.version import get_commit |
|
||||||
from tools.lib.logreader import LogReader |
|
||||||
|
|
||||||
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, "ref_commit") |
|
||||||
|
|
||||||
ref_commit = get_commit() |
|
||||||
if ref_commit is None: |
|
||||||
raise Exception("couldn't get ref commit") |
|
||||||
with open(ref_commit_fn, "w") as f: |
|
||||||
f.write(ref_commit) |
|
||||||
|
|
||||||
for car_brand, segment in segments: |
|
||||||
r, n = segment.rsplit("--", 1) |
|
||||||
lr = LogReader(get_url(r, n)) |
|
||||||
|
|
||||||
for cfg in CONFIGS: |
|
||||||
log_msgs = replay_process(cfg, lr) |
|
||||||
log_fn = os.path.join(process_replay_dir, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2") |
|
||||||
save_log(log_fn, log_msgs) |
|
||||||
|
|
||||||
if not no_upload: |
|
||||||
upload_file(log_fn, os.path.basename(log_fn)) |
|
||||||
os.remove(log_fn) |
|
||||||
|
|
||||||
print("done") |
|
||||||
@ -0,0 +1,131 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
# type: ignore |
||||||
|
|
||||||
|
import os |
||||||
|
import time |
||||||
|
import argparse |
||||||
|
import signal |
||||||
|
from collections import defaultdict |
||||||
|
|
||||||
|
import cereal.messaging as messaging |
||||||
|
|
||||||
|
def sigint_handler(signal, frame): |
||||||
|
print("handler!") |
||||||
|
exit(0) |
||||||
|
signal.signal(signal.SIGINT, sigint_handler) |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
|
||||||
|
parser = argparse.ArgumentParser(description='Sniff a communication socket') |
||||||
|
parser.add_argument('control_type', help="[pid|indi|lqr|angle]") |
||||||
|
parser.add_argument('--addr', default='127.0.0.1', help="IP address for optional ZMQ listener, default to msgq") |
||||||
|
parser.add_argument('--group', default='all', help="speed group to display, [crawl|slow|medium|fast|veryfast|germany|all], default to all") |
||||||
|
args = parser.parse_args() |
||||||
|
|
||||||
|
if args.addr != "127.0.0.1": |
||||||
|
os.environ["ZMQ"] = "1" |
||||||
|
messaging.context = messaging.Context() |
||||||
|
|
||||||
|
all_groups = {"germany": (45, "45 - up m/s // 162 - up km/h // 101 - up mph"), |
||||||
|
"veryfast": (35, "35 - 45 m/s // 126 - 162 km/h // 78 - 101 mph"), |
||||||
|
"fast": (25, "25 - 35 m/s // 90 - 126 km/h // 56 - 78 mph"), |
||||||
|
"medium": (15, "15 - 25 m/s // 54 - 90 km/h // 34 - 56 mph"), |
||||||
|
"slow": (5, " 5 - 15 m/s // 18 - 54 km/h // 11 - 34 mph"), |
||||||
|
"crawl": (0, " 0 - 5 m/s // 0 - 18 km/h // 0 - 11 mph")} |
||||||
|
|
||||||
|
if args.group == "all": |
||||||
|
display_groups = all_groups.keys() |
||||||
|
elif args.group in all_groups.keys(): |
||||||
|
display_groups = [args.group] |
||||||
|
else: |
||||||
|
raise ValueError("invalid speed group, see help") |
||||||
|
|
||||||
|
speed_group_stats = {} |
||||||
|
for group in all_groups: |
||||||
|
speed_group_stats[group] = defaultdict(lambda: {'err': 0, "cnt": 0, "=": 0, "+": 0, "-": 0, "steer": 0, "limited": 0, "saturated": 0, "dpp": 0}) |
||||||
|
|
||||||
|
carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True) |
||||||
|
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'lateralPlan'], addr=args.addr) |
||||||
|
time.sleep(1) # Make sure all submaster data is available before going further |
||||||
|
|
||||||
|
msg_cnt = 0 |
||||||
|
cnt = 0 |
||||||
|
total_error = 0 |
||||||
|
|
||||||
|
while messaging.recv_one(carControl): |
||||||
|
sm.update() |
||||||
|
msg_cnt += 1 |
||||||
|
|
||||||
|
if args.control_type == "pid": |
||||||
|
control_state = sm['controlsState'].lateralControlState.pidState |
||||||
|
elif args.control_type == "indi": |
||||||
|
control_state = sm['controlsState'].lateralControlState.indiState |
||||||
|
elif args.control_type == "lqr": |
||||||
|
control_state = sm['controlsState'].lateralControlState.lqrState |
||||||
|
elif args.control_type == "angle": |
||||||
|
control_state = sm['controlsState'].lateralControlState.angleState |
||||||
|
else: |
||||||
|
raise ValueError("invalid lateral control type, see help") |
||||||
|
|
||||||
|
v_ego = sm['carState'].vEgo |
||||||
|
active = sm['controlsState'].active |
||||||
|
steer = sm['carControl'].actuatorsOutput.steer |
||||||
|
standstill = sm['carState'].standstill |
||||||
|
steer_limited = sm['carState'].steeringRateLimited |
||||||
|
overriding = sm['carState'].steeringPressed |
||||||
|
changing_lanes = sm['lateralPlan'].laneChangeState != 0 |
||||||
|
d_path_points = sm['lateralPlan'].dPathPoints |
||||||
|
# must be engaged, not at standstill, not overriding steering, and not changing lanes |
||||||
|
if active and not standstill and not overriding and not changing_lanes: |
||||||
|
cnt += 1 |
||||||
|
|
||||||
|
# wait 5 seconds after engage / standstill / override / lane change |
||||||
|
if cnt >= 500: |
||||||
|
actual_angle = control_state.steeringAngleDeg |
||||||
|
desired_angle = control_state.steeringAngleDesiredDeg |
||||||
|
|
||||||
|
# calculate error before rounding, then round for stats grouping |
||||||
|
angle_error = abs(desired_angle - actual_angle) |
||||||
|
actual_angle = round(actual_angle, 1) |
||||||
|
desired_angle = round(desired_angle, 1) |
||||||
|
angle_error = round(angle_error, 2) |
||||||
|
angle_abs = int(abs(round(desired_angle, 0))) |
||||||
|
|
||||||
|
for group, group_props in all_groups.items(): |
||||||
|
if v_ego > group_props[0]: |
||||||
|
# collect stats |
||||||
|
speed_group_stats[group][angle_abs]["cnt"] += 1 |
||||||
|
speed_group_stats[group][angle_abs]["err"] += angle_error |
||||||
|
speed_group_stats[group][angle_abs]["steer"] += abs(steer) |
||||||
|
if len(d_path_points): |
||||||
|
speed_group_stats[group][angle_abs]["dpp"] += abs(d_path_points[0]) |
||||||
|
if steer_limited: |
||||||
|
speed_group_stats[group][angle_abs]["limited"] += 1 |
||||||
|
if control_state.saturated: |
||||||
|
speed_group_stats[group][angle_abs]["saturated"] += 1 |
||||||
|
if actual_angle == desired_angle: |
||||||
|
speed_group_stats[group][angle_abs]["="] += 1 |
||||||
|
else: |
||||||
|
if desired_angle == 0.: |
||||||
|
overshoot = True |
||||||
|
else: |
||||||
|
overshoot = desired_angle < actual_angle if desired_angle > 0. else desired_angle > actual_angle |
||||||
|
speed_group_stats[group][angle_abs]["+" if overshoot else "-"] += 1 |
||||||
|
break |
||||||
|
else: |
||||||
|
cnt = 0 |
||||||
|
|
||||||
|
if msg_cnt % 100 == 0: |
||||||
|
print(chr(27) + "[2J") |
||||||
|
if cnt != 0: |
||||||
|
print("COLLECTING ...\n") |
||||||
|
else: |
||||||
|
print("DISABLED (not active, standstill, steering override, or lane change)\n") |
||||||
|
for group in display_groups: |
||||||
|
if len(speed_group_stats[group]) > 0: |
||||||
|
print(f"speed group: {group:10s} {all_groups[group][1]:>96s}") |
||||||
|
print(f" {'-'*118}") |
||||||
|
for k in sorted(speed_group_stats[group].keys()): |
||||||
|
v = speed_group_stats[group][k] |
||||||
|
print(f' {k:#2}° | actuator:{int(v["steer"] / v["cnt"] * 100):#3}% | error: {round(v["err"] / v["cnt"], 2):2.2f}° | -:{int(v["-"] / v["cnt"] * 100):#3}% | =:{int(v["="] / v["cnt"] * 100):#3}% | +:{int(v["+"] / v["cnt"] * 100):#3}% | lim:{v["limited"]:#5} | sat:{v["saturated"]:#5} | path dev: {round(v["dpp"] / v["cnt"], 2):2.2f}m | total: {v["cnt"]:#5}') |
||||||
|
print("") |
||||||
Loading…
Reference in new issue