spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

add frame id

latency_logging_2
Lukas Petersson 2022-03-21 17:39:46 -07:00
parent 932778e21b
commit 419da793af
7 changed files with 34 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
common/logging_extra.py
View File

@ -165,11 +165,12 @@ class SwagLogger(logging.Logger):
else:
self.info(evt)
def timestamp(self, event_name, *args, **kwargs):
def timestamp(self, event_name, frame_id, *args, **kwargs):
tstp = NiceOrderedDict()
tstp['timestamp'] = NiceOrderedDict()
tstp['timestamp']["event"] = event_name
tstp['timestamp']["time"] = sec_since_boot()
tstp['timestamp']["frameId"] = frame_id
tstp['timestamp']["time"] = sec_since_boot()/1e9
if args:
tstp['timestamp']['args'] = args
tstp['timestamp'].update(kwargs)

10
selfdrive/camerad/cameras/camera_common.cc
View File

@ -375,8 +375,8 @@ void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thre
uint32_t cnt = 0;
while (!do_exit) {
//cloudlog.timestamp(cs.buf.cur_frame_data.frame_id, "camerad", thread_name+": Start");
LOGT(": start");
//cloudlog.timestamp(thread_name+": Start");
uint32_t frame_id = cs->buf.cur_frame_data.frame_id;
LOGT((std::string(thread_name)+std::string(": Start")).c_str(), frame_id);
if (!cs->buf.acquire()) continue;
callback(cameras, cs, cnt);
@ -384,14 +384,14 @@ void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thre
if (cs == &(cameras->road_cam) && cameras->pm && cnt % 100 == 3) {
// this takes 10ms???
//cloudlog.timestamp(thread_name+": Send thumbnail");
LOGT(": Send thumbnail");
LOGT((std::string(thread_name)+std::string(": Send thumbnail")).c_str(), frame_id);
publish_thumbnail(cameras->pm, &(cs->buf));
//cloudlog.timestamp(thread_name+": Send thumbnail done");
LOGT(": Send thumbnail done");
LOGT((std::string(thread_name)+std::string(": Send thumbnail done")).c_str(), frame_id);
}
cs->buf.release();
//cloudlog.timestamp(thread_name+": End");
LOGT(": End");
LOGT((std::string(thread_name)+std::string(": End")).c_str(), frame_id);
++cnt;
}
return NULL;

12
selfdrive/common/swaglog.cc
View File

@ -100,15 +100,3 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func
log(levelnum, filename, lineno, func, msg_buf, log_s);
free(msg_buf);
}
//void cloudlog_t(int levelnum, const char* filename, int lineno, const char* func,
// const char* fmt, ...) {
// json11::Json timestamp = json11::Json::object {
// {"timestamp":{
// {"event", fmt},
// {"time", nanos_since_boot()}
// }
// }
// };
// cloudlog_e(levelnum, filename, lineno, func, timestamp);
//}

6
selfdrive/common/swaglog.h
View File

@ -19,10 +19,10 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func
__func__, \
fmt, ## __VA_ARGS__)
#define cloudlog_t(lvl, fmt, ...) \
#define cloudlog_t(lvl, event_name, frame_id, ...) \
{ \
uint64_t ns = nanos_since_boot(); \
std::string msg = std::string("{'timestamp': {'event':") + std::string(fmt) + std::string("},{'time':") + std::to_string(ns); \
std::string msg = std::string("{'timestamp': {'event':") + std::string(event_name) + std::string("},{'frameId':")+ std::to_string(frame_id) + std::string("},{'time':") + std::to_string(ns); \
cloudlog(lvl, msg.c_str(), ## __VA_ARGS__); \
}
@ -56,7 +56,7 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func
} \
}
#define LOGT(fmt, ...) cloudlog_t(CLOUDLOG_TIMESTAMP, fmt, ## __VA_ARGS__)
#define LOGT(event_name , frame_id, ...) cloudlog_t(CLOUDLOG_TIMESTAMP, event_name, frame_id, ## __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__)

15
selfdrive/controls/controlsd.py
View File

