LibMultiSense/include/details/legacy/channel.hh

Go to the documentation of this file.

 
#pragma once
 
#include <set>
 
#include "MultiSense/MultiSenseChannel.hh"
 
#include <wire/Protocol.hh>
#include <utility/BufferStream.hh>
#include <wire/ImageMetaMessage.hh>
 
#include "details/legacy/ip.hh"
#include "details/legacy/message.hh"
#include "details/legacy/storage.hh"
#include "details/legacy/udp.hh"
#include "details/legacy/utilities.hh"
 
namespace multisense{
namespace legacy{
 
template <typename T>
class FrameNotifier
{
public:
 
    FrameNotifier() = default;
 
    ~FrameNotifier()
    {
        m_cv.notify_all();
    }
 
    void set_and_notify(const T &in_frame)
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        m_frame = in_frame;
        m_cv.notify_all();
    }
 
    template <class Rep, class Period>
    std::optional<T> wait(const std::optional<std::chrono::duration<Rep, Period>>& timeout)
    {
        std::unique_lock<std::mutex> lock(m_mutex);
 
        std::optional<T> output_frame = std::nullopt;
        if (timeout)
        {
            if (std::cv_status::no_timeout == m_cv.wait_for(lock, timeout.value()))
            {
                output_frame = std::move(m_frame);
            }
        }
        else
        {
            m_cv.wait(lock);
            output_frame = std::move(m_frame);
        }
 
        //
        // Reset the frame
        //
        m_frame = std::nullopt;
 
        return output_frame;
    }
 
    std::optional<T> wait()
    {
        const std::optional<std::chrono::milliseconds> timeout = std::nullopt;
        return wait(timeout);
    }
 
private:
 
    std::mutex m_mutex;
    std::condition_variable m_cv;
    std::optional<T> m_frame;
};
 
 
class LegacyChannel : public Channel
{
public:
    explicit LegacyChannel(const Config &config);
 
    virtual ~LegacyChannel();
 
    Status start_streams(const std::vector<DataSource> &sources) final override;
 
    Status stop_streams(const std::vector<DataSource> &sources) final override;
 
    void add_image_frame_callback(std::function<void(const ImageFrame&)> callback) final override;
 
    void add_imu_frame_callback(std::function<void(const ImuFrame&)> callback) final override;
 
    Status connect(const Config &config) final override;
 
    void disconnect() final override;
 
    std::optional<ImageFrame> get_next_image_frame() final override;
 
    std::optional<ImuFrame> get_next_imu_frame() final override;
 
    MultiSenseConfig get_config() final override;
 
    Status set_config(const MultiSenseConfig &config) final override;
 
    StereoCalibration get_calibration() final override;
 
    Status set_calibration(const StereoCalibration &calibration) final override;
 
    MultiSenseInfo get_info() final override;
 
    Status set_device_info(const MultiSenseInfo::DeviceInfo &device_info, const std::string &key) final override;
 
    std::optional<MultiSenseStatus> get_system_status() final override;
 
    Status set_network_config(const MultiSenseInfo::NetworkInfo &config,
                              const std::optional<std::string> &broadcast_interface) final override;
 
private:
 
    Status set_mtu(uint16_t mtu);
 
    Status set_mtu(const std::optional<uint16_t> &mtu);
 
    std::optional<MultiSenseConfig> query_configuration(bool has_aux_camera, bool has_imu, bool ptp_enabled);
 
    std::optional<StereoCalibration> query_calibration();
 
    std::optional<MultiSenseInfo> query_info();
 
    std::optional<MultiSenseInfo::DeviceInfo> query_device_info();
 
    void image_meta_callback(std::shared_ptr<const std::vector<uint8_t>> data);
 
    void image_callback(std::shared_ptr<const std::vector<uint8_t>> data);
 
    void disparity_callback(std::shared_ptr<const std::vector<uint8_t>> data);
 
    void imu_callback(std::shared_ptr<const std::vector<uint8_t>> data);
 
    void handle_and_dispatch(Image image,
                            int64_t frame_id,
                            const StereoCalibration &calibration,
                            const TimeT &capture_time,
                            const TimeT &ptp_capture_time);
 
    std::mutex m_mutex{};
 
    std::mutex m_image_callback_mutex{};
 
    std::mutex m_imu_callback_mutex{};
 
    std::atomic_bool m_connected = false;
 
    std::atomic_uint16_t m_current_mtu = 1500;
 
    Config m_config{};
 
    NetworkSocket m_socket{};
 
    std::atomic_uint16_t m_transmit_id = 0;
 
    StereoCalibration m_calibration{};
 
    MultiSenseInfo m_info{};
 
    MultiSenseConfig m_multisense_config{};
 
    std::set<DataSource> m_active_streams{};
 
    std::function<void(const ImageFrame&)> m_user_image_frame_callback{};
 
    std::function<void(const ImuFrame&)> m_user_imu_frame_callback{};
 
    FrameNotifier<ImageFrame> m_image_frame_notifier{};
 
    FrameNotifier<ImuFrame> m_imu_frame_notifier{};
 
    std::map<int64_t, crl::multisense::details::wire::ImageMeta> m_meta_cache{};
 
    std::map<int64_t, ImageFrame> m_frame_buffer{};
 
    std::atomic_uint32_t m_max_batched_imu_messages = 0;
 
    ImuSampleScalars m_imu_scalars{};
 
    ImuFrame m_current_imu_frame{};
 
    std::shared_ptr<BufferPool> m_buffer_pool = nullptr;
 
    std::unique_ptr<UdpReceiver> m_udp_receiver = nullptr;
 
    MessageAssembler m_message_assembler;
};
 
}
}