bug fixes

.gitignore

@@ -54,6 +54,7 @@ selfdrive/camerad/test/ae_gray_test
 selfdrive/modeld/_modeld
 selfdrive/modeld/_dmonitoringmodeld
 /src/
+tools/latency_logging/timestamps.json
 
 one
 openpilot

cereal

@@ -1 +1 @@
-Subproject commit 64b6014193a69fe2e2b3b7a17f9e228162081ddf
+Subproject commit f546024f76a1d7d8f502f8cf67cb847e29327ba2

common/logging_extra.py

@@ -7,9 +7,11 @@ import uuid
 import socket
 import logging
 import traceback
+import time
 from threading import local
-from collections import OrderedDict
+from collections import OrderedDict, defaultdict
 from contextlib import contextmanager
+from common.realtime import sec_since_boot
 
 def json_handler(obj):
   # if isinstance(obj, (datetime.date, datetime.time)):
@@ -163,6 +165,18 @@ class SwagLogger(logging.Logger):
     else:
       self.info(evt)
 
+  def timestamp(self, event_name, frame_id, translate=False, time=0,*args, **kwargs):
+    tstp = NiceOrderedDict()
+    tstp['timestamp'] = NiceOrderedDict()
+    tstp['timestamp']["event"] = event_name
+    tstp['timestamp']["frameId"] = frame_id
+    tstp['timestamp']["time"] = time if time else sec_since_boot()*1e9
+    tstp['timestamp']["translate"] = translate
+    if args:
+      tstp['timestamp']['args'] = args
+    tstp['timestamp'].update(kwargs)
+    self.debug(tstp)
+
   def findCaller(self, stack_info=False, stacklevel=1):
     """
     Find the stack frame of the caller so that we can note the source

selfdrive/boardd/boardd.cc

@@ -226,10 +226,13 @@ void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
     capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
     cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
 
+    LOGTT("sendcan received", event.getLogMonoTime(), true);
+
     //Dont send if older than 1 second
     if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) {
       for (const auto& panda : pandas) {
         panda->can_send(event.getSendcan());
+	LOGTT((std::string("sendcan sent to panda: ")+std::string(panda->usb_serial)).c_str(), event.getLogMonoTime() , true);
       }
     }
   }

selfdrive/boardd/pandad.py

@@ -127,6 +127,7 @@ def main() -> NoReturn:
     first_run = False
 
     # run boardd with all connected serials as arguments
+    os.environ['MANAGER_DAEMON'] = 'boardd'
     os.chdir(os.path.join(BASEDIR, "selfdrive/boardd"))
     subprocess.run(["./boardd", *panda_serials], check=True)
 

selfdrive/camerad/cameras/camera_common.cc

@@ -157,6 +157,7 @@ bool CameraBuf::acquire() {
   }
 
   cur_frame_data = camera_bufs_metadata[cur_buf_idx];
+  LOGT("Start acquire camera buffer", cur_frame_data.frame_id);
   cur_rgb_buf = vipc_server->get_buffer(rgb_type);
   cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl;
   cl_event event;
@@ -194,7 +195,7 @@ bool CameraBuf::acquire() {
   cur_yuv_buf->set_frame_id(cur_frame_data.frame_id);
   vipc_server->send(cur_rgb_buf, &extra);
   vipc_server->send(cur_yuv_buf, &extra);
-
+  LOGT("End acquire camera buffer", cur_frame_data.frame_id);
   return true;
 }
 
@@ -376,13 +377,22 @@ void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thre
   while (!do_exit) {
     if (!cs->buf.acquire()) continue;
 
+    //TODO: is this frame id fake, maybe it is created in the callback
+    uint32_t frame_id = cs->buf.cur_frame_data.frame_id;
+    LOGT((std::string(thread_name)+std::string(": Start")).c_str(), frame_id);
+
     callback(cameras, cs, cnt);
 
+    LOGT((std::string(thread_name)+std::string(": END")).c_str(), frame_id);
+
     if (cs == &(cameras->road_cam) && cameras->pm && cnt % 100 == 3) {
       // this takes 10ms???
+      LOGT((std::string(thread_name)+std::string(": Send thumbnail")).c_str(), frame_id);
       publish_thumbnail(cameras->pm, &(cs->buf));
+      LOGT((std::string(thread_name)+std::string(": Send thumbnail done")).c_str(), frame_id);
     }
     cs->buf.release();
+    LOGT((std::string(thread_name)+std::string(": Released")).c_str(), frame_id);
     ++cnt;
   }
   return NULL;
@@ -440,6 +450,8 @@ void common_process_driver_camera(MultiCameraState *s, CameraState *c, int cnt)
   if (env_send_driver) {
     framed.setImage(get_frame_image(&c->buf));
   }
+  uint32_t frame_id = c->buf.cur_frame_data.frame_id;
+  LOGT("DriverCamera: Image set", frame_id);
   s->pm->send("driverCameraState", msg);
 }
 

selfdrive/camerad/cameras/camera_qcom2.cc

@@ -1032,8 +1032,11 @@ 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));
   }
+  uint32_t frame_id = c->buf.cur_frame_data.frame_id;
+  LOGT((std::string(c == &s->road_cam ? "RoadCamera" : "WideRoadCamera")+std::string(": Image set")).c_str(), frame_id);
   if (c == &s->road_cam) {
     framed.setTransform(b->yuv_transform.v);
+    LOGT((std::string(c == &s->road_cam ? "RoadCamera" : "WideRoadCamera")+std::string(": Transformed")).c_str(), frame_id);
   }
   s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg);
 

selfdrive/common/swaglog.cc

@@ -8,6 +8,7 @@
 #include <cstring>
 #include <mutex>
 #include <string>
+#include <chrono>
 
 #include <zmq.h>
 #include "json11.hpp"

selfdrive/common/swaglog.h

@@ -2,6 +2,7 @@
 
 #include "selfdrive/common/timing.h"
 
+#define CLOUDLOG_TIMESTAMP 10
 #define CLOUDLOG_DEBUG 10
 #define CLOUDLOG_INFO 20
 #define CLOUDLOG_WARNING 30
@@ -13,10 +14,25 @@
 void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
                 const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
 
+
 #define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
                                            __func__, \
                                            fmt, ## __VA_ARGS__)
 
+#define cloudlog_t_translate(lvl, event_name, frame_id, translate,...)                                 \
+{                                                                 \
+  uint64_t ns = nanos_since_boot();                               \
+  std::string json_msg = std::string("{'timestamp': {'event': '") + std::string(event_name) + std::string("' ,'frameId':")+ std::to_string(frame_id) + std::string(",'time':") + std::to_string(ns)+ std::string(",'translate':") + std::to_string(translate)+std::string("}}");   \
+  cloudlog(lvl, json_msg.c_str(), ## __VA_ARGS__);                             \
+}
+
+#define cloudlog_t(lvl, event_name, frame_id,...)                                 \
+{                                                                 \
+  uint64_t ns = nanos_since_boot();                               \
+  std::string json_msg = std::string("{'timestamp': {'event': '") + std::string(event_name) + std::string("' ,'frameId':")+ std::to_string(frame_id) + std::string(",'time':") + std::to_string(ns)+ std::string(",'translate':") + std::string("false")+std::string("}}");   \
+  cloudlog(lvl, json_msg.c_str(), ## __VA_ARGS__);                             \
+}
+
 #define cloudlog_rl(burst, millis, lvl, fmt, ...)   \
 {                                                   \
   static uint64_t __begin = 0;                      \
@@ -46,6 +62,8 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func
   }                                                 \
 }
 
+#define LOGTT(event_name , frame_id, translate, ...) cloudlog_t_translate(CLOUDLOG_TIMESTAMP, event_name, frame_id, translate,## __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__)

selfdrive/controls/controlsd.py

@@ -486,6 +486,9 @@ class Controls:
     long_plan = self.sm['longitudinalPlan']
 
     CC = car.CarControl.new_message()
+    # lat and long should have the same id (and they do)
+    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 \
@@ -624,10 +627,14 @@ class Controls:
     if current_alert:
       hudControl.visualAlert = current_alert.visual_alert
 
+    frame_id = CC.frameId
     if not self.read_only and self.initialized:
       # 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))
+      sendcan_bytes = can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)
+      self.pm.send('sendcan', sendcan_bytes)
+      sendcanMonoTime = log.Event.from_bytes(sendcan_bytes).logMonoTime
+      cloudlog.event("translation", logMonoTime=sendcanMonoTime, frameId=frame_id, debug=True)
       CC.actuatorsOutput = self.last_actuators
 
     force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
@@ -721,9 +728,11 @@ class Controls:
 
     # Sample data from sockets and get a carState
     CS = self.data_sample()
+    cloudlog.timestamp("Data sampled", self.sm['lateralPlan'].frameId)
     self.prof.checkpoint("Sample")
 
     self.update_events(CS)
+    cloudlog.timestamp("Events updated", self.sm['lateralPlan'].frameId)
 
     if not self.read_only and self.initialized:
       # Update control state

selfdrive/controls/lib/lateral_planner.py

@@ -103,6 +103,7 @@ class LateralPlanner:
     plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'modelV2'])
 
     lateralPlan = plan_send.lateralPlan
+    lateralPlan.frameId = sm['modelV2'].frameId
     lateralPlan.laneWidth = float(self.LP.lane_width)
     lateralPlan.dPathPoints = self.y_pts.tolist()
     lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()

selfdrive/controls/lib/longitudinal_planner.py

@@ -116,6 +116,7 @@ class Planner:
     plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState'])
 
     longitudinalPlan = plan_send.longitudinalPlan
+    longitudinalPlan.frameId = sm['modelV2'].frameId
     longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
     longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
 

selfdrive/controls/plannerd.py

@@ -37,8 +37,10 @@ def plannerd_thread(sm=None, pm=None):
 
     if sm.updated['modelV2']:
       lateral_planner.update(sm)
+      cloudlog.timestamp("lateral planner updated", sm['modelV2'].frameId)
       lateral_planner.publish(sm, pm)
       longitudinal_planner.update(sm)
+      cloudlog.timestamp("longitudinal planner updated",sm['modelV2'].frameId)
       longitudinal_planner.publish(sm, pm)
 
 

selfdrive/manager/manager.py

@@ -130,11 +130,14 @@ def manager_thread() -> None:
   ensure_running(managed_processes.values(), started=False, not_run=ignore)
 
   started_prev = False
-  sm = messaging.SubMaster(['deviceState'])
+  logEvents = ['roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'modelV2',  'lateralPlan', 'longitudinalPlan', 'carControl', 'sendcan']
+  sm = messaging.SubMaster(['deviceState']+logEvents)
   pm = messaging.PubMaster(['managerState'])
 
   while True:
     sm.update()
+    logTimestamps(sm, logEvents)
+
     not_run = ignore[:]
 
     started = sm['deviceState'].started
@@ -169,6 +172,17 @@ def manager_thread() -> None:
     if shutdown:
       break
 
+def logTimestamps(sm, events):
+  for event in events:
+    if sm.updated[event]:
+      pubTime = sm.logMonoTime[event]
+      recTime = sm.rcv_time[event] * 1e9
+      if event != "sendcan":
+        cloudlog.timestamp(event+": Published", sm[event].frameId, False, time=pubTime)
+        cloudlog.timestamp(event+": Received", sm[event].frameId, False, time=recTime)
+      else:
+        cloudlog.timestamp(event+": Published", sm.logMonoTime[event], True, time=pubTime)
+        cloudlog.timestamp(event+": Published", sm.logMonoTime[event], True, time=recTime)
 
 def main() -> None:
   prepare_only = os.getenv("PREPAREONLY") is not None

selfdrive/modeld/modeld.cc

@@ -89,7 +89,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       LOGE("vipc_client_main no frame");
       continue;
     }
-
+    LOGT("vicp read main", meta_main.frame_id)
     if (use_extra_client) {
       // Keep receiving extra frames until frame id matches main camera
       do {
@@ -100,6 +100,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
         LOGE("vipc_client_extra no frame");
         continue;
       }
+      LOGT("vicp read extra", meta_main.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)",
@@ -133,8 +134,10 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       vec_desire[desire] = 1.0;
     }
 
+    LOGT("Model start", meta_main.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);
+    LOGT("Model end", meta_main.frame_id);
     double mt2 = millis_since_boot();
     float model_execution_time = (mt2 - mt1) / 1000.0;
 

selfdrive/modeld/models/driving.cc

@@ -11,6 +11,7 @@
 #include "selfdrive/common/clutil.h"
 #include "selfdrive/common/params.h"
 #include "selfdrive/common/timing.h"
+#include "selfdrive/common/swaglog.h"
 
 constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
 constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;

tools/latency_logging/print_json.py

@@ -0,0 +1,54 @@
+import json
+import matplotlib.pyplot as plt
+import matplotlib.patches as mpatches
+from collections import defaultdict
+from tabulate import tabulate
+
+
+json_file = open('timestamps.json')
+timestamps = json.load(json_file)
+
+
+for frame_id, services in timestamps.items():
+    t0 = min([min([min(times) for times in events.values()]) for events in services.values()])
+    print(frame_id)
+    d = defaultdict( lambda: ("","",[]))
+    for service, events in services.items():
+        for event, times in events.items():
+            key = (min(times)-t0)/1e6
+            times = [(int(time)-t0)/1e6 for time in times]
+            d[key] = (service, event, times)
+    s = sorted(d.items())
+    print(tabulate([[item[1][0], item[1][1], item[1][2]] for item in s], headers=["service", "event", "time (ms)"]))
+    print()
+
+'''
+fig, gnt = plt.subplots()
+maxx = max([max([max(events.values()) for events in services.values()]) for services in timestamps.values()])/1e6
+gnt.set_xlim(0, 150)
+maxy = len(timestamps)
+gnt.set_ylim(0, maxy)
+
+avg_times = defaultdict(list)
+
+count = 0
+for frame_id, services in timestamps.items():
+    t0 = min([min(events.values())for events in services.values()])
+    service_bars = []
+    event_bars = []
+    print(frame_id)
+    for service, events in services.items():
+        start = min(events.values())
+        end = max(events.values())
+        #service_bars.append(((start-t0)/1e6, (end-start)/1e6))
+        for event, time in events.items():
+            t = (time-t0)/1e6
+            event_bars.append((t, 0.1))
+            avg_times[service+"."+event].append(t)
+            print("    ", service+"."+event, t)
+    #gnt.broken_barh(service_bars, (count, 0.9), facecolors=("blue"))
+    gnt.broken_barh(event_bars, (count, 0.9), facecolors=("black"))
+    count+=1
+print(tabulate([[event, sum(times)/len(times), max(times), len(times)] for event, times in avg_times.items()], headers=["event", "avg", "max", "len"]))
+plt.show(block=True)
+'''

tools/latency_logging/read_cloudlog.py

@@ -0,0 +1,49 @@
+from selfdrive.swaglog import cloudlog
+from tools.lib.route import Route
+from tools.lib.logreader import LogReader
+
+import json
+from collections import defaultdict
+
+
+timestamps = defaultdict(lambda: defaultdict(lambda: defaultdict(list)))
+translationdict = {}
+
+
+
+#r = Route("9f583b1d93915c31|2022-03-23--14-47-10") clean commit
+#r = Route("9f583b1d93915c31|2022-03-24--19-31-10") current (no pub tlogs) update()
+#r = Route("9f583b1d93915c31|2022-03-24--20-10-09") # current update(0)
+r = Route("9f583b1d93915c31|2022-03-25--13-09-51") # rec and pub
+lr = LogReader(r.log_paths()[0])
+
+for msg in lr:
+  if msg.which() == "logMessage" and "translation" in msg.logMessage:
+    msg = msg.logMessage.replace("'", '"').replace('"{', "{").replace('}"', "}")
+    jmsg = json.loads(msg)
+    logMonoTime = jmsg['msg']['logMonoTime'] 
+    frame_id = jmsg['msg']['frameId']
+    translationdict[logMonoTime] = frame_id
+
+i = 0
+for msg in lr:
+  if msg.which() == "logMessage" and "timestamp" in msg.logMessage:
+    msg = msg.logMessage.replace("'", '"').replace('"{', "{").replace('}"', "}")
+    jmsg = json.loads(msg)
+    service = jmsg['ctx']['daemon']
+    event = jmsg['msg']['timestamp']['event']
+    time = jmsg['msg']['timestamp']['time']
+    frame_id = jmsg['msg']['timestamp']['frameId']
+    if jmsg['msg']['timestamp']['translate']:
+        try:
+            frame_id = translationdict[frame_id]
+        except:
+            i+=1
+    timestamps[frame_id][service][event].append(time)
+
+print("Num failed translations:",i)
+
+del timestamps[0]
+
+with open('timestamps.json', 'w') as outfile:
+    json.dump(timestamps, outfile)

tools/latency_logging/test.py

@@ -0,0 +1,8 @@
+import cereal.messaging as messaging
+
+# in subscriber
+sm = messaging.SubMaster(['sendcan'])
+while 1:
+  sm.update()
+  if sm.updated['sendcan']:
+      print(sm.rcv_time['sendcan'], sm.logMonoTime['sendcan'])