message.hh

Go to the documentation of this file.

 
#pragma once
 
#include <atomic>
#include <condition_variable>
#include <deque>
#include <functional>
#include <map>
#include <mutex>
#include <optional>
 
#include <utility/BufferStream.hh>
#include <wire/Protocol.hh>
#include <wire/AckMessage.hh>
 
#include "details/legacy/storage.hh"
 
namespace multisense{
namespace legacy{
 
template <typename T>
T deserialize(const std::vector<uint8_t>& data)
{
    using namespace crl::multisense::details;
 
    utility::BufferStreamReader stream{data.data(), data.size()};
 
    wire::IdType id = 0;
    wire::VersionType version = 0;
 
    stream & id;
    stream & version;
    T m(stream, version);
 
    return m;
}
 
int64_t unwrap_sequence_id(uint16_t current_wire_id, int32_t previous_wire_id, int64_t current_sequence_id);
 
bool header_valid(const std::vector<uint8_t> &raw_data);
 
crl::multisense::details::wire::IdType get_message_type(const std::vector<uint8_t>& raw_buffer);
 
std::optional<uint32_t> get_full_message_size(const std::vector<uint8_t> &raw_data);
 
template <typename T>
std::vector<uint8_t> serialize(const T& message, uint16_t sequence_id, size_t mtu)
{
    using namespace crl::multisense::details;
 
    const wire::IdType id = T::ID;
    const wire::VersionType version = T::VERSION;
 
    std::vector<uint8_t> output_buffer(mtu - wire::COMBINED_HEADER_LENGTH, static_cast<uint8_t>(0));
 
    utility::BufferStreamWriter stream(output_buffer.data(), output_buffer.size());
 
    wire::Header& header = *(reinterpret_cast<wire::Header*>(stream.data()));
    header.magic = wire::HEADER_MAGIC;
    header.version = wire::HEADER_VERSION;
    header.group = wire::HEADER_GROUP;
    header.flags = 0;
    header.sequenceIdentifier = sequence_id;
 
    stream.seek(sizeof(wire::Header));
    stream & id;
    stream & version;
    const_cast<T*>(&message)->serialize(stream, version);
 
    header.messageLength = static_cast<uint32_t>(stream.tell() - sizeof(wire::Header));
    header.byteOffset = 0;
 
    output_buffer.resize(stream.tell());
 
    return output_buffer;
}
 
class MessageCondition
{
public:
 
    MessageCondition() = default;
 
    ~MessageCondition()
    {
        m_cv.notify_all();
    }
 
    void unset()
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        m_notified = false;
    }
 
    void set_and_notify(std::shared_ptr<std::vector<uint8_t>> data)
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        m_data = *data;
        m_notified = true;
        m_cv.notify_all();
    }
 
    template <typename T, 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);
 
        if (timeout)
        {
            if (m_cv.wait_for(lock, timeout.value(), [this]{return this->m_notified;}))
            {
                return std::make_optional(deserialize<T>(m_data));
            }
        }
        else
        {
            m_cv.wait(lock, [this]{return this->m_notified;});
            return std::make_optional(deserialize<T>(m_data));
        }
 
        return std::nullopt;
    }
 
    template <typename T>
    std::optional<T> wait()
    {
        const std::optional<std::chrono::milliseconds> timeout = std::nullopt;
        return wait<T>(timeout);
    }
 
private:
 
    std::mutex m_mutex;
    std::condition_variable m_cv;
    std::vector<uint8_t> m_data;
    bool m_notified = false;
 
};
 
struct MessageStatistics
{
    size_t received_messages = 0;
 
    size_t dropped_messages = 0;
 
    size_t invalid_packets = 0;
};
 
class MessageAssembler
{
public:
    MessageAssembler(std::shared_ptr<BufferPool> buffer_pool);
 
    ~MessageAssembler() = default;
 
    bool process_packet(const std::vector<uint8_t> &raw_data);
 
    std::shared_ptr<MessageCondition> register_message(const crl::multisense::details::wire::IdType &message_id);
 
    void remove_registration(const crl::multisense::details::wire::IdType &message_id);
 
    void register_callback(const crl::multisense::details::wire::IdType& message_id,
                           std::function<void(std::shared_ptr<const std::vector<uint8_t>>)> callback);
 
    void remove_callback(const crl::multisense::details::wire::IdType& message_id);
 
    MessageStatistics get_message_statistics() const
    {
        return MessageStatistics{m_dispatched_messages,
                                 (m_processing_messages ? (m_received_messages - 1 - m_dispatched_messages) :
                                                          (m_received_messages - m_dispatched_messages)),
                                 m_invalid_packets};
    }
 
private:
 
    struct InternalMessage
    {
        crl::multisense::details::wire::IdType type = 0;
        size_t bytes_written = 0;
        std::shared_ptr<std::vector<uint8_t>> data;
    };
 
    std::tuple<std::shared_ptr<std::vector<uint8_t>>, std::deque<int64_t>>
        get_buffer(uint32_t message_size, std::deque<int64_t> ordered_messages);
 
    bool write_data(InternalMessage &message, const std::vector<uint8_t> &raw_data);
 
    void dispatch(const crl::multisense::details::wire::IdType& message_id,
                  std::shared_ptr<std::vector<uint8_t>> data);
 
    std::mutex m_condition_mutex;
 
    std::mutex m_callback_mutex;
 
    std::shared_ptr<BufferPool> m_buffer_pool = nullptr;
 
    int32_t m_previous_wire_id = -1;
 
    int64_t m_current_sequence_id = 0;
 
    std::deque<int64_t> m_small_ordered_messages;
    std::deque<int64_t> m_large_ordered_messages;
 
    std::map<int64_t, InternalMessage> m_active_messages;
 
    std::map<crl::multisense::details::wire::IdType, std::shared_ptr<MessageCondition>> m_conditions;
 
    std::map<crl::multisense::details::wire::IdType,
             std::function<void(std::shared_ptr<const std::vector<uint8_t>>)>> m_callbacks;
 
    std::atomic_size_t m_received_messages = 0;
 
    std::atomic_bool m_processing_messages = false;
 
    std::atomic_size_t m_dispatched_messages = 0;
 
    std::atomic_size_t m_invalid_packets = 0;
};
 
}
}