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