json bug fix

.gitignore

@@ -35,6 +35,7 @@ a.out
 config.json
 clcache
 compile_commands.json
+tools/latency_logging/timestamps.json
 
 persist
 board/obj/

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,17 @@ class SwagLogger(logging.Logger):
     else:
       self.info(evt)
 
+  def timestamp(self, event_name, *args, **kwargs):
+    tstp = NiceOrderedDict()
+    tstp['timestamp'] = NiceOrderedDict()
+    tstp['timestamp']["event"] = event_name
+    tstp['timestamp']["time"] = sec_since_boot()*1e9
+    tstp['timestamp']["translate"] = False
+    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

@@ -1,6 +1,7 @@
 #include "selfdrive/boardd/boardd.h"
 
 #include <sched.h>
+#include <string>
 #include <sys/cdefs.h>
 #include <sys/resource.h>
 #include <sys/types.h>
@@ -226,10 +227,17 @@ 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>();
 
+//setenv("FRAME_ID", std::to_string(event.getLogMonoTime()).c_str(),1);
+    set_frame_id(event.getLogMonoTime());
+    LOGW(event.getLogMonoTime());
+
+    LOGTT("sendcan received", 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() , 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,8 @@ bool CameraBuf::acquire() {
   }
 
   cur_frame_data = camera_bufs_metadata[cur_buf_idx];
+  set_frame_id(cur_frame_data.frame_id);
+  LOGT("Start acquire camera buffer");
   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 +196,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");
   return true;
 }
 
@@ -375,14 +377,22 @@ void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thre
   uint32_t cnt = 0;
   while (!do_exit) {
     if (!cs->buf.acquire()) continue;
+    //setenv("FRAME_ID", std::to_string(cs->buf.cur_frame_data.frame_id).c_str(),1);
+    set_frame_id(cs->buf.cur_frame_data.frame_id);
+    LOGT((std::string(thread_name)+std::string(": Start")).c_str());
 
     callback(cameras, cs, cnt);
 
+    LOGT((std::string(thread_name)+std::string(": END")).c_str());
+
     if (cs == &(cameras->road_cam) && cameras->pm && cnt % 100 == 3) {
       // this takes 10ms???
+      LOGT((std::string(thread_name)+std::string(": Send thumbnail")).c_str());
       publish_thumbnail(cameras->pm, &(cs->buf));
+      LOGT((std::string(thread_name)+std::string(": Send thumbnail done")).c_str());
     }
     cs->buf.release();
+    LOGT((std::string(thread_name)+std::string(": Released")).c_str());
     ++cnt;
   }
   return NULL;
@@ -440,7 +450,9 @@ void common_process_driver_camera(MultiCameraState *s, CameraState *c, int cnt)
   if (env_send_driver) {
     framed.setImage(get_frame_image(&c->buf));
   }
+  LOGT("DriverCamera: Image set");
   s->pm->send("driverCameraState", msg);
+  LOGT("DriverCamera: Published");
 }
 
 

selfdrive/camerad/cameras/camera_qcom2.cc

@@ -1032,10 +1032,13 @@ 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((std::string(c == &s->road_cam ? "RoadCamera" : "WideRoadCamera")+std::string(": Image set")).c_str());
   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());
   }
   s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg);
+  LOGT((std::string(c == &s->road_cam ? "RoadCamera" : "WideRoadCamera")+std::string(": Published")).c_str());
 
   const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986);
   const int skip = 2;

selfdrive/common/swaglog.cc

@@ -8,6 +8,7 @@
 #include <cstring>
 #include <mutex>
 #include <string>
+#include <chrono>
 
 #include <zmq.h>
 #include "json11.hpp"
@@ -60,10 +61,16 @@ class SwaglogState : public LogState {
 
     initialized = true;
   }
+
 };
 
 static SwaglogState s = {};
 
