예제 #1
0
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();
        }
    }
}
예제 #2
0
    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);
    }
예제 #3
0
    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);
    }
예제 #4
0
    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);
    }
예제 #5
0
        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());
                }
            }
        }
예제 #6
0
    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);
    }