Add openpilot tools
This commit is contained in:
286
tools/cabana/streams/abstractstream.cc
Normal file
286
tools/cabana/streams/abstractstream.cc
Normal file
@@ -0,0 +1,286 @@
|
||||
#include "tools/cabana/streams/abstractstream.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <utility>
|
||||
|
||||
#include "common/timing.h"
|
||||
#include "tools/cabana/settings.h"
|
||||
|
||||
static const int EVENT_NEXT_BUFFER_SIZE = 6 * 1024 * 1024; // 6MB
|
||||
|
||||
AbstractStream *can = nullptr;
|
||||
|
||||
StreamNotifier *StreamNotifier::instance() {
|
||||
static StreamNotifier notifier;
|
||||
return ¬ifier;
|
||||
}
|
||||
|
||||
AbstractStream::AbstractStream(QObject *parent) : QObject(parent) {
|
||||
assert(parent != nullptr);
|
||||
event_buffer_ = std::make_unique<MonotonicBuffer>(EVENT_NEXT_BUFFER_SIZE);
|
||||
|
||||
QObject::connect(this, &AbstractStream::privateUpdateLastMsgsSignal, this, &AbstractStream::updateLastMessages, Qt::QueuedConnection);
|
||||
QObject::connect(this, &AbstractStream::seekedTo, this, &AbstractStream::updateLastMsgsTo);
|
||||
QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &AbstractStream::updateMasks);
|
||||
QObject::connect(dbc(), &DBCManager::maskUpdated, this, &AbstractStream::updateMasks);
|
||||
QObject::connect(this, &AbstractStream::streamStarted, [this]() {
|
||||
emit StreamNotifier::instance()->changingStream();
|
||||
delete can;
|
||||
can = this;
|
||||
emit StreamNotifier::instance()->streamStarted();
|
||||
});
|
||||
}
|
||||
|
||||
void AbstractStream::updateMasks() {
|
||||
std::lock_guard lk(mutex_);
|
||||
masks_.clear();
|
||||
if (!settings.suppress_defined_signals)
|
||||
return;
|
||||
|
||||
for (const auto s : sources) {
|
||||
for (const auto &[address, m] : dbc()->getMessages(s)) {
|
||||
masks_[{.source = (uint8_t)s, .address = address}] = m.mask;
|
||||
}
|
||||
}
|
||||
// clear bit change counts
|
||||
for (auto &[id, m] : messages_) {
|
||||
auto &mask = masks_[id];
|
||||
const int size = std::min(mask.size(), m.last_changes.size());
|
||||
for (int i = 0; i < size; ++i) {
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
if (((mask[i] >> (7 - j)) & 1) != 0) m.last_changes[i].bit_change_counts[j] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AbstractStream::suppressDefinedSignals(bool suppress) {
|
||||
settings.suppress_defined_signals = suppress;
|
||||
updateMasks();
|
||||
}
|
||||
|
||||
size_t AbstractStream::suppressHighlighted() {
|
||||
std::lock_guard lk(mutex_);
|
||||
size_t cnt = 0;
|
||||
const double cur_ts = currentSec();
|
||||
for (auto &[_, m] : messages_) {
|
||||
for (auto &last_change : m.last_changes) {
|
||||
const double dt = cur_ts - last_change.ts;
|
||||
if (dt < 2.0) {
|
||||
last_change.suppressed = true;
|
||||
}
|
||||
// clear bit change counts
|
||||
last_change.bit_change_counts.fill(0);
|
||||
cnt += last_change.suppressed;
|
||||
}
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
void AbstractStream::clearSuppressed() {
|
||||
std::lock_guard lk(mutex_);
|
||||
for (auto &[_, m] : messages_) {
|
||||
std::for_each(m.last_changes.begin(), m.last_changes.end(), [](auto &c) { c.suppressed = false; });
|
||||
}
|
||||
}
|
||||
|
||||
void AbstractStream::updateLastMessages() {
|
||||
auto prev_src_size = sources.size();
|
||||
auto prev_msg_size = last_msgs.size();
|
||||
std::set<MessageId> msgs;
|
||||
{
|
||||
std::lock_guard lk(mutex_);
|
||||
for (const auto &id : new_msgs_) {
|
||||
const auto &can_data = messages_[id];
|
||||
current_sec_ = std::max(current_sec_, can_data.ts);
|
||||
last_msgs[id] = can_data;
|
||||
sources.insert(id.source);
|
||||
}
|
||||
msgs = std::move(new_msgs_);
|
||||
}
|
||||
|
||||
if (sources.size() != prev_src_size) {
|
||||
updateMasks();
|
||||
emit sourcesUpdated(sources);
|
||||
}
|
||||
emit msgsReceived(&msgs, prev_msg_size != last_msgs.size());
|
||||
}
|
||||
|
||||
void AbstractStream::updateEvent(const MessageId &id, double sec, const uint8_t *data, uint8_t size) {
|
||||
std::lock_guard lk(mutex_);
|
||||
messages_[id].compute(id, data, size, sec, getSpeed(), masks_[id]);
|
||||
new_msgs_.insert(id);
|
||||
}
|
||||
|
||||
const std::vector<const CanEvent *> &AbstractStream::events(const MessageId &id) const {
|
||||
static std::vector<const CanEvent *> empty_events;
|
||||
auto it = events_.find(id);
|
||||
return it != events_.end() ? it->second : empty_events;
|
||||
}
|
||||
|
||||
const CanData &AbstractStream::lastMessage(const MessageId &id) {
|
||||
static CanData empty_data = {};
|
||||
auto it = last_msgs.find(id);
|
||||
return it != last_msgs.end() ? it->second : empty_data;
|
||||
}
|
||||
|
||||
// it is thread safe to update data in updateLastMsgsTo.
|
||||
// updateLastMsgsTo is always called in UI thread.
|
||||
void AbstractStream::updateLastMsgsTo(double sec) {
|
||||
new_msgs_.clear();
|
||||
messages_.clear();
|
||||
|
||||
current_sec_ = sec;
|
||||
uint64_t last_ts = (sec + routeStartTime()) * 1e9;
|
||||
for (const auto &[id, ev] : events_) {
|
||||
auto it = std::upper_bound(ev.begin(), ev.end(), last_ts, CompareCanEvent());
|
||||
if (it != ev.begin()) {
|
||||
auto prev = std::prev(it);
|
||||
double ts = (*prev)->mono_time / 1e9 - routeStartTime();
|
||||
auto &m = messages_[id];
|
||||
m.compute(id, (*prev)->dat, (*prev)->size, ts, getSpeed(), {});
|
||||
m.count = std::distance(ev.begin(), prev) + 1;
|
||||
}
|
||||
}
|
||||
|
||||
bool id_changed = messages_.size() != last_msgs.size() ||
|
||||
std::any_of(messages_.cbegin(), messages_.cend(),
|
||||
[this](const auto &m) { return !last_msgs.count(m.first); });
|
||||
last_msgs = messages_;
|
||||
emit msgsReceived(nullptr, id_changed);
|
||||
}
|
||||
|
||||
const CanEvent *AbstractStream::newEvent(uint64_t mono_time, const cereal::CanData::Reader &c) {
|
||||
auto dat = c.getDat();
|
||||
CanEvent *e = (CanEvent *)event_buffer_->allocate(sizeof(CanEvent) + sizeof(uint8_t) * dat.size());
|
||||
e->src = c.getSrc();
|
||||
e->address = c.getAddress();
|
||||
e->mono_time = mono_time;
|
||||
e->size = dat.size();
|
||||
memcpy(e->dat, (uint8_t *)dat.begin(), e->size);
|
||||
return e;
|
||||
}
|
||||
|
||||
void AbstractStream::mergeEvents(const std::vector<const CanEvent *> &events) {
|
||||
static MessageEventsMap msg_events;
|
||||
std::for_each(msg_events.begin(), msg_events.end(), [](auto &e) { e.second.clear(); });
|
||||
for (auto e : events) {
|
||||
msg_events[{.source = e->src, .address = e->address}].push_back(e);
|
||||
}
|
||||
|
||||
if (!events.empty()) {
|
||||
for (const auto &[id, new_e] : msg_events) {
|
||||
if (!new_e.empty()) {
|
||||
auto &e = events_[id];
|
||||
auto pos = std::upper_bound(e.cbegin(), e.cend(), new_e.front()->mono_time, CompareCanEvent());
|
||||
e.insert(pos, new_e.cbegin(), new_e.cend());
|
||||
}
|
||||
}
|
||||
auto pos = std::upper_bound(all_events_.cbegin(), all_events_.cend(), events.front()->mono_time, CompareCanEvent());
|
||||
all_events_.insert(pos, events.cbegin(), events.cend());
|
||||
emit eventsMerged(msg_events);
|
||||
}
|
||||
lastest_event_ts = all_events_.empty() ? 0 : all_events_.back()->mono_time;
|
||||
}
|
||||
|
||||
// CanData
|
||||
|
||||
namespace {
|
||||
|
||||
enum Color { GREYISH_BLUE, CYAN, RED};
|
||||
QColor getColor(int c) {
|
||||
constexpr int start_alpha = 128;
|
||||
static const QColor colors[] = {
|
||||
[GREYISH_BLUE] = QColor(102, 86, 169, start_alpha / 2),
|
||||
[CYAN] = QColor(0, 187, 255, start_alpha),
|
||||
[RED] = QColor(255, 0, 0, start_alpha),
|
||||
};
|
||||
return settings.theme == LIGHT_THEME ? colors[c] : colors[c].lighter(135);
|
||||
}
|
||||
|
||||
inline QColor blend(const QColor &a, const QColor &b) {
|
||||
return QColor((a.red() + b.red()) / 2, (a.green() + b.green()) / 2, (a.blue() + b.blue()) / 2, (a.alpha() + b.alpha()) / 2);
|
||||
}
|
||||
|
||||
// Calculate the frequency of the past minute.
|
||||
double calc_freq(const MessageId &msg_id, double current_sec) {
|
||||
const auto &events = can->events(msg_id);
|
||||
uint64_t cur_mono_time = (can->routeStartTime() + current_sec) * 1e9;
|
||||
uint64_t first_mono_time = std::max<int64_t>(0, cur_mono_time - 59 * 1e9);
|
||||
auto first = std::lower_bound(events.begin(), events.end(), first_mono_time, CompareCanEvent());
|
||||
auto second = std::lower_bound(first, events.end(), cur_mono_time, CompareCanEvent());
|
||||
if (first != events.end() && second != events.end()) {
|
||||
double duration = ((*second)->mono_time - (*first)->mono_time) / 1e9;
|
||||
uint32_t count = std::distance(first, second);
|
||||
return count / std::max(1.0, duration);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void CanData::compute(const MessageId &msg_id, const uint8_t *can_data, const int size, double current_sec,
|
||||
double playback_speed, const std::vector<uint8_t> &mask, double in_freq) {
|
||||
ts = current_sec;
|
||||
++count;
|
||||
|
||||
if (auto sec = seconds_since_boot(); (sec - last_freq_update_ts) >= 1) {
|
||||
last_freq_update_ts = sec;
|
||||
freq = !in_freq ? calc_freq(msg_id, ts) : in_freq;
|
||||
}
|
||||
|
||||
if (dat.size() != size) {
|
||||
dat.resize(size);
|
||||
colors.assign(size, QColor(0, 0, 0, 0));
|
||||
last_changes.resize(size);
|
||||
std::for_each(last_changes.begin(), last_changes.end(), [current_sec](auto &c) { c.ts = current_sec; });
|
||||
} else {
|
||||
constexpr int periodic_threshold = 10;
|
||||
constexpr float fade_time = 2.0;
|
||||
const float alpha_delta = 1.0 / (freq + 1) / (fade_time * playback_speed);
|
||||
|
||||
for (int i = 0; i < size; ++i) {
|
||||
auto &last_change = last_changes[i];
|
||||
|
||||
uint8_t mask_byte = last_change.suppressed ? 0x00 : 0xFF;
|
||||
if (i < mask.size()) mask_byte &= ~(mask[i]);
|
||||
|
||||
const uint8_t last = dat[i] & mask_byte;
|
||||
const uint8_t cur = can_data[i] & mask_byte;
|
||||
if (last != cur) {
|
||||
const int delta = cur - last;
|
||||
// Keep track if signal is changing randomly, or mostly moving in the same direction
|
||||
if (std::signbit(delta) == std::signbit(last_change.delta)) {
|
||||
last_change.same_delta_counter = std::min(16, last_change.same_delta_counter + 1);
|
||||
} else {
|
||||
last_change.same_delta_counter = std::max(0, last_change.same_delta_counter - 4);
|
||||
}
|
||||
|
||||
const double delta_t = ts - last_change.ts;
|
||||
// Mostly moves in the same direction, color based on delta up/down
|
||||
if (delta_t * freq > periodic_threshold || last_change.same_delta_counter > 8) {
|
||||
// Last change was while ago, choose color based on delta up or down
|
||||
colors[i] = getColor(cur > last ? CYAN : RED);
|
||||
} else {
|
||||
// Periodic changes
|
||||
colors[i] = blend(colors[i], getColor(GREYISH_BLUE));
|
||||
}
|
||||
|
||||
// Track bit level changes
|
||||
const uint8_t tmp = (cur ^ last);
|
||||
for (int bit = 0; bit < 8; bit++) {
|
||||
if (tmp & (1 << (7 - bit))) {
|
||||
last_change.bit_change_counts[bit] += 1;
|
||||
}
|
||||
}
|
||||
|
||||
last_change.ts = ts;
|
||||
last_change.delta = delta;
|
||||
} else {
|
||||
// Fade out
|
||||
colors[i].setAlphaF(std::max(0.0, colors[i].alphaF() - alpha_delta));
|
||||
}
|
||||
}
|
||||
}
|
||||
memcpy(dat.data(), can_data, size);
|
||||
}
|
||||
157
tools/cabana/streams/abstractstream.h
Normal file
157
tools/cabana/streams/abstractstream.h
Normal file
@@ -0,0 +1,157 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <set>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <QColor>
|
||||
#include <QDateTime>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "tools/cabana/dbc/dbcmanager.h"
|
||||
#include "tools/cabana/util.h"
|
||||
|
||||
struct CanData {
|
||||
void compute(const MessageId &msg_id, const uint8_t *dat, const int size, double current_sec,
|
||||
double playback_speed, const std::vector<uint8_t> &mask, double in_freq = 0);
|
||||
|
||||
double ts = 0.;
|
||||
uint32_t count = 0;
|
||||
double freq = 0;
|
||||
std::vector<uint8_t> dat;
|
||||
std::vector<QColor> colors;
|
||||
|
||||
struct ByteLastChange {
|
||||
double ts;
|
||||
int delta;
|
||||
int same_delta_counter;
|
||||
bool suppressed;
|
||||
std::array<uint32_t, 8> bit_change_counts;
|
||||
};
|
||||
std::vector<ByteLastChange> last_changes;
|
||||
double last_freq_update_ts = 0;
|
||||
};
|
||||
|
||||
struct CanEvent {
|
||||
uint8_t src;
|
||||
uint32_t address;
|
||||
uint64_t mono_time;
|
||||
uint8_t size;
|
||||
uint8_t dat[];
|
||||
};
|
||||
|
||||
struct CompareCanEvent {
|
||||
constexpr bool operator()(const CanEvent *const e, uint64_t ts) const { return e->mono_time < ts; }
|
||||
constexpr bool operator()(uint64_t ts, const CanEvent *const e) const { return ts < e->mono_time; }
|
||||
};
|
||||
|
||||
struct BusConfig {
|
||||
int can_speed_kbps = 500;
|
||||
int data_speed_kbps = 2000;
|
||||
bool can_fd = false;
|
||||
};
|
||||
|
||||
typedef std::unordered_map<MessageId, std::vector<const CanEvent *>> MessageEventsMap;
|
||||
|
||||
class AbstractStream : public QObject {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
AbstractStream(QObject *parent);
|
||||
virtual ~AbstractStream() {}
|
||||
virtual void start() = 0;
|
||||
virtual bool liveStreaming() const { return true; }
|
||||
virtual void seekTo(double ts) {}
|
||||
virtual QString routeName() const = 0;
|
||||
virtual QString carFingerprint() const { return ""; }
|
||||
virtual QDateTime beginDateTime() const { return {}; }
|
||||
virtual double routeStartTime() const { return 0; }
|
||||
inline double currentSec() const { return current_sec_; }
|
||||
virtual double totalSeconds() const { return lastEventMonoTime() / 1e9 - routeStartTime(); }
|
||||
virtual void setSpeed(float speed) {}
|
||||
virtual double getSpeed() { return 1; }
|
||||
virtual bool isPaused() const { return false; }
|
||||
virtual void pause(bool pause) {}
|
||||
|
||||
inline const std::unordered_map<MessageId, CanData> &lastMessages() const { return last_msgs; }
|
||||
inline const MessageEventsMap &eventsMap() const { return events_; }
|
||||
inline const std::vector<const CanEvent *> &allEvents() const { return all_events_; }
|
||||
const CanData &lastMessage(const MessageId &id);
|
||||
const std::vector<const CanEvent *> &events(const MessageId &id) const;
|
||||
|
||||
size_t suppressHighlighted();
|
||||
void clearSuppressed();
|
||||
void suppressDefinedSignals(bool suppress);
|
||||
|
||||
signals:
|
||||
void paused();
|
||||
void resume();
|
||||
void seekedTo(double sec);
|
||||
void streamStarted();
|
||||
void eventsMerged(const MessageEventsMap &events_map);
|
||||
void msgsReceived(const std::set<MessageId> *new_msgs, bool has_new_ids);
|
||||
void sourcesUpdated(const SourceSet &s);
|
||||
void privateUpdateLastMsgsSignal();
|
||||
|
||||
public:
|
||||
SourceSet sources;
|
||||
|
||||
protected:
|
||||
void mergeEvents(const std::vector<const CanEvent *> &events);
|
||||
const CanEvent *newEvent(uint64_t mono_time, const cereal::CanData::Reader &c);
|
||||
void updateEvent(const MessageId &id, double sec, const uint8_t *data, uint8_t size);
|
||||
uint64_t lastEventMonoTime() const { return lastest_event_ts; }
|
||||
|
||||
std::vector<const CanEvent *> all_events_;
|
||||
uint64_t lastest_event_ts = 0;
|
||||
|
||||
private:
|
||||
void updateLastMessages();
|
||||
void updateLastMsgsTo(double sec);
|
||||
void updateMasks();
|
||||
|
||||
double current_sec_ = 0;
|
||||
MessageEventsMap events_;
|
||||
std::unordered_map<MessageId, CanData> last_msgs;
|
||||
std::unique_ptr<MonotonicBuffer> event_buffer_;
|
||||
|
||||
// Members accessed in multiple threads. (mutex protected)
|
||||
std::mutex mutex_;
|
||||
std::set<MessageId> new_msgs_;
|
||||
std::unordered_map<MessageId, CanData> messages_;
|
||||
std::unordered_map<MessageId, std::vector<uint8_t>> masks_;
|
||||
};
|
||||
|
||||
class AbstractOpenStreamWidget : public QWidget {
|
||||
public:
|
||||
AbstractOpenStreamWidget(AbstractStream **stream, QWidget *parent = nullptr) : stream(stream), QWidget(parent) {}
|
||||
virtual bool open() = 0;
|
||||
virtual QString title() = 0;
|
||||
|
||||
protected:
|
||||
AbstractStream **stream = nullptr;
|
||||
};
|
||||
|
||||
class DummyStream : public AbstractStream {
|
||||
Q_OBJECT
|
||||
public:
|
||||
DummyStream(QObject *parent) : AbstractStream(parent) {}
|
||||
QString routeName() const override { return tr("No Stream"); }
|
||||
void start() override { emit streamStarted(); }
|
||||
};
|
||||
|
||||
class StreamNotifier : public QObject {
|
||||
Q_OBJECT
|
||||
public:
|
||||
StreamNotifier(QObject *parent = nullptr) : QObject(parent) {}
|
||||
static StreamNotifier* instance();
|
||||
signals:
|
||||
void streamStarted();
|
||||
void changingStream();
|
||||
};
|
||||
|
||||
// A global pointer referring to the unique AbstractStream object
|
||||
extern AbstractStream *can;
|
||||
70
tools/cabana/streams/devicestream.cc
Normal file
70
tools/cabana/streams/devicestream.cc
Normal file
@@ -0,0 +1,70 @@
|
||||
#include "tools/cabana/streams/devicestream.h"
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include <QButtonGroup>
|
||||
#include <QFormLayout>
|
||||
#include <QRadioButton>
|
||||
#include <QRegularExpression>
|
||||
#include <QRegularExpressionValidator>
|
||||
#include <QThread>
|
||||
|
||||
// DeviceStream
|
||||
|
||||
DeviceStream::DeviceStream(QObject *parent, QString address) : zmq_address(address), LiveStream(parent) {
|
||||
}
|
||||
|
||||
void DeviceStream::streamThread() {
|
||||
zmq_address.isEmpty() ? unsetenv("ZMQ") : setenv("ZMQ", "1", 1);
|
||||
|
||||
std::unique_ptr<Context> context(Context::create());
|
||||
std::string address = zmq_address.isEmpty() ? "127.0.0.1" : zmq_address.toStdString();
|
||||
std::unique_ptr<SubSocket> sock(SubSocket::create(context.get(), "can", address));
|
||||
assert(sock != NULL);
|
||||
// run as fast as messages come in
|
||||
while (!QThread::currentThread()->isInterruptionRequested()) {
|
||||
std::unique_ptr<Message> msg(sock->receive(true));
|
||||
if (!msg) {
|
||||
QThread::msleep(50);
|
||||
continue;
|
||||
}
|
||||
handleEvent(kj::ArrayPtr<capnp::word>((capnp::word*)msg->getData(), msg->getSize() / sizeof(capnp::word)));
|
||||
}
|
||||
}
|
||||
|
||||
AbstractOpenStreamWidget *DeviceStream::widget(AbstractStream **stream) {
|
||||
return new OpenDeviceWidget(stream);
|
||||
}
|
||||
|
||||
// OpenDeviceWidget
|
||||
|
||||
OpenDeviceWidget::OpenDeviceWidget(AbstractStream **stream) : AbstractOpenStreamWidget(stream) {
|
||||
QRadioButton *msgq = new QRadioButton(tr("MSGQ"));
|
||||
QRadioButton *zmq = new QRadioButton(tr("ZMQ"));
|
||||
ip_address = new QLineEdit(this);
|
||||
ip_address->setPlaceholderText(tr("Enter device Ip Address"));
|
||||
QString ip_range = "(?:[0-1]?[0-9]?[0-9]|2[0-4][0-9]|25[0-5])";
|
||||
QString pattern("^" + ip_range + "\\." + ip_range + "\\." + ip_range + "\\." + ip_range + "$");
|
||||
QRegularExpression re(pattern);
|
||||
ip_address->setValidator(new QRegularExpressionValidator(re, this));
|
||||
|
||||
group = new QButtonGroup(this);
|
||||
group->addButton(msgq, 0);
|
||||
group->addButton(zmq, 1);
|
||||
|
||||
QFormLayout *form_layout = new QFormLayout(this);
|
||||
form_layout->addRow(msgq);
|
||||
form_layout->addRow(zmq, ip_address);
|
||||
QObject::connect(group, qOverload<QAbstractButton *, bool>(&QButtonGroup::buttonToggled), [=](QAbstractButton *button, bool checked) {
|
||||
ip_address->setEnabled(button == zmq && checked);
|
||||
});
|
||||
zmq->setChecked(true);
|
||||
}
|
||||
|
||||
bool OpenDeviceWidget::open() {
|
||||
QString ip = ip_address->text().isEmpty() ? "127.0.0.1" : ip_address->text();
|
||||
bool msgq = group->checkedId() == 0;
|
||||
*stream = new DeviceStream(qApp, msgq ? "" : ip);
|
||||
return true;
|
||||
}
|
||||
30
tools/cabana/streams/devicestream.h
Normal file
30
tools/cabana/streams/devicestream.h
Normal file
@@ -0,0 +1,30 @@
|
||||
#pragma once
|
||||
|
||||
#include "tools/cabana/streams/livestream.h"
|
||||
|
||||
class DeviceStream : public LiveStream {
|
||||
Q_OBJECT
|
||||
public:
|
||||
DeviceStream(QObject *parent, QString address = {});
|
||||
static AbstractOpenStreamWidget *widget(AbstractStream **stream);
|
||||
inline QString routeName() const override {
|
||||
return QString("Live Streaming From %1").arg(zmq_address.isEmpty() ? "127.0.0.1" : zmq_address);
|
||||
}
|
||||
|
||||
protected:
|
||||
void streamThread() override;
|
||||
const QString zmq_address;
|
||||
};
|
||||
|
||||
class OpenDeviceWidget : public AbstractOpenStreamWidget {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
OpenDeviceWidget(AbstractStream **stream);
|
||||
bool open() override;
|
||||
QString title() override { return tr("&Device"); }
|
||||
|
||||
private:
|
||||
QLineEdit *ip_address;
|
||||
QButtonGroup *group;
|
||||
};
|
||||
140
tools/cabana/streams/livestream.cc
Normal file
140
tools/cabana/streams/livestream.cc
Normal file
@@ -0,0 +1,140 @@
|
||||
#include "tools/cabana/streams/livestream.h"
|
||||
|
||||
#include <QThread>
|
||||
#include <algorithm>
|
||||
#include <fstream>
|
||||
#include <memory>
|
||||
|
||||
#include "common/timing.h"
|
||||
#include "common/util.h"
|
||||
|
||||
struct LiveStream::Logger {
|
||||
Logger() : start_ts(seconds_since_epoch()), segment_num(-1) {}
|
||||
|
||||
void write(kj::ArrayPtr<capnp::word> data) {
|
||||
int n = (seconds_since_epoch() - start_ts) / 60.0;
|
||||
if (std::exchange(segment_num, n) != segment_num) {
|
||||
QString dir = QString("%1/%2--%3")
|
||||
.arg(settings.log_path)
|
||||
.arg(QDateTime::fromSecsSinceEpoch(start_ts).toString("yyyy-MM-dd--hh-mm-ss"))
|
||||
.arg(n);
|
||||
util::create_directories(dir.toStdString(), 0755);
|
||||
fs.reset(new std::ofstream((dir + "/rlog").toStdString(), std::ios::binary | std::ios::out));
|
||||
}
|
||||
|
||||
auto bytes = data.asBytes();
|
||||
fs->write((const char*)bytes.begin(), bytes.size());
|
||||
}
|
||||
|
||||
std::unique_ptr<std::ofstream> fs;
|
||||
int segment_num;
|
||||
uint64_t start_ts;
|
||||
};
|
||||
|
||||
LiveStream::LiveStream(QObject *parent) : AbstractStream(parent) {
|
||||
if (settings.log_livestream) {
|
||||
logger = std::make_unique<Logger>();
|
||||
}
|
||||
stream_thread = new QThread(this);
|
||||
|
||||
QObject::connect(&settings, &Settings::changed, this, &LiveStream::startUpdateTimer);
|
||||
QObject::connect(stream_thread, &QThread::started, [=]() { streamThread(); });
|
||||
QObject::connect(stream_thread, &QThread::finished, stream_thread, &QThread::deleteLater);
|
||||
}
|
||||
|
||||
void LiveStream::startUpdateTimer() {
|
||||
update_timer.stop();
|
||||
update_timer.start(1000.0 / settings.fps, this);
|
||||
timer_id = update_timer.timerId();
|
||||
}
|
||||
|
||||
void LiveStream::start() {
|
||||
emit streamStarted();
|
||||
stream_thread->start();
|
||||
startUpdateTimer();
|
||||
begin_date_time = QDateTime::currentDateTime();
|
||||
}
|
||||
|
||||
LiveStream::~LiveStream() {
|
||||
update_timer.stop();
|
||||
stream_thread->requestInterruption();
|
||||
stream_thread->quit();
|
||||
stream_thread->wait();
|
||||
}
|
||||
|
||||
// called in streamThread
|
||||
void LiveStream::handleEvent(kj::ArrayPtr<capnp::word> data) {
|
||||
if (logger) {
|
||||
logger->write(data);
|
||||
}
|
||||
|
||||
capnp::FlatArrayMessageReader reader(data);
|
||||
auto event = reader.getRoot<cereal::Event>();
|
||||
if (event.which() == cereal::Event::Which::CAN) {
|
||||
const uint64_t mono_time = event.getLogMonoTime();
|
||||
std::lock_guard lk(lock);
|
||||
for (const auto &c : event.getCan()) {
|
||||
received_events_.push_back(newEvent(mono_time, c));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void LiveStream::timerEvent(QTimerEvent *event) {
|
||||
if (event->timerId() == timer_id) {
|
||||
{
|
||||
// merge events received from live stream thread.
|
||||
std::lock_guard lk(lock);
|
||||
mergeEvents(received_events_);
|
||||
received_events_.clear();
|
||||
}
|
||||
if (!all_events_.empty()) {
|
||||
begin_event_ts = all_events_.front()->mono_time;
|
||||
updateEvents();
|
||||
return;
|
||||
}
|
||||
}
|
||||
QObject::timerEvent(event);
|
||||
}
|
||||
|
||||
void LiveStream::updateEvents() {
|
||||
static double prev_speed = 1.0;
|
||||
|
||||
if (first_update_ts == 0) {
|
||||
first_update_ts = nanos_since_boot();
|
||||
first_event_ts = current_event_ts = all_events_.back()->mono_time;
|
||||
}
|
||||
|
||||
if (paused_ || prev_speed != speed_) {
|
||||
prev_speed = speed_;
|
||||
first_update_ts = nanos_since_boot();
|
||||
first_event_ts = current_event_ts;
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t last_ts = post_last_event && speed_ == 1.0
|
||||
? all_events_.back()->mono_time
|
||||
: first_event_ts + (nanos_since_boot() - first_update_ts) * speed_;
|
||||
auto first = std::upper_bound(all_events_.cbegin(), all_events_.cend(), current_event_ts, CompareCanEvent());
|
||||
auto last = std::upper_bound(first, all_events_.cend(), last_ts, CompareCanEvent());
|
||||
|
||||
for (auto it = first; it != last; ++it) {
|
||||
const CanEvent *e = *it;
|
||||
MessageId id = {.source = e->src, .address = e->address};
|
||||
updateEvent(id, (e->mono_time - begin_event_ts) / 1e9, e->dat, e->size);
|
||||
current_event_ts = e->mono_time;
|
||||
}
|
||||
emit privateUpdateLastMsgsSignal();
|
||||
}
|
||||
|
||||
void LiveStream::seekTo(double sec) {
|
||||
sec = std::max(0.0, sec);
|
||||
first_update_ts = nanos_since_boot();
|
||||
current_event_ts = first_event_ts = std::min<uint64_t>(sec * 1e9 + begin_event_ts, lastEventMonoTime());
|
||||
post_last_event = (first_event_ts == lastEventMonoTime());
|
||||
emit seekedTo((current_event_ts - begin_event_ts) / 1e9);
|
||||
}
|
||||
|
||||
void LiveStream::pause(bool pause) {
|
||||
paused_ = pause;
|
||||
emit(pause ? paused() : resume());
|
||||
}
|
||||
52
tools/cabana/streams/livestream.h
Normal file
52
tools/cabana/streams/livestream.h
Normal file
@@ -0,0 +1,52 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include <QBasicTimer>
|
||||
|
||||
#include "tools/cabana/streams/abstractstream.h"
|
||||
|
||||
class LiveStream : public AbstractStream {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
LiveStream(QObject *parent);
|
||||
virtual ~LiveStream();
|
||||
void start() override;
|
||||
inline QDateTime beginDateTime() const { return begin_date_time; }
|
||||
inline double routeStartTime() const override { return begin_event_ts / 1e9; }
|
||||
void setSpeed(float speed) override { speed_ = speed; }
|
||||
double getSpeed() override { return speed_; }
|
||||
bool isPaused() const override { return paused_; }
|
||||
void pause(bool pause) override;
|
||||
void seekTo(double sec) override;
|
||||
|
||||
protected:
|
||||
virtual void streamThread() = 0;
|
||||
void handleEvent(kj::ArrayPtr<capnp::word> event);
|
||||
|
||||
private:
|
||||
void startUpdateTimer();
|
||||
void timerEvent(QTimerEvent *event) override;
|
||||
void updateEvents();
|
||||
|
||||
std::mutex lock;
|
||||
QThread *stream_thread;
|
||||
std::vector<const CanEvent *> received_events_;
|
||||
|
||||
int timer_id;
|
||||
QBasicTimer update_timer;
|
||||
|
||||
QDateTime begin_date_time;
|
||||
uint64_t begin_event_ts = 0;
|
||||
uint64_t current_event_ts = 0;
|
||||
uint64_t first_event_ts = 0;
|
||||
uint64_t first_update_ts = 0;
|
||||
bool post_last_event = true;
|
||||
double speed_ = 1;
|
||||
bool paused_ = false;
|
||||
|
||||
struct Logger;
|
||||
std::unique_ptr<Logger> logger;
|
||||
};
|
||||
220
tools/cabana/streams/pandastream.cc
Normal file
220
tools/cabana/streams/pandastream.cc
Normal file
@@ -0,0 +1,220 @@
|
||||
#include "tools/cabana/streams/pandastream.h"
|
||||
|
||||
#include <QDebug>
|
||||
#include <QCheckBox>
|
||||
#include <QLabel>
|
||||
#include <QMessageBox>
|
||||
#include <QPushButton>
|
||||
#include <QThread>
|
||||
#include <QVBoxLayout>
|
||||
|
||||
// TODO: remove clearLayout
|
||||
static void clearLayout(QLayout* layout) {
|
||||
while (layout->count() > 0) {
|
||||
QLayoutItem* item = layout->takeAt(0);
|
||||
if (QWidget* widget = item->widget()) {
|
||||
widget->deleteLater();
|
||||
}
|
||||
if (QLayout* childLayout = item->layout()) {
|
||||
clearLayout(childLayout);
|
||||
}
|
||||
delete item;
|
||||
}
|
||||
}
|
||||
|
||||
PandaStream::PandaStream(QObject *parent, PandaStreamConfig config_) : config(config_), LiveStream(parent) {
|
||||
if (config.serial.isEmpty()) {
|
||||
auto serials = Panda::list();
|
||||
if (serials.size() == 0) {
|
||||
throw std::runtime_error("No panda found");
|
||||
}
|
||||
config.serial = QString::fromStdString(serials[0]);
|
||||
}
|
||||
|
||||
qDebug() << "Connecting to panda with serial" << config.serial;
|
||||
if (!connect()) {
|
||||
throw std::runtime_error("Failed to connect to panda");
|
||||
}
|
||||
}
|
||||
|
||||
bool PandaStream::connect() {
|
||||
try {
|
||||
panda.reset(new Panda(config.serial.toStdString()));
|
||||
config.bus_config.resize(3);
|
||||
qDebug() << "Connected";
|
||||
} catch (const std::exception& e) {
|
||||
return false;
|
||||
}
|
||||
|
||||
panda->set_safety_model(cereal::CarParams::SafetyModel::SILENT);
|
||||
|
||||
for (int bus = 0; bus < config.bus_config.size(); bus++) {
|
||||
panda->set_can_speed_kbps(bus, config.bus_config[bus].can_speed_kbps);
|
||||
|
||||
// CAN-FD
|
||||
if (panda->hw_type == cereal::PandaState::PandaType::RED_PANDA || panda->hw_type == cereal::PandaState::PandaType::RED_PANDA_V2) {
|
||||
if (config.bus_config[bus].can_fd) {
|
||||
panda->set_data_speed_kbps(bus, config.bus_config[bus].data_speed_kbps);
|
||||
} else {
|
||||
// Hack to disable can-fd by setting data speed to a low value
|
||||
panda->set_data_speed_kbps(bus, 10);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void PandaStream::streamThread() {
|
||||
std::vector<can_frame> raw_can_data;
|
||||
|
||||
while (!QThread::currentThread()->isInterruptionRequested()) {
|
||||
QThread::msleep(1);
|
||||
|
||||
if (!panda->connected()) {
|
||||
qDebug() << "Connection to panda lost. Attempting reconnect.";
|
||||
if (!connect()){
|
||||
QThread::msleep(1000);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
raw_can_data.clear();
|
||||
if (!panda->can_receive(raw_can_data)) {
|
||||
qDebug() << "failed to receive";
|
||||
continue;
|
||||
}
|
||||
|
||||
MessageBuilder msg;
|
||||
auto evt = msg.initEvent();
|
||||
auto canData = evt.initCan(raw_can_data.size());
|
||||
for (uint i = 0; i<raw_can_data.size(); i++) {
|
||||
canData[i].setAddress(raw_can_data[i].address);
|
||||
canData[i].setBusTime(raw_can_data[i].busTime);
|
||||
canData[i].setDat(kj::arrayPtr((uint8_t*)raw_can_data[i].dat.data(), raw_can_data[i].dat.size()));
|
||||
canData[i].setSrc(raw_can_data[i].src);
|
||||
}
|
||||
|
||||
handleEvent(capnp::messageToFlatArray(msg));
|
||||
|
||||
panda->send_heartbeat(false);
|
||||
}
|
||||
}
|
||||
|
||||
AbstractOpenStreamWidget *PandaStream::widget(AbstractStream **stream) {
|
||||
return new OpenPandaWidget(stream);
|
||||
}
|
||||
|
||||
// OpenPandaWidget
|
||||
|
||||
OpenPandaWidget::OpenPandaWidget(AbstractStream **stream) : AbstractOpenStreamWidget(stream) {
|
||||
QVBoxLayout *main_layout = new QVBoxLayout(this);
|
||||
main_layout->addStretch(1);
|
||||
|
||||
QFormLayout *form_layout = new QFormLayout();
|
||||
|
||||
QHBoxLayout *serial_layout = new QHBoxLayout();
|
||||
serial_edit = new QComboBox();
|
||||
serial_edit->setFixedWidth(300);
|
||||
serial_layout->addWidget(serial_edit);
|
||||
|
||||
QPushButton *refresh = new QPushButton(tr("Refresh"));
|
||||
refresh->setFixedWidth(100);
|
||||
serial_layout->addWidget(refresh);
|
||||
form_layout->addRow(tr("Serial"), serial_layout);
|
||||
main_layout->addLayout(form_layout);
|
||||
|
||||
config_layout = new QFormLayout();
|
||||
main_layout->addLayout(config_layout);
|
||||
|
||||
main_layout->addStretch(1);
|
||||
|
||||
QObject::connect(refresh, &QPushButton::clicked, this, &OpenPandaWidget::refreshSerials);
|
||||
QObject::connect(serial_edit, &QComboBox::currentTextChanged, this, &OpenPandaWidget::buildConfigForm);
|
||||
|
||||
// Populate serials
|
||||
refreshSerials();
|
||||
buildConfigForm();
|
||||
}
|
||||
|
||||
void OpenPandaWidget::refreshSerials() {
|
||||
serial_edit->clear();
|
||||
for (auto serial : Panda::list()) {
|
||||
serial_edit->addItem(QString::fromStdString(serial));
|
||||
}
|
||||
}
|
||||
|
||||
void OpenPandaWidget::buildConfigForm() {
|
||||
clearLayout(config_layout);
|
||||
QString serial = serial_edit->currentText();
|
||||
|
||||
bool has_fd = false;
|
||||
bool has_panda = !serial.isEmpty();
|
||||
|
||||
if (has_panda) {
|
||||
try {
|
||||
Panda panda = Panda(serial.toStdString());
|
||||
has_fd = (panda.hw_type == cereal::PandaState::PandaType::RED_PANDA) || (panda.hw_type == cereal::PandaState::PandaType::RED_PANDA_V2);
|
||||
} catch (const std::exception& e) {
|
||||
has_panda = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (has_panda) {
|
||||
config.serial = serial;
|
||||
config.bus_config.resize(3);
|
||||
for (int i = 0; i < config.bus_config.size(); i++) {
|
||||
QHBoxLayout *bus_layout = new QHBoxLayout;
|
||||
|
||||
// CAN Speed
|
||||
bus_layout->addWidget(new QLabel(tr("CAN Speed (kbps):")));
|
||||
QComboBox *can_speed = new QComboBox;
|
||||
for (int j = 0; j < std::size(speeds); j++) {
|
||||
can_speed->addItem(QString::number(speeds[j]));
|
||||
|
||||
if (data_speeds[j] == config.bus_config[i].can_speed_kbps) {
|
||||
can_speed->setCurrentIndex(j);
|
||||
}
|
||||
}
|
||||
QObject::connect(can_speed, qOverload<int>(&QComboBox::currentIndexChanged), [=](int index) {config.bus_config[i].can_speed_kbps = speeds[index];});
|
||||
bus_layout->addWidget(can_speed);
|
||||
|
||||
// CAN-FD Speed
|
||||
if (has_fd) {
|
||||
QCheckBox *enable_fd = new QCheckBox("CAN-FD");
|
||||
bus_layout->addWidget(enable_fd);
|
||||
bus_layout->addWidget(new QLabel(tr("Data Speed (kbps):")));
|
||||
QComboBox *data_speed = new QComboBox;
|
||||
for (int j = 0; j < std::size(data_speeds); j++) {
|
||||
data_speed->addItem(QString::number(data_speeds[j]));
|
||||
|
||||
if (data_speeds[j] == config.bus_config[i].data_speed_kbps) {
|
||||
data_speed->setCurrentIndex(j);
|
||||
}
|
||||
}
|
||||
|
||||
data_speed->setEnabled(false);
|
||||
bus_layout->addWidget(data_speed);
|
||||
|
||||
QObject::connect(data_speed, qOverload<int>(&QComboBox::currentIndexChanged), [=](int index) {config.bus_config[i].data_speed_kbps = data_speeds[index];});
|
||||
QObject::connect(enable_fd, &QCheckBox::stateChanged, data_speed, &QComboBox::setEnabled);
|
||||
QObject::connect(enable_fd, &QCheckBox::stateChanged, [=](int state) {config.bus_config[i].can_fd = (bool)state;});
|
||||
}
|
||||
|
||||
config_layout->addRow(tr("Bus %1:").arg(i), bus_layout);
|
||||
}
|
||||
} else {
|
||||
config.serial = "";
|
||||
config_layout->addWidget(new QLabel(tr("No panda found")));
|
||||
}
|
||||
}
|
||||
|
||||
bool OpenPandaWidget::open() {
|
||||
try {
|
||||
*stream = new PandaStream(qApp, config);
|
||||
} catch (std::exception &e) {
|
||||
QMessageBox::warning(nullptr, tr("Warning"), tr("Failed to connect to panda: '%1'").arg(e.what()));
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
52
tools/cabana/streams/pandastream.h
Normal file
52
tools/cabana/streams/pandastream.h
Normal file
@@ -0,0 +1,52 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include <QComboBox>
|
||||
#include <QFormLayout>
|
||||
|
||||
#include "tools/cabana/streams/livestream.h"
|
||||
#include "selfdrive/boardd/panda.h"
|
||||
|
||||
const uint32_t speeds[] = {10U, 20U, 50U, 100U, 125U, 250U, 500U, 1000U};
|
||||
const uint32_t data_speeds[] = {10U, 20U, 50U, 100U, 125U, 250U, 500U, 1000U, 2000U, 5000U};
|
||||
|
||||
struct PandaStreamConfig {
|
||||
QString serial = "";
|
||||
std::vector<BusConfig> bus_config;
|
||||
};
|
||||
|
||||
class PandaStream : public LiveStream {
|
||||
Q_OBJECT
|
||||
public:
|
||||
PandaStream(QObject *parent, PandaStreamConfig config_ = {});
|
||||
static AbstractOpenStreamWidget *widget(AbstractStream **stream);
|
||||
inline QString routeName() const override {
|
||||
return QString("Live Streaming From Panda %1").arg(config.serial);
|
||||
}
|
||||
|
||||
protected:
|
||||
void streamThread() override;
|
||||
bool connect();
|
||||
|
||||
std::unique_ptr<Panda> panda;
|
||||
PandaStreamConfig config = {};
|
||||
};
|
||||
|
||||
class OpenPandaWidget : public AbstractOpenStreamWidget {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
OpenPandaWidget(AbstractStream **stream);
|
||||
bool open() override;
|
||||
QString title() override { return tr("&Panda"); }
|
||||
|
||||
private:
|
||||
void refreshSerials();
|
||||
void buildConfigForm();
|
||||
|
||||
QComboBox *serial_edit;
|
||||
QFormLayout *config_layout;
|
||||
PandaStreamConfig config = {};
|
||||
};
|
||||
145
tools/cabana/streams/replaystream.cc
Normal file
145
tools/cabana/streams/replaystream.cc
Normal file
@@ -0,0 +1,145 @@
|
||||
#include "tools/cabana/streams/replaystream.h"
|
||||
|
||||
#include <QLabel>
|
||||
#include <QFileDialog>
|
||||
#include <QGridLayout>
|
||||
#include <QMessageBox>
|
||||
#include <QPushButton>
|
||||
|
||||
ReplayStream::ReplayStream(QObject *parent) : AbstractStream(parent) {
|
||||
unsetenv("ZMQ");
|
||||
setenv("COMMA_CACHE", "/tmp/comma_download_cache", 1);
|
||||
|
||||
// TODO: Remove when OpenpilotPrefix supports ZMQ
|
||||
#ifndef __APPLE__
|
||||
op_prefix = std::make_unique<OpenpilotPrefix>();
|
||||
#endif
|
||||
|
||||
QObject::connect(&settings, &Settings::changed, this, [this]() {
|
||||
if (replay) replay->setSegmentCacheLimit(settings.max_cached_minutes);
|
||||
});
|
||||
}
|
||||
|
||||
static bool event_filter(const Event *e, void *opaque) {
|
||||
return ((ReplayStream *)opaque)->eventFilter(e);
|
||||
}
|
||||
|
||||
void ReplayStream::mergeSegments() {
|
||||
for (auto &[n, seg] : replay->segments()) {
|
||||
if (seg && seg->isLoaded() && !processed_segments.count(n)) {
|
||||
processed_segments.insert(n);
|
||||
|
||||
std::vector<const CanEvent *> new_events;
|
||||
new_events.reserve(seg->log->events.size());
|
||||
for (auto it = seg->log->events.cbegin(); it != seg->log->events.cend(); ++it) {
|
||||
if ((*it)->which == cereal::Event::Which::CAN) {
|
||||
const uint64_t ts = (*it)->mono_time;
|
||||
for (const auto &c : (*it)->event.getCan()) {
|
||||
new_events.push_back(newEvent(ts, c));
|
||||
}
|
||||
}
|
||||
}
|
||||
mergeEvents(new_events);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool ReplayStream::loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags) {
|
||||
replay.reset(new Replay(route, {"can", "roadEncodeIdx", "driverEncodeIdx", "wideRoadEncodeIdx", "carParams"},
|
||||
{}, nullptr, replay_flags, data_dir, this));
|
||||
replay->setSegmentCacheLimit(settings.max_cached_minutes);
|
||||
replay->installEventFilter(event_filter, this);
|
||||
QObject::connect(replay.get(), &Replay::seekedTo, this, &AbstractStream::seekedTo);
|
||||
QObject::connect(replay.get(), &Replay::segmentsMerged, this, &ReplayStream::mergeSegments);
|
||||
QObject::connect(replay.get(), &Replay::qLogLoaded, this, &ReplayStream::qLogLoaded, Qt::QueuedConnection);
|
||||
return replay->load();
|
||||
}
|
||||
|
||||
void ReplayStream::start() {
|
||||
emit streamStarted();
|
||||
replay->start();
|
||||
}
|
||||
|
||||
bool ReplayStream::eventFilter(const Event *event) {
|
||||
static double prev_update_ts = 0;
|
||||
if (event->which == cereal::Event::Which::CAN) {
|
||||
double current_sec = event->mono_time / 1e9 - routeStartTime();
|
||||
for (const auto &c : event->event.getCan()) {
|
||||
MessageId id = {.source = c.getSrc(), .address = c.getAddress()};
|
||||
const auto dat = c.getDat();
|
||||
updateEvent(id, current_sec, (const uint8_t*)dat.begin(), dat.size());
|
||||
}
|
||||
}
|
||||
|
||||
double ts = millis_since_boot();
|
||||
if ((ts - prev_update_ts) > (1000.0 / settings.fps)) {
|
||||
emit privateUpdateLastMsgsSignal();
|
||||
prev_update_ts = ts;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void ReplayStream::pause(bool pause) {
|
||||
replay->pause(pause);
|
||||
emit(pause ? paused() : resume());
|
||||
}
|
||||
|
||||
|
||||
AbstractOpenStreamWidget *ReplayStream::widget(AbstractStream **stream) {
|
||||
return new OpenReplayWidget(stream);
|
||||
}
|
||||
|
||||
// OpenReplayWidget
|
||||
|
||||
OpenReplayWidget::OpenReplayWidget(AbstractStream **stream) : AbstractOpenStreamWidget(stream) {
|
||||
// TODO: get route list from api.comma.ai
|
||||
QGridLayout *grid_layout = new QGridLayout(this);
|
||||
grid_layout->addWidget(new QLabel(tr("Route")), 0, 0);
|
||||
grid_layout->addWidget(route_edit = new QLineEdit(this), 0, 1);
|
||||
route_edit->setPlaceholderText(tr("Enter remote route name or click browse to select a local route"));
|
||||
auto file_btn = new QPushButton(tr("Browse..."), this);
|
||||
grid_layout->addWidget(file_btn, 0, 2);
|
||||
|
||||
grid_layout->addWidget(new QLabel(tr("Camera")), 1, 0);
|
||||
QHBoxLayout *camera_layout = new QHBoxLayout();
|
||||
for (auto c : {tr("Road camera"), tr("Driver camera"), tr("Wide road camera")})
|
||||
camera_layout->addWidget(cameras.emplace_back(new QCheckBox(c, this)));
|
||||
camera_layout->addStretch(1);
|
||||
grid_layout->addItem(camera_layout, 1, 1);
|
||||
|
||||
setMinimumWidth(550);
|
||||
QObject::connect(file_btn, &QPushButton::clicked, [=]() {
|
||||
QString dir = QFileDialog::getExistingDirectory(this, tr("Open Local Route"), settings.last_route_dir);
|
||||
if (!dir.isEmpty()) {
|
||||
route_edit->setText(dir);
|
||||
settings.last_route_dir = QFileInfo(dir).absolutePath();
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
bool OpenReplayWidget::open() {
|
||||
QString route = route_edit->text();
|
||||
QString data_dir;
|
||||
if (int idx = route.lastIndexOf('/'); idx != -1) {
|
||||
data_dir = route.mid(0, idx + 1);
|
||||
route = route.mid(idx + 1);
|
||||
}
|
||||
|
||||
bool is_valid_format = Route::parseRoute(route).str.size() > 0;
|
||||
if (!is_valid_format) {
|
||||
QMessageBox::warning(nullptr, tr("Warning"), tr("Invalid route format: '%1'").arg(route));
|
||||
} else {
|
||||
auto replay_stream = std::make_unique<ReplayStream>(qApp);
|
||||
uint32_t flags = REPLAY_FLAG_NONE;
|
||||
if (cameras[1]->isChecked()) flags |= REPLAY_FLAG_DCAM;
|
||||
if (cameras[2]->isChecked()) flags |= REPLAY_FLAG_ECAM;
|
||||
if (flags == REPLAY_FLAG_NONE && !cameras[0]->isChecked()) flags = REPLAY_FLAG_NO_VIPC;
|
||||
|
||||
if (replay_stream->loadRoute(route, data_dir, flags)) {
|
||||
*stream = replay_stream.release();
|
||||
} else {
|
||||
QMessageBox::warning(nullptr, tr("Warning"), tr("Failed to load route: '%1'").arg(route));
|
||||
}
|
||||
}
|
||||
return *stream != nullptr;
|
||||
}
|
||||
57
tools/cabana/streams/replaystream.h
Normal file
57
tools/cabana/streams/replaystream.h
Normal file
@@ -0,0 +1,57 @@
|
||||
#pragma once
|
||||
|
||||
#include <QCheckBox>
|
||||
#include <algorithm>
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
#include "common/prefix.h"
|
||||
#include "tools/cabana/streams/abstractstream.h"
|
||||
#include "tools/replay/replay.h"
|
||||
|
||||
class ReplayStream : public AbstractStream {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
ReplayStream(QObject *parent);
|
||||
void start() override;
|
||||
bool loadRoute(const QString &route, const QString &data_dir, uint32_t replay_flags = REPLAY_FLAG_NONE);
|
||||
bool eventFilter(const Event *event);
|
||||
void seekTo(double ts) override { replay->seekTo(std::max(double(0), ts), false); }
|
||||
bool liveStreaming() const override { return false; }
|
||||
inline QString routeName() const override { return replay->route()->name(); }
|
||||
inline QString carFingerprint() const override { return replay->carFingerprint().c_str(); }
|
||||
double totalSeconds() const override { return replay->totalSeconds(); }
|
||||
inline QDateTime beginDateTime() const { return replay->route()->datetime(); }
|
||||
inline double routeStartTime() const override { return replay->routeStartTime() / (double)1e9; }
|
||||
inline const Route *route() const { return replay->route(); }
|
||||
inline void setSpeed(float speed) override { replay->setSpeed(speed); }
|
||||
inline float getSpeed() const { return replay->getSpeed(); }
|
||||
inline Replay *getReplay() const { return replay.get(); }
|
||||
inline bool isPaused() const override { return replay->isPaused(); }
|
||||
void pause(bool pause) override;
|
||||
static AbstractOpenStreamWidget *widget(AbstractStream **stream);
|
||||
|
||||
signals:
|
||||
void qLogLoaded(int segnum, std::shared_ptr<LogReader> qlog);
|
||||
|
||||
private:
|
||||
void mergeSegments();
|
||||
std::unique_ptr<Replay> replay = nullptr;
|
||||
std::set<int> processed_segments;
|
||||
std::unique_ptr<OpenpilotPrefix> op_prefix;
|
||||
};
|
||||
|
||||
class OpenReplayWidget : public AbstractOpenStreamWidget {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
OpenReplayWidget(AbstractStream **stream);
|
||||
bool open() override;
|
||||
QString title() override { return tr("&Replay"); }
|
||||
|
||||
private:
|
||||
QLineEdit *route_edit;
|
||||
std::vector<QCheckBox *> cameras;
|
||||
};
|
||||
115
tools/cabana/streams/socketcanstream.cc
Normal file
115
tools/cabana/streams/socketcanstream.cc
Normal file
@@ -0,0 +1,115 @@
|
||||
#include "tools/cabana/streams/socketcanstream.h"
|
||||
|
||||
#include <QDebug>
|
||||
#include <QFormLayout>
|
||||
#include <QHBoxLayout>
|
||||
#include <QMessageBox>
|
||||
#include <QPushButton>
|
||||
#include <QThread>
|
||||
|
||||
SocketCanStream::SocketCanStream(QObject *parent, SocketCanStreamConfig config_) : config(config_), LiveStream(parent) {
|
||||
if (!available()) {
|
||||
throw std::runtime_error("SocketCAN plugin not available");
|
||||
}
|
||||
|
||||
qDebug() << "Connecting to SocketCAN device" << config.device;
|
||||
if (!connect()) {
|
||||
throw std::runtime_error("Failed to connect to SocketCAN device");
|
||||
}
|
||||
}
|
||||
|
||||
bool SocketCanStream::available() {
|
||||
return QCanBus::instance()->plugins().contains("socketcan");
|
||||
}
|
||||
|
||||
bool SocketCanStream::connect() {
|
||||
// Connecting might generate some warnings about missing socketcan/libsocketcan libraries
|
||||
// These are expected and can be ignored, we don't need the advanced features of libsocketcan
|
||||
QString errorString;
|
||||
device.reset(QCanBus::instance()->createDevice("socketcan", config.device, &errorString));
|
||||
|
||||
if (!device) {
|
||||
qDebug() << "Failed to create SocketCAN device" << errorString;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!device->connectDevice()) {
|
||||
qDebug() << "Failed to connect to device";
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SocketCanStream::streamThread() {
|
||||
while (!QThread::currentThread()->isInterruptionRequested()) {
|
||||
QThread::msleep(1);
|
||||
|
||||
auto frames = device->readAllFrames();
|
||||
if (frames.size() == 0) continue;
|
||||
|
||||
MessageBuilder msg;
|
||||
auto evt = msg.initEvent();
|
||||
auto canData = evt.initCan(frames.size());
|
||||
|
||||
for (uint i = 0; i < frames.size(); i++) {
|
||||
if (!frames[i].isValid()) continue;
|
||||
|
||||
canData[i].setAddress(frames[i].frameId());
|
||||
canData[i].setSrc(0);
|
||||
|
||||
auto payload = frames[i].payload();
|
||||
canData[i].setDat(kj::arrayPtr((uint8_t*)payload.data(), payload.size()));
|
||||
}
|
||||
|
||||
handleEvent(capnp::messageToFlatArray(msg));
|
||||
}
|
||||
}
|
||||
|
||||
AbstractOpenStreamWidget *SocketCanStream::widget(AbstractStream **stream) {
|
||||
return new OpenSocketCanWidget(stream);
|
||||
}
|
||||
|
||||
OpenSocketCanWidget::OpenSocketCanWidget(AbstractStream **stream) : AbstractOpenStreamWidget(stream) {
|
||||
QVBoxLayout *main_layout = new QVBoxLayout(this);
|
||||
main_layout->addStretch(1);
|
||||
|
||||
QFormLayout *form_layout = new QFormLayout();
|
||||
|
||||
QHBoxLayout *device_layout = new QHBoxLayout();
|
||||
device_edit = new QComboBox();
|
||||
device_edit->setFixedWidth(300);
|
||||
device_layout->addWidget(device_edit);
|
||||
|
||||
QPushButton *refresh = new QPushButton(tr("Refresh"));
|
||||
refresh->setFixedWidth(100);
|
||||
device_layout->addWidget(refresh);
|
||||
form_layout->addRow(tr("Device"), device_layout);
|
||||
main_layout->addLayout(form_layout);
|
||||
|
||||
main_layout->addStretch(1);
|
||||
|
||||
QObject::connect(refresh, &QPushButton::clicked, this, &OpenSocketCanWidget::refreshDevices);
|
||||
QObject::connect(device_edit, &QComboBox::currentTextChanged, this, [=]{ config.device = device_edit->currentText(); });
|
||||
|
||||
// Populate devices
|
||||
refreshDevices();
|
||||
}
|
||||
|
||||
void OpenSocketCanWidget::refreshDevices() {
|
||||
device_edit->clear();
|
||||
for (auto device : QCanBus::instance()->availableDevices(QStringLiteral("socketcan"))) {
|
||||
device_edit->addItem(device.name());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool OpenSocketCanWidget::open() {
|
||||
try {
|
||||
*stream = new SocketCanStream(qApp, config);
|
||||
} catch (std::exception &e) {
|
||||
QMessageBox::warning(nullptr, tr("Warning"), tr("Failed to connect to SocketCAN device: '%1'").arg(e.what()));
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
48
tools/cabana/streams/socketcanstream.h
Normal file
48
tools/cabana/streams/socketcanstream.h
Normal file
@@ -0,0 +1,48 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <QtSerialBus/QCanBus>
|
||||
#include <QtSerialBus/QCanBusDevice>
|
||||
#include <QtSerialBus/QCanBusDeviceInfo>
|
||||
#include <QComboBox>
|
||||
|
||||
#include "tools/cabana/streams/livestream.h"
|
||||
|
||||
struct SocketCanStreamConfig {
|
||||
QString device = ""; // TODO: support multiple devices/buses at once
|
||||
};
|
||||
|
||||
class SocketCanStream : public LiveStream {
|
||||
Q_OBJECT
|
||||
public:
|
||||
SocketCanStream(QObject *parent, SocketCanStreamConfig config_ = {});
|
||||
static AbstractOpenStreamWidget *widget(AbstractStream **stream);
|
||||
static bool available();
|
||||
|
||||
inline QString routeName() const override {
|
||||
return QString("Live Streaming From Socket CAN %1").arg(config.device);
|
||||
}
|
||||
|
||||
protected:
|
||||
void streamThread() override;
|
||||
bool connect();
|
||||
|
||||
SocketCanStreamConfig config = {};
|
||||
std::unique_ptr<QCanBusDevice> device;
|
||||
};
|
||||
|
||||
class OpenSocketCanWidget : public AbstractOpenStreamWidget {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
OpenSocketCanWidget(AbstractStream **stream);
|
||||
bool open() override;
|
||||
QString title() override { return tr("&SocketCAN"); }
|
||||
|
||||
private:
|
||||
void refreshDevices();
|
||||
|
||||
QComboBox *device_edit;
|
||||
SocketCanStreamConfig config = {};
|
||||
};
|
||||
Reference in New Issue
Block a user