@ -487,7 +487,8 @@ class Controls:
CC = car.CarControl.new_message()
# lat and long should have the same id (and they do)
CC.frameId = lat_plan.frameId
frame_id = lat_plan.frameId
CC.frameId = frame_id
CC.enabled = self.enabled
# Check which actuators can be enabled
CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
@ -630,7 +631,7 @@ class Controls:
# send car controls over can
self.last_actuators, can_sends = self.CI.apply(CC)
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
cloudlog.timestamp("sendcan Published")
cloudlog.timestamp("sendcan Published", frame_id)
CC.actuatorsOutput = self.last_actuators
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
@ -687,7 +688,7 @@ class Controls:
controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat)
cloudlog.timestamp("controlState Published")
cloudlog.timestamp("controlState Published", frame_id)
# carState
car_events = self.events.to_msg()
@ -696,14 +697,14 @@ class Controls:
cs_send.carState = CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
cloudlog.timestamp("carState Published")
cloudlog.timestamp("carState Published", frame_id)
# carEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('carEvents', len(self.events))
ce_send.carEvents = car_events
self.pm.send('carEvents', ce_send)
cloudlog.timestamp("carEvents Published")
cloudlog.timestamp("carEvents Published", frame_id)
self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment)
@ -711,14 +712,14 @@ class Controls:
cp_send = messaging.new_message('carParams')
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
cloudlog.timestamp("carParams Published")
cloudlog.timestamp("carParams Published", frame_id)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
cloudlog.timestamp("carControl Published")
cloudlog.timestamp("carControl Published", frame_id)
# copy CarControl to pass to CarInterface on the next iteration
self.CC = CC

11
selfdrive/controls/plannerd.py
View File

@ -36,15 +36,16 @@ def plannerd_thread(sm=None, pm=None):
sm.update()
if sm.updated['modelV2']:
cloudlog.timestamp("Model updated")
frame_id = sm['modelV2'].frameId
cloudlog.timestamp("Model updated", frame_id)
lateral_planner.update(sm)
cloudlog.timestamp("lateral planner updated")
cloudlog.timestamp("lateral planner updated", frame_id)
lateral_planner.publish(sm, pm)
cloudlog.timestamp("lateral planner published")
cloudlog.timestamp("lateral planner published", frame_id)
longitudinal_planner.update(sm)
cloudlog.timestamp("longitudinal planner updated")
cloudlog.timestamp("longitudinal planner updated",frame_id)
longitudinal_planner.publish(sm, pm)
cloudlog.timestamp("longitudinal planner published")
cloudlog.timestamp("longitudinal planner published", frame_id)
def main(sm=None, pm=None):

18
selfdrive/modeld/modeld.cc
View File

@ -89,7 +89,11 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
LOGE("vipc_client_main no frame");
continue;
}
//cloudlog.timestamp("vicp read main")
// TODO: path planner timeout?
sm.update(0);
int desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire());
frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId();
LOGT("vicp read main", frame_id)
if (use_extra_client) {
// Keep receiving extra frames until frame id matches main camera
@ -101,7 +105,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
LOGE("vipc_client_extra no frame");
continue;
}
//cloudlog.timestamp("vicp read extra")
LOGT("vicp read extra", frame_id)
if (std::abs((int64_t)meta_main.timestamp_sof - (int64_t)meta_extra.timestamp_sof) > 10000000ULL) {
LOGE("frames out of sync! main: %d (%.5f), extra: %d (%.5f)",
@ -114,10 +118,6 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
meta_extra = meta_main;
}
// TODO: path planner timeout?
sm.update(0);
int desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire());
frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId();
if (sm.updated("liveCalibration")) {
auto extrinsic_matrix = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix();
Eigen::Matrix<float, 3, 4> extrinsic_matrix_eigen;
@ -135,10 +135,10 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
vec_desire[desire] = 1.0;
}
//cloudlog.timestamp("Model start")
LOGT("Model start", frame_id);
double mt1 = millis_since_boot();
ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire);
//cloudlog.timestamp("Model end")
LOGT("Model end", frame_id);
double mt2 = millis_since_boot();
float model_execution_time = (mt2 - mt1) / 1000.0;
@ -157,7 +157,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
kj::ArrayPtr<const float>(model.output.data(), model.output.size()), live_calib_seen);
posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);
//cloudlog.timestamp("Model published")
LOGT("Model published", frame_id);
//printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
last = mt1;