Skip to content

Commit

Permalink
Disambiguate frame id in latencylogger (commaai#24164)
Browse files Browse the repository at this point in the history 
* add tools to disambiguate frame id

* Update selfdrive/camerad/cameras/camera_qcom2.cc

Co-authored-by: Gijs Koning <gijs-koning@live.nl>

* bug fix

* remove duplicate code

* PR comments

* bug fix

Co-authored-by: Gijs Koning <gijs-koning@live.nl>
Co-authored-by: Comma Device <device@comma.ai>
  • Loading branch information
@lukaspetersson @gijskoning @spektor56
3 people authored and spektor56 committed May 7, 2022
1 parent 0f44275 commit 3f46dc2
Show file tree
Hide file tree
Showing 5 changed files with 115 additions and 79 deletions.
4 changes: 2 additions & 2 deletions selfdrive/camerad/cameras/camera_qcom2.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1092,10 +1092,10 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
if ((c == &s->road_cam && env_send_road) || (c == &s->wide_road_cam && env_send_wide_road)) {
framed.setImage(get_frame_image(b));
}
LOGT("%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera");
LOGT(c->buf.cur_frame_data.frame_id, "%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera");
if (c == &s->road_cam) {
framed.setTransform(b->yuv_transform.v);
LOGT("%s: Transformed", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera");
LOGT(c->buf.cur_frame_data.frame_id, "%s: Transformed", "RoadCamera");
}
s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg);

Expand Down
62 changes: 40 additions & 22 deletions selfdrive/common/swaglog.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

#include <cassert>
#include <cstring>
#include <limits>
#include <mutex>
#include <string>

Expand Down Expand Up @@ -64,6 +65,7 @@ class SwaglogState : public LogState {

static SwaglogState s = {};
bool LOG_TIMESTAMPS = getenv("LOG_TIMESTAMPS");
uint32_t NO_FRAME_ID = std::numeric_limits<uint32_t>::max();

static void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) {
if (levelnum >= s.print_level) {
Expand All @@ -72,14 +74,9 @@ static void log(int levelnum, const char* filename, int lineno, const char* func
char levelnum_c = levelnum;
zmq_send(s.sock, (levelnum_c + log_s).c_str(), log_s.length() + 1, ZMQ_NOBLOCK);
}
static void cloudlog_common(int levelnum, bool is_timestamp, const char* filename, int lineno, const char* func,
const char* fmt, va_list args) {
char* msg_buf = nullptr;
int ret = vasprintf(&msg_buf, fmt, args);
if (ret <= 0 || !msg_buf) return;

static void cloudlog_common(int levelnum, const char* filename, int lineno, const char* func,
char* msg_buf, json11::Json::object msg_j={}) {
std::lock_guard lk(s.lock);

if (!s.initialized) s.initialize();

json11::Json::object log_j = json11::Json::object {
Expand All @@ -90,17 +87,10 @@ static void cloudlog_common(int levelnum, bool is_timestamp, const char* filenam
{"funcname", func},
{"created", seconds_since_epoch()}
};

if (is_timestamp) {
json11::Json::object tspt_j = json11::Json::object {
{"timestamp", json11::Json::object{
{"event", msg_buf},
{"time", std::to_string(nanos_since_boot())}}
}
};
log_j["msg"] = tspt_j;
} else {
if (msg_j.empty()) {
log_j["msg"] = msg_buf;
} else {
log_j["msg"] = msg_j;
}

std::string log_s = ((json11::Json)log_j).dump();
Expand All @@ -109,18 +99,46 @@ static void cloudlog_common(int levelnum, bool is_timestamp, const char* filenam
}

void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...){
const char* fmt, ...) {
va_list args;
va_start(args, fmt);
cloudlog_common(levelnum, false, filename, lineno, func, fmt, args);
char* msg_buf = nullptr;
int ret = vasprintf(&msg_buf, fmt, args);
va_end(args);
if (ret <= 0 || !msg_buf) return;
cloudlog_common(levelnum, filename, lineno, func, msg_buf);
}

void cloudlog_t(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...){
void cloudlog_t_common(int levelnum, const char* filename, int lineno, const char* func,
uint32_t frame_id, const char* fmt, va_list args) {
if (!LOG_TIMESTAMPS) return;
char* msg_buf = nullptr;
int ret = vasprintf(&msg_buf, fmt, args);
if (ret <= 0 || !msg_buf) return;
json11::Json::object tspt_j = json11::Json::object{
{"event", msg_buf},
{"time", std::to_string(nanos_since_boot())}
};
if (frame_id < NO_FRAME_ID) {
tspt_j["frame_id"] = std::to_string(frame_id);
}
tspt_j = json11::Json::object{{"timestamp", tspt_j}};
cloudlog_common(levelnum, filename, lineno, func, msg_buf, tspt_j);
}


void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) {
va_list args;
va_start(args, fmt);
cloudlog_t_common(levelnum, filename, lineno, func, NO_FRAME_ID, fmt, args);
va_end(args);
}
void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
uint32_t frame_id, const char* fmt, ...) {
va_list args;
va_start(args, fmt);
cloudlog_common(levelnum, true, filename, lineno, func, fmt, args);
cloudlog_t_common(levelnum, filename, lineno, func, frame_id, fmt, args);
va_end(args);
}

18 changes: 12 additions & 6 deletions selfdrive/common/swaglog.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,21 @@
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;

void cloudlog_t(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;

void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
uint32_t frame_id, const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;


#define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
__func__, \
fmt, ## __VA_ARGS__);

#define cloudlog_t_m(lvl, fmt, ...) cloudlog_t(lvl, __FILE__, __LINE__, \
__func__, \
fmt, ## __VA_ARGS__);
#define cloudlog_t(lvl, ...) cloudlog_te(lvl, __FILE__, __LINE__, \
__func__, \
__VA_ARGS__);


#define cloudlog_rl(burst, millis, lvl, fmt, ...) \
{ \
Expand Down Expand Up @@ -52,7 +57,8 @@ void cloudlog_t(int levelnum, const char* filename, int lineno, const char* func
} \
}

#define LOGT(fmt, ...) cloudlog_t_m(CLOUDLOG_DEBUG, fmt,## __VA_ARGS__)

#define LOGT(...) cloudlog_t(CLOUDLOG_DEBUG, __VA_ARGS__)
#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
Expand Down
103 changes: 55 additions & 48 deletions tools/latencylogger/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@ optional arguments:
--demo Use the demo route instead of providing one (default: False)
--plot If a plot should be generated (default: False)
```
To timestamp an event, use `LOGT("msg")` in c++ code or `cloudlog.timestamp("msg")` in python code. If the print is warning for frameId assignment ambiguity, use `LOGT(frameId ,"msg")`.

## Examples
Plotting with relative starts each process at time=0 and gives a nice overview. Timestamps are visualized as diamonds. The opacity allows for visualization of overlapping services.
Expand All @@ -31,58 +32,64 @@ Plotting without relative provides info about the frames relative time.

Printed timestamps of a frame with internal durations.
```
Frame ID: 303
Frame ID: 371
camerad
roadCameraState start of frame 0.0
wideRoadCameraState start of frame 0.091926
RoadCamera: Image set 1.691696
RoadCamera: Transformed 1.812841
roadCameraState published 51.775466
wideRoadCameraState published 54.935164
roadCameraState.processingTime 1.6455530421808362
wideRoadCameraState.processingTime 4.790564067661762
wideRoadCameraState start of frame 0.0
roadCameraState start of frame 0.072395
wideRoadCameraState published 47.804745
WideRoadCamera: Image set 47.839849
roadCameraState published 48.319166
RoadCamera: Image set 48.354478
RoadCamera: Transformed 48.430258
wideRoadCameraState.processingTime 16.733376309275627
roadCameraState.processingTime 16.218071803450584
modeld
Image added 56.628788
Extra image added 57.459923
Execution finished 75.091306
modelV2 published 75.24797
modelV2.modelExecutionTime 20.00947669148445
Image added 51.346522
Extra image added 53.179467
Execution finished 71.584437
modelV2 published 71.76881
modelV2.modelExecutionTime 22.54236489534378
modelV2.gpuExecutionTime 0.0
plannerd
lateralPlan published 80.426861
longitudinalPlan published 85.722781
lateralPlan.solverExecutionTime 1.0600379901006818
longitudinalPlan.solverExecutionTime 1.3830000534653664
lateralPlan published 77.381862
longitudinalPlan published 84.207972
lateralPlan.solverExecutionTime 1.3547739945352077
longitudinalPlan.solverExecutionTime 2.0179999992251396
controlsd
Data sampled 89.436221
Events updated 90.356522
sendcan published 91.396092
controlsState published 91.77843
Data sampled 99.885876
Events updated 100.696855
sendcan published 101.600489
controlsState published 101.941839
Data sampled 110.087669
Events updated 111.025365
sendcan published 112.305921
controlsState published 112.70451
Data sampled 119.692803
Events updated 120.56774
sendcan published 121.735016
controlsState published 122.142823
Data sampled 129.264761
Events updated 130.024282
sendcan published 130.950364
controlsState published 131.281558
Data sampled 78.909759
Events updated 79.711884
sendcan published 80.721038
controlsState published 81.081398
Data sampled 88.663748
Events updated 89.535403
sendcan published 90.587889
controlsState published 91.019707
Data sampled 98.667003
Events updated 99.661261
sendcan published 100.776507
controlsState published 101.198794
Data sampled 108.967078
Events updated 109.95842
sendcan published 111.263142
controlsState published 111.678085
Data sampled 118.574923
Events updated 119.608555
sendcan published 120.73427
controlsState published 121.111036
Data sampled 128.596408
Events updated 129.382283
sendcan published 130.330083
controlsState published 130.676485
boardd
sending sendcan to panda: 250027001751393037323631 101.705487
sendcan sent to panda: 250027001751393037323631 102.042462
sending sendcan to panda: 250027001751393037323631 112.416961
sendcan sent to panda: 250027001751393037323631 112.792269
sending sendcan to panda: 250027001751393037323631 121.850952
sendcan sent to panda: 250027001751393037323631 122.231103
sending sendcan to panda: 250027001751393037323631 131.045206
sendcan sent to panda: 250027001751393037323631 131.351296
sending sendcan to panda: 250027001751393037323631 141.340592
sendcan sent to panda: 250027001751393037323631 141.700744
sending sendcan to panda: 250027001751393037323631 90.7257
sendcan sent to panda: 250027001751393037323631 91.078143
sending sendcan to panda: 250027001751393037323631 100.941766
sendcan sent to panda: 250027001751393037323631 101.306865
sending sendcan to panda: 250027001751393037323631 111.411786
sendcan sent to panda: 250027001751393037323631 111.754074
sending sendcan to panda: 250027001751393037323631 120.875987
sendcan sent to panda: 250027001751393037323631 121.188535
sending sendcan to panda: 250027001751393037323631 130.454248
sendcan sent to panda: 250027001751393037323631 130.757994
sending sendcan to panda: 250027001751393037323631 140.353234
```
7 changes: 6 additions & 1 deletion tools/latencylogger/latency_logger.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,6 @@ def read_logs(lr):
assert len(frame_mismatches) < 20, "Too many frame mismatches"
return (data, frame_mismatches)


def find_frame_id(time, service, start_times, end_times):
for frame_id in reversed(start_times):
if start_times[frame_id][service] and end_times[frame_id][service]:
Expand All @@ -108,6 +107,10 @@ def insert_cloudlogs(lr, timestamps, start_times, end_times):
# Filter out controlsd messages which arrive before the camera loop
continue

if "frame_id" in jmsg['msg']['timestamp']:
timestamps[int(jmsg['msg']['timestamp']['frame_id'])][service].append((event, time))
continue

if service == "boardd":
timestamps[latest_controls_frameid][service].append((event, time))
end_times[latest_controls_frameid][service] = time
Expand All @@ -117,6 +120,8 @@ def insert_cloudlogs(lr, timestamps, start_times, end_times):
if frame_id:
if service == 'controlsd':
latest_controls_frameid = frame_id
if next(frame_id_gen, False):
event += " (warning: ambiguity)"
timestamps[frame_id][service].append((event, time))
else:
failed_inserts += 1
Expand Down

0 comments on commit 3f46dc2

Please sign in to comment.