void pipeline_processing_block::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;
}
for (int s : _streams_ids)
{
if (!_last_set[s])
return;
}
std::vector<frame_holder> set;
for (auto&& s : _last_set)
{
set.push_back(s.second.clone());
}
auto fref = source->allocate_composite_frame(std::move(set));
if (!fref)
{
LOG_ERROR("Failed to allocate composite frame");
return;
}
_queue->enqueue(fref);
}
else
{
LOG_ERROR("Non composite frame arrived to pipeline::handle_frame");
assert(false);
}
}
syncer_proccess_unit::syncer_proccess_unit()
: _matcher((new timestamp_composite_matcher({})))
{
_matcher->set_callback([this](frame_holder f, syncronization_environment env)
{
std::stringstream ss;
ss << "SYNCED: ";
auto composite = dynamic_cast<composite_frame*>(f.frame);
for (int i = 0; i < composite->get_embedded_frames_count(); i++)
{
auto matched = composite->get_frame(i);
ss << matched->get_stream()->get_stream_type() << " " << matched->get_frame_number() << ", "<<std::fixed<< matched->get_frame_timestamp()<<"\n";
}
LOG_DEBUG(ss.str());
env.matches.enqueue(std::move(f));
});
auto f = [&](frame_holder frame, synthetic_source_interface* source)
{
single_consumer_queue<frame_holder> matches;
{
std::lock_guard<std::mutex> lock(_mutex);
_matcher->dispatch(std::move(frame), { source, matches });
}
frame_holder f;
while (matches.try_dequeue(&f))
{
get_source().frame_ready(std::move(f));
}
};
set_processing_callback(std::shared_ptr<rs2_frame_processor_callback>(
new internal_frame_processor_callback<decltype(f)>(f)));
}
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());
}
}
}
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);
}
