|
|
@ -4,7 +4,6 @@ |
|
|
|
#include <csignal> |
|
|
|
#include <csignal> |
|
|
|
#include "cereal/services.h" |
|
|
|
#include "cereal/services.h" |
|
|
|
#include "common/params.h" |
|
|
|
#include "common/params.h" |
|
|
|
#include "common/timing.h" |
|
|
|
|
|
|
|
#include "tools/replay/util.h" |
|
|
|
#include "tools/replay/util.h" |
|
|
|
|
|
|
|
|
|
|
|
static void interrupt_sleep_handler(int signal) {} |
|
|
|
static void interrupt_sleep_handler(int signal) {} |
|
|
@ -12,145 +11,124 @@ static void interrupt_sleep_handler(int signal) {} |
|
|
|
// Helper function to notify events with safety checks
|
|
|
|
// Helper function to notify events with safety checks
|
|
|
|
template <typename Callback, typename... Args> |
|
|
|
template <typename Callback, typename... Args> |
|
|
|
void notifyEvent(Callback &callback, Args &&...args) { |
|
|
|
void notifyEvent(Callback &callback, Args &&...args) { |
|
|
|
if (callback) { |
|
|
|
if (callback) callback(std::forward<Args>(args)...); |
|
|
|
callback(std::forward<Args>(args)...); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
Replay::Replay(const std::string &route, std::vector<std::string> allow, std::vector<std::string> block, SubMaster *sm_, |
|
|
|
Replay::Replay(const std::string &route, std::vector<std::string> allow, std::vector<std::string> block, |
|
|
|
uint32_t flags, const std::string &data_dir, QObject *parent) : sm(sm_), flags_(flags), QObject(parent) { |
|
|
|
SubMaster *sm, uint32_t flags, const std::string &data_dir) |
|
|
|
// Register signal handler for SIGUSR1
|
|
|
|
: sm_(sm), flags_(flags), seg_mgr_(std::make_unique<SegmentManager>(route, flags, data_dir)) { |
|
|
|
std::signal(SIGUSR1, interrupt_sleep_handler); |
|
|
|
std::signal(SIGUSR1, interrupt_sleep_handler); |
|
|
|
|
|
|
|
|
|
|
|
if (!(flags_ & REPLAY_FLAG_ALL_SERVICES)) { |
|
|
|
if (!(flags_ & REPLAY_FLAG_ALL_SERVICES)) { |
|
|
|
block.insert(block.end(), {"uiDebug", "userFlag"}); |
|
|
|
block.insert(block.end(), {"uiDebug", "userFlag"}); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
setupServices(allow, block); |
|
|
|
|
|
|
|
setupSegmentManager(!allow.empty() || !block.empty()); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void Replay::setupServices(const std::vector<std::string> &allow, const std::vector<std::string> &block) { |
|
|
|
auto event_schema = capnp::Schema::from<cereal::Event>().asStruct(); |
|
|
|
auto event_schema = capnp::Schema::from<cereal::Event>().asStruct(); |
|
|
|
sockets_.resize(event_schema.getUnionFields().size()); |
|
|
|
sockets_.resize(event_schema.getUnionFields().size(), nullptr); |
|
|
|
std::vector<std::string> active_services; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
std::vector<const char *> active_services; |
|
|
|
for (const auto &[name, _] : services) { |
|
|
|
for (const auto &[name, _] : services) { |
|
|
|
bool in_block = std::find(block.begin(), block.end(), name) != block.end(); |
|
|
|
bool is_blocked = std::find(block.begin(), block.end(), name) != block.end(); |
|
|
|
bool in_allow = std::find(allow.begin(), allow.end(), name) != allow.end(); |
|
|
|
bool is_allowed = allow.empty() || std::find(allow.begin(), allow.end(), name) != allow.end(); |
|
|
|
if (!in_block && (allow.empty() || in_allow)) { |
|
|
|
if (is_allowed && !is_blocked) { |
|
|
|
uint16_t which = event_schema.getFieldByName(name).getProto().getDiscriminantValue(); |
|
|
|
uint16_t which = event_schema.getFieldByName(name).getProto().getDiscriminantValue(); |
|
|
|
sockets_[which] = name.c_str(); |
|
|
|
sockets_[which] = name.c_str(); |
|
|
|
active_services.push_back(name); |
|
|
|
active_services.push_back(name.c_str()); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
rInfo("active services: %s", join(active_services, ", ").c_str()); |
|
|
|
if (!allow.empty()) { |
|
|
|
if (!sm_) { |
|
|
|
for (int i = 0; i < sockets_.size(); ++i) { |
|
|
|
pm_ = std::make_unique<PubMaster>(active_services); |
|
|
|
filters_.push_back(i == cereal::Event::Which::INIT_DATA || i == cereal::Event::Which::CAR_PARAMS || sockets_[i]); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
rInfo("active services: %s", join(active_services, ", ").c_str()); |
|
|
|
void Replay::setupSegmentManager(bool has_filters) { |
|
|
|
rInfo("loading route %s", route.c_str()); |
|
|
|
seg_mgr_->setCallback([this]() { handleSegmentMerge(); }); |
|
|
|
|
|
|
|
|
|
|
|
if (sm == nullptr) { |
|
|
|
if (has_filters) { |
|
|
|
std::vector<const char *> socket_names; |
|
|
|
std::vector<bool> filters(sockets_.size(), false); |
|
|
|
std::copy_if(sockets_.begin(), sockets_.end(), std::back_inserter(socket_names), |
|
|
|
for (size_t i = 0; i < sockets_.size(); ++i) { |
|
|
|
[](const char *name) { return name != nullptr; }); |
|
|
|
filters[i] = (i == cereal::Event::Which::INIT_DATA || i == cereal::Event::Which::CAR_PARAMS || sockets_[i]); |
|
|
|
pm = std::make_unique<PubMaster>(socket_names); |
|
|
|
} |
|
|
|
|
|
|
|
seg_mgr_->setFilters(filters); |
|
|
|
} |
|
|
|
} |
|
|
|
route_ = std::make_unique<Route>(route, data_dir); |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
Replay::~Replay() { |
|
|
|
Replay::~Replay() { |
|
|
|
stop(); |
|
|
|
seg_mgr_.reset(); |
|
|
|
} |
|
|
|
if (stream_thread_.joinable()) { |
|
|
|
|
|
|
|
|
|
|
|
void Replay::stop() { |
|
|
|
|
|
|
|
exit_ = true; |
|
|
|
|
|
|
|
if (stream_thread_ != nullptr) { |
|
|
|
|
|
|
|
rInfo("shutdown: in progress..."); |
|
|
|
rInfo("shutdown: in progress..."); |
|
|
|
pauseStreamThread(); |
|
|
|
interruptStream([this]() { |
|
|
|
stream_cv_.notify_one(); |
|
|
|
exit_ = true; |
|
|
|
stream_thread_->quit(); |
|
|
|
return false; |
|
|
|
stream_thread_->wait(); |
|
|
|
}); |
|
|
|
stream_thread_->deleteLater(); |
|
|
|
stream_thread_.join(); |
|
|
|
stream_thread_ = nullptr; |
|
|
|
|
|
|
|
rInfo("shutdown: done"); |
|
|
|
rInfo("shutdown: done"); |
|
|
|
} |
|
|
|
} |
|
|
|
camera_server_.reset(nullptr); |
|
|
|
camera_server_.reset(); |
|
|
|
segments_.clear(); |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
bool Replay::load() { |
|
|
|
bool Replay::load() { |
|
|
|
if (!route_->load()) { |
|
|
|
rInfo("loading route %s", seg_mgr_->route_.name().c_str()); |
|
|
|
rError("failed to load route %s from %s", route_->name().c_str(), |
|
|
|
if (!seg_mgr_->load()) return false; |
|
|
|
route_->dir().empty() ? "server" : route_->dir().c_str()); |
|
|
|
|
|
|
|
return false; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
for (auto &[n, f] : route_->segments()) { |
|
|
|
min_seconds_ = seg_mgr_->route_.segments().begin()->first * 60; |
|
|
|
bool has_log = !f.rlog.empty() || !f.qlog.empty(); |
|
|
|
max_seconds_ = (seg_mgr_->route_.segments().rbegin()->first + 1) * 60; |
|
|
|
bool has_video = !f.road_cam.empty() || !f.qcamera.empty(); |
|
|
|
|
|
|
|
if (has_log && (has_video || hasFlag(REPLAY_FLAG_NO_VIPC))) { |
|
|
|
|
|
|
|
segments_.insert({n, nullptr}); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
if (segments_.empty()) { |
|
|
|
|
|
|
|
rInfo("no valid segments in route: %s", route_->name().c_str()); |
|
|
|
|
|
|
|
return false; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
rInfo("load route %s with %zu valid segments", route_->name().c_str(), segments_.size()); |
|
|
|
|
|
|
|
max_seconds_ = (segments_.rbegin()->first + 1) * 60; |
|
|
|
|
|
|
|
return true; |
|
|
|
return true; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void Replay::start(int seconds) { |
|
|
|
void Replay::interruptStream(const std::function<bool()> &update_fn) { |
|
|
|
seekTo(route_->identifier().begin_segment * 60 + seconds, false); |
|
|
|
if (stream_thread_.joinable() && stream_thread_id) { |
|
|
|
} |
|
|
|
pthread_kill(stream_thread_id, SIGUSR1); // Interrupt sleep in stream thread
|
|
|
|
|
|
|
|
} |
|
|
|
void Replay::updateEvents(const std::function<bool()> &update_events_function) { |
|
|
|
|
|
|
|
pauseStreamThread(); |
|
|
|
|
|
|
|
{ |
|
|
|
{ |
|
|
|
std::unique_lock lk(stream_lock_); |
|
|
|
interrupt_requested_ = true; |
|
|
|
events_ready_ = update_events_function(); |
|
|
|
std::unique_lock lock(stream_lock_); |
|
|
|
paused_ = user_paused_; |
|
|
|
events_ready_ = update_fn(); |
|
|
|
|
|
|
|
interrupt_requested_ = user_paused_; |
|
|
|
} |
|
|
|
} |
|
|
|
stream_cv_.notify_one(); |
|
|
|
stream_cv_.notify_one(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void Replay::seekTo(double seconds, bool relative) { |
|
|
|
void Replay::seekTo(double seconds, bool relative) { |
|
|
|
updateEvents([&]() { |
|
|
|
double target_time = relative ? seconds + currentSeconds() : seconds; |
|
|
|
double target_time = relative ? seconds + currentSeconds() : seconds; |
|
|
|
target_time = std::max(0.0, target_time); |
|
|
|
target_time = std::max(double(0.0), target_time); |
|
|
|
int target_segment = target_time / 60; |
|
|
|
int target_segment = (int)target_time / 60; |
|
|
|
if (!seg_mgr_->hasSegment(target_segment)) { |
|
|
|
if (segments_.count(target_segment) == 0) { |
|
|
|
rWarning("Invalid seek to %.2f s (segment %d)", target_time, target_segment); |
|
|
|
rWarning("Can't seek to %.2f s segment %d is invalid", target_time, target_segment); |
|
|
|
return; |
|
|
|
return true; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
if (target_time > max_seconds_) { |
|
|
|
|
|
|
|
rWarning("Can't seek to %.2f s, time is invalid", target_time); |
|
|
|
|
|
|
|
return true; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
rInfo("Seeking to %d s, segment %d", (int)target_time, target_segment); |
|
|
|
rInfo("Seeking to %d s, segment %d", (int)target_time, target_segment); |
|
|
|
|
|
|
|
notifyEvent(onSeeking, target_time); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
double seeked_to_sec = -1; |
|
|
|
|
|
|
|
interruptStream([&]() { |
|
|
|
current_segment_ = target_segment; |
|
|
|
current_segment_ = target_segment; |
|
|
|
cur_mono_time_ = route_start_ts_ + target_time * 1e9; |
|
|
|
cur_mono_time_ = route_start_ts_ + target_time * 1e9; |
|
|
|
seeking_to_ = target_time; |
|
|
|
seeking_to_ = target_time; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (event_data_->isSegmentLoaded(target_segment)) { |
|
|
|
|
|
|
|
seeked_to_sec = *seeking_to_; |
|
|
|
|
|
|
|
seeking_to_.reset(); |
|
|
|
|
|
|
|
} |
|
|
|
return false; |
|
|
|
return false; |
|
|
|
}); |
|
|
|
}); |
|
|
|
|
|
|
|
|
|
|
|
checkSeekProgress(); |
|
|
|
checkSeekProgress(seeked_to_sec); |
|
|
|
updateSegmentsCache(); |
|
|
|
seg_mgr_->setCurrentSegment(target_segment); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void Replay::checkSeekProgress() { |
|
|
|
void Replay::checkSeekProgress(double seeked_to_sec) { |
|
|
|
if (seeking_to_) { |
|
|
|
if (seeked_to_sec >= 0) { |
|
|
|
auto it = segments_.find(int(*seeking_to_ / 60)); |
|
|
|
if (onSeekedTo) { |
|
|
|
if (it != segments_.end() && it->second && it->second->isLoaded()) { |
|
|
|
onSeekedTo(seeked_to_sec); |
|
|
|
emit seekedTo(*seeking_to_); |
|
|
|
|
|
|
|
seeking_to_ = std::nullopt; |
|
|
|
|
|
|
|
// wake up stream thread
|
|
|
|
|
|
|
|
updateEvents([]() { return true; }); |
|
|
|
|
|
|
|
} else { |
|
|
|
} else { |
|
|
|
// Emit signal indicating the ongoing seek operation
|
|
|
|
interruptStream([]() { return true; }); |
|
|
|
emit seeking(*seeking_to_); |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
@ -163,125 +141,45 @@ void Replay::seekToFlag(FindFlag flag) { |
|
|
|
|
|
|
|
|
|
|
|
void Replay::pause(bool pause) { |
|
|
|
void Replay::pause(bool pause) { |
|
|
|
if (user_paused_ != pause) { |
|
|
|
if (user_paused_ != pause) { |
|
|
|
pauseStreamThread(); |
|
|
|
interruptStream([=]() { |
|
|
|
{ |
|
|
|
|
|
|
|
std::unique_lock lk(stream_lock_); |
|
|
|
|
|
|
|
rWarning("%s at %.2f s", pause ? "paused..." : "resuming", currentSeconds()); |
|
|
|
rWarning("%s at %.2f s", pause ? "paused..." : "resuming", currentSeconds()); |
|
|
|
paused_ = user_paused_ = pause; |
|
|
|
user_paused_ = pause; |
|
|
|
} |
|
|
|
return !pause; |
|
|
|
stream_cv_.notify_one(); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void Replay::pauseStreamThread() { |
|
|
|
|
|
|
|
paused_ = true; |
|
|
|
|
|
|
|
// Send SIGUSR1 to interrupt clock_nanosleep
|
|
|
|
|
|
|
|
if (stream_thread_ && stream_thread_id) { |
|
|
|
|
|
|
|
pthread_kill(stream_thread_id, SIGUSR1); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void Replay::segmentLoadFinished(int seg_num, bool success) { |
|
|
|
|
|
|
|
if (!success) { |
|
|
|
|
|
|
|
rWarning("failed to load segment %d, removing it from current replay list", seg_num); |
|
|
|
|
|
|
|
updateEvents([&]() { |
|
|
|
|
|
|
|
segments_.erase(seg_num); |
|
|
|
|
|
|
|
return !segments_.empty(); |
|
|
|
|
|
|
|
}); |
|
|
|
}); |
|
|
|
} |
|
|
|
} |
|
|
|
QMetaObject::invokeMethod(this, &Replay::updateSegmentsCache, Qt::QueuedConnection); |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void Replay::updateSegmentsCache() { |
|
|
|
void Replay::handleSegmentMerge() { |
|
|
|
auto cur = segments_.lower_bound(current_segment_.load()); |
|
|
|
if (exit_) return; |
|
|
|
if (cur == segments_.end()) return; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Calculate the range of segments to load
|
|
|
|
|
|
|
|
auto begin = std::prev(cur, std::min<int>(segment_cache_limit / 2, std::distance(segments_.begin(), cur))); |
|
|
|
|
|
|
|
auto end = std::next(begin, std::min<int>(segment_cache_limit, std::distance(begin, segments_.end()))); |
|
|
|
|
|
|
|
begin = std::prev(end, std::min<int>(segment_cache_limit, std::distance(segments_.begin(), end))); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
loadSegmentInRange(begin, cur, end); |
|
|
|
|
|
|
|
mergeSegments(begin, end); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// free segments out of current semgnt window.
|
|
|
|
double seeked_to_sec = -1; |
|
|
|
std::for_each(segments_.begin(), begin, [](auto &e) { e.second.reset(nullptr); }); |
|
|
|
interruptStream([&]() { |
|
|
|
std::for_each(end, segments_.end(), [](auto &e) { e.second.reset(nullptr); }); |
|
|
|
event_data_ = seg_mgr_->getEventData(); |
|
|
|
|
|
|
|
notifyEvent(onSegmentsMerged); |
|
|
|
|
|
|
|
|
|
|
|
// start stream thread
|
|
|
|
bool segment_loaded = event_data_->isSegmentLoaded(current_segment_); |
|
|
|
const auto &cur_segment = cur->second; |
|
|
|
if (seeking_to_ && segment_loaded) { |
|
|
|
if (stream_thread_ == nullptr && cur_segment->isLoaded()) { |
|
|
|
seeked_to_sec = *seeking_to_; |
|
|
|
startStream(cur_segment.get()); |
|
|
|
seeking_to_.reset(); |
|
|
|
} |
|
|
|
return false; |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void Replay::loadSegmentInRange(SegmentMap::iterator begin, SegmentMap::iterator cur, SegmentMap::iterator end) { |
|
|
|
|
|
|
|
auto loadNextSegment = [this](auto first, auto last) { |
|
|
|
|
|
|
|
auto it = std::find_if(first, last, [](const auto &seg_it) { return !seg_it.second || !seg_it.second->isLoaded(); }); |
|
|
|
|
|
|
|
if (it != last && !it->second) { |
|
|
|
|
|
|
|
rDebug("loading segment %d...", it->first); |
|
|
|
|
|
|
|
it->second = std::make_unique<Segment>(it->first, route_->at(it->first), flags_, filters_, |
|
|
|
|
|
|
|
[this](int seg_num, bool success) { |
|
|
|
|
|
|
|
segmentLoadFinished(seg_num, success); |
|
|
|
|
|
|
|
}); |
|
|
|
|
|
|
|
return true; |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
return false; |
|
|
|
return segment_loaded; |
|
|
|
}; |
|
|
|
}); |
|
|
|
|
|
|
|
|
|
|
|
// Try loading forward segments, then reverse segments
|
|
|
|
|
|
|
|
if (!loadNextSegment(cur, end)) { |
|
|
|
|
|
|
|
loadNextSegment(std::make_reverse_iterator(cur), std::make_reverse_iterator(begin)); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap::iterator &end) { |
|
|
|
|
|
|
|
std::set<int> segments_to_merge; |
|
|
|
|
|
|
|
size_t new_events_size = 0; |
|
|
|
|
|
|
|
for (auto it = begin; it != end; ++it) { |
|
|
|
|
|
|
|
if (it->second && it->second->isLoaded()) { |
|
|
|
|
|
|
|
segments_to_merge.insert(it->first); |
|
|
|
|
|
|
|
new_events_size += it->second->log->events.size(); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (segments_to_merge == merged_segments_) return; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
rDebug("merge segments %s", std::accumulate(segments_to_merge.begin(), segments_to_merge.end(), std::string{}, |
|
|
|
|
|
|
|
[](auto & a, int b) { return a + (a.empty() ? "" : ", ") + std::to_string(b); }).c_str()); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
std::vector<Event> new_events; |
|
|
|
|
|
|
|
new_events.reserve(new_events_size); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Merge events from segments_to_merge into new_events
|
|
|
|
|
|
|
|
for (int n : segments_to_merge) { |
|
|
|
|
|
|
|
size_t size = new_events.size(); |
|
|
|
|
|
|
|
const auto &events = segments_.at(n)->log->events; |
|
|
|
|
|
|
|
std::copy_if(events.begin(), events.end(), std::back_inserter(new_events), |
|
|
|
|
|
|
|
[this](const Event &e) { return e.which < sockets_.size() && sockets_[e.which] != nullptr; }); |
|
|
|
|
|
|
|
std::inplace_merge(new_events.begin(), new_events.begin() + size, new_events.end()); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (stream_thread_) { |
|
|
|
checkSeekProgress(seeked_to_sec); |
|
|
|
emit segmentsMerged(); |
|
|
|
if (!stream_thread_.joinable() && !event_data_->events.empty()) { |
|
|
|
|
|
|
|
startStream(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
updateEvents([&]() { |
|
|
|
|
|
|
|
events_.swap(new_events); |
|
|
|
|
|
|
|
merged_segments_ = segments_to_merge; |
|
|
|
|
|
|
|
// Wake up the stream thread if the current segment is loaded or invalid.
|
|
|
|
|
|
|
|
return !seeking_to_ && (isSegmentMerged(current_segment_) || (segments_.count(current_segment_) == 0)); |
|
|
|
|
|
|
|
}); |
|
|
|
|
|
|
|
checkSeekProgress(); |
|
|
|
|
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void Replay::startStream(const Segment *cur_segment) { |
|
|
|
void Replay::startStream() { |
|
|
|
|
|
|
|
const auto &cur_segment = event_data_->segments.begin()->second; |
|
|
|
const auto &events = cur_segment->log->events; |
|
|
|
const auto &events = cur_segment->log->events; |
|
|
|
route_start_ts_ = events.front().mono_time; |
|
|
|
route_start_ts_ = events.front().mono_time; |
|
|
|
cur_mono_time_ += route_start_ts_ - 1; |
|
|
|
cur_mono_time_ += route_start_ts_ - 1; |
|
|
|
|
|
|
|
|
|
|
|
// get datetime from INIT_DATA, fallback to datetime in the route name
|
|
|
|
// get datetime from INIT_DATA, fallback to datetime in the route name
|
|
|
|
route_date_time_ = route()->datetime(); |
|
|
|
route_date_time_ = route().datetime(); |
|
|
|
auto it = std::find_if(events.cbegin(), events.cend(), |
|
|
|
auto it = std::find_if(events.cbegin(), events.cend(), |
|
|
|
[](const Event &e) { return e.which == cereal::Event::Which::INIT_DATA; }); |
|
|
|
[](const Event &e) { return e.which == cereal::Event::Which::INIT_DATA; }); |
|
|
|
if (it != events.cend()) { |
|
|
|
if (it != events.cend()) { |
|
|
@ -299,6 +197,7 @@ void Replay::startStream(const Segment *cur_segment) { |
|
|
|
capnp::FlatArrayMessageReader reader(it->data); |
|
|
|
capnp::FlatArrayMessageReader reader(it->data); |
|
|
|
auto event = reader.getRoot<cereal::Event>(); |
|
|
|
auto event = reader.getRoot<cereal::Event>(); |
|
|
|
car_fingerprint_ = event.getCarParams().getCarFingerprint(); |
|
|
|
car_fingerprint_ = event.getCarParams().getCarFingerprint(); |
|
|
|
|
|
|
|
|
|
|
|
capnp::MallocMessageBuilder builder; |
|
|
|
capnp::MallocMessageBuilder builder; |
|
|
|
builder.setRoot(event.getCarParams()); |
|
|
|
builder.setRoot(event.getCarParams()); |
|
|
|
auto words = capnp::messageToFlatArray(builder); |
|
|
|
auto words = capnp::messageToFlatArray(builder); |
|
|
@ -320,26 +219,18 @@ void Replay::startStream(const Segment *cur_segment) { |
|
|
|
camera_server_ = std::make_unique<CameraServer>(camera_size); |
|
|
|
camera_server_ = std::make_unique<CameraServer>(camera_size); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
emit segmentsMerged(); |
|
|
|
timeline_.initialize(seg_mgr_->route_, route_start_ts_, !(flags_ & REPLAY_FLAG_NO_FILE_CACHE), |
|
|
|
|
|
|
|
[this](std::shared_ptr<LogReader> log) { notifyEvent(onQLogLoaded, log); }); |
|
|
|
timeline_.initialize(*route_, route_start_ts_, !(flags_ & REPLAY_FLAG_NO_FILE_CACHE), |
|
|
|
|
|
|
|
[this](std::shared_ptr<LogReader> log) { |
|
|
|
|
|
|
|
notifyEvent(onQLogLoaded, log); |
|
|
|
|
|
|
|
}); |
|
|
|
|
|
|
|
// start stream thread
|
|
|
|
|
|
|
|
stream_thread_ = new QThread(); |
|
|
|
|
|
|
|
QObject::connect(stream_thread_, &QThread::started, [=]() { streamThread(); }); |
|
|
|
|
|
|
|
stream_thread_->start(); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
emit streamStarted(); |
|
|
|
stream_thread_ = std::thread(&Replay::streamThread, this); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void Replay::publishMessage(const Event *e) { |
|
|
|
void Replay::publishMessage(const Event *e) { |
|
|
|
if (event_filter && event_filter(e, filter_opaque)) return; |
|
|
|
if (event_filter_ && event_filter_(e)) return; |
|
|
|
|
|
|
|
|
|
|
|
if (sm == nullptr) { |
|
|
|
if (!sm_) { |
|
|
|
auto bytes = e->data.asBytes(); |
|
|
|
auto bytes = e->data.asBytes(); |
|
|
|
int ret = pm->send(sockets_[e->which], (capnp::byte *)bytes.begin(), bytes.size()); |
|
|
|
int ret = pm_->send(sockets_[e->which], (capnp::byte *)bytes.begin(), bytes.size()); |
|
|
|
if (ret == -1) { |
|
|
|
if (ret == -1) { |
|
|
|
rWarning("stop publishing %s due to multiple publishers error", sockets_[e->which]); |
|
|
|
rWarning("stop publishing %s due to multiple publishers error", sockets_[e->which]); |
|
|
|
sockets_[e->which] = nullptr; |
|
|
|
sockets_[e->which] = nullptr; |
|
|
@ -347,7 +238,7 @@ void Replay::publishMessage(const Event *e) { |
|
|
|
} else { |
|
|
|
} else { |
|
|
|
capnp::FlatArrayMessageReader reader(e->data); |
|
|
|
capnp::FlatArrayMessageReader reader(e->data); |
|
|
|
auto event = reader.getRoot<cereal::Event>(); |
|
|
|
auto event = reader.getRoot<cereal::Event>(); |
|
|
|
sm->update_msgs(nanos_since_boot(), {{sockets_[e->which], event}}); |
|
|
|
sm_->update_msgs(nanos_since_boot(), {{sockets_[e->which], event}}); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
@ -363,9 +254,9 @@ void Replay::publishFrame(const Event *e) { |
|
|
|
if ((cam == DriverCam && !hasFlag(REPLAY_FLAG_DCAM)) || (cam == WideRoadCam && !hasFlag(REPLAY_FLAG_ECAM))) |
|
|
|
if ((cam == DriverCam && !hasFlag(REPLAY_FLAG_DCAM)) || (cam == WideRoadCam && !hasFlag(REPLAY_FLAG_ECAM))) |
|
|
|
return; // Camera isdisabled
|
|
|
|
return; // Camera isdisabled
|
|
|
|
|
|
|
|
|
|
|
|
if (isSegmentMerged(e->eidx_segnum)) { |
|
|
|
auto seg_it = event_data_->segments.find(e->eidx_segnum); |
|
|
|
auto &segment = segments_.at(e->eidx_segnum); |
|
|
|
if (seg_it != event_data_->segments.end()) { |
|
|
|
if (auto &frame = segment->frames[cam]; frame) { |
|
|
|
if (auto &frame = seg_it->second->frames[cam]; frame) { |
|
|
|
camera_server_->pushFrame(cam, frame.get(), e); |
|
|
|
camera_server_->pushFrame(cam, frame.get(), e); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
@ -377,32 +268,33 @@ void Replay::streamThread() { |
|
|
|
std::unique_lock lk(stream_lock_); |
|
|
|
std::unique_lock lk(stream_lock_); |
|
|
|
|
|
|
|
|
|
|
|
while (true) { |
|
|
|
while (true) { |
|
|
|
stream_cv_.wait(lk, [=]() { return exit_ || ( events_ready_ && !paused_); }); |
|
|
|
stream_cv_.wait(lk, [this]() { return exit_ || (events_ready_ && !interrupt_requested_); }); |
|
|
|
if (exit_) break; |
|
|
|
if (exit_) break; |
|
|
|
|
|
|
|
|
|
|
|
Event event(cur_which, cur_mono_time_, {}); |
|
|
|
const auto &events = event_data_->events; |
|
|
|
auto first = std::upper_bound(events_.cbegin(), events_.cend(), event); |
|
|
|
auto first = std::upper_bound(events.cbegin(), events.cend(), Event(cur_which, cur_mono_time_, {})); |
|
|
|
if (first == events_.cend()) { |
|
|
|
if (first == events.cend()) { |
|
|
|
rInfo("waiting for events..."); |
|
|
|
rInfo("waiting for events..."); |
|
|
|
events_ready_ = false; |
|
|
|
events_ready_ = false; |
|
|
|
continue; |
|
|
|
continue; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
auto it = publishEvents(first, events_.cend()); |
|
|
|
auto it = publishEvents(first, events.cend()); |
|
|
|
|
|
|
|
|
|
|
|
// Ensure frames are sent before unlocking to prevent race conditions
|
|
|
|
// Ensure frames are sent before unlocking to prevent race conditions
|
|
|
|
if (camera_server_) { |
|
|
|
if (camera_server_) { |
|
|
|
camera_server_->waitForSent(); |
|
|
|
camera_server_->waitForSent(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
if (it != events_.cend()) { |
|
|
|
if (it != events.cend()) { |
|
|
|
cur_which = it->which; |
|
|
|
cur_which = it->which; |
|
|
|
} else if (!hasFlag(REPLAY_FLAG_NO_LOOP)) { |
|
|
|
} else if (!hasFlag(REPLAY_FLAG_NO_LOOP)) { |
|
|
|
// Check for loop end and restart if necessary
|
|
|
|
int last_segment = seg_mgr_->route_.segments().rbegin()->first; |
|
|
|
int last_segment = segments_.rbegin()->first; |
|
|
|
if (event_data_->isSegmentLoaded(last_segment)) { |
|
|
|
if (current_segment_ >= last_segment && isSegmentMerged(last_segment)) { |
|
|
|
|
|
|
|
rInfo("reaches the end of route, restart from beginning"); |
|
|
|
rInfo("reaches the end of route, restart from beginning"); |
|
|
|
QMetaObject::invokeMethod(this, std::bind(&Replay::seekTo, this, minSeconds(), false), Qt::QueuedConnection); |
|
|
|
stream_lock_.unlock(); |
|
|
|
|
|
|
|
seekTo(minSeconds(), false); |
|
|
|
|
|
|
|
stream_lock_.lock(); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
@ -414,16 +306,16 @@ std::vector<Event>::const_iterator Replay::publishEvents(std::vector<Event>::con |
|
|
|
uint64_t loop_start_ts = nanos_since_boot(); |
|
|
|
uint64_t loop_start_ts = nanos_since_boot(); |
|
|
|
double prev_replay_speed = speed_; |
|
|
|
double prev_replay_speed = speed_; |
|
|
|
|
|
|
|
|
|
|
|
for (; !paused_ && first != last; ++first) { |
|
|
|
for (; !interrupt_requested_ && first != last; ++first) { |
|
|
|
const Event &evt = *first; |
|
|
|
const Event &evt = *first; |
|
|
|
int segment = toSeconds(evt.mono_time) / 60; |
|
|
|
int segment = toSeconds(evt.mono_time) / 60; |
|
|
|
|
|
|
|
|
|
|
|
if (current_segment_ != segment) { |
|
|
|
if (current_segment_ != segment) { |
|
|
|
current_segment_ = segment; |
|
|
|
current_segment_ = segment; |
|
|
|
QMetaObject::invokeMethod(this, &Replay::updateSegmentsCache, Qt::QueuedConnection); |
|
|
|
seg_mgr_->setCurrentSegment(current_segment_); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// Skip events if socket is not present
|
|
|
|
// Skip events if socket is not present
|
|
|
|
if (!sockets_[evt.which]) continue; |
|
|
|
if (!sockets_[evt.which]) continue; |
|
|
|
|
|
|
|
|
|
|
|
cur_mono_time_ = evt.mono_time; |
|
|
|
cur_mono_time_ = evt.mono_time; |
|
|
@ -438,10 +330,10 @@ std::vector<Event>::const_iterator Replay::publishEvents(std::vector<Event>::con |
|
|
|
loop_start_ts = current_nanos; |
|
|
|
loop_start_ts = current_nanos; |
|
|
|
prev_replay_speed = speed_; |
|
|
|
prev_replay_speed = speed_; |
|
|
|
} else if (time_diff > 0) { |
|
|
|
} else if (time_diff > 0) { |
|
|
|
precise_nano_sleep(time_diff, paused_); |
|
|
|
precise_nano_sleep(time_diff, interrupt_requested_); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
if (paused_) break; |
|
|
|
if (interrupt_requested_) break; |
|
|
|
|
|
|
|
|
|
|
|
if (evt.eidx_segnum == -1) { |
|
|
|
if (evt.eidx_segnum == -1) { |
|
|
|
publishMessage(&evt); |
|
|
|
publishMessage(&evt); |
|
|
|