utilities.hh

Go to the documentation of this file.

 
#pragma once
 
#include <utility/Exception.hh>
#include <wire/Protocol.hh>
#include <utility/BufferStream.hh>
#include <wire/AckMessage.hh>
#include <wire/ImuDataMessage.hh>
#include <wire/ImuInfoMessage.hh>
 
#include "details/legacy/message.hh"
#include "details/legacy/udp.hh"
 
#include "MultiSense/MultiSenseTypes.hh"
 
namespace multisense{
namespace legacy{
 
constexpr crl::multisense::details::wire::SourceType all_sources = {
    crl::multisense::details::wire::SOURCE_LUMA_LEFT |
    crl::multisense::details::wire::SOURCE_LUMA_RIGHT |
    crl::multisense::details::wire::SOURCE_COMPRESSED_LEFT |
    crl::multisense::details::wire::SOURCE_COMPRESSED_RIGHT |
    crl::multisense::details::wire::SOURCE_LUMA_RECT_LEFT |
    crl::multisense::details::wire::SOURCE_LUMA_RECT_RIGHT |
    crl::multisense::details::wire::SOURCE_COMPRESSED_RECTIFIED_LEFT |
    crl::multisense::details::wire::SOURCE_COMPRESSED_RECTIFIED_RIGHT |
    crl::multisense::details::wire::SOURCE_DISPARITY |
    crl::multisense::details::wire::SOURCE_COMPRESSED_AUX |
    crl::multisense::details::wire::SOURCE_COMPRESSED_RECTIFIED_AUX |
    crl::multisense::details::wire::SOURCE_LUMA_AUX |
    crl::multisense::details::wire::SOURCE_LUMA_RECT_AUX |
    crl::multisense::details::wire::SOURCE_CHROMA_AUX |
    crl::multisense::details::wire::SOURCE_CHROMA_RECT_AUX |
    crl::multisense::details::wire::SOURCE_DISPARITY_COST |
    crl::multisense::details::wire::SOURCE_IMU
};
 
struct ImuSampleScalars
{
    double accelerometer_scale = 1.0;
 
    double gyroscope_scale = 1.0;
 
