diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 4fcf1d06dc08a915bc4eb823fe663c23dde39436..92b668bbd6d75471b120601a0687d2d400a41216 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -6,7 +6,7 @@ variables: GIT_SUBMODULE_STRATEGY: recursive - CMAKE_ARGS_WINDOWS: '-DCMAKE_GENERATOR_PLATFORM=x64 -DNVPIPE_DIR="D:/Build/NvPipe" -DEigen3_DIR="C:/Program Files (x86)/Eigen3/share/eigen3/cmake" -DOpenCV_DIR="D:/Build/opencv-4.1.1" -DCUDA_TOOLKIT_ROOT_DIR="C:/Program Files/NVIDIA GPU Computing Toolkit/CUDA/v10.1"' + CMAKE_ARGS_WINDOWS: '-DCMAKE_GENERATOR_PLATFORM=x64 -DPORTAUDIO_DIR="D:/Build/portaudio" -DNVPIPE_DIR="D:/Build/NvPipe" -DEigen3_DIR="C:/Program Files (x86)/Eigen3/share/eigen3/cmake" -DOpenCV_DIR="D:/Build/opencv-4.1.1" -DCUDA_TOOLKIT_ROOT_DIR="C:/Program Files/NVIDIA GPU Computing Toolkit/CUDA/v10.1"' stages: - all @@ -25,7 +25,7 @@ linux: script: - mkdir build - cd build - - cmake .. -DWITH_OPTFLOW=TRUE -DBUILD_CALIBRATION=TRUE -DCMAKE_BUILD_TYPE=Release + - cmake .. -DWITH_OPTFLOW=TRUE -DUSE_CPPCHECK=FALSE -DBUILD_CALIBRATION=TRUE -DCMAKE_BUILD_TYPE=Release - make - ctest --output-on-failure diff --git a/CMakeLists.txt b/CMakeLists.txt index 19f41d1b5f19ed2d182e3b415922b741c5cf409b..37d4fa1008247dcf60123d83e0ce5293e917491a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ option(WITH_NVPIPE "Use NvPipe for compression if available" ON) option(WITH_OPTFLOW "Use NVIDIA Optical Flow if available" OFF) option(WITH_OPENVR "Build with OpenVR support" OFF) option(WITH_FIXSTARS "Use Fixstars libSGM if available" ON) +option(USE_CPPCHECK "Apply cppcheck during build" ON) option(BUILD_VISION "Enable the vision component" ON) option(BUILD_RECONSTRUCT "Enable the reconstruction component" ON) option(BUILD_RENDERER "Enable the renderer component" ON) @@ -94,25 +95,6 @@ else() endif() if (BUILD_GUI) - #find_library( NANOGUI_LIBRARY NAMES nanogui libnanogui PATHS ${NANOGUI_DIR} PATH_SUFFIXES lib) - #if (NANOGUI_LIBRARY) - # set(HAVE_NANOGUI TRUE) - # add_library(nanogui UNKNOWN IMPORTED) - # #set_property(TARGET nanogui PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${NANOGUI_EXTRA_INCS}) - # set_property(TARGET nanogui PROPERTY IMPORTED_LOCATION ${NANOGUI_LIBRARY}) - # message(STATUS "Found NanoGUI: ${NANOGUI_LIBRARY}") - - # if(WIN32) - # # Find include - # find_path(NANOGUI_INCLUDE_DIRS - # NAMES nanogui/nanogui.h - # PATHS "C:/Program Files/NanoGUI" "C:/Program Files (x86)/NanoGUI" ${NANOGUI_DIR} - # PATH_SUFFIXES include - # ) - # set_property(TARGET nanogui PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${NANOGUI_INCLUDE_DIRS}) - # endif() - #endif() - set(HAVE_NANOGUI TRUE) # Disable building extras we won't need (pure C++ project) @@ -150,6 +132,30 @@ else() add_library(nvpipe INTERFACE) endif() +# Portaudio v19 library +find_library( PORTAUDIO_LIBRARY NAMES portaudio PATHS ${PORTAUDIO_DIR} PATH_SUFFIXES lib) +if (PORTAUDIO_LIBRARY) + set(HAVE_PORTAUDIO TRUE) + add_library(portaudio UNKNOWN IMPORTED) + #set_property(TARGET nanogui PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${NANOGUI_EXTRA_INCS}) + set_property(TARGET portaudio PROPERTY IMPORTED_LOCATION ${PORTAUDIO_LIBRARY}) + message(STATUS "Found Portaudio: ${PORTAUDIO_LIBRARY}") + + if(WIN32) + # Find include + find_path(PORTAUDIO_INCLUDE_DIRS + NAMES portaudio.h + PATHS "C:/Program Files/Portaudio" "C:/Program Files (x86)/Portaudio" ${PORTAUDIO_DIR} + PATH_SUFFIXES include + ) + set_property(TARGET portaudio PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${PORTAUDIO_INCLUDE_DIRS}) + endif() +else() + set(PORTAUDIO_LIBRARY "") + add_library(portaudio INTERFACE) + message(WARNING "Portaudio not found - sound disabled") +endif() + find_program( NODE_NPM NAMES npm ) if (NODE_NPM) message(STATUS "Found NPM: ${NODE_NPM}") @@ -209,10 +215,12 @@ check_include_file_cxx("opencv2/viz.hpp" HAVE_VIZ) check_include_file_cxx("opencv2/cudastereo.hpp" HAVE_OPENCVCUDA) # Optional source problem check -find_program(CPPCHECK_FOUND cppcheck) -if (CPPCHECK_FOUND) - message(STATUS "Found cppcheck: will perform source checks") - set(CMAKE_CXX_CPPCHECK "cppcheck" "-D__align__(A)" "-DCUDARTAPI" "--enable=warning,performance,style" "--inline-suppr" "--std=c++14" "--suppress=*:*catch.hpp" "--suppress=*:*elas*" "--suppress=*:*nanogui*" "--suppress=*:*json.hpp" "--quiet") +if (USE_CPPCHECK) + find_program(CPPCHECK_FOUND cppcheck) + if (CPPCHECK_FOUND) + message(STATUS "Found cppcheck: will perform source checks") + set(CMAKE_CXX_CPPCHECK "cppcheck" "-D__align__(A)" "-DCUDARTAPI" "--enable=warning,performance,style" "--inline-suppr" "--std=c++14" "--suppress=*:*catch.hpp" "--suppress=*:*elas*" "--suppress=*:*nanogui*" "--suppress=*:*json.hpp" "--quiet") + endif() endif() # include_directories(${PROJECT_SOURCE_DIR}/common/cpp/include) @@ -239,11 +247,13 @@ set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) add_subdirectory(components/common/cpp) add_subdirectory(components/codecs) +add_subdirectory(components/structures) add_subdirectory(components/net) add_subdirectory(components/rgbd-sources) add_subdirectory(components/control/cpp) add_subdirectory(components/operators) add_subdirectory(components/streams) +add_subdirectory(components/audio) add_subdirectory(applications/calibration) #add_subdirectory(applications/groupview) #add_subdirectory(applications/player) diff --git a/applications/gui/src/src_window.cpp b/applications/gui/src/src_window.cpp index baa3a6f4152756f36c71f99f27e793d746f4bee8..cc1f2aa0aadc0e622e468fb548eddea9eaa067ba 100644 --- a/applications/gui/src/src_window.cpp +++ b/applications/gui/src/src_window.cpp @@ -80,8 +80,10 @@ SourceWindow::SourceWindow(ftl::gui::Screen *screen) cycle_ = 0; receiver_->onFrameSet([this](ftl::rgbd::FrameSet &fs) { + fs.swapTo(frameset_); + // Request the channels required by current camera configuration - interceptor_->select(fs.id, _aggregateChannels()); + interceptor_->select(frameset_.id, _aggregateChannels()); /*if (fs.frames[0].hasChannel(Channel::Data)) { int data = 0; @@ -90,30 +92,39 @@ SourceWindow::SourceWindow(ftl::gui::Screen *screen) }*/ const auto *cstream = interceptor_; - _createDefaultCameras(fs, cstream->available(fs.id).has(Channel::Depth)); + _createDefaultCameras(frameset_, cstream->available(fs.id).has(Channel::Depth)); //LOG(INFO) << "Channels = " << (unsigned int)cstream->available(fs.id); // Enforce interpolated colour - for (int i=0; i<fs.frames.size(); ++i) { - fs.frames[i].createTexture<uchar4>(Channel::Colour, true); + for (int i=0; i<frameset_.frames.size(); ++i) { + frameset_.frames[i].createTexture<uchar4>(Channel::Colour, true); } - pre_pipeline_->apply(fs, fs, 0); + pre_pipeline_->apply(frameset_, frameset_, 0); int i=0; for (auto cam : cameras_) { // Only update the camera periodically unless the active camera if (screen_->activeCamera() == cam.second.camera || - (screen_->activeCamera() == nullptr && cycle_ % cameras_.size() == i++)) cam.second.camera->update(fs); + (screen_->activeCamera() == nullptr && cycle_ % cameras_.size() == i++)) cam.second.camera->update(frameset_); - cam.second.camera->update(cstream->available(fs.id)); + cam.second.camera->update(cstream->available(frameset_.id)); } ++cycle_; return true; }); + speaker_ = ftl::create<ftl::audio::Speaker>(screen_->root(), "speaker_test"); + + receiver_->onAudio([this](ftl::audio::FrameSet &fs) { + //LOG(INFO) << "Audio delay required = " << (ts - frameset_.timestamp) << "ms"; + speaker_->setDelay(fs.timestamp - frameset_.timestamp + ftl::timer::getInterval()); // Add Xms for local render time + speaker_->queue(fs.timestamp, fs.frames[0]); + return true; + }); + _updateCameras(screen_->control()->getNet()->findAll<string>("list_streams")); // Also check for a file on command line. diff --git a/applications/gui/src/src_window.hpp b/applications/gui/src/src_window.hpp index 5e8164954d090e089f99bf75514891f2c1f51221..1aa9b5345aacc8b926a0e1c2902586218c08741d 100644 --- a/applications/gui/src/src_window.hpp +++ b/applications/gui/src/src_window.hpp @@ -16,6 +16,8 @@ #include <ftl/streams/receiver.hpp> #include <ftl/streams/filestream.hpp> +#include <ftl/audio/speaker.hpp> + class VirtualCameraView; namespace ftl { @@ -68,6 +70,10 @@ class SourceWindow : public nanogui::Window { ftl::operators::Graph *pre_pipeline_; MUTEX mutex_; + ftl::audio::Speaker *speaker_; + + ftl::rgbd::FrameSet frameset_; + void _updateCameras(const std::vector<std::string> &netcams); void _createDefaultCameras(ftl::rgbd::FrameSet &fs, bool makevirtual); ftl::codecs::Channels<0> _aggregateChannels(); diff --git a/applications/reconstruct/CMakeLists.txt b/applications/reconstruct/CMakeLists.txt index b039e8466ee5374a33daa29716ec1c04c5fd2619..61af68ef71818a0d1499151a52ae0d9b6bb76690 100644 --- a/applications/reconstruct/CMakeLists.txt +++ b/applications/reconstruct/CMakeLists.txt @@ -37,6 +37,6 @@ set_property(TARGET ftl-reconstruct PROPERTY CUDA_SEPARABLE_COMPILATION ON) endif() #target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include) -target_link_libraries(ftl-reconstruct ftlcommon ftlrgbd Threads::Threads ${OpenCV_LIBS} ftlctrl ftlnet ftlrender ftloperators ftlstreams) +target_link_libraries(ftl-reconstruct ftlcommon ftlrgbd Threads::Threads ${OpenCV_LIBS} ftlctrl ftlnet ftlrender ftloperators ftlstreams ftlaudio) diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp index 6d91839dc540386400a0d959246299b1a293e10c..9869a7915f0ecbdd5da96ac703535dc3ea25a053 100644 --- a/applications/reconstruct/src/main.cpp +++ b/applications/reconstruct/src/main.cpp @@ -52,6 +52,8 @@ #include <ftl/streams/sender.hpp> #include <ftl/streams/netstream.hpp> +#include <ftl/audio/source.hpp> + #include <cuda_profiler_api.h> #ifdef WIN32 @@ -143,6 +145,8 @@ static void run(ftl::Configurable *root) { outstream->begin(); sender->setStream(outstream); + ftl::audio::Source *audioSrc = nullptr; + std::vector<Source*> sources; // Create a vector of all input RGB-Depth sources if (root->getConfig()["sources"].size() > 0) { @@ -194,6 +198,14 @@ static void run(ftl::Configurable *root) { }); groups.push_back(reconstr); ++i; + + // TODO: Temporary reconstruction local audio source for testing + audioSrc = ftl::create<ftl::audio::Source>(root, "audio_test"); + + audioSrc->onFrameSet([sender](ftl::audio::FrameSet &fs) { + sender->post(fs); + return true; + }); } } @@ -229,6 +241,11 @@ static void run(ftl::Configurable *root) { return reconstr->post(fs); }); + gen->onAudio([sender](ftl::audio::FrameSet &fs) { + sender->post(fs); + return true; + }); + int i = groups.size(); reconstr->onFrameSet([sender,i](ftl::rgbd::FrameSet &fs) { fs.id = i; @@ -242,10 +259,11 @@ static void run(ftl::Configurable *root) { } } - LOG(INFO) << "Start timer"; ftl::timer::start(true); + if (audioSrc) delete audioSrc; + LOG(INFO) << "Shutting down..."; ftl::timer::stop(); ctrl.stop(); diff --git a/applications/vision/CMakeLists.txt b/applications/vision/CMakeLists.txt index 14fc1ad0077d633121878e9399d2187e21e44648..78d4bb28c548e41bf0b26b5e04019db75e21f63b 100644 --- a/applications/vision/CMakeLists.txt +++ b/applications/vision/CMakeLists.txt @@ -21,6 +21,6 @@ set_property(TARGET ftl-vision PROPERTY CUDA_SEPARABLE_COMPILATION OFF) endif() #target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include) -target_link_libraries(ftl-vision ftlrgbd ftlcommon ftlstreams ftlctrl ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} ftlnet) +target_link_libraries(ftl-vision ftlrgbd ftlcommon ftlstreams ftlctrl ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} ftlnet ftlaudio) diff --git a/applications/vision/src/main.cpp b/applications/vision/src/main.cpp index da166f00cdc2071150417b9775af3bea49eb01a0..5cc9c175b6b421f53d438454c48aaa539b27daf4 100644 --- a/applications/vision/src/main.cpp +++ b/applications/vision/src/main.cpp @@ -26,6 +26,8 @@ #include <ftl/streams/netstream.hpp> #include <ftl/streams/sender.hpp> +#include <ftl/audio/source.hpp> + #include "opencv2/imgproc.hpp" #include "opencv2/imgcodecs.hpp" #include "opencv2/highgui.hpp" @@ -75,6 +77,13 @@ static void run(ftl::Configurable *root) { sender->post(fs); return true; }); + + // TODO: TEMPORARY + ftl::audio::Source *audioSrc = ftl::create<ftl::audio::Source>(root, "audio_test"); + audioSrc->onFrameSet([sender](ftl::audio::FrameSet &fs) { + sender->post(fs); + return true; + }); auto pipeline = ftl::config::create<ftl::operators::Graph>(root, "pipeline"); pipeline->append<ftl::operators::DepthChannel>("depth"); // Ensure there is a depth channel diff --git a/components/audio/CMakeLists.txt b/components/audio/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..767184b2b91ab77c8a5fcb6e4ea842e586ac24f8 --- /dev/null +++ b/components/audio/CMakeLists.txt @@ -0,0 +1,18 @@ +set(AUDIOSRC + src/source.cpp + src/frame.cpp + src/portaudio.cpp + src/speaker.cpp +) + +add_library(ftlaudio ${AUDIOSRC}) + +target_include_directories(ftlaudio PUBLIC + $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> + $<INSTALL_INTERFACE:include> + PRIVATE src) + +target_link_libraries(ftlaudio ftlcommon Eigen3::Eigen ftlstreams ftldata portaudio) + +#add_subdirectory(test) + diff --git a/components/audio/include/ftl/audio/audio.hpp b/components/audio/include/ftl/audio/audio.hpp new file mode 100644 index 0000000000000000000000000000000000000000..12939b6653f8655b08631392f97ad3889c2d2fe1 --- /dev/null +++ b/components/audio/include/ftl/audio/audio.hpp @@ -0,0 +1,25 @@ +#ifndef _FTL_AUDIO_AUDIO_HPP_ +#define _FTL_AUDIO_AUDIO_HPP_ + +#include <vector> + +namespace ftl { +namespace audio { + +class Audio { + public: + Audio() {}; + + size_t size() const { return data_.size()*sizeof(short); } + + std::vector<short> &data() { return data_; } + const std::vector<short> &data() const { return data_; } + + private: + std::vector<short> data_; +}; + +} +} + +#endif // _FTL_AUDIO_AUDIO_HPP_ diff --git a/components/audio/include/ftl/audio/buffer.hpp b/components/audio/include/ftl/audio/buffer.hpp new file mode 100644 index 0000000000000000000000000000000000000000..b96efc3399bb42824fb91d5555c6fc5da5e034ac --- /dev/null +++ b/components/audio/include/ftl/audio/buffer.hpp @@ -0,0 +1,132 @@ +#ifndef _FTL_AUDIO_BUFFER_HPP_ +#define _FTL_AUDIO_BUFFER_HPP_ + +#include <vector> + +namespace ftl { +namespace audio { + +//static constexpr int kBufferCount = 100; + +/** + * A fast circular buffer to capture, play and manipulate audio data. + * This class can be used directly with portaudio. The hardware uses + * `readFrame` and `writeFrame` to consume or append audio data. A more + * advanced `write` function allows for non-frame aligned data and for time + * dilation / shifting, and amplitude control. + */ +template <typename T, int CHAN, int FRAME, int SIZE> +class FixedBuffer { + public: + typedef T type; + + FixedBuffer() : write_position_(0), read_position_(-1), offset_(0), rate_(44100), + cur_delay_(0.0f), req_delay_(0.0f) {} + explicit FixedBuffer(int rate) : write_position_(0), read_position_(-1), + offset_(0), rate_(rate), cur_delay_(0.0f), req_delay_(0.0f) {} + + int sampleRate() const { return rate_; } + + inline int channels() const { return CHAN; } + inline int frameSize() const { return FRAME; } + inline int maxFrames() const { return SIZE; } + + void setDelay(float d) { + req_delay_ = d * static_cast<float>(rate_); + } + + float delay() const { return cur_delay_ / static_cast<float>(rate_); } + + inline void writeFrame(const T *d) { + const T *in = d; + T *out = &data_[(write_position_++) % SIZE][0]; + for (size_t i=0; i<CHAN*FRAME; ++i) *out++ = *in++; + if (write_position_ > 5 && read_position_ < 0) read_position_ = 0; + } + + inline void readFrame(T *d) { + T *out = d; + if (read_position_ < 0 || read_position_ >= write_position_-1) { + for (size_t i=0; i<CHAN*FRAME; ++i) *out++ = 0; + } else { + T *in = &data_[(read_position_++) % SIZE][0]; + for (size_t i=0; i<CHAN*FRAME; ++i) *out++ = *in++; + } + } + + int size() const { return (read_position_>=0) ? write_position_ - 2 - read_position_ : 0; } + int frames() const { return (read_position_>=0) ? write_position_ - 2 - read_position_ : 0; } + + /** + * Append sound samples to the end of the buffer. The samples may be over + * or under sampled so as to gradually introduce or remove a requested + * delay and hence change the latency of the audio. + */ + void write(const std::vector<T> &in); + + private: + int write_position_; + int read_position_; + int offset_; + T data_[SIZE][CHAN*FRAME]; + int rate_; + + float cur_delay_; + float req_delay_; +}; + +// ==== Implementations ======================================================== + +template <typename T, int CHAN> +static T fracIndex(const std::vector<T> &in, float ix, int c) { + const int i1 = static_cast<int>(ix); + const int i2 = static_cast<int>(ix+1.0f); + const float alpha = ix - static_cast<float>(i1); + return (i2*CHAN+CHAN >= in.size()) ? in[i1*CHAN+c] : in[i1*CHAN+c]*(1.0f-alpha) + in[i2*CHAN+c]*alpha; +} + +inline float clamp(float v, float c) { return (v < -c) ? -c : (v > c) ? c : v; } + +template <typename T, int CHAN, int FRAME, int SIZE> +void FixedBuffer<T,CHAN,FRAME,SIZE>::write(const std::vector<T> &in) { + float i=0.0f; + float s = static_cast<float>(in.size()) / static_cast<float>(CHAN); + + while (i <= s-1.0f) { + T *ptr = data_[write_position_ % SIZE]+offset_; + + for (int c=0; c<CHAN; ++c) *ptr++ = fracIndex<T,CHAN>(in, i, c); + + const float d = 0.6f*clamp((req_delay_ - cur_delay_) / static_cast<float>(rate_), 0.5f); + i += 1.0f - d; // FIXME: Is this correct? Seems to function but perhaps not ideal + + /*if (d > 0.0f) { // Increase delay = oversample with increment < 1.0 + //i += 1.0f * (1.0f - d); + i += 1.0f - d; + } else { // Decrease delay = undersample with increment > 1.0 + //i += 1.0f / (1.0f + d); + i += 1.0f - d; + }*/ + cur_delay_ += d; + + offset_+= CHAN; + if (offset_ == CHAN*FRAME) { + offset_ = 0; + ++write_position_; + } + } + if (write_position_ > 20 && read_position_ < 0) read_position_ = 0; +} + +// ==== Common forms =========================================================== + +template <int SIZE> +using StereoBuffer16 = ftl::audio::FixedBuffer<short,2,256,SIZE>; + +template <int SIZE> +using MonoBuffer16 = ftl::audio::FixedBuffer<short,1,256,SIZE>; + +} +} + +#endif // _FTL_AUDIO_BUFFER_HPP_ diff --git a/components/audio/include/ftl/audio/frame.hpp b/components/audio/include/ftl/audio/frame.hpp new file mode 100644 index 0000000000000000000000000000000000000000..8546986093dd2693210edd4de3a53ac3fe26c216 --- /dev/null +++ b/components/audio/include/ftl/audio/frame.hpp @@ -0,0 +1,47 @@ +#pragma once +#ifndef _FTL_AUDIO_FRAME_HPP_ +#define _FTL_AUDIO_FRAME_HPP_ + +#include <ftl/data/framestate.hpp> +#include <ftl/data/frame.hpp> +#include <ftl/audio/audio.hpp> + +namespace ftl { +namespace audio { + +struct AudioSettings { + int sample_rate; + int frame_size; +}; + +struct AudioData { + template <typename T> + const T &as() const { + throw ftl::exception("Type not valid for audio channel"); + } + + template <typename T> + T &as() { + throw ftl::exception("Type not valid for audio channel"); + } + + template <typename T> + T &make() { + throw ftl::exception("Type not valid for audio channel"); + } + + Audio data; +}; + +// Specialisations for getting Audio data. +template <> Audio &AudioData::as<Audio>(); +template <> const Audio &AudioData::as<Audio>() const; +template <> Audio &AudioData::make<Audio>(); + +typedef ftl::data::FrameState<AudioSettings,2> FrameState; +typedef ftl::data::Frame<32,2,FrameState,AudioData> Frame; + +} +} + +#endif // _FTL_AUDIO_FRAME_HPP_ \ No newline at end of file diff --git a/components/audio/include/ftl/audio/frameset.hpp b/components/audio/include/ftl/audio/frameset.hpp new file mode 100644 index 0000000000000000000000000000000000000000..02027e88e0328008a3e7a312de04e5dda34eb629 --- /dev/null +++ b/components/audio/include/ftl/audio/frameset.hpp @@ -0,0 +1,15 @@ +#ifndef _FTL_AUDIO_FRAMESET_HPP_ +#define _FTL_AUDIO_FRAMESET_HPP_ + +#include <ftl/audio/frame.hpp> +#include <ftl/data/frameset.hpp> + +namespace ftl { +namespace audio { + +typedef ftl::data::FrameSet<ftl::audio::Frame> FrameSet; + +} +} + +#endif // _FTL_AUDIO_FRAMESET_HPP_ diff --git a/components/audio/include/ftl/audio/portaudio.hpp b/components/audio/include/ftl/audio/portaudio.hpp new file mode 100644 index 0000000000000000000000000000000000000000..64b285115f4d3cef364893a94acd1c093523d3c1 --- /dev/null +++ b/components/audio/include/ftl/audio/portaudio.hpp @@ -0,0 +1,14 @@ +#ifndef _FTL_AUDIO_PORTAUDIO_HPP_ +#define _FTL_AUDIO_PORTAUDIO_HPP_ + +namespace ftl { +namespace audio { + +void pa_init(); + +void pa_final(); + +} +} + +#endif // _FTL_AUDIO_PORTAUDIO_HPP_ diff --git a/components/audio/include/ftl/audio/source.hpp b/components/audio/include/ftl/audio/source.hpp new file mode 100644 index 0000000000000000000000000000000000000000..87aafadd6e7388a613774b9cb713943d74ab5a5e --- /dev/null +++ b/components/audio/include/ftl/audio/source.hpp @@ -0,0 +1,57 @@ +#ifndef _FTL_AUDIO_SOURCE_HPP_ +#define _FTL_AUDIO_SOURCE_HPP_ + +#include <ftl/audio/buffer.hpp> +#include <ftl/audio/frameset.hpp> +#include <ftl/configurable.hpp> +#include <ftl/config.h> + +#ifdef HAVE_PORTAUDIO +#include <portaudio.h> +#endif + +namespace ftl { +namespace audio { + +static constexpr int kFrameSize = 256; + +typedef ftl::data::Generator<ftl::audio::FrameSet> Generator; + +class Source : public ftl::Configurable, public ftl::audio::Generator { + public: + explicit Source(nlohmann::json &config); + ~Source(); + + /** Number of frames in last frameset. This can change over time. */ + size_t size() override; + + /** + * Get the persistent state object for a frame. An exception is thrown + * for a bad index. + */ + ftl::audio::FrameState &state(int ix) override; + + /** Register a callback to receive new frame sets. */ + void onFrameSet(const ftl::audio::FrameSet::Callback &) override; + + private: + ftl::audio::FrameState state_; + bool active_; + ftl::timer::TimerHandle timer_hp_; + ftl::timer::TimerHandle timer_main_; + ftl::audio::FrameSet::Callback cb_; + + ftl::audio::StereoBuffer16<100> buffer_; + int to_read_; + + ftl::audio::FrameSet frameset_; + + #ifdef HAVE_PORTAUDIO + PaStream *stream_; + #endif +}; + +} +} + +#endif // _FTL_AUDIO_SOURCE_HPP_ diff --git a/components/audio/include/ftl/audio/speaker.hpp b/components/audio/include/ftl/audio/speaker.hpp new file mode 100644 index 0000000000000000000000000000000000000000..b70c6e65f0bc249261d16b4ec0d1f8cd5ec92dd3 --- /dev/null +++ b/components/audio/include/ftl/audio/speaker.hpp @@ -0,0 +1,38 @@ +#ifndef _FTL_AUDIO_SPEAKER_HPP_ +#define _FTL_AUDIO_SPEAKER_HPP_ + +#include <ftl/configurable.hpp> +#include <ftl/audio/buffer.hpp> +#include <ftl/audio/frameset.hpp> +#include <ftl/config.h> + +#ifdef HAVE_PORTAUDIO +#include <portaudio.h> +#endif + +namespace ftl { +namespace audio { + +class Speaker : public ftl::Configurable { + public: + explicit Speaker(nlohmann::json &config); + ~Speaker(); + + void queue(int64_t ts, ftl::audio::Frame &fs); + + void setDelay(int64_t ms); + + private: + ftl::audio::StereoBuffer16<2000> buffer_; + bool active_; + float extra_delay_; + + #ifdef HAVE_PORTAUDIO + PaStream *stream_; + #endif +}; + +} +} + +#endif // _FTL_SPEAKER_HPP_ diff --git a/components/audio/src/frame.cpp b/components/audio/src/frame.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a73e7847c981598bd11d76ec40e9a02df4b491ff --- /dev/null +++ b/components/audio/src/frame.cpp @@ -0,0 +1,17 @@ +#include <ftl/audio/frame.hpp> +#include <ftl/audio/audio.hpp> + +using ftl::audio::Audio; +using ftl::audio::AudioData; + +template <> Audio &AudioData::as<Audio>() { + return data; +} + +template <> const Audio &AudioData::as<Audio>() const { + return data; +} + +template <> Audio &AudioData::make<Audio>() { + return data; +} diff --git a/components/audio/src/portaudio.cpp b/components/audio/src/portaudio.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7395877c21bdb11ef8a08e0f796e8837c6691988 --- /dev/null +++ b/components/audio/src/portaudio.cpp @@ -0,0 +1,47 @@ +#include <ftl/audio/portaudio.hpp> +#include <ftl/config.h> +#include <loguru.hpp> + +#include <atomic> + +static std::atomic<int> counter = 0; + +#ifdef HAVE_PORTAUDIO + +#include <portaudio.h> + +void ftl::audio::pa_init() { + // TODO: Mutex lock? + if (counter == 0) { + auto err = Pa_Initialize(); + if (err != paNoError) { + LOG(ERROR) << "Portaudio failed to initialise: " << Pa_GetErrorText(err); + counter = 1000; + } + + // List devices + int numDevices = Pa_GetDeviceCount(); + + if (numDevices == 0) LOG(WARNING) << "No audio devices found"; + else LOG(INFO) << "Audio devices:"; + + for (int i=0; i<numDevices; ++i) { + const PaDeviceInfo *deviceInfo = Pa_GetDeviceInfo(i); + LOG(INFO) << " -- (" << i << ") " << deviceInfo->name << " - Inputs=" << deviceInfo->maxInputChannels << " Outputs=" << deviceInfo->maxOutputChannels; + } + } + ++counter; +} + +void ftl::audio::pa_final() { + // TODO: Mutex lock? + --counter; + if (counter == 0) { + auto err = Pa_Terminate(); + if (err != paNoError) { + LOG(ERROR) << "Portaudio failed to terminate: " << Pa_GetErrorText(err); + counter = -1000; + } + } +} +#endif \ No newline at end of file diff --git a/components/audio/src/source.cpp b/components/audio/src/source.cpp new file mode 100644 index 0000000000000000000000000000000000000000..15b99c08611732fd51751f8bd63c18ffc1ec196b --- /dev/null +++ b/components/audio/src/source.cpp @@ -0,0 +1,182 @@ +#include <ftl/audio/source.hpp> +#include <ftl/audio/audio.hpp> +#include <ftl/audio/portaudio.hpp> + +using ftl::audio::Source; +using ftl::audio::Frame; +using ftl::audio::FrameSet; +using ftl::audio::FrameState; +using ftl::audio::Audio; +using ftl::codecs::Channel; + +#ifdef HAVE_PORTAUDIO + +//static double ltime = 0.0; + +/* Portaudio callback to receive audio data. */ +static int pa_source_callback(const void *input, void *output, + unsigned long frameCount, const PaStreamCallbackTimeInfo *timeInfo, + PaStreamCallbackFlags statusFlags, void *userData) { + + auto *buffer = (ftl::audio::StereoBuffer16<100>*)userData; + short *in = (short*)input; + + //short *out = (short*)output; + //buffer->readFrame(out); + + //if (timeInfo->currentTime - ltime < (1.0 / 128.0)) return 0; + //ltime = timeInfo->inputBufferAdcTime; + + //int i=0; + //while (i < frameCount) { + buffer->writeFrame(in); + //i+=2*ftl::audio::kFrameSize; + // + + return 0; +} + +#endif + +Source::Source(nlohmann::json &config) : ftl::Configurable(config), buffer_(48000) { + if (!value("enabled",true)) { + active_ = false; + return; + } + + #ifdef HAVE_PORTAUDIO + ftl::audio::pa_init(); + + PaStreamParameters inputParameters; + //bzero( &inputParameters, sizeof( inputParameters ) ); + inputParameters.channelCount = 2; + inputParameters.device = value("audio_device",-1); + //inputParameters.hostApiSpecificStreamInfo = NULL; + inputParameters.sampleFormat = paInt16; + inputParameters.suggestedLatency = (inputParameters.device >= 0) ? Pa_GetDeviceInfo(inputParameters.device)->defaultLowInputLatency : 0; + inputParameters.hostApiSpecificStreamInfo = NULL; + + PaError err; + + if (inputParameters.device >= 0) { + err = Pa_OpenStream( + &stream_, + &inputParameters, + NULL, + 48000, // Sample rate + ftl::audio::kFrameSize, // Size of single frame + paNoFlag, + pa_source_callback, + &this->buffer_ + ); + } else { + err = Pa_OpenDefaultStream( + &stream_, + 2, + 0, + paInt16, + 48000, // Sample rate + ftl::audio::kFrameSize, // Size of single frame + pa_source_callback, + &this->buffer_ + ); + } + + if (err != paNoError) { + LOG(ERROR) << "Portaudio open stream error: " << Pa_GetErrorText(err); + active_ = false; + return; + } else { + active_ = true; + } + + err = Pa_StartStream(stream_); + + if (err != paNoError) { + LOG(ERROR) << "Portaudio start stream error: " << Pa_GetErrorText(err); + //active_ = false; + return; + } + + to_read_ = 0; + + timer_hp_ = ftl::timer::add(ftl::timer::kTimerHighPrecision, [this](int64_t ts) { + to_read_ = buffer_.size(); + return true; + }); + + timer_main_ = ftl::timer::add(ftl::timer::kTimerMain, [this](int64_t ts) { + + // Remove one interval since the audio starts from the last frame + frameset_.timestamp = ts - ftl::timer::getInterval(); + + frameset_.count = 1; + frameset_.stale = false; + + if (to_read_ < 1) return true; + + if (frameset_.frames.size() < 1) frameset_.frames.emplace_back(); + + auto &frame = frameset_.frames[0]; + frame.reset(); + std::vector<short> &data = frame.create<Audio>(Channel::Audio).data(); + + data.resize(2*ftl::audio::kFrameSize*to_read_); + short *ptr = data.data(); + for (int i=0; i<to_read_; ++i) { + buffer_.readFrame(ptr); + ptr += 2*ftl::audio::kFrameSize; + } + + // Then do something with the data! + //LOG(INFO) << "Audio Frames Sent: " << to_read_ << " - " << ltime; + if (cb_) cb_(frameset_); + + return true; + }); + + #else // No portaudio + + active_ = false; + LOG(ERROR) << "No audio support"; + + #endif +} + +Source::~Source() { + if (active_) { + active_ = false; + + #ifdef HAVE_PORTAUDIO + auto err = Pa_StopStream(stream_); + + if (err != paNoError) { + LOG(ERROR) << "Portaudio stop stream error: " << Pa_GetErrorText(err); + //active_ = false; + } + + err = Pa_CloseStream(stream_); + + if (err != paNoError) { + LOG(ERROR) << "Portaudio close stream error: " << Pa_GetErrorText(err); + } + #endif + } + + #ifdef HAVE_PORTAUDIO + ftl::audio::pa_final(); + #endif +} + +size_t Source::size() { + return 1; +} + +ftl::audio::FrameState &Source::state(int ix) { + if (ix < 0 || ix > 1) throw ftl::exception("State index out-of-bounds"); + return state_; +} + +void Source::onFrameSet(const ftl::audio::FrameSet::Callback &cb) { + cb_ = cb; +} diff --git a/components/audio/src/speaker.cpp b/components/audio/src/speaker.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a0b40f932baa9d6222ac0d7378d164e862a618d3 --- /dev/null +++ b/components/audio/src/speaker.cpp @@ -0,0 +1,110 @@ +#include <ftl/audio/speaker.hpp> +#include <ftl/audio/audio.hpp> +#include <ftl/audio/portaudio.hpp> + +using ftl::audio::Speaker; +using ftl::audio::Frame; +using ftl::audio::FrameSet; +using ftl::audio::FrameState; +using ftl::audio::Audio; +using ftl::codecs::Channel; + +#ifdef HAVE_PORTAUDIO + +/* Portaudio callback to receive audio data. */ +static int pa_speaker_callback(const void *input, void *output, + unsigned long frameCount, const PaStreamCallbackTimeInfo *timeInfo, + PaStreamCallbackFlags statusFlags, void *userData) { + + auto *buffer = (ftl::audio::StereoBuffer16<2000>*)userData; + short *out = (short*)output; + + buffer->readFrame(out); + + return 0; +} + +#endif + +Speaker::Speaker(nlohmann::json &config) : ftl::Configurable(config), buffer_(48000) { + #ifdef HAVE_PORTAUDIO + ftl::audio::pa_init(); + + auto err = Pa_OpenDefaultStream( + &stream_, + 0, + 2, + paInt16, + 48000, // Sample rate + 256, // Size of single frame + pa_speaker_callback, + &this->buffer_ + ); + + if (err != paNoError) { + LOG(ERROR) << "Portaudio open stream error: " << Pa_GetErrorText(err); + active_ = false; + return; + } else { + active_ = true; + } + + err = Pa_StartStream(stream_); + + if (err != paNoError) { + LOG(ERROR) << "Portaudio start stream error: " << Pa_GetErrorText(err); + //active_ = false; + return; + } + + #else // No portaudio + + active_ = false; + LOG(ERROR) << "No audio support"; + + #endif + + extra_delay_ = value("delay",0.0f); + on("delay", [this](const ftl::config::Event &e) { + extra_delay_ = value("delay",0.0f); + }); +} + +Speaker::~Speaker() { + if (active_) { + active_ = false; + + #ifdef HAVE_PORTAUDIO + auto err = Pa_StopStream(stream_); + + if (err != paNoError) { + LOG(ERROR) << "Portaudio stop stream error: " << Pa_GetErrorText(err); + //active_ = false; + } + + err = Pa_CloseStream(stream_); + + if (err != paNoError) { + LOG(ERROR) << "Portaudio close stream error: " << Pa_GetErrorText(err); + } + #endif + } + + #ifdef HAVE_PORTAUDIO + ftl::audio::pa_final(); + #endif +} + +void Speaker::queue(int64_t ts, ftl::audio::Frame &frame) { + auto &audio = frame.get<ftl::audio::Audio>(Channel::Audio); + + LOG(INFO) << "Buffer Fullness (" << ts << "): " << buffer_.size(); + buffer_.write(audio.data()); + LOG(INFO) << "Audio delay: " << buffer_.delay() << "s"; +} + +void Speaker::setDelay(int64_t ms) { + float d = static_cast<float>(ms) / 1000.0f + extra_delay_; + if (d < 0.0f) d = 0.0f; // Clamp to 0 delay (not ideal to be exactly 0) + buffer_.setDelay(d); +} diff --git a/components/codecs/include/ftl/codecs/channels.hpp b/components/codecs/include/ftl/codecs/channels.hpp index ac5b19a12d735a8e68fe27f5c08940996b52bfcd..707702830c2d01e02d42c4a99442e497de601c4c 100644 --- a/components/codecs/include/ftl/codecs/channels.hpp +++ b/components/codecs/include/ftl/codecs/channels.hpp @@ -34,15 +34,20 @@ enum struct Channel : int { Disparity = 18, Smoothing = 19, // 32F + Audio = 32, AudioLeft = 32, AudioRight = 33, Configuration = 64, // JSON Data + Settings1 = 65, Calibration = 65, // Camera Parameters Object Pose = 66, // Eigen::Matrix4d + Settings2 = 67, Calibration2 = 67, // Right camera parameters Index = 68, Control = 69, // For stream and encoder control + Settings3 = 70, + Data = 2048 // Custom data, any codec. }; diff --git a/components/common/cpp/include/ftl/config.h.in b/components/common/cpp/include/ftl/config.h.in index 52fa616b01676ad071723f2d5ea5b6ef0fdedbbe..f36fe3b73b61d3d32b5a2ee564430cf56d7eec89 100644 --- a/components/common/cpp/include/ftl/config.h.in +++ b/components/common/cpp/include/ftl/config.h.in @@ -24,6 +24,7 @@ #cmakedefine HAVE_LIBARCHIVE #cmakedefine HAVE_OPENVR #cmakedefine HAVE_NVPIPE +#cmakedefine HAVE_PORTAUDIO extern const char *FTL_BRANCH; extern const char *FTL_VERSION_LONG; diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt index 405712691cd9cea781fd8cab5e41fdfccef4e05c..06db227d0feaa10e899a7e482dccba042cbd5dd1 100644 --- a/components/rgbd-sources/CMakeLists.txt +++ b/components/rgbd-sources/CMakeLists.txt @@ -41,7 +41,7 @@ if (CUDA_FOUND) set_property(TARGET ftlrgbd PROPERTY CUDA_SEPARABLE_COMPILATION OFF) endif() -target_link_libraries(ftlrgbd ftlcommon ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} Eigen3::Eigen ${REALSENSE_LIBRARY} ftlnet ${LibArchive_LIBRARIES} ftlcodecs ftloperators) +target_link_libraries(ftlrgbd ftlcommon ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} Eigen3::Eigen ${REALSENSE_LIBRARY} ftlnet ${LibArchive_LIBRARIES} ftlcodecs ftloperators ftldata) add_subdirectory(test) diff --git a/components/rgbd-sources/include/ftl/rgbd/frame.hpp b/components/rgbd-sources/include/ftl/rgbd/frame.hpp index 9bdbea42063c750e1129c698461f14bf3b36c444..b894cf07c80e80b51247a89a2b3162595b76e1d9 100644 --- a/components/rgbd-sources/include/ftl/rgbd/frame.hpp +++ b/components/rgbd-sources/include/ftl/rgbd/frame.hpp @@ -8,13 +8,15 @@ #include <opencv2/core/cuda.hpp> #include <opencv2/core/cuda_stream_accessor.hpp> +#include <ftl/data/frame.hpp> + #include <ftl/codecs/channels.hpp> #include <ftl/rgbd/format.hpp> #include <ftl/rgbd/camera.hpp> #include <ftl/codecs/codecs.hpp> #include <ftl/codecs/packet.hpp> #include <ftl/utility/vectorbuffer.hpp> - +#include <ftl/data/framestate.hpp> #include <ftl/cuda_common.hpp> #include <type_traits> @@ -26,136 +28,49 @@ namespace ftl { namespace rgbd { -// TODO: interpolation for scaling depends on channel type; -// NN for depth/disparity/optflow, linear/cubic/etc. for RGB - -class Frame; -class Source; - -/** - * Represent state that is persistent across frames. Such state may or may not - * change from one frame to the next so a record of what has changed must be - * kept. Changing state should be done at origin and not in the frame. State - * that is marked as changed will then be send into a stream and the changed - * status will be cleared, allowing data to only be sent/saved when actual - * changes occur. - */ -class FrameState { - public: - FrameState(); - FrameState(FrameState &); - FrameState(FrameState &&); - - /** - * Update the pose and mark as changed. - */ - void setPose(const Eigen::Matrix4d &pose); - - /** - * Update the left camera intrinsics and mark as changed. - */ - void setLeft(const ftl::rgbd::Camera &p); - - /** - * Update the right camera intrinsics and mark as changed. - */ - void setRight(const ftl::rgbd::Camera &p); - - /** - * Get the current camera pose. - */ - inline const Eigen::Matrix4d &getPose() const { return pose_; } - - /** - * Get the left camera intrinsics. - */ - inline const ftl::rgbd::Camera &getLeft() const { return camera_left_; } - - /** - * Get the right camera intrinsics. - */ - inline const ftl::rgbd::Camera &getRight() const { return camera_right_; } +typedef ftl::data::FrameState<ftl::rgbd::Camera,2> FrameState; - /** - * Get a modifiable pose reference that does not change the changed status. - * @attention Should only be used internally. - * @todo Make private eventually. - */ - inline Eigen::Matrix4d &getPose() { return pose_; } - - /** - * Get a modifiable left camera intrinsics reference that does not change - * the changed status. Modifications made using this will not be propagated. - * @attention Should only be used internally. - * @todo Make private eventually. - */ - inline ftl::rgbd::Camera &getLeft() { return camera_left_; } +struct VideoData { + ftl::cuda::TextureObjectBase tex; + cv::cuda::GpuMat gpu; + cv::Mat host; + bool isgpu; + std::list<ftl::codecs::Packet> encoded; - /** - * Get a modifiable right camera intrinsics reference that does not change - * the changed status. Modifications made using this will not be propagated. - * @attention Should only be used internally. - * @todo Make private eventually. - */ - inline ftl::rgbd::Camera &getRight() { return camera_right_; } - - /** - * Get a named config property. - */ template <typename T> - std::optional<T> get(const std::string &name) { - try { - return config_[name].get<T>(); - } catch (...) { - return {}; - } - } + T &as() { + throw ftl::exception("Unsupported type for Video data channel"); + }; - /** - * Set a named config property. Also makes state as changed to be resent. - */ template <typename T> - void set(const std::string &name, T value) { - config_[name] = value; - changed_ += ftl::codecs::Channel::Configuration; - } - - inline const nlohmann::json &getConfig() const { return config_; } - - inline nlohmann::json &getConfig() { return config_; } - - /** - * Check if pose of intrinsics have been modified and not yet forwarded. - * Once forwarded through a pipeline / stream the changed status is cleared. - */ - inline bool hasChanged(ftl::codecs::Channel c) const { return changed_.has(c); } + const T &as() const { + throw ftl::exception("Unsupported type for Video data channel"); + }; - /** - * Copy assignment will clear the changed status of the original. - */ - FrameState &operator=(FrameState &); + template <typename T> + T &make() { + throw ftl::exception("Unsupported type for Video data channel"); + }; +}; - FrameState &operator=(FrameState &&); +// Specialisations for cv mat types +template <> cv::Mat &VideoData::as<cv::Mat>(); +template <> const cv::Mat &VideoData::as<cv::Mat>() const; +template <> cv::cuda::GpuMat &VideoData::as<cv::cuda::GpuMat>(); +template <> const cv::cuda::GpuMat &VideoData::as<cv::cuda::GpuMat>() const; - /** - * Clear the changed status to unchanged. - */ - inline void clear() { changed_.clear(); } - - private: - Eigen::Matrix4d pose_; - ftl::rgbd::Camera camera_left_; - ftl::rgbd::Camera camera_right_; - nlohmann::json config_; - ftl::codecs::Channels<64> changed_; // Have the state channels changed? -}; +template <> cv::Mat &VideoData::make<cv::Mat>(); +template <> cv::cuda::GpuMat &VideoData::make<cv::cuda::GpuMat>(); /** * Manage a set of image channels corresponding to a single camera frame. */ -class Frame { +class Frame : public ftl::data::Frame<0,32,ftl::rgbd::FrameState,VideoData> { +//class Frame { public: - Frame() : origin_(nullptr) {} + using ftl::data::Frame<0,32,ftl::rgbd::FrameState,VideoData>::create; + + Frame() {} // Prevent frame copy, instead use a move. //Frame(const Frame &)=delete; @@ -172,18 +87,14 @@ public: inline void upload(const ftl::codecs::Channels<0> &c, cudaStream_t stream=0) { upload(c, cv::cuda::StreamAccessor::wrapStream(stream)); }; /** - * Perform a buffer swap of the selected channels. This is intended to be - * a copy from `this` to the passed frame object but by buffer swap - * instead of memory copy, meaning `this` may become invalid afterwards. + * Get an existing CUDA texture object. */ - void swapTo(ftl::codecs::Channels<0>, Frame &); - - void swapChannels(ftl::codecs::Channel, ftl::codecs::Channel); + template <typename T> const ftl::cuda::TextureObject<T> &getTexture(ftl::codecs::Channel) const; /** - * Does a host or device memory copy into the given frame. + * Get an existing CUDA texture object. */ - void copyTo(ftl::codecs::Channels<0>, Frame &); + template <typename T> ftl::cuda::TextureObject<T> &getTexture(ftl::codecs::Channel); /** * Create a channel with a given format. This will discard any existing @@ -192,17 +103,6 @@ public: */ template <typename T> T &create(ftl::codecs::Channel c, const ftl::rgbd::FormatBase &f); - /** - * Create a channel but without any format. - */ - template <typename T> T &create(ftl::codecs::Channel c); - - /** - * Set the value of a channel. Some channels should not be modified via the - * non-const get method, for example the data channels. - */ - template <typename T> void create(ftl::codecs::Channel channel, const T &value); - /** * Create a CUDA texture object for a channel. This version takes a format * argument to also create (or recreate) the associated GpuMat. @@ -245,16 +145,6 @@ public: void resetTexture(ftl::codecs::Channel c); - /** - * Reset all channels without releasing memory. - */ - void reset(); - - /** - * Reset all channels and release memory. - */ - void resetFull(); - /** * Check if any specified channels are empty or missing. */ @@ -264,36 +154,24 @@ public: * Check if a specific channel is missing or has no memory allocated. */ inline bool empty(ftl::codecs::Channel c) { - auto &m = _get(c); + auto &m = getData(c); return !hasChannel(c) || (m.host.empty() && m.gpu.empty()); } - /** - * Is there valid data in channel (either host or gpu). This does not - * verify that any memory or data exists for the channel. - */ - inline bool hasChannel(ftl::codecs::Channel channel) const { - int c = static_cast<int>(channel); - if (c >= 64 && c <= 68) return true; - else if (c >= 2048) return data_channels_.has(channel); - else if (c >= 32) return false; - else return channels_.has(channel); - } - /** * Obtain a mask of all available channels in the frame. */ - inline ftl::codecs::Channels<0> getChannels() const { return channels_; } - inline ftl::codecs::Channels<0> getVideoChannels() const { return channels_; } + inline ftl::codecs::Channels<0> getVideoChannels() const { return getChannels(); } - inline ftl::codecs::Channels<2048> getDataChannels() const { return data_channels_; } + inline const ftl::rgbd::Camera &getLeftCamera() const { return getLeft(); } + inline const ftl::rgbd::Camera &getRightCamera() const { return getRight(); } /** * Is the channel data currently located on GPU. This also returns false if * the channel does not exist. */ inline bool isGPU(ftl::codecs::Channel channel) const { - return channels_.has(channel) && gpu_.has(channel); + return hasChannel(channel) && getData(channel).isgpu; } /** @@ -301,195 +179,21 @@ public: * false if the channel does not exist. */ inline bool isCPU(ftl::codecs::Channel channel) const { - return channels_.has(channel) && !gpu_.has(channel); - } - - /** - * Does this frame have new data for a channel. This is compared with a - * previous frame and always returns true for image data. It may return - * false for persistent state data (calibration, pose etc). - */ - inline bool hasChanged(ftl::codecs::Channel c) const { - return (static_cast<int>(c) < 32) ? true : state_.hasChanged(c); + return hasChannel(channel) && !getData(channel).isgpu; } - - /** - * Method to get reference to the channel content. - * @param Channel type - * @return Const reference to channel data - * - * Result is valid only if hasChannel() is true. Host/Gpu transfer is - * performed, if necessary, but with a warning since an explicit upload or - * download should be used. - */ - template <typename T> const T& get(ftl::codecs::Channel channel) const; - - template <typename T> void get(ftl::codecs::Channel channel, T ¶ms) const; - - /** - * Method to get reference to the channel content. - * @param Channel type - * @return Reference to channel data - * - * Result is valid only if hasChannel() is true. Host/Gpu transfer is - * performed, if necessary, but with a warning since an explicit upload or - * download should be used. - */ - template <typename T> T& get(ftl::codecs::Channel channel); - - /** - * Get an existing CUDA texture object. - */ - template <typename T> const ftl::cuda::TextureObject<T> &getTexture(ftl::codecs::Channel) const; - - /** - * Get an existing CUDA texture object. - */ - template <typename T> ftl::cuda::TextureObject<T> &getTexture(ftl::codecs::Channel); - - /** - * Wrapper accessor function to get frame pose. - */ - const Eigen::Matrix4d &getPose() const; - - /** - * Change the pose of the origin state and mark as changed. - */ - void setPose(const Eigen::Matrix4d &pose); - - /** - * Wrapper to access left camera intrinsics channel. - */ - const ftl::rgbd::Camera &getLeftCamera() const; - - /** - * Wrapper to access right camera intrinsics channel. - */ - const ftl::rgbd::Camera &getRightCamera() const; - - /** - * Change left camera intrinsics in the origin state. This should send - * the changed parameters in reverse through a stream. - */ - void setLeftCamera(const ftl::rgbd::Camera &c); - - /** - * Change right camera intrinsics in the origin state. This should send - * the changed parameters in reverse through a stream. - */ - void setRightCamera(const ftl::rgbd::Camera &c); - - /** - * Dump the current frame config object to a json string. - */ - std::string getConfigString() const; - - /** - * Access the raw data channel vector object. - */ - const std::vector<unsigned char> &getRawData(ftl::codecs::Channel c) const; - - void createRawData(ftl::codecs::Channel c, const std::vector<unsigned char> &v); - - /** - * Wrapper to access a config property. If the property does not exist or - * is not of the requested type then the returned optional is false. - */ - template <typename T> - std::optional<T> get(const std::string &name) { return state_.get<T>(name); } - - /** - * Modify a config property. This does not modify the origin config so - * will not get transmitted over the stream. - * @todo Modify origin to send backwards over a stream. - */ - template <typename T> - void set(const std::string &name, T value) { state_.set(name, value); } - - /** - * Set the persistent state for the frame. This can only be done after - * construction or a reset. Multiple calls to this otherwise will throw - * an exception. The pointer must remain valid for the life of the frame. - */ - void setOrigin(ftl::rgbd::FrameState *state); - - /** - * Get the original frame state object. This can be a nullptr in some rare - * cases. When wishing to change state (pose, calibration etc) then those - * changes must be done on this origin, either directly or via wrappers. - */ - FrameState *origin() const { return origin_; } - -private: - struct ChannelData { - ftl::cuda::TextureObjectBase tex; - cv::Mat host; - cv::cuda::GpuMat gpu; - std::list<ftl::codecs::Packet> encoded; - }; - - std::array<ChannelData, ftl::codecs::Channels<0>::kMax> data_; - std::unordered_map<int, std::vector<unsigned char>> data_data_; - - ftl::codecs::Channels<0> channels_; // Does it have a channel - ftl::codecs::Channels<0> gpu_; // Is the channel on a GPU - ftl::codecs::Channels<2048> data_channels_; - - // Persistent state - FrameState state_; - FrameState *origin_; - - /* Lookup internal state for a given channel. */ - inline ChannelData &_get(ftl::codecs::Channel c) { return data_[static_cast<unsigned int>(c)]; } - inline const ChannelData &_get(ftl::codecs::Channel c) const { return data_[static_cast<unsigned int>(c)]; } }; // Specialisations -template<> const cv::Mat& Frame::get(ftl::codecs::Channel channel) const; -template<> const cv::cuda::GpuMat& Frame::get(ftl::codecs::Channel channel) const; -template<> cv::Mat& Frame::get(ftl::codecs::Channel channel); -template<> cv::cuda::GpuMat& Frame::get(ftl::codecs::Channel channel); - -//template<> const Eigen::Matrix4d &Frame::get(ftl::codecs::Channel channel) const; -template<> const ftl::rgbd::Camera &Frame::get(ftl::codecs::Channel channel) const; - -// Default data channel implementation -template <typename T> -void Frame::get(ftl::codecs::Channel channel, T ¶ms) const { - if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw ftl::exception("Cannot use generic type with non data channel"); - if (!hasChannel(channel)) throw ftl::exception("Data channel does not exist"); - - const auto &i = data_data_.find(static_cast<int>(channel)); - if (i == data_data_.end()) throw ftl::exception("Data channel does not exist"); - - auto unpacked = msgpack::unpack((const char*)(*i).second.data(), (*i).second.size()); - unpacked.get().convert(params); -} - template <> cv::Mat &Frame::create(ftl::codecs::Channel c, const ftl::rgbd::FormatBase &); template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c, const ftl::rgbd::FormatBase &); -template <> cv::Mat &Frame::create(ftl::codecs::Channel c); -template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c); - -template <typename T> -void Frame::create(ftl::codecs::Channel channel, const T &value) { - if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw ftl::exception("Cannot use generic type with non data channel"); - - data_channels_ += channel; - - auto &v = *std::get<0>(data_data_.insert({static_cast<int>(channel),{}})); - v.second.resize(0); - ftl::util::FTLVectorBuffer buf(v.second); - msgpack::pack(buf, value); -} template <typename T> ftl::cuda::TextureObject<T> &Frame::getTexture(ftl::codecs::Channel c) { - if (!channels_.has(c)) throw ftl::exception(ftl::Formatter() << "Texture channel does not exist: " << (int)c); - if (!gpu_.has(c)) throw ftl::exception("Texture channel is not on GPU"); + if (!hasChannel(c)) throw ftl::exception(ftl::Formatter() << "Texture channel does not exist: " << (int)c); - auto &m = _get(c); + auto &m = getData(c); + if (!m.isgpu) throw ftl::exception("Texture channel is not on GPU"); if (m.tex.cvType() != ftl::traits::OpenCVType<T>::value || m.tex.width() != m.gpu.cols || m.tex.height() != m.gpu.rows || m.gpu.type() != m.tex.cvType()) { LOG(ERROR) << "Texture has not been created for channel = " << (int)c; @@ -501,10 +205,11 @@ ftl::cuda::TextureObject<T> &Frame::getTexture(ftl::codecs::Channel c) { template <typename T> ftl::cuda::TextureObject<T> &Frame::createTexture(ftl::codecs::Channel c, const ftl::rgbd::Format<T> &f, bool interpolated) { - if (!channels_.has(c)) channels_ += c; - if (!gpu_.has(c)) gpu_ += c; + //if (!hasChannel(c)) channels_ += c; + //using ftl::data::Frame<0,32,ftl::rgbd::FrameState,VideoData>::create; - auto &m = _get(c); + ftl::data::Frame<0,32,ftl::rgbd::FrameState,VideoData>::create<cv::cuda::GpuMat>(c); + auto &m = getData(c); if (f.empty()) { throw ftl::exception("createTexture needs a non-empty format"); @@ -533,15 +238,15 @@ ftl::cuda::TextureObject<T> &Frame::createTexture(ftl::codecs::Channel c, const template <typename T> ftl::cuda::TextureObject<T> &Frame::createTexture(ftl::codecs::Channel c, bool interpolated) { - if (!channels_.has(c)) throw ftl::exception(ftl::Formatter() << "createTexture needs a format if the channel does not exist: " << (int)c); + if (!hasChannel(c)) throw ftl::exception(ftl::Formatter() << "createTexture needs a format if the channel does not exist: " << (int)c); - auto &m = _get(c); + auto &m = getData(c); - if (isCPU(c) && !m.host.empty()) { + if (!m.isgpu && !m.host.empty()) { m.gpu.create(m.host.size(), m.host.type()); // TODO: Should this upload to GPU or not? //gpu_ += c; - } else if (isCPU(c) || (isGPU(c) && m.gpu.empty())) { + } else if (!m.isgpu || (m.isgpu && m.gpu.empty())) { throw ftl::exception("createTexture needs a format if no memory is allocated"); } diff --git a/components/rgbd-sources/src/frame.cpp b/components/rgbd-sources/src/frame.cpp index 6fb82e19e70a39a7d1e33b8d8fbfef841ac5d071..37ceb7b288296f2f890634b28dbdb5168678a58c 100644 --- a/components/rgbd-sources/src/frame.cpp +++ b/components/rgbd-sources/src/frame.cpp @@ -5,74 +5,50 @@ using ftl::rgbd::Frame; using ftl::rgbd::FrameState; using ftl::codecs::Channels; using ftl::codecs::Channel; +using ftl::rgbd::VideoData; static cv::Mat none; static cv::cuda::GpuMat noneGPU; -FrameState::FrameState() : camera_left_({0}), camera_right_({0}), config_(nlohmann::json::value_t::object) { - pose_ = Eigen::Matrix4d::Identity(); +template <> +cv::Mat &VideoData::as<cv::Mat>() { + if (isgpu) throw ftl::exception("Host request for GPU data without download"); + return host; } -FrameState::FrameState(FrameState &f) { - pose_ = f.pose_; - camera_left_ = f.camera_left_; - camera_right_ = f.camera_right_; - changed_ = f.changed_; - config_ = f.config_; - // TODO: Add mutex lock - f.changed_.clear(); +template <> +const cv::Mat &VideoData::as<cv::Mat>() const { + if (isgpu) throw ftl::exception("Host request for GPU data without download"); + return host; } -FrameState::FrameState(FrameState &&f) { - pose_ = f.pose_; - camera_left_ = f.camera_left_; - camera_right_ = f.camera_right_; - changed_ = f.changed_; - config_ = std::move(f.config_); - // TODO: Add mutex lock - f.changed_.clear(); +template <> +cv::cuda::GpuMat &VideoData::as<cv::cuda::GpuMat>() { + if (!isgpu) throw ftl::exception("GPU request for Host data without upload"); + return gpu; } -FrameState &FrameState::operator=(FrameState &f) { - pose_ = f.pose_; - camera_left_ = f.camera_left_; - camera_right_ = f.camera_right_; - changed_ = f.changed_; - config_ = f.config_; - // TODO: Add mutex lock - f.changed_.clear(); - return *this; +template <> +const cv::cuda::GpuMat &VideoData::as<cv::cuda::GpuMat>() const { + if (!isgpu) throw ftl::exception("GPU request for Host data without upload"); + return gpu; } -FrameState &FrameState::operator=(FrameState &&f) { - pose_ = f.pose_; - camera_left_ = f.camera_left_; - camera_right_ = f.camera_right_; - changed_ = f.changed_; - config_ = std::move(f.config_); - // TODO: Add mutex lock - f.changed_.clear(); - return *this; +template <> +cv::Mat &VideoData::make<cv::Mat>() { + isgpu = false; + return host; } -void FrameState::setPose(const Eigen::Matrix4d &pose) { - pose_ = pose; - changed_ += Channel::Pose; -} - -void FrameState::setLeft(const ftl::rgbd::Camera &p) { - camera_left_ = p; - changed_ += Channel::Calibration; -} - -void FrameState::setRight(const ftl::rgbd::Camera &p) { - camera_right_ = p; - changed_ += Channel::Calibration2; +template <> +cv::cuda::GpuMat &VideoData::make<cv::cuda::GpuMat>() { + isgpu = true; + return gpu; } // ============================================================================= -void Frame::reset() { +/*void Frame::reset() { origin_ = nullptr; channels_.clear(); gpu_.clear(); @@ -91,7 +67,7 @@ void Frame::resetFull() { data_[i].host = cv::Mat(); data_[i].encoded.clear(); } -} +}*/ void Frame::download(Channel c, cv::cuda::Stream stream) { download(Channels(c), stream); @@ -103,25 +79,27 @@ void Frame::upload(Channel c, cv::cuda::Stream stream) { void Frame::download(Channels<0> c, cv::cuda::Stream stream) { for (size_t i=0u; i<Channels<0>::kMax; ++i) { - if (c.has(i) && channels_.has(i) && gpu_.has(i)) { - data_[i].gpu.download(data_[i].host, stream); - gpu_ -= i; + if (c.has(i) && hasChannel(static_cast<Channel>(i)) && isGPU(static_cast<Channel>(i))) { + auto &data = getData(static_cast<Channel>(i)); + data.gpu.download(data.host, stream); + data.isgpu = false; } } } void Frame::upload(Channels<0> c, cv::cuda::Stream stream) { for (size_t i=0u; i<Channels<0>::kMax; ++i) { - if (c.has(i) && channels_.has(i) && !gpu_.has(i)) { - data_[i].gpu.upload(data_[i].host, stream); - gpu_ += i; + if (c.has(i) && hasChannel(static_cast<Channel>(i)) && !isGPU(static_cast<Channel>(i))) { + auto &data = getData(static_cast<Channel>(i)); + data.gpu.upload(data.host, stream); + data.isgpu = true; } } } void Frame::pushPacket(ftl::codecs::Channel c, ftl::codecs::Packet &pkt) { if (hasChannel(c)) { - auto &m1 = _get(c); + auto &m1 = getData(c); m1.encoded.emplace_back() = std::move(pkt); } else { LOG(ERROR) << "Channel " << (int)c << " doesn't exist for packet push"; @@ -133,17 +111,17 @@ const std::list<ftl::codecs::Packet> &Frame::getPackets(ftl::codecs::Channel c) throw ftl::exception(ftl::Formatter() << "Frame channel does not exist: " << (int)c); } - auto &m1 = _get(c); + auto &m1 = getData(c); return m1.encoded; } void Frame::mergeEncoding(ftl::rgbd::Frame &f) { //LOG(INFO) << "MERGE " << (unsigned int)f.channels_; - for (auto c : channels_) { + for (auto c : getChannels()) { //if (!f.hasChannel(c)) f.create<cv::cuda::GpuMat>(c); if (f.hasChannel(c)) { - auto &m1 = _get(c); - auto &m2 = f._get(c); + auto &m1 = getData(c); + auto &m2 = f.getData(c); m1.encoded.splice(m1.encoded.begin(), m2.encoded); //LOG(INFO) << "SPLICED: " << m1.encoded.size(); } @@ -157,7 +135,7 @@ bool Frame::empty(ftl::codecs::Channels<0> channels) { return false; } -void Frame::swapTo(ftl::codecs::Channels<0> channels, Frame &f) { +/*void Frame::swapTo(ftl::codecs::Channels<0> channels, Frame &f) { f.reset(); f.origin_ = origin_; f.state_ = state_; @@ -330,16 +308,15 @@ template<> const nlohmann::json& Frame::get(ftl::codecs::Channel channel) const } throw ftl::exception(ftl::Formatter() << "Invalid configuration channel: " << (int)channel); -} +}*/ template <> cv::Mat &Frame::create(ftl::codecs::Channel c, const ftl::rgbd::FormatBase &f) { if (c == Channel::None) { throw ftl::exception("Cannot create a None channel"); } - channels_ += c; - gpu_ -= c; - - auto &m = _get(c); + + create<cv::Mat>(c); + auto &m = getData(c); m.encoded.clear(); // Remove all old encoded data @@ -354,10 +331,9 @@ template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c, const ftl::r if (c == Channel::None) { throw ftl::exception("Cannot create a None channel"); } - channels_ += c; - gpu_ += c; - auto &m = _get(c); + create<cv::cuda::GpuMat>(c); + auto &m = getData(c); m.encoded.clear(); // Remove all old encoded data @@ -369,11 +345,11 @@ template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c, const ftl::r } void Frame::clearPackets(ftl::codecs::Channel c) { - auto &m = _get(c); + auto &m = getData(c); m.encoded.clear(); } -template <> cv::Mat &Frame::create(ftl::codecs::Channel c) { +/*template <> cv::Mat &Frame::create(ftl::codecs::Channel c) { if (c == Channel::None) { throw ftl::exception("Cannot create a None channel"); } @@ -399,14 +375,14 @@ template <> cv::cuda::GpuMat &Frame::create(ftl::codecs::Channel c) { m.encoded.clear(); // Remove all old encoded data return m.gpu; -} +}*/ void Frame::resetTexture(ftl::codecs::Channel c) { - auto &m = _get(c); + auto &m = getData(c); m.tex.free(); } -void Frame::setOrigin(ftl::rgbd::FrameState *state) { +/*void Frame::setOrigin(ftl::rgbd::FrameState *state) { if (origin_ != nullptr) { throw ftl::exception("Can only set origin once after reset"); } @@ -454,4 +430,4 @@ void ftl::rgbd::Frame::createRawData(ftl::codecs::Channel c, const std::vector<u data_data_.insert({static_cast<int>(c), v}); data_channels_ += c; } - +*/ diff --git a/components/rgbd-sources/src/frameset.cpp b/components/rgbd-sources/src/frameset.cpp index 9cda59838d92f41d0d10e699b3bd7cb530947345..859c3d5d7b306b3666c9f86afc93bf0fdc437584 100644 --- a/components/rgbd-sources/src/frameset.cpp +++ b/components/rgbd-sources/src/frameset.cpp @@ -49,7 +49,7 @@ void FrameSet::resetFull() { //count = 0; //stale = false; for (auto &f : frames) { - f.resetFull(); + //f.resetFull(); } } diff --git a/components/rgbd-sources/test/CMakeLists.txt b/components/rgbd-sources/test/CMakeLists.txt index e16a37c5b7606f49bc3f06d7424ad12f726daee1..9065105d61d56ef02061f580b92d9f9409e1be5a 100644 --- a/components/rgbd-sources/test/CMakeLists.txt +++ b/components/rgbd-sources/test/CMakeLists.txt @@ -6,7 +6,7 @@ add_executable(source_unit ) target_include_directories(source_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include") target_link_libraries(source_unit - ftlcommon ftlcodecs ftlnet Eigen3::Eigen ${CUDA_LIBRARIES}) + ftlcommon ftlcodecs ftlnet Eigen3::Eigen ftldata ${CUDA_LIBRARIES}) add_test(SourceUnitTest source_unit) @@ -18,6 +18,6 @@ add_executable(frame_unit ) target_include_directories(frame_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include") target_link_libraries(frame_unit - ftlcommon ftlcodecs) + ftlcommon ftlcodecs ftldata) add_test(FrameUnitTest frame_unit) diff --git a/components/rgbd-sources/test/frame_unit.cpp b/components/rgbd-sources/test/frame_unit.cpp index 1c7db3e498e2776a0fd75447d6573bfdf3255588..a84025ec71df640cf0e77897cd29e799a5d026c9 100644 --- a/components/rgbd-sources/test/frame_unit.cpp +++ b/components/rgbd-sources/test/frame_unit.cpp @@ -109,7 +109,7 @@ TEST_CASE("Frame::get()", "") { hadexception = true; } - REQUIRE( !hadexception ); + REQUIRE( hadexception ); } SECTION("get a gpu mat from cpu channel") { @@ -130,7 +130,7 @@ TEST_CASE("Frame::get()", "") { hadexception = true; } - REQUIRE( !hadexception ); + REQUIRE( hadexception ); } } diff --git a/components/streams/CMakeLists.txt b/components/streams/CMakeLists.txt index 063f1d7678785408b31cc45aa75b0d311deabf5a..d971532fb5cd5a8b81130fa9b52a16f2a9623ed7 100644 --- a/components/streams/CMakeLists.txt +++ b/components/streams/CMakeLists.txt @@ -19,6 +19,6 @@ target_include_directories(ftlstreams PUBLIC PRIVATE src) #target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include) -target_link_libraries(ftlstreams ftlrgbd ftlcommon ${OpenCV_LIBS} Eigen3::Eigen ftlnet ftlcodecs) +target_link_libraries(ftlstreams ftlrgbd ftlcommon ${OpenCV_LIBS} Eigen3::Eigen ftlnet ftlcodecs ftlaudio) add_subdirectory(test) \ No newline at end of file diff --git a/components/streams/include/ftl/streams/receiver.hpp b/components/streams/include/ftl/streams/receiver.hpp index 1d1fe816c0c8d6070b69b29d58a1d34089dc340e..f79f6000e32d49201348faa83787cd59bd7a9007 100644 --- a/components/streams/include/ftl/streams/receiver.hpp +++ b/components/streams/include/ftl/streams/receiver.hpp @@ -3,6 +3,7 @@ #include <functional> #include <ftl/rgbd/frameset.hpp> +#include <ftl/audio/frameset.hpp> #include <ftl/streams/stream.hpp> #include <ftl/codecs/decoder.hpp> @@ -37,18 +38,19 @@ class Receiver : public ftl::Configurable, public ftl::rgbd::Generator { */ void onFrameSet(const ftl::rgbd::VideoCallback &cb) override; - // void onFrameSet(const AudioCallback &cb); + void onAudio(const ftl::audio::FrameSet::Callback &cb); private: ftl::stream::Stream *stream_; ftl::rgbd::VideoCallback fs_callback_; + ftl::audio::FrameSet::Callback audio_cb_; ftl::rgbd::Builder builder_; ftl::codecs::Channel second_channel_; int64_t timestamp_; SHARED_MUTEX mutex_; - struct InternalStates { - InternalStates(); + struct InternalVideoStates { + InternalVideoStates(); int64_t timestamp; ftl::rgbd::FrameState state; @@ -59,11 +61,27 @@ class Receiver : public ftl::Configurable, public ftl::rgbd::Generator { ftl::codecs::Channels<0> completed; }; - std::vector<InternalStates*> frames_; + struct InternalAudioStates { + InternalAudioStates(); - void _processConfig(InternalStates &frame, const ftl::codecs::Packet &pkt); - void _createDecoder(InternalStates &frame, int chan, const ftl::codecs::Packet &pkt); - InternalStates &_getFrame(const ftl::codecs::StreamPacket &spkt, int ix=0); + int64_t timestamp; + ftl::audio::FrameState state; + ftl::audio::Frame frame; + MUTEX mutex; + ftl::codecs::Channels<0> completed; + }; + + std::vector<InternalVideoStates*> video_frames_; + std::vector<InternalAudioStates*> audio_frames_; + + void _processConfig(InternalVideoStates &frame, const ftl::codecs::Packet &pkt); + void _processState(const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt); + void _processData(const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt); + void _processAudio(const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt); + void _processVideo(const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt); + void _createDecoder(InternalVideoStates &frame, int chan, const ftl::codecs::Packet &pkt); + InternalVideoStates &_getVideoFrame(const ftl::codecs::StreamPacket &spkt, int ix=0); + InternalAudioStates &_getAudioFrame(const ftl::codecs::StreamPacket &spkt, int ix=0); }; } diff --git a/components/streams/include/ftl/streams/sender.hpp b/components/streams/include/ftl/streams/sender.hpp index 31022ee458fea27e6cef31ebcf1008f0c6741b47..c9c4091f50214144e86fe55a3c6f04a003ab8836 100644 --- a/components/streams/include/ftl/streams/sender.hpp +++ b/components/streams/include/ftl/streams/sender.hpp @@ -3,6 +3,7 @@ #include <functional> #include <ftl/rgbd/frameset.hpp> +#include <ftl/audio/frameset.hpp> #include <ftl/streams/stream.hpp> #include <ftl/codecs/encoder.hpp> @@ -27,6 +28,11 @@ class Sender : public ftl::Configurable { */ void post(const ftl::rgbd::FrameSet &fs); + /** + * Encode and transmit a set of audio channels. + */ + void post(const ftl::audio::FrameSet &fs); + //void onStateChange(const std::function<void(ftl::codecs::Channel, int, int)>&); void onRequest(const ftl::stream::StreamCallback &); diff --git a/components/streams/src/receiver.cpp b/components/streams/src/receiver.cpp index 5e525ea0fdc3e4d061d210a4a942be1ba644dfac..8d07365d7ad5a8e9c5f614e2099c8afaced538c5 100644 --- a/components/streams/src/receiver.cpp +++ b/components/streams/src/receiver.cpp @@ -25,6 +25,10 @@ Receiver::~Receiver() { } +void Receiver::onAudio(const ftl::audio::FrameSet::Callback &cb) { + audio_cb_ = cb; +} + /*void Receiver::_processConfig(InternalStates &frame, const ftl::codecs::Packet &pkt) { std::string cfg; auto unpacked = msgpack::unpack((const char*)pkt.data.data(), pkt.data.size()); @@ -35,7 +39,7 @@ Receiver::~Receiver() { //host_->set(std::get<0>(cfg), nlohmann::json::parse(std::get<1>(cfg))); }*/ -void Receiver::_createDecoder(InternalStates &frame, int chan, const ftl::codecs::Packet &pkt) { +void Receiver::_createDecoder(InternalVideoStates &frame, int chan, const ftl::codecs::Packet &pkt) { //UNIQUE_LOCK(mutex_,lk); auto *decoder = frame.decoders[chan]; if (decoder) { @@ -51,188 +55,239 @@ void Receiver::_createDecoder(InternalStates &frame, int chan, const ftl::codecs frame.decoders[chan] = ftl::codecs::allocateDecoder(pkt); } -Receiver::InternalStates::InternalStates() { +Receiver::InternalVideoStates::InternalVideoStates() { for (int i=0; i<32; ++i) decoders[i] = nullptr; } -Receiver::InternalStates &Receiver::_getFrame(const StreamPacket &spkt, int ix) { +Receiver::InternalVideoStates &Receiver::_getVideoFrame(const StreamPacket &spkt, int ix) { int fn = spkt.frameNumber()+ix; UNIQUE_LOCK(mutex_, lk); - while (frames_.size() <= fn) { + while (video_frames_.size() <= fn) { //frames_.resize(spkt.frameNumber()+1); - frames_.push_back(new InternalStates); - frames_[frames_.size()-1]->state.set("name",std::string("Source ")+std::to_string(fn+1)); + video_frames_.push_back(new InternalVideoStates); + video_frames_[video_frames_.size()-1]->state.set("name",std::string("Source ")+std::to_string(fn+1)); } - auto &f = *frames_[fn]; + auto &f = *video_frames_[fn]; if (!f.frame.origin()) f.frame.setOrigin(&f.state); return f; } -void Receiver::setStream(ftl::stream::Stream *s) { - if (stream_) { - stream_->onPacket(nullptr); +Receiver::InternalAudioStates::InternalAudioStates() { + +} + +Receiver::InternalAudioStates &Receiver::_getAudioFrame(const StreamPacket &spkt, int ix) { + int fn = spkt.frameNumber()+ix; + + UNIQUE_LOCK(mutex_, lk); + while (audio_frames_.size() <= fn) { + //frames_.resize(spkt.frameNumber()+1); + audio_frames_.push_back(new InternalAudioStates); + audio_frames_[audio_frames_.size()-1]->state.set("name",std::string("Source ")+std::to_string(fn+1)); } + auto &f = *audio_frames_[fn]; + if (!f.frame.origin()) f.frame.setOrigin(&f.state); + return f; +} - stream_ = s; +void Receiver::_processState(const StreamPacket &spkt, const Packet &pkt) { + for (int i=0; i<pkt.frame_count; ++i) { + InternalVideoStates &frame = _getVideoFrame(spkt,i); + + // Deal with the special channels... + switch (spkt.channel) { + case Channel::Configuration : frame.state.getConfig() = nlohmann::json::parse(parseConfig(pkt)); break; + case Channel::Calibration : frame.state.getLeft() = parseCalibration(pkt); break; + case Channel::Calibration2 : frame.state.getRight() = parseCalibration(pkt); break; + case Channel::Pose : frame.state.getPose() = parsePose(pkt); break; + default: break; + } + } +} - s->onPacket([this](const StreamPacket &spkt, const Packet &pkt) { - //const ftl::codecs::Channel chan = second_channel_; - const ftl::codecs::Channel rchan = spkt.channel; - const unsigned int channum = (unsigned int)spkt.channel; +void Receiver::_processData(const StreamPacket &spkt, const Packet &pkt) { + InternalVideoStates &frame = _getVideoFrame(spkt); + frame.frame.createRawData(spkt.channel, pkt.data); +} - //LOG(INFO) << "PACKET: " << spkt.timestamp << ", " << (int)spkt.channel << ", " << (int)pkt.codec << ", " << (int)pkt.definition; +void Receiver::_processAudio(const StreamPacket &spkt, const Packet &pkt) { + // Audio Data + InternalAudioStates &frame = _getAudioFrame(spkt); + + frame.timestamp = spkt.timestamp; + auto &audio = frame.frame.create<ftl::audio::Audio>(spkt.channel); + size_t size = pkt.data.size()/sizeof(short); + audio.data().resize(size); + auto *ptr = (short*)pkt.data.data(); + for (int i=0; i<size; i++) audio.data()[i] = ptr[i]; + + if (audio_cb_) { + // Create an audio frameset wrapper. + ftl::audio::FrameSet fs; + fs.timestamp = frame.timestamp; + fs.count = 1; + fs.stale = false; + frame.frame.swapTo(fs.frames.emplace_back()); + + audio_cb_(fs); + } +} - // TODO: Allow for multiple framesets - if (spkt.frameSetID() > 0) return; +void Receiver::_processVideo(const StreamPacket &spkt, const Packet &pkt) { + const ftl::codecs::Channel rchan = spkt.channel; + const unsigned int channum = (unsigned int)spkt.channel; + InternalVideoStates &iframe = _getVideoFrame(spkt); + + auto [tx,ty] = ftl::codecs::chooseTileConfig(pkt.frame_count); + int width = ftl::codecs::getWidth(pkt.definition); + int height = ftl::codecs::getHeight(pkt.definition); + + for (int i=0; i<pkt.frame_count; ++i) { + InternalVideoStates &frame = _getVideoFrame(spkt,i); + + // Packets are for unwanted channel. + //if (rchan != Channel::Colour && rchan != chan) return; + + if (frame.frame.hasChannel(spkt.channel)) { + // FIXME: Is this a corruption in recording or in playback? + // Seems to occur in same place in ftl file, one channel is missing + LOG(ERROR) << "Previous frame not complete: " << frame.timestamp; + //LOG(ERROR) << " --- " << (string)spkt; + UNIQUE_LOCK(frame.mutex, lk); + frame.frame.reset(); + frame.completed.clear(); + } + frame.timestamp = spkt.timestamp; - // Too many frames, so ignore. - if (spkt.frameNumber() >= value("max_frames",32)) return; + // Add channel to frame and allocate memory if required + const cv::Size size = cv::Size(width, height); + frame.frame.create<cv::cuda::GpuMat>(spkt.channel).create(size, (isFloatChannel(rchan) ? CV_32FC1 : CV_8UC4)); + } - // Dummy no data packet. - if (pkt.data.size() == 0) return; + Packet tmppkt = pkt; + iframe.frame.pushPacket(spkt.channel, tmppkt); - InternalStates &iframe = _getFrame(spkt); + //LOG(INFO) << " CODEC = " << (int)pkt.codec << " " << (int)pkt.flags << " " << (int)spkt.channel; + //LOG(INFO) << "Decode surface: " << (width*tx) << "x" << (height*ty); - auto [tx,ty] = ftl::codecs::chooseTileConfig(pkt.frame_count); - int width = ftl::codecs::getWidth(pkt.definition); - int height = ftl::codecs::getHeight(pkt.definition); + auto &surface = iframe.surface[static_cast<int>(spkt.channel)]; + surface.create(height*ty, width*tx, ((isFloatChannel(spkt.channel)) ? ((pkt.flags & 0x2) ? CV_16UC4 : CV_16U) : CV_8UC4)); - //if (spkt.timestamp > frame.timestamp && !frame.completed.empty()) { - // LOG(WARNING) << "Next frame received"; - //return; - //} + // Do the actual decode + _createDecoder(iframe, channum, pkt); + auto *decoder = iframe.decoders[channum]; + if (!decoder) { + LOG(ERROR) << "No frame decoder available"; + return; + } - if (channum >= 2048) { - InternalStates &frame = _getFrame(spkt); - frame.frame.createRawData(spkt.channel, pkt.data); - return; - } else if (channum >= 64) { - for (int i=0; i<pkt.frame_count; ++i) { - InternalStates &frame = _getFrame(spkt,i); - - // Deal with the special channels... - switch (rchan) { - case Channel::Configuration : frame.state.getConfig() = nlohmann::json::parse(parseConfig(pkt)); break; - case Channel::Calibration : frame.state.getLeft() = parseCalibration(pkt); break; - case Channel::Calibration2 : frame.state.getRight() = parseCalibration(pkt); break; - case Channel::Pose : frame.state.getPose() = parsePose(pkt); break; - default: break; - } - } - return; + try { + //LOG(INFO) << "TYPE = " << frame.frame.get<cv::cuda::GpuMat>(spkt.channel).type(); + if (!decoder->decode(pkt, surface)) { + LOG(ERROR) << "Decode failed"; + //return; } + } catch (std::exception &e) { + LOG(ERROR) << "Decode failed for " << spkt.timestamp << ": " << e.what(); + //return; + } - for (int i=0; i<pkt.frame_count; ++i) { - InternalStates &frame = _getFrame(spkt,i); + /*if (spkt.channel == Channel::Depth && (pkt.flags & 0x2)) { + cv::Mat tmp; + surface.download(tmp); + cv::imshow("Test", tmp); + cv::waitKey(1); + }*/ + + for (int i=0; i<pkt.frame_count; ++i) { + InternalVideoStates &frame = _getVideoFrame(spkt,i); + + cv::Rect roi((i % tx)*width, (i / tx)*height, width, height); + cv::cuda::GpuMat sroi = surface(roi); + + // Do colour conversion + if (isFloatChannel(rchan) && (pkt.flags & 0x2)) { + //LOG(INFO) << "VUYA Convert"; + ftl::cuda::vuya_to_depth(frame.frame.get<cv::cuda::GpuMat>(spkt.channel), sroi, 16.0f, decoder->stream()); + } else if (isFloatChannel(rchan)) { + sroi.convertTo(frame.frame.get<cv::cuda::GpuMat>(spkt.channel), CV_32FC1, 1.0f/1000.0f, decoder->stream()); + } else { + cv::cuda::cvtColor(sroi, frame.frame.get<cv::cuda::GpuMat>(spkt.channel), cv::COLOR_RGBA2BGRA, 0, decoder->stream()); + } + } - // Packets are for unwanted channel. - //if (rchan != Channel::Colour && rchan != chan) return; + decoder->stream().waitForCompletion(); - if (frame.frame.hasChannel(spkt.channel)) { - // FIXME: Is this a corruption in recording or in playback? - // Seems to occur in same place in ftl file, one channel is missing - LOG(ERROR) << "Previous frame not complete: " << frame.timestamp; - //LOG(ERROR) << " --- " << (string)spkt; - UNIQUE_LOCK(frame.mutex, lk); - frame.frame.reset(); - frame.completed.clear(); - } - frame.timestamp = spkt.timestamp; + for (int i=0; i<pkt.frame_count; ++i) { + InternalVideoStates &frame = _getVideoFrame(spkt,i); + + frame.completed += spkt.channel; + + // Complete if all requested frames are found + auto sel = stream_->selected(spkt.frameSetID()); - // Add channel to frame and allocate memory if required - const cv::Size size = cv::Size(width, height); - frame.frame.create<cv::cuda::GpuMat>(spkt.channel).create(size, (isFloatChannel(rchan) ? CV_32FC1 : CV_8UC4)); - } + if ((frame.completed & sel) == sel) { + UNIQUE_LOCK(frame.mutex, lk); + if ((frame.completed & sel) == sel) { + timestamp_ = frame.timestamp; - Packet tmppkt = pkt; - iframe.frame.pushPacket(spkt.channel, tmppkt); + //LOG(INFO) << "BUILDER PUSH: " << timestamp_ << ", " << spkt.frameNumber(); - //LOG(INFO) << " CODEC = " << (int)pkt.codec << " " << (int)pkt.flags << " " << (int)spkt.channel; - //LOG(INFO) << "Decode surface: " << (width*tx) << "x" << (height*ty); + if (frame.state.getLeft().width == 0) { + LOG(WARNING) << "Missing calibration, skipping frame"; + //frame.frame.reset(); + //frame.completed.clear(); + //return; + } - auto &surface = iframe.surface[static_cast<int>(spkt.channel)]; - surface.create(height*ty, width*tx, ((isFloatChannel(spkt.channel)) ? ((pkt.flags & 0x2) ? CV_16UC4 : CV_16U) : CV_8UC4)); + // TODO: Have multiple builders for different framesets. + builder_.push(frame.timestamp, spkt.frameNumber()+i, frame.frame); - // Do the actual decode - _createDecoder(iframe, channum, pkt); - auto *decoder = iframe.decoders[channum]; - if (!decoder) { - LOG(ERROR) << "No frame decoder available"; - return; - } + // Check for any state changes and send them back + if (frame.state.hasChanged(Channel::Pose)) injectPose(stream_, frame.frame, frame.timestamp, spkt.frameNumber()+i); + if (frame.state.hasChanged(Channel::Calibration)) injectCalibration(stream_, frame.frame, frame.timestamp, spkt.frameNumber()+i); + if (frame.state.hasChanged(Channel::Calibration2)) injectCalibration(stream_, frame.frame, frame.timestamp, spkt.frameNumber()+i, true); - try { - //LOG(INFO) << "TYPE = " << frame.frame.get<cv::cuda::GpuMat>(spkt.channel).type(); - if (!decoder->decode(pkt, surface)) { - LOG(ERROR) << "Decode failed"; - //return; + frame.frame.reset(); + frame.completed.clear(); } - } catch (std::exception &e) { - LOG(ERROR) << "Decode failed for " << spkt.timestamp << ": " << e.what(); - //return; } + } +} - /*if (spkt.channel == Channel::Depth && (pkt.flags & 0x2)) { - cv::Mat tmp; - surface.download(tmp); - cv::imshow("Test", tmp); - cv::waitKey(1); - }*/ - - for (int i=0; i<pkt.frame_count; ++i) { - InternalStates &frame = _getFrame(spkt,i); - - cv::Rect roi((i % tx)*width, (i / tx)*height, width, height); - cv::cuda::GpuMat sroi = surface(roi); - - // Do colour conversion - if (isFloatChannel(rchan) && (pkt.flags & 0x2)) { - //LOG(INFO) << "VUYA Convert"; - ftl::cuda::vuya_to_depth(frame.frame.get<cv::cuda::GpuMat>(spkt.channel), sroi, 16.0f, decoder->stream()); - } else if (isFloatChannel(rchan)) { - sroi.convertTo(frame.frame.get<cv::cuda::GpuMat>(spkt.channel), CV_32FC1, 1.0f/1000.0f, decoder->stream()); - } else { - cv::cuda::cvtColor(sroi, frame.frame.get<cv::cuda::GpuMat>(spkt.channel), cv::COLOR_RGBA2BGRA, 0, decoder->stream()); - } - } +void Receiver::setStream(ftl::stream::Stream *s) { + if (stream_) { + stream_->onPacket(nullptr); + } - decoder->stream().waitForCompletion(); + stream_ = s; - for (int i=0; i<pkt.frame_count; ++i) { - InternalStates &frame = _getFrame(spkt,i); - - frame.completed += spkt.channel; - - // Complete if all requested frames are found - auto sel = stream_->selected(spkt.frameSetID()); + s->onPacket([this](const StreamPacket &spkt, const Packet &pkt) { + //const ftl::codecs::Channel chan = second_channel_; + const ftl::codecs::Channel rchan = spkt.channel; + const unsigned int channum = (unsigned int)spkt.channel; - if ((frame.completed & sel) == sel) { - UNIQUE_LOCK(frame.mutex, lk); - if ((frame.completed & sel) == sel) { - timestamp_ = frame.timestamp; + //LOG(INFO) << "PACKET: " << spkt.timestamp << ", " << (int)spkt.channel << ", " << (int)pkt.codec << ", " << (int)pkt.definition; - //LOG(INFO) << "BUILDER PUSH: " << timestamp_ << ", " << spkt.frameNumber(); + // TODO: Allow for multiple framesets + if (spkt.frameSetID() > 0) return; - if (frame.state.getLeft().width == 0) { - LOG(WARNING) << "Missing calibration, skipping frame"; - //frame.frame.reset(); - //frame.completed.clear(); - //return; - } + // Too many frames, so ignore. + if (spkt.frameNumber() >= value("max_frames",32)) return; - // TODO: Have multiple builders for different framesets. - builder_.push(frame.timestamp, spkt.frameNumber()+i, frame.frame); + // Dummy no data packet. + if (pkt.data.size() == 0) return; - // Check for any state changes and send them back - if (frame.state.hasChanged(Channel::Pose)) injectPose(stream_, frame.frame, frame.timestamp, spkt.frameNumber()+i); - if (frame.state.hasChanged(Channel::Calibration)) injectCalibration(stream_, frame.frame, frame.timestamp, spkt.frameNumber()+i); - if (frame.state.hasChanged(Channel::Calibration2)) injectCalibration(stream_, frame.frame, frame.timestamp, spkt.frameNumber()+i, true); - frame.frame.reset(); - frame.completed.clear(); - } - } + if (channum >= 2048) { + _processData(spkt,pkt); + } else if (channum >= 64) { + _processState(spkt,pkt); + } else if (channum >= 32 && channum < 64) { + _processAudio(spkt,pkt); + } else { + _processVideo(spkt,pkt); } }); } diff --git a/components/streams/src/sender.cpp b/components/streams/src/sender.cpp index db7bb7c04467ab623c64df93a32d4a2921c8b8bf..a32a4a848766b7a57bf28c52e7cd984fa2a1da43 100644 --- a/components/streams/src/sender.cpp +++ b/components/streams/src/sender.cpp @@ -52,6 +52,40 @@ void Sender::onRequest(const ftl::stream::StreamCallback &cb) { reqcb_ = cb; } +void Sender::post(const ftl::audio::FrameSet &fs) { + if (!stream_) return; + + //if (fs.stale) return; + //fs.stale = true; + + for (int i=0; i<fs.frames.size(); ++i) { + if (!fs.frames[i].hasChannel(Channel::Audio)) continue; + + auto &data = fs.frames[i].get<ftl::audio::Audio>(Channel::Audio); + + StreamPacket spkt; + spkt.version = 4; + spkt.timestamp = fs.timestamp; + spkt.streamID = 0; //fs.id; + spkt.frame_number = i; + spkt.channel = Channel::Audio; + + ftl::codecs::Packet pkt; + pkt.codec = ftl::codecs::codec_t::RAW; + pkt.definition = ftl::codecs::definition_t::Any; + pkt.frame_count = 1; + pkt.flags = 0; + pkt.bitrate = 0; + + const unsigned char *ptr = (unsigned char*)data.data().data(); + pkt.data = std::move(std::vector<unsigned char>(ptr, ptr+data.size())); // TODO: Reduce copy... + + stream_->post(spkt, pkt); + + //LOG(INFO) << "SENT AUDIO: " << fs.timestamp << " - " << pkt.data.size(); + } +} + template <typename T> static void writeValue(std::vector<unsigned char> &data, T value) { unsigned char *pvalue_start = (unsigned char*)&value; diff --git a/components/streams/test/CMakeLists.txt b/components/streams/test/CMakeLists.txt index 859e1bcbd8c41b1fbcbe0bd11bef30b3d0ca1691..cb2b1e5a194cb056841bddffad02ae9ffe64a483 100644 --- a/components/streams/test/CMakeLists.txt +++ b/components/streams/test/CMakeLists.txt @@ -46,7 +46,7 @@ add_executable(sender_unit ) target_include_directories(sender_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include") target_link_libraries(sender_unit - ftlcommon ftlcodecs ftlrgbd) + ftlcommon ftlcodecs ftlrgbd ftlaudio) add_test(SenderUnitTest sender_unit) @@ -61,6 +61,6 @@ add_executable(receiver_unit ) target_include_directories(receiver_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include") target_link_libraries(receiver_unit - ftlcommon ftlcodecs ftlrgbd) + ftlcommon ftlcodecs ftlrgbd ftlaudio) add_test(ReceiverUnitTest receiver_unit) diff --git a/components/structures/CMakeLists.txt b/components/structures/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..fbbddc846b25e50493300c31a7ed57b87bc2bcbf --- /dev/null +++ b/components/structures/CMakeLists.txt @@ -0,0 +1,10 @@ + +add_library(ftldata INTERFACE) + +target_include_directories(ftldata INTERFACE + ${CMAKE_CURRENT_SOURCE_DIR}/include) + +target_link_libraries(ftldata INTERFACE ftlcommon Eigen3::Eigen ftlcodecs) + +#add_subdirectory(test) + diff --git a/components/structures/include/ftl/data/frame.hpp b/components/structures/include/ftl/data/frame.hpp new file mode 100644 index 0000000000000000000000000000000000000000..eb8ae3e00bd05f59391d07d98a31c84cd457744d --- /dev/null +++ b/components/structures/include/ftl/data/frame.hpp @@ -0,0 +1,466 @@ +#pragma once +#ifndef _FTL_DATA_FRAME_HPP_ +#define _FTL_DATA_FRAME_HPP_ + +#include <ftl/configuration.hpp> +#include <ftl/exception.hpp> + +#include <ftl/codecs/channels.hpp> +#include <ftl/codecs/codecs.hpp> +//#include <ftl/codecs/packet.hpp> +#include <ftl/utility/vectorbuffer.hpp> + +#include <type_traits> +#include <array> +//#include <list> +#include <unordered_map> + +#include <Eigen/Eigen> + +namespace ftl { +namespace data { + +/** + * Manage a set of channels corresponding to a single frame. There are three + * kinds of channel in a frame: 1) the data type of interest (DoI) + * (eg. audio, video, etc), 2) Persistent state and 3) Generic meta data. + * The DoI is a template arg and could be in any form. Different DoIs will use + * different frame instances, ie. audio and video frame types. Persistent state + * may or may not change between frames but is always available. Finally, + * generic data is a small amount of information about the primary data that may + * or may not exist each frame, and is not required to exist. + * + * There is no specification for frame rates, intervals or synchronisation at + * this level. A frame is a quantum of data of any temporal size which can be + * added to a FrameSet to be synchronised with other frames. + * + * Use this template class either by inheriting it or just by providing the + * template arguments. It is not abstract and can work directly. + * + * The template DATA parameter must be a class or struct that implements three + * methods: 1) `const T& at<T>()` to cast to const type, 2) `T& at<T>()` to cast + * to non-const type, and 3) `T& make<T>() to create data as a type. + * + * The STATE parameter must be an instance of `ftl::data::FrameState`. + * + * @see ftl::data::FrameState + * @see ftl::data::FrameSet + * @see ftl::rgbd::FrameState + * @see ftl::rgbd::Frame + */ +template <int BASE, int N, typename STATE, typename DATA> +class Frame { + static_assert(N <= ftl::codecs::Channels<BASE>::kMax, "Too many channels requested"); + +public: + Frame() : origin_(nullptr) {} + + // Prevent frame copy, instead use a move. + //Frame(const Frame &)=delete; + //Frame &operator=(const Frame &)=delete; + + /** + * Perform a buffer swap of the selected channels. This is intended to be + * a copy from `this` to the passed frame object but by buffer swap + * instead of memory copy, meaning `this` may become invalid afterwards. + */ + void swapTo(ftl::codecs::Channels<BASE>, Frame &); + + void swapTo(Frame &); + + void swapChannels(ftl::codecs::Channel, ftl::codecs::Channel); + + /** + * Does a host or device memory copy into the given frame. + */ + void copyTo(ftl::codecs::Channels<BASE>, Frame &); + + /** + * Create a channel but without any format. + */ + template <typename T> T &create(ftl::codecs::Channel c); + + /** + * Set the value of a channel. Some channels should not be modified via the + * non-const get method, for example the data channels. + */ + template <typename T> void create(ftl::codecs::Channel channel, const T &value); + + /** + * Append encoded data for a channel. This will move the data, invalidating + * the original packet structure. It is to be used to allow data that is + * already encoded to be transmitted or saved again without re-encoding. + * A called to `create` will clear all encoded data for that channel. + */ + //void pushPacket(ftl::codecs::Channel c, ftl::codecs::Packet &pkt); + + /** + * Obtain a list of any existing encodings for this channel. + */ + //const std::list<ftl::codecs::Packet> &getPackets(ftl::codecs::Channel c) const; + + /** + * Clear any existing encoded packets. Used when the channel data is + * modified and the encodings are therefore out-of-date. + */ + //void clearPackets(ftl::codecs::Channel c); + + /** + * Reset all channels without releasing memory. + */ + void reset(); + + /** + * Reset all channels and release memory. + */ + //void resetFull(); + + /** + * Is there valid data in channel (either host or gpu). This does not + * verify that any memory or data exists for the channel. + */ + bool hasChannel(ftl::codecs::Channel channel) const { + int c = static_cast<int>(channel); + if (c >= 64 && c <= 68) return true; + else if (c >= 2048) return data_channels_.has(channel); + else if (c < BASE || c >= BASE+N) return false; + else return channels_.has(channel); + } + + /** + * Obtain a mask of all available channels in the frame. + */ + inline ftl::codecs::Channels<BASE> getChannels() const { return channels_; } + + inline ftl::codecs::Channels<2048> getDataChannels() const { return data_channels_; } + + /** + * Does this frame have new data for a channel. This is compared with a + * previous frame and always returns true for image data. It may return + * false for persistent state data (calibration, pose etc). + */ + inline bool hasChanged(ftl::codecs::Channel c) const { + return (static_cast<int>(c) < 64) ? true : state_.hasChanged(c); + } + + /** + * Method to get reference to the channel content. + * @param Channel type + * @return Const reference to channel data + * + * Result is valid only if hasChannel() is true. Host/Gpu transfer is + * performed, if necessary, but with a warning since an explicit upload or + * download should be used. + */ + template <typename T> const T& get(ftl::codecs::Channel channel) const; + + /** + * Get the data from a data channel. This only works for the data channels + * and will throw an exception with any others. + */ + template <typename T> void get(ftl::codecs::Channel channel, T ¶ms) const; + + /** + * Method to get reference to the channel content. + * @param Channel type + * @return Reference to channel data + * + * Result is valid only if hasChannel() is true. + */ + template <typename T> T& get(ftl::codecs::Channel channel); + + /** + * Wrapper accessor function to get frame pose. + */ + const Eigen::Matrix4d &getPose() const; + + /** + * Change the pose of the origin state and mark as changed. + */ + void setPose(const Eigen::Matrix4d &pose); + + /** + * Wrapper to access left settings channel. + */ + const typename STATE::Settings &getSettings() const; + + const typename STATE::Settings &getLeft() const; + const typename STATE::Settings &getRight() const; + + void setLeft(const typename STATE::Settings &); + void setRight(const typename STATE::Settings &); + + /** + * Change left settings in the origin state. This should send + * the changed parameters in reverse through a stream. + */ + void setSettings(const typename STATE::Settings &c); + + /** + * Dump the current frame config object to a json string. + */ + std::string getConfigString() const; + + /** + * Access the raw data channel vector object. + */ + const std::vector<unsigned char> &getRawData(ftl::codecs::Channel c) const; + + /** + * Provide raw data for a data channel. + */ + void createRawData(ftl::codecs::Channel c, const std::vector<unsigned char> &v); + + /** + * Wrapper to access a config property. If the property does not exist or + * is not of the requested type then the returned optional is false. + */ + template <class T> + std::optional<T> get(const std::string &name) { return state_.template get<T>(name); } + + /** + * Modify a config property. This does not modify the origin config so + * will not get transmitted over the stream. + * @todo Modify origin to send backwards over a stream. + */ + template <typename T> + void set(const std::string &name, T value) { state_.set(name, value); } + + /** + * Set the persistent state for the frame. This can only be done after + * construction or a reset. Multiple calls to this otherwise will throw + * an exception. The pointer must remain valid for the life of the frame. + */ + void setOrigin(STATE *state); + + /** + * Get the original frame state object. This can be a nullptr in some rare + * cases. When wishing to change state (pose, calibration etc) then those + * changes must be done on this origin, either directly or via wrappers. + */ + STATE *origin() const { return origin_; } + + typedef STATE State; + +protected: + /* Lookup internal state for a given channel. */ + inline DATA &getData(ftl::codecs::Channel c) { return data_[static_cast<unsigned int>(c)-BASE]; } + inline const DATA &getData(ftl::codecs::Channel c) const { return data_[static_cast<unsigned int>(c)-BASE]; } + +private: + std::array<DATA, N> data_; + + std::unordered_map<int, std::vector<unsigned char>> data_data_; + + ftl::codecs::Channels<BASE> channels_; // Does it have a channel + ftl::codecs::Channels<2048> data_channels_; + + // Persistent state + STATE state_; + STATE *origin_; +}; + +} +} + +// ==== Implementations ======================================================== + +template <int BASE, int N, typename STATE, typename DATA> +void ftl::data::Frame<BASE,N,STATE,DATA>::reset() { + origin_ = nullptr; + channels_.clear(); + data_channels_.clear(); +} + +template <int BASE, int N, typename STATE, typename DATA> +void ftl::data::Frame<BASE,N,STATE,DATA>::swapTo(ftl::codecs::Channels<BASE> channels, Frame<BASE,N,STATE,DATA> &f) { + f.reset(); + f.origin_ = origin_; + f.state_ = state_; + + // For all channels in this frame object + for (auto c : channels_) { + // Should we swap this channel? + if (channels.has(c)) { + f.channels_ += c; + f.getData(c) = std::move(getData(c)); + } + } + + f.data_data_ = std::move(data_data_); + f.data_channels_ = data_channels_; + data_channels_.clear(); + channels_.clear(); +} + +template <int BASE, int N, typename STATE, typename DATA> +void ftl::data::Frame<BASE,N,STATE,DATA>::swapTo(Frame<BASE,N,STATE,DATA> &f) { + swapTo(ftl::codecs::Channels<BASE>::All(), f); +} + +template <int BASE, int N, typename STATE, typename DATA> +void ftl::data::Frame<BASE,N,STATE,DATA>::swapChannels(ftl::codecs::Channel a, ftl::codecs::Channel b) { + auto &m1 = getData(a); + auto &m2 = getData(b); + + auto temp = std::move(m2); + m2 = std::move(m1); + m1 = std::move(temp); +} + +template <int BASE, int N, typename STATE, typename DATA> +void ftl::data::Frame<BASE,N,STATE,DATA>::copyTo(ftl::codecs::Channels<BASE> channels, Frame<BASE,N,STATE,DATA> &f) { + f.reset(); + f.origin_ = origin_; + f.state_ = state_; + + // For all channels in this frame object + for (auto c : channels_) { + // Should we copy this channel? + if (channels.has(c)) { + f.channels_ += c; + f.getData(c) = getData(c); + } + } + + f.data_data_ = data_data_; + f.data_channels_ = data_channels_; +} + +template <int BASE, int N, typename STATE, typename DATA> +template <typename T> +T& ftl::data::Frame<BASE,N,STATE,DATA>::get(ftl::codecs::Channel channel) { + if (channel == ftl::codecs::Channel::None) { + throw ftl::exception("Attempting to get channel 'None'"); + } + + // Add channel if not already there + if (!channels_.has(channel)) { + throw ftl::exception(ftl::Formatter() << "Frame channel does not exist: " << (int)channel); + } + + return getData(channel).template as<T>(); +} + +template <int BASE, int N, typename STATE, typename DATA> +template <typename T> +const T& ftl::data::Frame<BASE,N,STATE,DATA>::get(ftl::codecs::Channel channel) const { + if (channel == ftl::codecs::Channel::None) { + throw ftl::exception("Attempting to get channel 'None'"); + } else if (channel == ftl::codecs::Channel::Pose) { + return state_.template as<T,ftl::codecs::Channel::Pose>(); + } else if (channel == ftl::codecs::Channel::Calibration) { + return state_.template as<T,ftl::codecs::Channel::Calibration>(); + } else if (channel == ftl::codecs::Channel::Calibration2) { + return state_.template as<T,ftl::codecs::Channel::Calibration2>(); + } else if (channel == ftl::codecs::Channel::Configuration) { + return state_.template as<T,ftl::codecs::Channel::Configuration>(); + } + + // Add channel if not already there + if (!channels_.has(channel)) { + throw ftl::exception(ftl::Formatter() << "Frame channel does not exist: " << (int)channel); + } + + return getData(channel).template as<T>(); +} + +// Default data channel implementation +template <int BASE, int N, typename STATE, typename DATA> +template <typename T> +void ftl::data::Frame<BASE,N,STATE,DATA>::get(ftl::codecs::Channel channel, T ¶ms) const { + if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw ftl::exception("Cannot use generic type with non data channel"); + if (!hasChannel(channel)) throw ftl::exception("Data channel does not exist"); + + const auto &i = data_data_.find(static_cast<int>(channel)); + if (i == data_data_.end()) throw ftl::exception("Data channel does not exist"); + + auto unpacked = msgpack::unpack((const char*)(*i).second.data(), (*i).second.size()); + unpacked.get().convert(params); +} + +template <int BASE, int N, typename STATE, typename DATA> +template <typename T> +T &ftl::data::Frame<BASE,N,STATE,DATA>::create(ftl::codecs::Channel c) { + if (c == ftl::codecs::Channel::None) { + throw ftl::exception("Cannot create a None channel"); + } + channels_ += c; + + auto &m = getData(c); + return m.template make<T>(); +} + +template <int BASE, int N, typename STATE, typename DATA> +template <typename T> +void ftl::data::Frame<BASE,N,STATE,DATA>::create(ftl::codecs::Channel channel, const T &value) { + if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw ftl::exception("Cannot use generic type with non data channel"); + + data_channels_ += channel; + + auto &v = *std::get<0>(data_data_.insert({static_cast<int>(channel),{}})); + v.second.resize(0); + ftl::util::FTLVectorBuffer buf(v.second); + msgpack::pack(buf, value); +} + +template <int BASE, int N, typename STATE, typename DATA> +void ftl::data::Frame<BASE,N,STATE,DATA>::setOrigin(STATE *state) { + if (origin_ != nullptr) { + throw ftl::exception("Can only set origin once after reset"); + } + + origin_ = state; + state_ = *state; +} + +template <int BASE, int N, typename STATE, typename DATA> +const Eigen::Matrix4d &ftl::data::Frame<BASE,N,STATE,DATA>::getPose() const { + return get<Eigen::Matrix4d>(ftl::codecs::Channel::Pose); +} + +template <int BASE, int N, typename STATE, typename DATA> +const typename STATE::Settings &ftl::data::Frame<BASE,N,STATE,DATA>::getLeft() const { + return get<typename STATE::Settings>(ftl::codecs::Channel::Calibration); +} + +template <int BASE, int N, typename STATE, typename DATA> +const typename STATE::Settings &ftl::data::Frame<BASE,N,STATE,DATA>::getRight() const { + return get<typename STATE::Settings>(ftl::codecs::Channel::Calibration2); +} + +template <int BASE, int N, typename STATE, typename DATA> +void ftl::data::Frame<BASE,N,STATE,DATA>::setPose(const Eigen::Matrix4d &pose) { + if (origin_) origin_->setPose(pose); +} + +template <int BASE, int N, typename STATE, typename DATA> +void ftl::data::Frame<BASE,N,STATE,DATA>::setLeft(const typename STATE::Settings &c) { + if (origin_) origin_->setLeft(c); +} + +template <int BASE, int N, typename STATE, typename DATA> +void ftl::data::Frame<BASE,N,STATE,DATA>::setRight(const typename STATE::Settings &c) { + if (origin_) origin_->setRight(c); +} + +template <int BASE, int N, typename STATE, typename DATA> +std::string ftl::data::Frame<BASE,N,STATE,DATA>::getConfigString() const { + return get<nlohmann::json>(ftl::codecs::Channel::Configuration).dump(); +} + +template <int BASE, int N, typename STATE, typename DATA> +const std::vector<unsigned char> &ftl::data::Frame<BASE,N,STATE,DATA>::getRawData(ftl::codecs::Channel channel) const { + if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw ftl::exception("Non data channel"); + if (!hasChannel(channel)) throw ftl::exception("Data channel does not exist"); + + return data_data_.at(static_cast<int>(channel)); +} + +template <int BASE, int N, typename STATE, typename DATA> +void ftl::data::Frame<BASE,N,STATE,DATA>::createRawData(ftl::codecs::Channel c, const std::vector<unsigned char> &v) { + data_data_.insert({static_cast<int>(c), v}); + data_channels_ += c; +} + +#endif // _FTL_DATA_FRAME_HPP_ diff --git a/components/structures/include/ftl/data/frameset.hpp b/components/structures/include/ftl/data/frameset.hpp new file mode 100644 index 0000000000000000000000000000000000000000..e33ee2e660f80fd0e648d56722c9f4ab81bebdaf --- /dev/null +++ b/components/structures/include/ftl/data/frameset.hpp @@ -0,0 +1,96 @@ +#ifndef _FTL_DATA_FRAMESET_HPP_ +#define _FTL_DATA_FRAMESET_HPP_ + +#include <ftl/threads.hpp> +#include <ftl/timer.hpp> +#include <ftl/data/frame.hpp> +#include <functional> + +//#include <opencv2/opencv.hpp> +#include <vector> + +namespace ftl { +namespace data { + +// Allows a latency of 20 frames maximum +//static const size_t kMaxFramesets = 15; +static const size_t kMaxFramesInSet = 32; + +/** + * Represents a set of synchronised frames, each with two channels. This is + * used to collect all frames from multiple computers that have the same + * timestamp. + */ +template <typename FRAME> +struct FrameSet { + int id=0; + int64_t timestamp; // Millisecond timestamp of all frames + std::vector<FRAME> frames; + std::atomic<int> count; // Number of valid frames + std::atomic<unsigned int> mask; // Mask of all sources that contributed + bool stale; // True if buffers have been invalidated + SHARED_MUTEX mtx; + + /** + * Upload all specified host memory channels to GPU memory. + */ + //void upload(ftl::codecs::Channels<0>, cudaStream_t stream=0); + + /** + * Download all specified GPU memory channels to host memory. + */ + //void download(ftl::codecs::Channels<0>, cudaStream_t stream=0); + + /** + * Move the entire frameset to another frameset object. This will + * invalidate the current frameset object as all memory buffers will be + * moved. + */ + void swapTo(ftl::data::FrameSet<FRAME> &); + + /** + * Clear all channels and all memory allocations within those channels. + * This will perform a resetFull on all frames in the frameset. + */ + //void resetFull(); + + typedef FRAME Frame; + typedef std::function<bool(ftl::data::FrameSet<FRAME> &)> Callback; +}; + +/** + * Callback type for receiving video frames. + */ +//typedef std::function<bool(ftl::rgbd::FrameSet &)> VideoCallback; + +/** + * Abstract class for any generator of FrameSet structures. A generator + * produces (decoded) frame sets at regular frame intervals depending on the + * global timer settings. The `onFrameSet` callback may be triggered from any + * thread and also may drop frames and not be called for a given timestamp. + */ +template <typename FRAMESET> +class Generator { + public: + Generator() {} + virtual ~Generator() {} + + /** Number of frames in last frameset. This can change over time. */ + virtual size_t size()=0; + + /** + * Get the persistent state object for a frame. An exception is thrown + * for a bad index. + */ + virtual typename FRAMESET::Frame::State &state(int ix)=0; + + inline typename FRAMESET::Frame::State &operator[](int ix) { return state(ix); } + + /** Register a callback to receive new frame sets. */ + virtual void onFrameSet(const typename FRAMESET::Callback &)=0; +}; + +} +} + +#endif // _FTL_DATA_FRAMESET_HPP_ diff --git a/components/structures/include/ftl/data/framestate.hpp b/components/structures/include/ftl/data/framestate.hpp new file mode 100644 index 0000000000000000000000000000000000000000..7d7255f60b68492b440634539a08fe6c0ac3afe0 --- /dev/null +++ b/components/structures/include/ftl/data/framestate.hpp @@ -0,0 +1,298 @@ +#ifndef _FTL_DATA_FRAMESTATE_HPP_ +#define _FTL_DATA_FRAMESTATE_HPP_ + +#include <nlohmann/json.hpp> +#include <ftl/exception.hpp> +#include <ftl/codecs/channels.hpp> +#include <Eigen/Eigen> +#include <array> +#include <optional> +#include <string> + +namespace ftl { +namespace data { + +/** + * Represent state that is persistent across frames. Such state may or may not + * change from one frame to the next so a record of what has changed must be + * kept. Changing state should be done at origin and not in the frame. State + * that is marked as changed will then be send into a stream and the changed + * status will be cleared, allowing data to only be sent/saved when actual + * changes occur. + * + * The provided SETTINGS type must support MsgPack and be copyable. An example + * of settings is camera intrinsics. + * + * COUNT is the number of settings channels available. For example, video state + * has two settings channels, one for left camera and one for right camera. + */ +template <typename SETTINGS, int COUNT> +class FrameState { + public: + typedef SETTINGS Settings; + + FrameState(); + FrameState(FrameState &); + FrameState(FrameState &&); + + /** + * Update the pose and mark as changed. + */ + void setPose(const Eigen::Matrix4d &pose) { + pose_ = pose; + changed_ += ftl::codecs::Channel::Pose; + } + + /** + * Update the left settings and mark as changed. + */ + void setLeft(const SETTINGS &p) { + static_assert(COUNT > 0, "No settings channel"); + settings_[0] = p; + changed_ += ftl::codecs::Channel::Settings1; + } + + /** + * Update the right settings and mark as changed. + */ + void setRight(const SETTINGS &p) { + static_assert(COUNT > 1, "No second settings channel"); + settings_[1] = p; + changed_ += ftl::codecs::Channel::Settings2; + } + + /** + * Change settings using ID number. Necessary when more than 2 settings + * channels exist, otherwise use `setLeft` and `setRight`. + */ + template <int I> + void set(const SETTINGS &p) { + static_assert(I < COUNT, "Settings channel too large"); + settings_[I] = p; + changed_ += __idToChannel(I); + } + + /** + * Get the current pose. + */ + inline const Eigen::Matrix4d &getPose() const { return pose_; } + + /** + * Get the left settings. + */ + inline const SETTINGS &getLeft() const { return settings_[0]; } + + /** + * Get the right settings. + */ + inline const SETTINGS &getRight() const { return settings_[1]; } + + /** + * Get a modifiable pose reference that does not change the changed status. + * @attention Should only be used internally. + * @todo Make private eventually. + */ + inline Eigen::Matrix4d &getPose() { return pose_; } + + /** + * Get a modifiable left settings reference that does not change + * the changed status. Modifications made using this will not be propagated. + * @attention Should only be used internally. + * @todo Make private eventually. + */ + inline SETTINGS &getLeft() { return settings_[0]; } + + /** + * Get a modifiable right settings reference that does not change + * the changed status. Modifications made using this will not be propagated. + * @attention Should only be used internally. + * @todo Make private eventually. + */ + inline SETTINGS &getRight() { return settings_[1]; } + + /** + * Get a named config property. + */ + template <typename T> + std::optional<T> get(const std::string &name) { + try { + return config_[name].get<T>(); + } catch (...) { + return {}; + } + } + + /** + * Helper class to specialising channel based state access. + * @private + */ + template <typename T, ftl::codecs::Channel C, typename S, int N> struct As { + static const T &func(const ftl::data::FrameState<S,N> &t) { + throw ftl::exception("Type not supported for state channel"); + } + + static T &func(ftl::data::FrameState<S,N> &t) { + throw ftl::exception("Type not supported for state channel"); + } + }; + + // Specialise for pose + template <typename S, int N> + struct As<Eigen::Matrix4d,ftl::codecs::Channel::Pose,S,N> { + static const Eigen::Matrix4d &func(const ftl::data::FrameState<S,N> &t) { + return t.pose_; + } + + static Eigen::Matrix4d &func(ftl::data::FrameState<S,N> &t) { + return t.pose_; + } + }; + + // Specialise for settings 1 + template <typename S, int N> + struct As<S,ftl::codecs::Channel::Settings1,S,N> { + static const S &func(const ftl::data::FrameState<S,N> &t) { + return t.settings_[0]; + } + + static S &func(ftl::data::FrameState<S,N> &t) { + return t.settings_[0]; + } + }; + + // Specialise for settings 2 + template <typename S, int N> + struct As<S,ftl::codecs::Channel::Settings2,S,N> { + static const S &func(const ftl::data::FrameState<S,N> &t) { + return t.settings_[1]; + } + + static S &func(ftl::data::FrameState<S,N> &t) { + return t.settings_[1]; + } + }; + + // Specialise for config + template <typename S, int N> + struct As<nlohmann::json,ftl::codecs::Channel::Configuration,S,N> { + static const nlohmann::json &func(const ftl::data::FrameState<S,N> &t) { + return t.config_; + } + + static nlohmann::json &func(ftl::data::FrameState<S,N> &t) { + return t.config_; + } + }; + + /** + * Allow access to state items using a known channel number. By default + * these throw an exception unless specialised to accept a particular type + * for a particular channel. The specialisations are automatic for pose, + * config and SETTINGS items. + */ + template <typename T, ftl::codecs::Channel C> + T &as() { return As<T,C,SETTINGS,COUNT>::func(*this); } + + /** + * Allow access to state items using a known channel number. By default + * these throw an exception unless specialised to accept a particular type + * for a particular channel. The specialisations are automatic for pose, + * config and SETTINGS items. + */ + template <typename T, ftl::codecs::Channel C> + const T &as() const { + return As<T,C,SETTINGS,COUNT>::func(*this); + } + + /** + * Set a named config property. Also makes state as changed to be resent. + */ + template <typename T> + void set(const std::string &name, T value) { + config_[name] = value; + changed_ += ftl::codecs::Channel::Configuration; + } + + inline const nlohmann::json &getConfig() const { return config_; } + + inline nlohmann::json &getConfig() { return config_; } + + /** + * Check if pose or settings have been modified and not yet forwarded. + * Once forwarded through a pipeline / stream the changed status is cleared. + */ + inline bool hasChanged(ftl::codecs::Channel c) const { return changed_.has(c); } + + /** + * Copy assignment will clear the changed status of the original. + */ + FrameState &operator=(FrameState &); + + FrameState &operator=(FrameState &&); + + /** + * Clear the changed status to unchanged. + */ + inline void clear() { changed_.clear(); } + + private: + Eigen::Matrix4d pose_; + std::array<SETTINGS,COUNT> settings_; + nlohmann::json config_; + ftl::codecs::Channels<64> changed_; // Have the state channels changed? + + static inline ftl::codecs::Channel __idToChannel(int id) { + return (id == 0) ? ftl::codecs::Channel::Settings1 : (id == 1) ? + ftl::codecs::Channel::Settings2 : + static_cast<ftl::codecs::Channel>(static_cast<int>(ftl::codecs::Channel::Settings3)+(id-2)); + } +}; + +} +} + + +template <typename SETTINGS, int COUNT> +ftl::data::FrameState<SETTINGS,COUNT>::FrameState() : settings_({{0}}), config_(nlohmann::json::value_t::object) { + pose_ = Eigen::Matrix4d::Identity(); +} + +template <typename SETTINGS, int COUNT> +ftl::data::FrameState<SETTINGS,COUNT>::FrameState(ftl::data::FrameState<SETTINGS,COUNT> &f) { + pose_ = f.pose_; + settings_ = f.settings_; + changed_ = f.changed_; + config_ = f.config_; + f.changed_.clear(); +} + +template <typename SETTINGS, int COUNT> +ftl::data::FrameState<SETTINGS,COUNT>::FrameState(ftl::data::FrameState<SETTINGS,COUNT> &&f) { + pose_ = f.pose_; + settings_ = f.settings_; + changed_ = f.changed_; + config_ = std::move(f.config_); + f.changed_.clear(); +} + +template <typename SETTINGS, int COUNT> +ftl::data::FrameState<SETTINGS,COUNT> &ftl::data::FrameState<SETTINGS,COUNT>::operator=(ftl::data::FrameState<SETTINGS,COUNT> &f) { + pose_ = f.pose_; + settings_ = f.settings_; + changed_ = f.changed_; + config_ = f.config_; + f.changed_.clear(); + return *this; +} + +template <typename SETTINGS, int COUNT> +ftl::data::FrameState<SETTINGS,COUNT> &ftl::data::FrameState<SETTINGS,COUNT>::operator=(ftl::data::FrameState<SETTINGS,COUNT> &&f) { + pose_ = f.pose_; + settings_ = f.settings_; + changed_ = f.changed_; + config_ = std::move(f.config_); + f.changed_.clear(); + return *this; +} + +#endif // _FTL_DATA_FRAMESTATE_HPP_