Replay: seeking functionality (#20763)
iejMac
4 years ago
committed by
GitHub
parent
533bc30c2e
commit
f0d0b82b8d
8 changed files with 298 additions and 297 deletions
@ -1 +1 @@ |
||||
Subproject commit a6f4b6351d70e74220ffa7d9af917c3dea08a2ce |
||||
Subproject commit 3c895e7b33a06a4c087c7728a3e44986b360f3ab |
||||
@ -1,202 +0,0 @@ |
||||
#include "selfdrive/ui/replay/unlogger.h" |
||||
|
||||
#include <cmath> |
||||
#include <string> |
||||
#include <vector> |
||||
|
||||
#include <stdint.h> |
||||
#include <time.h> |
||||
|
||||
// include the dynamic struct
|
||||
#include <capnp/dynamic.h> |
||||
#include <capnp/schema.h> |
||||
|
||||
#include "cereal/gen/cpp/car.capnp.c++" |
||||
#include "cereal/gen/cpp/legacy.capnp.c++" |
||||
#include "cereal/gen/cpp/log.capnp.c++" |
||||
#include "cereal/services.h" |
||||
#include "selfdrive/common/timing.h" |
||||
|
||||
Unlogger::Unlogger(Events *events_, QReadWriteLock* events_lock_, QMap<int, FrameReader*> *frs_, int seek) |
||||
: events(events_), events_lock(events_lock_), frs(frs_) { |
||||
ctx = Context::create(); |
||||
|
||||
seek_request = seek*1e9; |
||||
|
||||
QStringList block = QString(getenv("BLOCK")).split(","); |
||||
qDebug() << "blocklist" << block; |
||||
|
||||
QStringList allow = QString(getenv("ALLOW")).split(","); |
||||
qDebug() << "allowlist" << allow; |
||||
|
||||
for (const auto& it : services) { |
||||
std::string name = it.name; |
||||
if (allow[0].size() > 0 && !allow.contains(name.c_str())) { |
||||
qDebug() << "not allowing" << name.c_str(); |
||||
continue; |
||||
} |
||||
|
||||
if (block.contains(name.c_str())) { |
||||
qDebug() << "blocking" << name.c_str(); |
||||
continue; |
||||
} |
||||
|
||||
PubSocket *sock = PubSocket::create(ctx, name); |
||||
if (sock == NULL) { |
||||
qDebug() << "FAILED" << name.c_str(); |
||||
continue; |
||||
} |
||||
|
||||
qDebug() << name.c_str(); |
||||
|
||||
socks.insert(name, sock); |
||||
} |
||||
} |
||||
|
||||
void Unlogger::process(SubMaster *sm) { |
||||
|
||||
qDebug() << "hello from unlogger thread"; |
||||
while (events->size() == 0) { |
||||
qDebug() << "waiting for events"; |
||||
QThread::sleep(1); |
||||
} |
||||
qDebug() << "got events"; |
||||
|
||||
// TODO: hack
|
||||
if (seek_request != 0) { |
||||
seek_request += events->begin().key(); |
||||
while (events->lowerBound(seek_request) == events->end()) { |
||||
qDebug() << "waiting for desired time"; |
||||
QThread::sleep(1); |
||||
} |
||||
} |
||||
|
||||
QElapsedTimer timer; |
||||
timer.start(); |
||||
|
||||
uint64_t last_elapsed = 0; |
||||
|
||||
// loops
|
||||
while (1) { |
||||
uint64_t t0 = (events->begin()+1).key(); |
||||
uint64_t t0r = timer.nsecsElapsed(); |
||||
qDebug() << "unlogging at" << t0; |
||||
|
||||
auto eit = events->lowerBound(t0); |
||||
while (eit != events->end()) { |
||||
|
||||
float time_to_end = ((events->lastKey() - eit.key())/1e9); |
||||
if (loading_segment && (time_to_end > 20.0)){ |
||||
loading_segment = false; |
||||
} |
||||
|
||||
while (paused) { |
||||
QThread::usleep(1000); |
||||
t0 = eit->getLogMonoTime(); |
||||
t0r = timer.nsecsElapsed(); |
||||
} |
||||
|
||||
if (seek_request != 0) { |
||||
t0 = seek_request; |
||||
qDebug() << "seeking to" << t0; |
||||
t0r = timer.nsecsElapsed(); |
||||
eit = events->lowerBound(t0); |
||||
seek_request = 0; |
||||
if (eit == events->end()) { |
||||
qWarning() << "seek off end"; |
||||
break; |
||||
} |
||||
} |
||||
|
||||
if (abs(((long long)tc-(long long)last_elapsed)) > 50e6) { |
||||
//qDebug() << "elapsed";
|
||||
emit elapsed(); |
||||
last_elapsed = tc; |
||||
} |
||||
|
||||
cereal::Event::Reader e = *eit; |
||||
|
||||
capnp::DynamicStruct::Reader e_ds = static_cast<capnp::DynamicStruct::Reader>(e); |
||||
std::string type; |
||||
KJ_IF_MAYBE(e_, e_ds.which()){ |
||||
type = e_->getProto().getName(); |
||||
} |
||||
|
||||
uint64_t tm = e.getLogMonoTime(); |
||||
auto it = socks.find(type); |
||||
tc = tm; |
||||
if (it != socks.end()) { |
||||
long etime = tm-t0; |
||||
|
||||
float timestamp = etime/1e9; |
||||
if(std::abs(timestamp-last_print) > 5.0){ |
||||
last_print = timestamp; |
||||
printf("at %f\n", last_print); |
||||
} |
||||
|
||||
long rtime = timer.nsecsElapsed() - t0r; |
||||
long us_behind = ((etime-rtime)*1e-3)+0.5; |
||||
if (us_behind > 0) { |
||||
if (us_behind > 1e6) { |
||||
qWarning() << "OVER ONE SECOND BEHIND, HACKING" << us_behind; |
||||
us_behind = 0; |
||||
t0 = tm; |
||||
t0r = timer.nsecsElapsed(); |
||||
} |
||||
QThread::usleep(us_behind); |
||||
//qDebug() << "sleeping" << us_behind << etime << timer.nsecsElapsed();
|
||||
} |
||||
|
||||
if (type == "roadCameraState") { |
||||
auto fr = e.getRoadCameraState(); |
||||
|
||||
auto it_ = eidx.find(fr.getFrameId()); |
||||
if (it_ != eidx.end()) { |
||||
auto pp = *it_; |
||||
//qDebug() << fr.getRoadCameraStateId() << pp;
|
||||
|
||||
if (frs->find(pp.first) != frs->end()) { |
||||
auto frm = (*frs)[pp.first]; |
||||
auto data = frm->get(pp.second); |
||||
|
||||
if (vipc_server == nullptr) { |
||||
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); |
||||
cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); |
||||
|
||||
vipc_server = new VisionIpcServer("camerad", device_id, context); |
||||
vipc_server->create_buffers(VisionStreamType::VISION_STREAM_RGB_BACK, 4, true, frm->width, frm->height); |
||||
|
||||
vipc_server->start_listener(); |
||||
} |
||||
|
||||
VisionBuf *buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_RGB_BACK); |
||||
memcpy(buf->addr, data, frm->getRGBSize()); |
||||
VisionIpcBufExtra extra = {}; |
||||
|
||||
vipc_server->send(buf, &extra, false); |
||||
} |
||||
} |
||||
} |
||||
|
||||
if (sm == nullptr){ |
||||
capnp::MallocMessageBuilder msg; |
||||
msg.setRoot(e); |
||||
auto words = capnp::messageToFlatArray(msg); |
||||
auto bytes = words.asBytes(); |
||||
|
||||
(*it)->send((char*)bytes.begin(), bytes.size()); |
||||
} else{ |
||||
std::vector<std::pair<std::string, cereal::Event::Reader>> messages; |
||||
messages.push_back({type, e}); |
||||
sm->update_msgs(nanos_since_boot(), messages); |
||||
} |
||||
} |
||||
++eit; |
||||
|
||||
if (time_to_end < 10.0 && !loading_segment){ |
||||
loading_segment = true; |
||||
emit loadSegment(); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
@ -1,42 +0,0 @@ |
||||
#pragma once |
||||
|
||||
#include <QReadWriteLock> |
||||
#include <QThread> |
||||
|
||||
#include "cereal/messaging/messaging.h" |
||||
#include "cereal/visionipc/visionipc_server.h" |
||||
#include "selfdrive/common/clutil.h" |
||||
#include "selfdrive/ui/replay/filereader.h" |
||||
#include "tools/clib/framereader.h" |
||||
|
||||
class Unlogger : public QObject { |
||||
Q_OBJECT |
||||
public: |
||||
Unlogger(Events *events_, QReadWriteLock* events_lock_, QMap<int, FrameReader*> *frs_, int seek); |
||||
uint64_t getCurrentTime() { return tc; } |
||||
void setSeekRequest(uint64_t seek_request_) { seek_request = seek_request_; } |
||||
void setPause(bool pause) { paused = pause; } |
||||
void togglePause() { paused = !paused; } |
||||
QMap<int, QPair<int, int> > eidx; |
||||
|
||||
public slots: |
||||
void process(SubMaster *sm = nullptr); |
||||
signals: |
||||
void elapsed(); |
||||
void finished(); |
||||
void loadSegment(); |
||||
private: |
||||
Events *events; |
||||
QReadWriteLock *events_lock; |
||||
QMap<int, FrameReader*> *frs; |
||||
QMap<std::string, PubSocket*> socks; |
||||
Context *ctx; |
||||
uint64_t tc = 0; |
||||
float last_print = 0; |
||||
uint64_t seek_request = 0; |
||||
bool paused = false; |
||||
bool loading_segment = false; |
||||
|
||||
VisionIpcServer *vipc_server = nullptr; |
||||
}; |
||||
|
||||
Loading…
Reference in new issue