Compare commits
31 Commits
master
...
latency_lo
Author | SHA1 | Date |
---|---|---|
Lukas Petersson | 1fee24c612 | |
Lukas Petersson | eb4dd2cb6e | |
Lukas Petersson | 0d989fb2e5 | |
Comma Device | 86139f0c52 | |
Lukas Petersson | 5bf4290d70 | |
Lukas Petersson | 861a545a46 | |
Comma Device | d5b1e31335 | |
Lukas Petersson | 5e44d0460a | |
Lukas Petersson | c4fdcb6f8d | |
Lukas Petersson | 1a3cb43a67 | |
Comma Device | 7b7655079a | |
Lukas Petersson | 6922fa21a4 | |
Comma Device | 25f07d207f | |
Comma Device | c6f082ca74 | |
Lukas Petersson | e66b2bf70a | |
Lukas Petersson | 1b50e24f65 | |
Lukas Petersson | 4e898fe88f | |
Comma Device | 548e6159fb | |
Comma Device | 08c0896f6a | |
Lukas Petersson | 6f4d041e83 | |
Lukas Petersson | 790a58c4fb | |
Lukas Petersson | 852702311d | |
Lukas Petersson | 419da793af | |
Lukas Petersson | 932778e21b | |
Lukas Petersson | 37925ac9f6 | |
Lukas Petersson | 2d7726a82b | |
Lukas Petersson | 9892fa63fe | |
Lukas Petersson | 337219c9f3 | |
Lukas Petersson | aad0b12223 | |
Lukas Petersson | 75adb2d076 | |
Bruce Wayne | ccfb061870 |
|
@ -35,6 +35,7 @@ a.out
|
||||||
config.json
|
config.json
|
||||||
clcache
|
clcache
|
||||||
compile_commands.json
|
compile_commands.json
|
||||||
|
tools/latency_logging/timestamps.json
|
||||||
|
|
||||||
persist
|
persist
|
||||||
board/obj/
|
board/obj/
|
||||||
|
|
2
cereal
2
cereal
|
@ -1 +1 @@
|
||||||
Subproject commit 64b6014193a69fe2e2b3b7a17f9e228162081ddf
|
Subproject commit f546024f76a1d7d8f502f8cf67cb847e29327ba2
|
|
@ -7,9 +7,11 @@ import uuid
|
||||||
import socket
|
import socket
|
||||||
import logging
|
import logging
|
||||||
import traceback
|
import traceback
|
||||||
|
import time
|
||||||
from threading import local
|
from threading import local
|
||||||
from collections import OrderedDict
|
from collections import OrderedDict, defaultdict
|
||||||
from contextlib import contextmanager
|
from contextlib import contextmanager
|
||||||
|
from common.realtime import sec_since_boot
|
||||||
|
|
||||||
def json_handler(obj):
|
def json_handler(obj):
|
||||||
# if isinstance(obj, (datetime.date, datetime.time)):
|
# if isinstance(obj, (datetime.date, datetime.time)):
|
||||||
|
@ -163,6 +165,17 @@ class SwagLogger(logging.Logger):
|
||||||
else:
|
else:
|
||||||
self.info(evt)
|
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):
|
def findCaller(self, stack_info=False, stacklevel=1):
|
||||||
"""
|
"""
|
||||||
Find the stack frame of the caller so that we can note the source
|
Find the stack frame of the caller so that we can note the source
|
||||||
|
|
|
@ -1,6 +1,7 @@
|
||||||
#include "selfdrive/boardd/boardd.h"
|
#include "selfdrive/boardd/boardd.h"
|
||||||
|
|
||||||
#include <sched.h>
|
#include <sched.h>
|
||||||
|
#include <string>
|
||||||
#include <sys/cdefs.h>
|
#include <sys/cdefs.h>
|
||||||
#include <sys/resource.h>
|
#include <sys/resource.h>
|
||||||
#include <sys/types.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()));
|
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
|
||||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
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
|
//Dont send if older than 1 second
|
||||||
if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) {
|
if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) {
|
||||||
for (const auto& panda : pandas) {
|
for (const auto& panda : pandas) {
|
||||||
panda->can_send(event.getSendcan());
|
panda->can_send(event.getSendcan());
|
||||||
|
LOGTT((std::string("sendcan sent to panda: ")+std::string(panda->usb_serial)).c_str() , true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -127,6 +127,7 @@ def main() -> NoReturn:
|
||||||
first_run = False
|
first_run = False
|
||||||
|
|
||||||
# run boardd with all connected serials as arguments
|
# run boardd with all connected serials as arguments
|
||||||
|
os.environ['MANAGER_DAEMON'] = 'boardd'
|
||||||
os.chdir(os.path.join(BASEDIR, "selfdrive/boardd"))
|
os.chdir(os.path.join(BASEDIR, "selfdrive/boardd"))
|
||||||
subprocess.run(["./boardd", *panda_serials], check=True)
|
subprocess.run(["./boardd", *panda_serials], check=True)
|
||||||
|
|
||||||
|
|
|
@ -157,6 +157,8 @@ bool CameraBuf::acquire() {
|
||||||
}
|
}
|
||||||
|
|
||||||
cur_frame_data = camera_bufs_metadata[cur_buf_idx];
|
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);
|
cur_rgb_buf = vipc_server->get_buffer(rgb_type);
|
||||||
cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl;
|
cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl;
|
||||||
cl_event event;
|
cl_event event;
|
||||||
|
@ -194,7 +196,7 @@ bool CameraBuf::acquire() {
|
||||||
cur_yuv_buf->set_frame_id(cur_frame_data.frame_id);
|
cur_yuv_buf->set_frame_id(cur_frame_data.frame_id);
|
||||||
vipc_server->send(cur_rgb_buf, &extra);
|
vipc_server->send(cur_rgb_buf, &extra);
|
||||||
vipc_server->send(cur_yuv_buf, &extra);
|
vipc_server->send(cur_yuv_buf, &extra);
|
||||||
|
LOGT("End acquire camera buffer");
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -375,14 +377,22 @@ void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thre
|
||||||
uint32_t cnt = 0;
|
uint32_t cnt = 0;
|
||||||
while (!do_exit) {
|
while (!do_exit) {
|
||||||
if (!cs->buf.acquire()) continue;
|
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);
|
callback(cameras, cs, cnt);
|
||||||
|
|
||||||
|
LOGT((std::string(thread_name)+std::string(": END")).c_str());
|
||||||
|
|
||||||
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???
|
||||||
|
LOGT((std::string(thread_name)+std::string(": Send thumbnail")).c_str());
|
||||||
publish_thumbnail(cameras->pm, &(cs->buf));
|
publish_thumbnail(cameras->pm, &(cs->buf));
|
||||||
|
LOGT((std::string(thread_name)+std::string(": Send thumbnail done")).c_str());
|
||||||
}
|
}
|
||||||
cs->buf.release();
|
cs->buf.release();
|
||||||
|
LOGT((std::string(thread_name)+std::string(": Released")).c_str());
|
||||||
++cnt;
|
++cnt;
|
||||||
}
|
}
|
||||||
return NULL;
|
return NULL;
|
||||||
|
@ -440,7 +450,9 @@ void common_process_driver_camera(MultiCameraState *s, CameraState *c, int cnt)
|
||||||
if (env_send_driver) {
|
if (env_send_driver) {
|
||||||
framed.setImage(get_frame_image(&c->buf));
|
framed.setImage(get_frame_image(&c->buf));
|
||||||
}
|
}
|
||||||
|
LOGT("DriverCamera: Image set");
|
||||||
s->pm->send("driverCameraState", msg);
|
s->pm->send("driverCameraState", msg);
|
||||||
|
LOGT("DriverCamera: Published");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -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)) {
|
if ((c == &s->road_cam && env_send_road) || (c == &s->wide_road_cam && env_send_wide_road)) {
|
||||||
framed.setImage(get_frame_image(b));
|
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) {
|
if (c == &s->road_cam) {
|
||||||
framed.setTransform(b->yuv_transform.v);
|
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);
|
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 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;
|
const int skip = 2;
|
||||||
|
|
|
@ -8,6 +8,7 @@
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
#include <chrono>
|
||||||
|
|
||||||
#include <zmq.h>
|
#include <zmq.h>
|
||||||
#include "json11.hpp"
|
#include "json11.hpp"
|
||||||
|
@ -60,10 +61,16 @@ class SwaglogState : public LogState {
|
||||||
|
|
||||||
initialized = true;
|
initialized = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
static SwaglogState s = {};
|
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) {
|
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) {
|
if (levelnum >= s.print_level) {
|
||||||
printf("%s: %s\n", filename, msg);
|
printf("%s: %s\n", filename, msg);
|
||||||
|
|
|
@ -2,6 +2,7 @@
|
||||||
|
|
||||||
#include "selfdrive/common/timing.h"
|
#include "selfdrive/common/timing.h"
|
||||||
|
|
||||||
|
#define CLOUDLOG_TIMESTAMP 10
|
||||||
#define CLOUDLOG_DEBUG 10
|
#define CLOUDLOG_DEBUG 10
|
||||||
#define CLOUDLOG_INFO 20
|
#define CLOUDLOG_INFO 20
|
||||||
#define CLOUDLOG_WARNING 30
|
#define CLOUDLOG_WARNING 30
|
||||||
|
@ -12,11 +13,26 @@
|
||||||
|
|
||||||
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
|
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
|
||||||
const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
|
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__, \
|
#define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
|
||||||
__func__, \
|
__func__, \
|
||||||
fmt, ## __VA_ARGS__)
|
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, ...) \
|
#define cloudlog_rl(burst, millis, lvl, fmt, ...) \
|
||||||
{ \
|
{ \
|
||||||
static uint64_t __begin = 0; \
|
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 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__)
|
||||||
|
|
|
@ -486,6 +486,9 @@ class Controls:
|
||||||
long_plan = self.sm['longitudinalPlan']
|
long_plan = self.sm['longitudinalPlan']
|
||||||
|
|
||||||
CC = car.CarControl.new_message()
|
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
|
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 \
|
||||||
|
@ -624,10 +627,15 @@ class Controls:
|
||||||
if current_alert:
|
if current_alert:
|
||||||
hudControl.visualAlert = current_alert.visual_alert
|
hudControl.visualAlert = current_alert.visual_alert
|
||||||
|
|
||||||
|
frame_id = CC.frameId
|
||||||
if not self.read_only and self.initialized:
|
if not self.read_only and self.initialized:
|
||||||
# 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))
|
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
|
CC.actuatorsOutput = self.last_actuators
|
||||||
|
|
||||||
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
|
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
|
||||||
|
@ -684,6 +692,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")
|
||||||
|
|
||||||
# carState
|
# carState
|
||||||
car_events = self.events.to_msg()
|
car_events = self.events.to_msg()
|
||||||
|
@ -692,12 +701,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")
|
||||||
|
|
||||||
# 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", 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)
|
||||||
|
@ -705,12 +716,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")
|
||||||
|
|
||||||
# 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")
|
||||||
|
|
||||||
# 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
|
||||||
|
@ -721,9 +734,13 @@ class Controls:
|
||||||
|
|
||||||
# Sample data from sockets and get a carState
|
# Sample data from sockets and get a carState
|
||||||
CS = self.data_sample()
|
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.prof.checkpoint("Sample")
|
||||||
|
|
||||||
self.update_events(CS)
|
self.update_events(CS)
|
||||||
|
cloudlog.timestamp("Events updated")
|
||||||
|
|
||||||
if not self.read_only and self.initialized:
|
if not self.read_only and self.initialized:
|
||||||
# Update control state
|
# Update control state
|
||||||
|
|
|
@ -103,6 +103,7 @@ class LateralPlanner:
|
||||||
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'modelV2'])
|
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'modelV2'])
|
||||||
|
|
||||||
lateralPlan = plan_send.lateralPlan
|
lateralPlan = plan_send.lateralPlan
|
||||||
|
lateralPlan.frameId = sm['modelV2'].frameId
|
||||||
lateralPlan.laneWidth = float(self.LP.lane_width)
|
lateralPlan.laneWidth = float(self.LP.lane_width)
|
||||||
lateralPlan.dPathPoints = self.y_pts.tolist()
|
lateralPlan.dPathPoints = self.y_pts.tolist()
|
||||||
lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
|
lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
|
||||||
|
|
|
@ -116,6 +116,7 @@ class Planner:
|
||||||
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState'])
|
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState'])
|
||||||
|
|
||||||
longitudinalPlan = plan_send.longitudinalPlan
|
longitudinalPlan = plan_send.longitudinalPlan
|
||||||
|
longitudinalPlan.frameId = sm['modelV2'].frameId
|
||||||
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
|
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
|
||||||
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
|
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
|
||||||
|
|
||||||
|
|
|
@ -1,4 +1,5 @@
|
||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
|
import os
|
||||||
from cereal import car
|
from cereal import car
|
||||||
from common.params import Params
|
from common.params import Params
|
||||||
from common.realtime import Priority, config_realtime_process
|
from common.realtime import Priority, config_realtime_process
|
||||||
|
@ -36,10 +37,17 @@ def plannerd_thread(sm=None, pm=None):
|
||||||
sm.update()
|
sm.update()
|
||||||
|
|
||||||
if sm.updated['modelV2']:
|
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)
|
lateral_planner.update(sm)
|
||||||
|
cloudlog.timestamp("lateral planner updated")
|
||||||
lateral_planner.publish(sm, pm)
|
lateral_planner.publish(sm, pm)
|
||||||
|
cloudlog.timestamp("lateral planner published")
|
||||||
longitudinal_planner.update(sm)
|
longitudinal_planner.update(sm)
|
||||||
|
cloudlog.timestamp("longitudinal planner updated")
|
||||||
longitudinal_planner.publish(sm, pm)
|
longitudinal_planner.publish(sm, pm)
|
||||||
|
cloudlog.timestamp("longitudinal planner published")
|
||||||
|
|
||||||
|
|
||||||
def main(sm=None, pm=None):
|
def main(sm=None, pm=None):
|
||||||
|
|
|
@ -2,8 +2,10 @@
|
||||||
#include <cstdlib>
|
#include <cstdlib>
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
#include <stdlib.h>
|
||||||
|
|
||||||
#include <eigen3/Eigen/Dense>
|
#include <eigen3/Eigen/Dense>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
#include "cereal/messaging/messaging.h"
|
#include "cereal/messaging/messaging.h"
|
||||||
#include "cereal/visionipc/visionipc_client.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");
|
LOGE("vipc_client_main no frame");
|
||||||
continue;
|
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) {
|
if (use_extra_client) {
|
||||||
// Keep receiving extra frames until frame id matches main camera
|
// Keep receiving extra frames until frame id matches main camera
|
||||||
do {
|
do {
|
||||||
|
@ -100,6 +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;
|
||||||
}
|
}
|
||||||
|
LOGT("vicp read extra");
|
||||||
|
|
||||||
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)",
|
||||||
|
@ -133,8 +139,10 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
|
||||||
vec_desire[desire] = 1.0;
|
vec_desire[desire] = 1.0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
LOGT("Model start");
|
||||||
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);
|
||||||
|
LOGT("Model end");
|
||||||
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;
|
||||||
|
|
||||||
|
@ -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);
|
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);
|
||||||
|
|
||||||
|
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);
|
//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;
|
||||||
last_vipc_frame_id = meta_main.frame_id;
|
last_vipc_frame_id = meta_main.frame_id;
|
||||||
|
|
|
@ -11,6 +11,7 @@
|
||||||
#include "selfdrive/common/clutil.h"
|
#include "selfdrive/common/clutil.h"
|
||||||
#include "selfdrive/common/params.h"
|
#include "selfdrive/common/params.h"
|
||||||
#include "selfdrive/common/timing.h"
|
#include "selfdrive/common/timing.h"
|
||||||
|
#include "selfdrive/common/swaglog.h"
|
||||||
|
|
||||||
constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
|
constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
|
||||||
constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
|
constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
|
||||||
|
|
|
@ -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)
|
||||||
|
'''
|
|
@ -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)
|
|
@ -0,0 +1,8 @@
|
||||||
|
|
||||||
|
import cereal.messaging as messaging
|
||||||
|
|
||||||
|
# in subscriber
|
||||||
|
sm = messaging.SubMaster(['lateralPlan'])
|
||||||
|
while 1:
|
||||||
|
sm.update()
|
||||||
|
print(sm['lateralPlan'])
|
File diff suppressed because one or more lines are too long
Loading…
Reference in New Issue