    double magnetometer_scale = 1.0;
};
 
bool is_image_source(const DataSource &source);
 
Status get_status(const crl::multisense::details::wire::Ack::AckStatus &status);
 
MultiSenseInfo::Version get_version(const crl::multisense::details::wire::VersionType &version);
 
MultiSenseConfig::MaxDisparities get_disparities(size_t disparity);
 
std::vector<DataSource> convert_sources(const crl::multisense::details::wire::SourceType &source);
 
crl::multisense::details::wire::SourceType convert_sources(const std::vector<DataSource> &sources);
 
std::vector<DataSource> expand_source(const DataSource &source);
 
ImuSample add_wire_sample(ImuSample sample,
                          const crl::multisense::details::wire::ImuSample &wire,
                          const ImuSampleScalars &scalars);
 
uint32_t get_rate_index(const std::vector<ImuRate> &rates, const ImuRate &rate);
 
uint32_t get_range_index(const std::vector<ImuRange> &ranges, const ImuRange &range);
 
double get_acceleration_scale(const std::string &units);
 
double get_gyroscope_scale(const std::string &units);
 
double get_magnetometer_scale(const std::string &units);
 
ImuSampleScalars get_imu_scalars(const crl::multisense::details::wire::ImuInfo &info);
 
template <typename QueryMessage, class Rep, class Period>
std::optional<crl::multisense::details::wire::Ack> wait_for_ack(MessageAssembler &assembler,
                                                               const NetworkSocket &socket,
                                                               const QueryMessage &query,
                                                               uint16_t sequence_id,
                                                               uint16_t mtu,
                                                               const std::optional<std::chrono::duration<Rep, Period>>& wait_time,
                                                               size_t attempts = 1)
{
    using namespace crl::multisense::details;
 
    std::optional<wire::Ack> output = std::nullopt;
 
    auto ack_waiter = assembler.register_message(MSG_ID(QueryMessage::ID));
 
    for (size_t i = 0 ; i < attempts ; ++i)
    {
        if(publish_data(socket, serialize(query, sequence_id, mtu)) < 0)
        {
            continue;
        }
 
        if (auto ack = ack_waiter->wait<wire::Ack>(wait_time); ack)
        {
            output = std::move(ack);
            break;
        }
    }
 
    assembler.remove_registration(MSG_ID(QueryMessage::ID));
 
    return output;
}
 
 
template <typename OutputMessage, typename QueryMessage, class Rep, class Period>
std::optional<OutputMessage> wait_for_data(MessageAssembler &assembler,
                                           const NetworkSocket &socket,
                                           const QueryMessage &query,
                                           uint16_t sequence_id,
                                           uint16_t mtu,
                                           const std::optional<std::chrono::duration<Rep, Period>>& wait_time,
                                           size_t attempts = 1)
{
    using namespace crl::multisense::details;
 
    std::optional<OutputMessage> output = std::nullopt;
 
    auto ack_waiter = assembler.register_message(MSG_ID(QueryMessage::ID));
    auto response_waiter = assembler.register_message(MSG_ID(OutputMessage::ID));
 
    for (size_t i = 0 ; i < attempts ; ++i)
    {
        if(publish_data(socket, serialize(query, sequence_id, mtu)) < 0)
        {
            continue;
        }
 
        if (auto ack = ack_waiter->wait<wire::Ack>(wait_time); ack)
        {
            if (ack->status != wire::Ack::Status_Ok)
            {
                continue;
            }
 
            if (auto response = response_waiter->wait<OutputMessage>(wait_time); response)
            {
                output = std::move(response);
                break;
            }
        }
    }
 
    assembler.remove_registration(MSG_ID(QueryMessage::ID));
    assembler.remove_registration(MSG_ID(OutputMessage::ID));
 
    return output;
}
 
template <typename T>
struct TimedResponse
{
    std::chrono::nanoseconds host_start_transmit_time{0};
 
    std::chrono::nanoseconds host_transmit_receive_roundtrip{0};
 
    T message{};
};
 
template <typename OutputMessage, typename QueryMessage, class Rep, class Period>
std::optional<TimedResponse<OutputMessage>> wait_for_data_timed(MessageAssembler &assembler,
                                                                const NetworkSocket &socket,
                                                                const QueryMessage &query,
                                                                uint16_t sequence_id,
                                                                uint16_t mtu,
                                                                const std::optional<std::chrono::duration<Rep, Period>>& wait_time,
                                                                size_t attempts = 1)
{
    using namespace crl::multisense::details;
 
    std::optional<TimedResponse<OutputMessage>> output = std::nullopt;
 
    auto ack_waiter = assembler.register_message(MSG_ID(QueryMessage::ID));
    auto response_waiter = assembler.register_message(MSG_ID(OutputMessage::ID));
 
    for (size_t i = 0 ; i < attempts ; ++i)
    {
        const auto serialized_data = serialize(query, sequence_id, mtu);
 
        const auto send = std::chrono::system_clock::now().time_since_epoch();
        const auto start = std::chrono::high_resolution_clock::now();
 
        if(publish_data(socket, serialized_data) < 0)
        {
            continue;
        }
 
        if (auto ack = ack_waiter->wait<wire::Ack>(wait_time); ack)
        {
            const auto end = std::chrono::high_resolution_clock::now();
 
            if (ack->status != wire::Ack::Status_Ok)
            {
                continue;
            }
 
            if (auto response = response_waiter->wait<OutputMessage>(wait_time); response)
            {
                output = TimedResponse<OutputMessage>{send, end - start, response.value()};
                break;
            }
        }
    }
 
    assembler.remove_registration(MSG_ID(QueryMessage::ID));
    assembler.remove_registration(MSG_ID(OutputMessage::ID));
 
    return output;
}
 
}
}