+
+void set_frame_id(uint64_t frame_id){
+  s.ctx_j["frame_id"] = std::to_string(frame_id).c_str();
+}
+
 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) {
     printf("%s: %s\n", filename, msg);

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
@@ -12,11 +13,26 @@
 
 void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
                 const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
+void set_frame_id(uint64_t frame_id);
 
 #define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
                                            __func__, \
                                            fmt, ## __VA_ARGS__)
 
+#define cloudlog_t_translate(lvl, event_name, translate,...)                                 \
+{                                                                 \
+  uint64_t ns = nanos_since_boot();                               \
+  std::string json_msg = std::string("{'timestamp': {'event': '") + std::string(event_name) + 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,...)                                 \
+{                                                                 \
+  uint64_t ns = nanos_since_boot();                               \
+  std::string json_msg = std::string("{'timestamp': {'event': '") + std::string(event_name)+ 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 , translate, ...) cloudlog_t_translate(CLOUDLOG_TIMESTAMP, event_name, translate,## __VA_ARGS__)
+#define LOGT(event_name , ...) cloudlog_t(CLOUDLOG_TIMESTAMP, event_name, ## __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,15 @@ 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)
+      cloudlog.timestamp("sendcan Published")
       CC.actuatorsOutput = self.last_actuators
 
     force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
@@ -684,6 +692,7 @@ class Controls:
       controlsState.lateralControlState.indiState = lac_log
 
     self.pm.send('controlsState', dat)
+    cloudlog.timestamp("controlState Published")
 
     # carState
     car_events = self.events.to_msg()
@@ -692,12 +701,14 @@ class Controls:
     cs_send.carState = CS
     cs_send.carState.events = car_events
     self.pm.send('carState', cs_send)
+    cloudlog.timestamp("carState Published")
 
     # 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", frame_id)
     self.events_prev = self.events.names.copy()
 
     # carParams - logged every 50 seconds (> 1 per segment)
@@ -705,12 +716,14 @@ class Controls:
       cp_send = messaging.new_message('carParams')
       cp_send.carParams = self.CP
       self.pm.send('carParams', cp_send)
+      cloudlog.timestamp("carParams Published")
 
     # 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")
 
     # copy CarControl to pass to CarInterface on the next iteration
     self.CC = CC
@@ -721,9 +734,13 @@ class Controls:
 
     # Sample data from sockets and get a carState
     CS = self.data_sample()
+    #os.environ["FRAME_ID"] = str(self.sm['lateralPlan'].frameId)
+    cloudlog.bind(frame_id=self.sm['lateralPlan'].frameId)
+    cloudlog.timestamp("Data sampled")
     self.prof.checkpoint("Sample")
 
     self.update_events(CS)
+    cloudlog.timestamp("Events updated")
 
     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

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import os
 from cereal import car
 from common.params import Params
 from common.realtime import Priority, config_realtime_process
@@ -36,10 +37,17 @@ def plannerd_thread(sm=None, pm=None):
     sm.update()
 
     if sm.updated['modelV2']:
+      #os.environ["FRAME_ID"] = str(sm['modelV2'].frameId)
+      cloudlog.bind(frame_id=sm['modelV2'].frameId)
+      cloudlog.timestamp("ModelV2 recived")
       lateral_planner.update(sm)
+      cloudlog.timestamp("lateral planner updated")
       lateral_planner.publish(sm, pm)
+      cloudlog.timestamp("lateral planner published")
       longitudinal_planner.update(sm)
+      cloudlog.timestamp("longitudinal planner updated")
       longitudinal_planner.publish(sm, pm)
+      cloudlog.timestamp("longitudinal planner published")
 
 
 def main(sm=None, pm=None):

selfdrive/modeld/modeld.cc

@@ -2,8 +2,10 @@
 #include <cstdlib>
 #include <mutex>
 #include <cmath>
+#include <stdlib.h>
 
 #include <eigen3/Eigen/Dense>
+#include <string>
 
 #include "cereal/messaging/messaging.h"
 #include "cereal/visionipc/visionipc_client.h"
@@ -89,7 +91,10 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       LOGE("vipc_client_main no frame");
       continue;
     }
-
+    //TODO: SET CONTEXT
+    //setenv("FRAME_ID", std::to_string(meta_main.frame_id).c_str(), 1);
+    set_frame_id(meta_main.frame_id);
+    LOGT("vicp read main");
     if (use_extra_client) {
       // Keep receiving extra frames until frame id matches main camera
       do {
@@ -100,6 +105,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
         LOGE("vipc_client_extra no frame");
         continue;
       }
+      LOGT("vicp read extra");
 
       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 +139,10 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       vec_desire[desire] = 1.0;
     }
 
+    LOGT("Model start");
     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");
     double mt2 = millis_since_boot();
     float model_execution_time = (mt2 - mt1) / 1000.0;
 
@@ -153,6 +161,8 @@ 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);
 
+    LOGT("Model published");
+
     //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_vipc_frame_id = meta_main.frame_id;

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,53 @@
+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,51 @@
+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")
+r = Route("9f583b1d93915c31|2022-03-24--16-15-18")
+lr = LogReader(r.log_paths()[0])
+'''
+for msg in lr:
+  if msg.which() == "logMessage":
+      print(msg)
+
+exit()
+'''
+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['ctx']['frame_id']
+    if jmsg['msg']['timestamp']['translate']:
+        try:
+            frame_id = translationdict[int(frame_id)]
+        except:
+            i+=1
+            continue
+    timestamps[int(frame_id)][service][event].append(time)
+
+print("Num failed translations:", i)
+
+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(['lateralPlan'])
+while 1:
+  sm.update()
+  print(sm['lateralPlan'])