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: else:
self.info(evt) self.info(evt)
def timestamp(self, event_name, *args, **kwargs): def timestamp(self, event_name, frame_id, *args, **kwargs):
tstp = NiceOrderedDict() tstp = NiceOrderedDict()
tstp['timestamp'] = NiceOrderedDict() tstp['timestamp'] = NiceOrderedDict()
tstp['timestamp']["event"] = event_name 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: if args:
tstp['timestamp']['args'] = args tstp['timestamp']['args'] = args
tstp['timestamp'].update(kwargs) 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; uint32_t cnt = 0;
while (!do_exit) { while (!do_exit) {
//cloudlog.timestamp(cs.buf.cur_frame_data.frame_id, "camerad", thread_name+": Start"); //cloudlog.timestamp(cs.buf.cur_frame_data.frame_id, "camerad", thread_name+": Start");
LOGT(": start"); uint32_t frame_id = cs->buf.cur_frame_data.frame_id;
//cloudlog.timestamp(thread_name+": Start"); LOGT((std::string(thread_name)+std::string(": Start")).c_str(), frame_id);
if (!cs->buf.acquire()) continue; if (!cs->buf.acquire()) continue;
callback(cameras, cs, cnt); 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) { if (cs == &(cameras->road_cam) && cameras->pm && cnt % 100 == 3) {
// this takes 10ms??? // this takes 10ms???
//cloudlog.timestamp(thread_name+": Send thumbnail"); //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)); publish_thumbnail(cameras->pm, &(cs->buf));
//cloudlog.timestamp(thread_name+": Send thumbnail done"); //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(); cs->buf.release();
//cloudlog.timestamp(thread_name+": End"); //cloudlog.timestamp(thread_name+": End");
LOGT(": End"); LOGT((std::string(thread_name)+std::string(": End")).c_str(), frame_id);
++cnt; ++cnt;
} }
return NULL; 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); log(levelnum, filename, lineno, func, msg_buf, log_s);
free(msg_buf); 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__, \ __func__, \
fmt, ## __VA_ARGS__) fmt, ## __VA_ARGS__)
#define cloudlog_t(lvl, fmt, ...) \ #define cloudlog_t(lvl, event_name, frame_id, ...) \
{ \ { \
uint64_t ns = nanos_since_boot(); \ 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__); \ 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 LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__) #define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, 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() CC = car.CarControl.new_message()
# lat and long should have the same id (and they do) # 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 CC.enabled = self.enabled
# Check which actuators can be enabled # Check which actuators can be enabled
CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
@ -630,7 +631,7 @@ class Controls:
# send car controls over can # send car controls over can
self.last_actuators, can_sends = self.CI.apply(CC) 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)) 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 CC.actuatorsOutput = self.last_actuators
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
@ -687,7 +688,7 @@ class Controls:
controlsState.lateralControlState.indiState = lac_log controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat) self.pm.send('controlsState', dat)
cloudlog.timestamp("controlState Published") cloudlog.timestamp("controlState Published", frame_id)
# carState # carState
car_events = self.events.to_msg() car_events = self.events.to_msg()
@ -696,14 +697,14 @@ class Controls:
cs_send.carState = CS cs_send.carState = CS
cs_send.carState.events = car_events cs_send.carState.events = car_events
self.pm.send('carState', cs_send) self.pm.send('carState', cs_send)
cloudlog.timestamp("carState Published") cloudlog.timestamp("carState Published", frame_id)
# carEvents - logged every second or on change # carEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): 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 = messaging.new_message('carEvents', len(self.events))
ce_send.carEvents = car_events ce_send.carEvents = car_events
self.pm.send('carEvents', ce_send) self.pm.send('carEvents', ce_send)
cloudlog.timestamp("carEvents Published") cloudlog.timestamp("carEvents Published", frame_id)
self.events_prev = self.events.names.copy() self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment) # carParams - logged every 50 seconds (> 1 per segment)
@ -711,14 +712,14 @@ class Controls:
cp_send = messaging.new_message('carParams') cp_send = messaging.new_message('carParams')
cp_send.carParams = self.CP cp_send.carParams = self.CP
self.pm.send('carParams', cp_send) self.pm.send('carParams', cp_send)
cloudlog.timestamp("carParams Published") cloudlog.timestamp("carParams Published", frame_id)
# carControl # carControl
cc_send = messaging.new_message('carControl') cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid cc_send.valid = CS.canValid
cc_send.carControl = CC cc_send.carControl = CC
self.pm.send('carControl', cc_send) 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 # copy CarControl to pass to CarInterface on the next iteration
self.CC = CC self.CC = CC

11
selfdrive/controls/plannerd.py
View File

@ -36,15 +36,16 @@ def plannerd_thread(sm=None, pm=None):
sm.update() sm.update()
if sm.updated['modelV2']: if sm.updated['modelV2']:
cloudlog.timestamp("Model updated") frame_id = sm['modelV2'].frameId
cloudlog.timestamp("Model updated", frame_id)
lateral_planner.update(sm) lateral_planner.update(sm)
cloudlog.timestamp("lateral planner updated") cloudlog.timestamp("lateral planner updated", frame_id)
lateral_planner.publish(sm, pm) lateral_planner.publish(sm, pm)
cloudlog.timestamp("lateral planner published") cloudlog.timestamp("lateral planner published", frame_id)
longitudinal_planner.update(sm) longitudinal_planner.update(sm)
cloudlog.timestamp("longitudinal planner updated") cloudlog.timestamp("longitudinal planner updated",frame_id)
longitudinal_planner.publish(sm, pm) longitudinal_planner.publish(sm, pm)
cloudlog.timestamp("longitudinal planner published") cloudlog.timestamp("longitudinal planner published", frame_id)
def main(sm=None, pm=None): 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"); LOGE("vipc_client_main no frame");
continue; 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) { if (use_extra_client) {
// Keep receiving extra frames until frame id matches main camera // 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"); LOGE("vipc_client_extra no frame");
continue; 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) { 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)", 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; 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")) { if (sm.updated("liveCalibration")) {
auto extrinsic_matrix = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix(); auto extrinsic_matrix = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix();
Eigen::Matrix<float, 3, 4> extrinsic_matrix_eigen; 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; vec_desire[desire] = 1.0;
} }
//cloudlog.timestamp("Model start") LOGT("Model start", frame_id);
double mt1 = millis_since_boot(); double mt1 = millis_since_boot();
ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire); 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(); double mt2 = millis_since_boot();
float model_execution_time = (mt2 - mt1) / 1000.0; 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); 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); 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); //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; last = mt1;