void playback_sensor::handle_frame(frame_holder frame, bool is_real_time)
{
if(frame == nullptr)
{
throw invalid_value_exception("null frame passed to handle_frame");
}
if(m_is_started)
{
frame->get_owner()->set_sensor(shared_from_this());
auto type = frame->get_stream()->get_stream_type();
auto index = static_cast<uint32_t>(frame->get_stream()->get_stream_index());
frame->set_stream(m_streams[std::make_pair(type, index)]);
frame->set_sensor(shared_from_this());
auto stream_id = frame.frame->get_stream()->get_unique_id();
//TODO: Ziv, remove usage of shared_ptr when frame_holder is cpoyable
auto pf = std::make_shared<frame_holder>(std::move(frame));
m_dispatchers.at(stream_id)->invoke([this, pf](dispatcher::cancellable_timer t)
{
frame_interface* pframe = nullptr;
std::swap((*pf).frame, pframe);
m_user_callback->on_frame((rs2_frame*)pframe);
});
if(is_real_time)
{
m_dispatchers.at(stream_id)->flush();
}
}
}
void identity_matcher::dispatch(frame_holder f, syncronization_environment env)
{
std::stringstream s;
s <<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<<std::fixed<< f->get_frame_timestamp()<<"\n";
LOG_DEBUG(s.str());
sync(std::move(f), env);
}
bool timestamp_composite_matcher::are_equivalent(frame_holder & a, frame_holder & b)
{
auto a_fps = a->get_stream()->get_framerate();
auto b_fps = b->get_stream()->get_framerate();
auto min_fps = std::min(a_fps, b_fps);
auto ts = extract_timestamps(a, b);
return are_equivalent(ts.first, ts.second, min_fps);
}
void composite_matcher::dispatch(frame_holder f, syncronization_environment env)
{
std::stringstream s;
s <<"DISPATCH "<<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<<std::fixed<< f->get_frame_timestamp()<<"\n";
LOG_DEBUG(s.str());
clean_inactive_streams(f);
auto matcher = find_matcher(f);
update_last_arrived(f, matcher.get());
matcher->dispatch(std::move(f), env);
}
std::pair<double, double> extract_timestamps(frame_holder & a, frame_holder & b)
{
if (a->get_frame_timestamp_domain() == b->get_frame_timestamp_domain())
return{ a->get_frame_timestamp(), b->get_frame_timestamp() };
else
{
return{ (double)a->get_frame_metadata(RS2_FRAME_METADATA_TIME_OF_ARRIVAL),
(double)b->get_frame_metadata(RS2_FRAME_METADATA_TIME_OF_ARRIVAL) };
}
}
void frame_number_composite_matcher::clean_inactive_streams(frame_holder& f)
{
std::vector<stream_id> inactive_matchers;
for(auto m: _matchers)
{
if(_last_arrived[m.second.get()] && (f->get_frame_number() - _last_arrived[m.second.get()]) > 5)
{
inactive_matchers.push_back(m.first);
m.second->set_active(false);
}
}
for(auto id: inactive_matchers)
{
_frames_queue[_matchers[id].get()].clear();
}
}
void aggregator::handle_frame(frame_holder frame, synthetic_source_interface* source)
{
std::lock_guard<std::mutex> lock(_mutex);
auto comp = dynamic_cast<composite_frame*>(frame.frame);
if (comp)
{
for (auto i = 0; i < comp->get_embedded_frames_count(); i++)
{
auto f = comp->get_frame(i);
f->acquire();
_last_set[f->get_stream()->get_unique_id()] = f;
}
// in case not all required streams were aggregated don't publish the frame set
for (int s : _streams_to_aggregate_ids)
{
if (!_last_set[s])
return;
}
// prepare the output frame set for wait_for_frames/poll_frames calls
std::vector<frame_holder> sync_set;
// prepare the output frame set for the callbacks
std::vector<frame_holder> async_set;
for (auto&& s : _last_set)
{
sync_set.push_back(s.second.clone());
// send only the synchronized frames to the user callback
if (std::find(_streams_to_sync_ids.begin(), _streams_to_sync_ids.end(),
s.second->get_stream()->get_unique_id()) != _streams_to_sync_ids.end())
async_set.push_back(s.second.clone());
}
frame_holder sync_fref = source->allocate_composite_frame(std::move(sync_set));
frame_holder async_fref = source->allocate_composite_frame(std::move(async_set));
if (!sync_fref || !async_fref)
{
LOG_ERROR("Failed to allocate composite frame");
return;
}
// for async pipeline usage - provide only the synchronized frames to the user via callback
source->frame_ready(async_fref.clone());
// for sync pipeline usage - push the aggregated to the output queue
_queue->enqueue(sync_fref.clone());
}
else
{
source->frame_ready(frame.clone());
_last_set[frame->get_stream()->get_unique_id()] = frame.clone();
if (_streams_to_sync_ids.empty() && _last_set.size() == _streams_to_aggregate_ids.size())
{
// prepare the output frame set for wait_for_frames/poll_frames calls
std::vector<frame_holder> sync_set;
for (auto&& s : _last_set)
sync_set.push_back(s.second.clone());
frame_holder sync_fref = source->allocate_composite_frame(std::move(sync_set));
if (!sync_fref)
{
LOG_ERROR("Failed to allocate composite frame");
return;
}
// for sync pipeline usage - push the aggregated to the output queue
_queue->enqueue(sync_fref.clone());
}
}
}
bool frame_number_composite_matcher::is_smaller_than(frame_holder & a, frame_holder & b)
{
return a->get_frame_number() < b->get_frame_number();
}
bool frame_number_composite_matcher::are_equivalent(frame_holder& a, frame_holder& b)
{
return a->get_frame_number() == b->get_frame_number();
}
void frame_number_composite_matcher::update_last_arrived(frame_holder& f, matcher* m)
{
_last_arrived[m] =f->get_frame_number();
}
void composite_matcher::sync(frame_holder f, syncronization_environment env)
{
std::stringstream s;
s <<"SYNC "<<_name<<"--> "<< f->get_stream()->get_stream_type() << " " << f->get_frame_number() << ", "<<std::fixed<< f->get_frame_timestamp()<<"\n";
LOG_DEBUG(s.str());
update_next_expected(f);
auto matcher = find_matcher(f);
_frames_queue[matcher.get()].enqueue(std::move(f));
std::vector<frame_holder*> frames_arrived;
std::vector<librealsense::matcher*> frames_arrived_matchers;
std::vector<librealsense::matcher*> synced_frames;
std::vector<librealsense::matcher*> missing_streams;
do
{
auto old_frames = false;
synced_frames.clear();
missing_streams.clear();
frames_arrived_matchers.clear();
frames_arrived.clear();
for (auto s = _frames_queue.begin(); s != _frames_queue.end(); s++)
{
frame_holder* f;
if (s->second.peek(&f))
{
frames_arrived.push_back(f);
frames_arrived_matchers.push_back(s->first);
}
else
{
missing_streams.push_back(s->first);
}
}
if (frames_arrived.size() == 0)
break;
frame_holder* curr_sync;
if (frames_arrived.size() > 0)
{
curr_sync = frames_arrived[0];
synced_frames.push_back(frames_arrived_matchers[0]);
}
for (auto i = 1; i < frames_arrived.size(); i++)
{
if (are_equivalent(*curr_sync, *frames_arrived[i]))
{
synced_frames.push_back(frames_arrived_matchers[i]);
}
else if (is_smaller_than(*frames_arrived[i], *curr_sync))
{
old_frames = true;
synced_frames.clear();
synced_frames.push_back(frames_arrived_matchers[i]);
curr_sync = frames_arrived[i];
}
else
{
old_frames = true;
}
}
if (!old_frames)
{
for (auto i : missing_streams)
{
if (!skip_missing_stream(synced_frames, i))
{
synced_frames.clear();
break;
}
}
}
if (synced_frames.size())
{
std::vector<frame_holder> match;
match.reserve(synced_frames.size());
for (auto index : synced_frames)
{
frame_holder frame;
_frames_queue[index].dequeue(&frame);
match.push_back(std::move(frame));
}
std::sort(match.begin(), match.end(), [](frame_holder& f1, frame_holder& f2)
{
return f1->get_stream()->get_unique_id()> f2->get_stream()->get_unique_id();
});
std::stringstream s;
s<<"MATCHED: ";
for(auto&& f: match)
{
auto composite = dynamic_cast<composite_frame*>(f.frame);
if(composite)
{
for (int i = 0; i < composite->get_embedded_frames_count(); i++)
{
auto matched = composite->get_frame(i);
s << matched->get_stream()->get_stream_type()<<" "<<matched->get_frame_timestamp()<<" ";
}
}
else {
s<<f->get_stream()->get_stream_type()<<" "<<(double)f->get_frame_timestamp()<<" ";
}
}
s<<"\n";
LOG_DEBUG(s.str());
frame_holder composite = env.source->allocate_composite_frame(std::move(match));
if (composite.frame)
{
s <<"SYNCED "<<_name<<"--> "<< composite->get_stream()->get_stream_type() << " " << composite->get_frame_number() << ", "<<std::fixed<< composite->get_frame_timestamp()<<"\n";
auto cb = begin_callback();
_callback(std::move(composite), env);
}
}
} while (synced_frames.size() > 0);
}